Attachment #342 for bug #609

This bugzilla service is closed. All entries have been migrated to https://gitlab.com/libeigen/eigen

View | Details | Raw Unified | Return to bug 609
Collapse All | Expand All




  * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode
  * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that
  * we have the following equality:
  * \code
  * mat == AngleAxisf(ea[0], Vector3f::UnitZ())
  *      * AngleAxisf(ea[1], Vector3f::UnitX())
  *      * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
  * This corresponds to the right-multiply conventions (with right hand side frames).
  * 
  * The returned angles are in the ranges [0:pi]x[0:pi]x[-pi:pi].
  * 
  * \sa class AngleAxis
  */
template<typename Derived>
inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
{
  using std::atan2;
  using std::sin;
  using std::cos;
  /* Implemented from Graphics Gems IV */
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)

  Matrix<Scalar,3,1> res;
  typedef Matrix<typename Derived::Scalar,2,1> Vector2;
  const Scalar epsilon = NumTraits<Scalar>::dummy_precision();

  const Index odd = ((a0+1)%3 == a1) ? 0 : 1;
  const Index i = a0;
  const Index j = (a0 + 1 + odd)%3;
  const Index k = (a0 + 2 - odd)%3;
  
  if (a0==a2)
  {
    res[0] = atan2(coeff(j,i), coeff(k,i));
    if((odd && res[0]<0) || ((!odd) && res[0]>0))
    if (s > epsilon)
    {
      res[0] = (res[0] > 0) ? res[0] - M_PI : res[0] +  M_PI;
      Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm();
      res[1] = -atan2(s2, coeff(i,i));
    }
    else
    {
      Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm();
      res[1] = atan2(s2, coeff(i,i));
    }
    
    // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles,
    // we can compute their respective rotation, and apply its inverse to M. Since the result must
    // be a rotation around x, we have:
    //
    //  c2  s1.s2 c1.s2                   1  0   0 
    //  0   c1    -s1       *    M    =   0  c3  s3
    //  -s2 s1.c2 c1.c2                   0 -s3  c3
    //
    //  Thus:  m11.c1 - m21.s1 = c3  &   m12.c1 - m22.s1 = s3
    
    Scalar s1 = sin(res[0]);
    Scalar c1 = cos(res[0]);
    res[2] = atan2(c1*coeff(j,k)-s1*coeff(k,k), c1*coeff(j,j) - s1 * coeff(k,j));
  } 
  else
  {
    res[0] = atan2(coeff(j,k), coeff(k,k));
    Scalar c2 = Vector2(coeff(i,i), coeff(i,j)).norm();
    if((odd && res[0]<0) || ((!odd) && res[0]>0)) {
      res[0] = (res[0] > 0) ? res[0] - M_PI : res[0] +  M_PI;
      res[1] = atan2(-coeff(i,k), -c2);
      res[2] = atan2(coeff(i,j), coeff(i,i));
    }
    else
      res[1] = atan2(-coeff(i,k), c2);
    Scalar s1 = sin(res[0]);
    Scalar c1 = cos(res[0]);
    res[2] = atan2(s1*coeff(k,i)-c1*coeff(j,i), c1*coeff(j,j) - s1 * coeff(k,j));
  }
  if (!odd)
    res = -res;
  
  return res;
}

} // end namespace Eigen

#endif // EIGEN_EULERANGLES_H




// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>

template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
{
  typedef Matrix<Scalar,3,3> Matrix3;
  typedef Matrix<Scalar,3,1> Vector3;
  typedef Quaternion<Scalar> Quaternionx;
  typedef AngleAxis<Scalar> AngleAxisx;
  
  Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
  Quaternionx q1;
  q1 = AngleAxisx(a, Vector3::Random().normalized());
  Matrix3 m;
  m = q1;

  #define VERIFY_EULER(I,J,K, X,Y,Z) { \
    Matrix3 m(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \
    Vector3 eabis = m.eulerAngles(I,J,K); \
    Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit##X()) * AngleAxisx(eabis[1], Vector3::Unit##Y()) * AngleAxisx(eabis[2], Vector3::Unit##Z())); \
    VERIFY_IS_APPROX(m,  mbis); \
    if(I!=K || ea[1]!=0) VERIFY_IS_APPROX(ea, eabis); \
  }
  VERIFY_EULER(0,1,2, X,Y,Z);
  VERIFY_EULER(0,1,0, X,Y,X);
  VERIFY_EULER(0,2,1, X,Z,Y);
  VERIFY_EULER(0,2,0, X,Z,X);

  VERIFY_EULER(1,2,0, Y,Z,X);
  VERIFY_EULER(1,2,1, Y,Z,Y);

  VERIFY_EULER(1,0,1, Y,X,Y);

  VERIFY_EULER(2,0,1, Z,X,Y);
  VERIFY_EULER(2,0,2, Z,X,Z);
  VERIFY_EULER(2,1,0, Z,Y,X);
  VERIFY_EULER(2,1,2, Z,Y,Z);
}

template<typename Scalar> void eulerangles(void)
{
  typedef Matrix<Scalar,3,3> Matrix3;
  typedef Matrix<Scalar,3,1> Vector3;
  typedef Array<Scalar,3,1> Array3;
  typedef Quaternion<Scalar> Quaternionx;
  typedef AngleAxis<Scalar> AngleAxisx;

  Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
  Quaternionx q1;
  q1 = AngleAxisx(a, Vector3::Random().normalized());
  Matrix3 m;
  m = q1;
  
  Vector3 ea = m.eulerAngles(0,1,2);
  check_all_var(ea);
  ea = m.eulerAngles(0,1,0);
  check_all_var(ea);
  
  ea = (Array3::Random() + Array3(1,1,0))*M_PI*Array3(0.5,0.5,1);
  check_all_var(ea);
  
  ea[2] = ea[0] = internal::random<Scalar>(0,M_PI);
  check_all_var(ea);
  
  ea[0] = ea[1] = internal::random<Scalar>(0,M_PI);
  check_all_var(ea);
  
  ea[1] = 0;
  check_all_var(ea);
  
  ea.head(2).setZero();
  check_all_var(ea);
  
  ea.setZero();
  check_all_var(ea);
}

void test_geo_eulerangles()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( eulerangles<float>() );
    CALL_SUBTEST_2( eulerangles<double>() );
  }
}

Return to bug 609

Lines 22-85 namespace Eigen { Link Here

(-)a/Eigen/src/Geometry/EulerAngles.h (-19 / +39 lines)
22	* \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode	22	* \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode
23	* "2" represents the z axis and "0" the x axis, etc. The returned angles are such that	23	* "2" represents the z axis and "0" the x axis, etc. The returned angles are such that
24	* we have the following equality:	24	* we have the following equality:
25	* \code	25	* \code
26	* mat == AngleAxisf(ea[0], Vector3f::UnitZ())	26	* mat == AngleAxisf(ea[0], Vector3f::UnitZ())
27	* * AngleAxisf(ea[1], Vector3f::UnitX())	27	* * AngleAxisf(ea[1], Vector3f::UnitX())
28	* * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode	28	* * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
29	* This corresponds to the right-multiply conventions (with right hand side frames).	29	* This corresponds to the right-multiply conventions (with right hand side frames).
		30	*
		31	* The returned angles are in the ranges [0:pi]x[0:pi]x[-pi:pi].
		32	*
		33	* \sa class AngleAxis
30	*/	34	*/
31	template<typename Derived>	35	template<typename Derived>
32	inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>	36	inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
33	MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const	37	MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
34	{	38	{
35	using std::atan2;	39	using std::atan2;
		40	using std::sin;
		41	using std::cos;
36	/* Implemented from Graphics Gems IV */	42	/* Implemented from Graphics Gems IV */
37	EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)	43	EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)
38		44
39	Matrix<Scalar,3,1> res;	45	Matrix<Scalar,3,1> res;
40	typedef Matrix<typename Derived::Scalar,2,1> Vector2;	46	typedef Matrix<typename Derived::Scalar,2,1> Vector2;
41	const Scalar epsilon = NumTraits<Scalar>::dummy_precision();	47	const Scalar epsilon = NumTraits<Scalar>::dummy_precision();
42		48
43	const Index odd = ((a0+1)%3 == a1) ? 0 : 1;	49	const Index odd = ((a0+1)%3 == a1) ? 0 : 1;
44	const Index i = a0;	50	const Index i = a0;
45	const Index j = (a0 + 1 + odd)%3;	51	const Index j = (a0 + 1 + odd)%3;
46	const Index k = (a0 + 2 - odd)%3;	52	const Index k = (a0 + 2 - odd)%3;
47		53
48	if (a0==a2)	54	if (a0==a2)
49	{	55	{
50	Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm();	56	res[0] = atan2(coeff(j,i), coeff(k,i));
51	res[1] = atan2(s, coeff(i,i));	57	if((odd && res[0]<0) \|\| ((!odd) && res[0]>0))
52	if (s > epsilon)
53	{	58	{
54	res[0] = atan2(coeff(j,i), coeff(k,i));	59	res[0] = (res[0] > 0) ? res[0] - M_PI : res[0] + M_PI;
55	res[2] = atan2(coeff(i,j),-coeff(i,k));	60	Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm();
		61	res[1] = -atan2(s2, coeff(i,i));
56	}	62	}
57	else	63	else
58	{	64	{
59	res[0] = Scalar(0);	65	Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm();
60	res[2] = (coeff(i,i)>0?1:-1)*atan2(-coeff(k,j), coeff(j,j));	66	res[1] = atan2(s2, coeff(i,i));
61	}	67	}
62	}	68
		69	// With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles,
		70	// we can compute their respective rotation, and apply its inverse to M. Since the result must
		71	// be a rotation around x, we have:
		72	//
		73	// c2 s1.s2 c1.s2 1 0 0
		74	// 0 c1 -s1 * M = 0 c3 s3
		75	// -s2 s1.c2 c1.c2 0 -s3 c3
		76	//
		77	// Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3
		78
		79	Scalar s1 = sin(res[0]);
		80	Scalar c1 = cos(res[0]);
		81	res[2] = atan2(c1coeff(j,k)-s1coeff(k,k), c1coeff(j,j) - s1 coeff(k,j));
		82	}
63	else	83	else
64	{	84	{
65	Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm();	85	res[0] = atan2(coeff(j,k), coeff(k,k));
66	res[1] = atan2(-coeff(i,k), c);	86	Scalar c2 = Vector2(coeff(i,i), coeff(i,j)).norm();
67	if (c > epsilon)	87	if((odd && res[0]<0) \|\| ((!odd) && res[0]>0)) {
68	{	88	res[0] = (res[0] > 0) ? res[0] - M_PI : res[0] + M_PI;
69	res[0] = atan2(coeff(j,k), coeff(k,k));	89	res[1] = atan2(-coeff(i,k), -c2);
70	res[2] = atan2(coeff(i,j), coeff(i,i));
71	}	90	}
72	else	91	else
73	{	92	res[1] = atan2(-coeff(i,k), c2);
74	res[0] = Scalar(0);	93	Scalar s1 = sin(res[0]);
75	res[2] = (coeff(i,k)>0?1:-1)*atan2(-coeff(k,j), coeff(j,j));	94	Scalar c1 = cos(res[0]);
76	}	95	res[2] = atan2(s1coeff(k,i)-c1coeff(j,i), c1coeff(j,j) - s1 coeff(k,j));
77	}	96	}
78	if (!odd)	97	if (!odd)
79	res = -res;	98	res = -res;
		99
80	return res;	100	return res;
81	}	101	}
82		102
83	} // end namespace Eigen	103	} // end namespace Eigen
84		104
85	#endif // EIGEN_EULERANGLES_H	105	#endif // EIGEN_EULERANGLES_H

Lines 1-38 Link Here

(-)a/test/geo_eulerangles.cpp (-12 / +46 lines)
1	// This file is part of Eigen, a lightweight C++ template library	1	// This file is part of Eigen, a lightweight C++ template library
2	// for linear algebra.	2	// for linear algebra.
3	//	3	//
4	// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>	4	// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
5	//	5	//
6	// This Source Code Form is subject to the terms of the Mozilla	6	// This Source Code Form is subject to the terms of the Mozilla
7	// Public License v. 2.0. If a copy of the MPL was not distributed	7	// Public License v. 2.0. If a copy of the MPL was not distributed
8	// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.	8	// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9		9
10	#include "main.h"	10	#include "main.h"
11	#include <Eigen/Geometry>	11	#include <Eigen/Geometry>
12	#include <Eigen/LU>	12	#include <Eigen/LU>
13	#include <Eigen/SVD>	13	#include <Eigen/SVD>
14		14
15	template<typename Scalar> void eulerangles(void)	15	template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
16	{	16	{
17	typedef Matrix<Scalar,3,3> Matrix3;	17	typedef Matrix<Scalar,3,3> Matrix3;
18	typedef Matrix<Scalar,3,1> Vector3;	18	typedef Matrix<Scalar,3,1> Vector3;
19	typedef Quaternion<Scalar> Quaternionx;
20	typedef AngleAxis<Scalar> AngleAxisx;	19	typedef AngleAxis<Scalar> AngleAxisx;
21		20
22	Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
23	Quaternionx q1;
24	q1 = AngleAxisx(a, Vector3::Random().normalized());
25	Matrix3 m;
26	m = q1;
27
28	#define VERIFY_EULER(I,J,K, X,Y,Z) { \	21	#define VERIFY_EULER(I,J,K, X,Y,Z) { \
29	Vector3 ea = m.eulerAngles(I,J,K); \	22	Matrix3 m(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \
30	VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \	23	Vector3 eabis = m.eulerAngles(I,J,K); \
		24	Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit##X()) * AngleAxisx(eabis[1], Vector3::Unit##Y()) * AngleAxisx(eabis[2], Vector3::Unit##Z())); \
		25	VERIFY_IS_APPROX(m, mbis); \
		26	if(I!=K \|\| ea[1]!=0) VERIFY_IS_APPROX(ea, eabis); \
31	}	27	}
32	VERIFY_EULER(0,1,2, X,Y,Z);	28	VERIFY_EULER(0,1,2, X,Y,Z);
33	VERIFY_EULER(0,1,0, X,Y,X);	29	VERIFY_EULER(0,1,0, X,Y,X);
34	VERIFY_EULER(0,2,1, X,Z,Y);	30	VERIFY_EULER(0,2,1, X,Z,Y);
35	VERIFY_EULER(0,2,0, X,Z,X);	31	VERIFY_EULER(0,2,0, X,Z,X);
36		32
37	VERIFY_EULER(1,2,0, Y,Z,X);	33	VERIFY_EULER(1,2,0, Y,Z,X);
38	VERIFY_EULER(1,2,1, Y,Z,Y);	34	VERIFY_EULER(1,2,1, Y,Z,Y);
Lines 40-54 template<typename Scalar> void eulerangl Link Here
40	VERIFY_EULER(1,0,1, Y,X,Y);	36	VERIFY_EULER(1,0,1, Y,X,Y);
41		37
42	VERIFY_EULER(2,0,1, Z,X,Y);	38	VERIFY_EULER(2,0,1, Z,X,Y);
43	VERIFY_EULER(2,0,2, Z,X,Z);	39	VERIFY_EULER(2,0,2, Z,X,Z);
44	VERIFY_EULER(2,1,0, Z,Y,X);	40	VERIFY_EULER(2,1,0, Z,Y,X);
45	VERIFY_EULER(2,1,2, Z,Y,Z);	41	VERIFY_EULER(2,1,2, Z,Y,Z);
46	}	42	}
47		43
		44	template<typename Scalar> void eulerangles(void)
		45	{
		46	typedef Matrix<Scalar,3,3> Matrix3;
		47	typedef Matrix<Scalar,3,1> Vector3;
		48	typedef Array<Scalar,3,1> Array3;
		49	typedef Quaternion<Scalar> Quaternionx;
		50	typedef AngleAxis<Scalar> AngleAxisx;
		51
		52	Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
		53	Quaternionx q1;
		54	q1 = AngleAxisx(a, Vector3::Random().normalized());
		55	Matrix3 m;
		56	m = q1;
		57
		58	Vector3 ea = m.eulerAngles(0,1,2);
		59	check_all_var(ea);
		60	ea = m.eulerAngles(0,1,0);
		61	check_all_var(ea);
		62
		63	ea = (Array3::Random() + Array3(1,1,0))M_PIArray3(0.5,0.5,1);
		64	check_all_var(ea);
		65
		66	ea[2] = ea[0] = internal::random<Scalar>(0,M_PI);
		67	check_all_var(ea);
		68
		69	ea[0] = ea[1] = internal::random<Scalar>(0,M_PI);
		70	check_all_var(ea);
		71
		72	ea[1] = 0;
		73	check_all_var(ea);
		74
		75	ea.head(2).setZero();
		76	check_all_var(ea);
		77
		78	ea.setZero();
		79	check_all_var(ea);
		80	}
		81
48	void test_geo_eulerangles()	82	void test_geo_eulerangles()
49	{	83	{
50	for(int i = 0; i < g_repeat; i++) {	84	for(int i = 0; i < g_repeat; i++) {
51	CALL_SUBTEST_1( eulerangles<float>() );	85	CALL_SUBTEST_1( eulerangles<float>() );
52	CALL_SUBTEST_2( eulerangles<double>() );	86	CALL_SUBTEST_2( eulerangles<double>() );
53	}	87	}
54	}	88	}