EulerAngles.cpp (9623B)
1 // This file is part of Eigen, a lightweight C++ template library 2 // for linear algebra. 3 // 4 // Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com> 5 // 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 8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 10 #include "main.h" 11 12 #include <unsupported/Eigen/EulerAngles> 13 14 using namespace Eigen; 15 16 // Unfortunately, we need to specialize it in order to work. (We could add it in main.h test framework) 17 template <typename Scalar, class System> 18 bool verifyIsApprox(const Eigen::EulerAngles<Scalar, System>& a, const Eigen::EulerAngles<Scalar, System>& b) 19 { 20 return verifyIsApprox(a.angles(), b.angles()); 21 } 22 23 // Verify that x is in the approxed range [a, b] 24 #define VERIFY_APPROXED_RANGE(a, x, b) \ 25 do { \ 26 VERIFY_IS_APPROX_OR_LESS_THAN(a, x); \ 27 VERIFY_IS_APPROX_OR_LESS_THAN(x, b); \ 28 } while(0) 29 30 const char X = EULER_X; 31 const char Y = EULER_Y; 32 const char Z = EULER_Z; 33 34 template<typename Scalar, class EulerSystem> 35 void verify_euler(const EulerAngles<Scalar, EulerSystem>& e) 36 { 37 typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType; 38 typedef Matrix<Scalar,3,3> Matrix3; 39 typedef Matrix<Scalar,3,1> Vector3; 40 typedef Quaternion<Scalar> QuaternionType; 41 typedef AngleAxis<Scalar> AngleAxisType; 42 43 const Scalar ONE = Scalar(1); 44 const Scalar HALF_PI = Scalar(EIGEN_PI / 2); 45 const Scalar PI = Scalar(EIGEN_PI); 46 47 // It's very important calc the acceptable precision depending on the distance from the pole. 48 const Scalar longitudeRadius = std::abs( 49 EulerSystem::IsTaitBryan ? 50 std::cos(e.beta()) : 51 std::sin(e.beta()) 52 ); 53 Scalar precision = test_precision<Scalar>() / longitudeRadius; 54 55 Scalar betaRangeStart, betaRangeEnd; 56 if (EulerSystem::IsTaitBryan) 57 { 58 betaRangeStart = -HALF_PI; 59 betaRangeEnd = HALF_PI; 60 } 61 else 62 { 63 if (!EulerSystem::IsBetaOpposite) 64 { 65 betaRangeStart = 0; 66 betaRangeEnd = PI; 67 } 68 else 69 { 70 betaRangeStart = -PI; 71 betaRangeEnd = 0; 72 } 73 } 74 75 const Vector3 I_ = EulerAnglesType::AlphaAxisVector(); 76 const Vector3 J_ = EulerAnglesType::BetaAxisVector(); 77 const Vector3 K_ = EulerAnglesType::GammaAxisVector(); 78 79 // Is approx checks 80 VERIFY(e.isApprox(e)); 81 VERIFY_IS_APPROX(e, e); 82 VERIFY_IS_NOT_APPROX(e, EulerAnglesType(e.alpha() + ONE, e.beta() + ONE, e.gamma() + ONE)); 83 84 const Matrix3 m(e); 85 VERIFY_IS_APPROX(Scalar(m.determinant()), ONE); 86 87 EulerAnglesType ebis(m); 88 89 // When no roll(acting like polar representation), we have the best precision. 90 // One of those cases is when the Euler angles are on the pole, and because it's singular case, 91 // the computation returns no roll. 92 if (ebis.beta() == 0) 93 precision = test_precision<Scalar>(); 94 95 // Check that eabis in range 96 VERIFY_APPROXED_RANGE(-PI, ebis.alpha(), PI); 97 VERIFY_APPROXED_RANGE(betaRangeStart, ebis.beta(), betaRangeEnd); 98 VERIFY_APPROXED_RANGE(-PI, ebis.gamma(), PI); 99 100 const Matrix3 mbis(AngleAxisType(ebis.alpha(), I_) * AngleAxisType(ebis.beta(), J_) * AngleAxisType(ebis.gamma(), K_)); 101 VERIFY_IS_APPROX(Scalar(mbis.determinant()), ONE); 102 VERIFY_IS_APPROX(mbis, ebis.toRotationMatrix()); 103 /*std::cout << "===================\n" << 104 "e: " << e << std::endl << 105 "eabis: " << eabis.transpose() << std::endl << 106 "m: " << m << std::endl << 107 "mbis: " << mbis << std::endl << 108 "X: " << (m * Vector3::UnitX()).transpose() << std::endl << 109 "X: " << (mbis * Vector3::UnitX()).transpose() << std::endl;*/ 110 VERIFY(m.isApprox(mbis, precision)); 111 112 // Test if ea and eabis are the same 113 // Need to check both singular and non-singular cases 114 // There are two singular cases. 115 // 1. When I==K and sin(ea(1)) == 0 116 // 2. When I!=K and cos(ea(1)) == 0 117 118 // TODO: Make this test work well, and use range saturation function. 119 /*// If I==K, and ea[1]==0, then there no unique solution. 120 // The remark apply in the case where I!=K, and |ea[1]| is close to +-pi/2. 121 if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(EIGEN_PI/2),test_precision<Scalar>())) ) 122 VERIFY_IS_APPROX(ea, eabis);*/ 123 124 // Quaternions 125 const QuaternionType q(e); 126 ebis = q; 127 const QuaternionType qbis(ebis); 128 VERIFY(internal::isApprox<Scalar>(std::abs(q.dot(qbis)), ONE, precision)); 129 //VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same 130 131 // A suggestion for simple product test when will be supported. 132 /*EulerAnglesType e2(PI/2, PI/2, PI/2); 133 Matrix3 m2(e2); 134 VERIFY_IS_APPROX(e*e2, m*m2);*/ 135 } 136 137 template<signed char A, signed char B, signed char C, typename Scalar> 138 void verify_euler_vec(const Matrix<Scalar,3,1>& ea) 139 { 140 verify_euler(EulerAngles<Scalar, EulerSystem<A, B, C> >(ea[0], ea[1], ea[2])); 141 } 142 143 template<signed char A, signed char B, signed char C, typename Scalar> 144 void verify_euler_all_neg(const Matrix<Scalar,3,1>& ea) 145 { 146 verify_euler_vec<+A,+B,+C>(ea); 147 verify_euler_vec<+A,+B,-C>(ea); 148 verify_euler_vec<+A,-B,+C>(ea); 149 verify_euler_vec<+A,-B,-C>(ea); 150 151 verify_euler_vec<-A,+B,+C>(ea); 152 verify_euler_vec<-A,+B,-C>(ea); 153 verify_euler_vec<-A,-B,+C>(ea); 154 verify_euler_vec<-A,-B,-C>(ea); 155 } 156 157 template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea) 158 { 159 verify_euler_all_neg<X,Y,Z>(ea); 160 verify_euler_all_neg<X,Y,X>(ea); 161 verify_euler_all_neg<X,Z,Y>(ea); 162 verify_euler_all_neg<X,Z,X>(ea); 163 164 verify_euler_all_neg<Y,Z,X>(ea); 165 verify_euler_all_neg<Y,Z,Y>(ea); 166 verify_euler_all_neg<Y,X,Z>(ea); 167 verify_euler_all_neg<Y,X,Y>(ea); 168 169 verify_euler_all_neg<Z,X,Y>(ea); 170 verify_euler_all_neg<Z,X,Z>(ea); 171 verify_euler_all_neg<Z,Y,X>(ea); 172 verify_euler_all_neg<Z,Y,Z>(ea); 173 } 174 175 template<typename Scalar> void check_singular_cases(const Scalar& singularBeta) 176 { 177 typedef Matrix<Scalar,3,1> Vector3; 178 const Scalar PI = Scalar(EIGEN_PI); 179 180 for (Scalar epsilon = NumTraits<Scalar>::epsilon(); epsilon < 1; epsilon *= Scalar(1.2)) 181 { 182 check_all_var(Vector3(PI/4, singularBeta, PI/3)); 183 check_all_var(Vector3(PI/4, singularBeta - epsilon, PI/3)); 184 check_all_var(Vector3(PI/4, singularBeta - Scalar(1.5)*epsilon, PI/3)); 185 check_all_var(Vector3(PI/4, singularBeta - 2*epsilon, PI/3)); 186 check_all_var(Vector3(PI*Scalar(0.8), singularBeta - epsilon, Scalar(0.9)*PI)); 187 check_all_var(Vector3(PI*Scalar(-0.9), singularBeta + epsilon, PI*Scalar(0.3))); 188 check_all_var(Vector3(PI*Scalar(-0.6), singularBeta + Scalar(1.5)*epsilon, PI*Scalar(0.3))); 189 check_all_var(Vector3(PI*Scalar(-0.5), singularBeta + 2*epsilon, PI*Scalar(0.4))); 190 check_all_var(Vector3(PI*Scalar(0.9), singularBeta + epsilon, Scalar(0.8)*PI)); 191 } 192 193 // This one for sanity, it had a problem with near pole cases in float scalar. 194 check_all_var(Vector3(PI*Scalar(0.8), singularBeta - Scalar(1E-6), Scalar(0.9)*PI)); 195 } 196 197 template<typename Scalar> void eulerangles_manual() 198 { 199 typedef Matrix<Scalar,3,1> Vector3; 200 typedef Matrix<Scalar,Dynamic,1> VectorX; 201 const Vector3 Zero = Vector3::Zero(); 202 const Scalar PI = Scalar(EIGEN_PI); 203 204 check_all_var(Zero); 205 206 // singular cases 207 check_singular_cases(PI/2); 208 check_singular_cases(-PI/2); 209 210 check_singular_cases(Scalar(0)); 211 check_singular_cases(Scalar(-0)); 212 213 check_singular_cases(PI); 214 check_singular_cases(-PI); 215 216 // non-singular cases 217 VectorX alpha = VectorX::LinSpaced(20, Scalar(-0.99) * PI, PI); 218 VectorX beta = VectorX::LinSpaced(20, Scalar(-0.49) * PI, Scalar(0.49) * PI); 219 VectorX gamma = VectorX::LinSpaced(20, Scalar(-0.99) * PI, PI); 220 for (int i = 0; i < alpha.size(); ++i) { 221 for (int j = 0; j < beta.size(); ++j) { 222 for (int k = 0; k < gamma.size(); ++k) { 223 check_all_var(Vector3(alpha(i), beta(j), gamma(k))); 224 } 225 } 226 } 227 } 228 229 template<typename Scalar> void eulerangles_rand() 230 { 231 typedef Matrix<Scalar,3,3> Matrix3; 232 typedef Matrix<Scalar,3,1> Vector3; 233 typedef Array<Scalar,3,1> Array3; 234 typedef Quaternion<Scalar> Quaternionx; 235 typedef AngleAxis<Scalar> AngleAxisType; 236 237 Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); 238 Quaternionx q1; 239 q1 = AngleAxisType(a, Vector3::Random().normalized()); 240 Matrix3 m; 241 m = q1; 242 243 Vector3 ea = m.eulerAngles(0,1,2); 244 check_all_var(ea); 245 ea = m.eulerAngles(0,1,0); 246 check_all_var(ea); 247 248 // Check with purely random Quaternion: 249 q1.coeffs() = Quaternionx::Coefficients::Random().normalized(); 250 m = q1; 251 ea = m.eulerAngles(0,1,2); 252 check_all_var(ea); 253 ea = m.eulerAngles(0,1,0); 254 check_all_var(ea); 255 256 // Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi]. 257 ea = (Array3::Random() + Array3(1,0,0))*Scalar(EIGEN_PI)*Array3(0.5,1,1); 258 check_all_var(ea); 259 260 ea[2] = ea[0] = internal::random<Scalar>(0,Scalar(EIGEN_PI)); 261 check_all_var(ea); 262 263 ea[0] = ea[1] = internal::random<Scalar>(0,Scalar(EIGEN_PI)); 264 check_all_var(ea); 265 266 ea[1] = 0; 267 check_all_var(ea); 268 269 ea.head(2).setZero(); 270 check_all_var(ea); 271 272 ea.setZero(); 273 check_all_var(ea); 274 } 275 276 EIGEN_DECLARE_TEST(EulerAngles) 277 { 278 // Simple cast test 279 EulerAnglesXYZd onesEd(1, 1, 1); 280 EulerAnglesXYZf onesEf = onesEd.cast<float>(); 281 VERIFY_IS_APPROX(onesEd, onesEf.cast<double>()); 282 283 // Simple Construction from Vector3 test 284 VERIFY_IS_APPROX(onesEd, EulerAnglesXYZd(Vector3d::Ones())); 285 286 CALL_SUBTEST_1( eulerangles_manual<float>() ); 287 CALL_SUBTEST_2( eulerangles_manual<double>() ); 288 289 for(int i = 0; i < g_repeat; i++) { 290 CALL_SUBTEST_3( eulerangles_rand<float>() ); 291 CALL_SUBTEST_4( eulerangles_rand<double>() ); 292 } 293 294 // TODO: Add tests for auto diff 295 // TODO: Add tests for complex numbers 296 }