All tests for SO3 now uncommented
							parent
							
								
									f7ad80673c
								
							
						
					
					
						commit
						d376d0158d
					
				|  | @ -115,7 +115,7 @@ typename SO<N>::MatrixDD SO<N>::ExpmapDerivative(const TangentVector& omega) { | |||
| 
 | ||||
| template <int N> | ||||
| typename SO<N>::TangentVector SO<N>::Logmap(const SO& R, ChartJacobian H) { | ||||
|   throw std::runtime_error("SO<N>::Logmap only implemented for SO3 and SO4."); | ||||
|   throw std::runtime_error("SO<N>::Logmap only implemented for SO3."); | ||||
| } | ||||
| 
 | ||||
| template <int N> | ||||
|  |  | |||
|  | @ -182,9 +182,10 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { | |||
|   } | ||||
| } | ||||
| 
 | ||||
| // Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
 | ||||
| //   return sot::DexpFunctor(omega).dexp();
 | ||||
| // }
 | ||||
| template <> | ||||
| Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { | ||||
|   return sot::DexpFunctor(omega).dexp(); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| /* Right Jacobian for Log map in SO(3) - equation (10.86) and following
 | ||||
|  | @ -254,19 +255,22 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { | |||
|     omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); | ||||
|   } | ||||
| 
 | ||||
|   if (H) *H = SO3::LogmapDerivative(omega); | ||||
|   if (H) *H = LogmapDerivative(omega); | ||||
|   return omega; | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| // local vectorize
 | ||||
| static Vector9 vec3(const Matrix3& R) { | ||||
|   return Eigen::Map<const Vector9>(R.data()); | ||||
| } | ||||
| 
 | ||||
| // so<3> generators
 | ||||
| static const std::vector<const Matrix3> G3({SO3::Hat(Vector3::Unit(0)), | ||||
|                                             SO3::Hat(Vector3::Unit(1)), | ||||
|                                             SO3::Hat(Vector3::Unit(2))}); | ||||
| 
 | ||||
| // vectorized generators
 | ||||
| static const Matrix93 P3 = | ||||
|     (Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished(); | ||||
| 
 | ||||
|  |  | |||
|  | @ -42,6 +42,15 @@ using SO3 = SO<3>; | |||
| //   static Matrix3 Hat(const Vector3 &xi); ///< make skew symmetric matrix
 | ||||
| //   static Vector3 Vee(const Matrix3 &X);  ///< inverse of Hat
 | ||||
| 
 | ||||
| // Below are all declarations of SO<3> specializations.
 | ||||
| // They are *defined* in SO3.cpp.
 | ||||
| 
 | ||||
| /// Adjoint map
 | ||||
| template <> | ||||
| Matrix3 SO3::AdjointMap() const { | ||||
|   return matrix_; | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Exponential map at identity - create a rotation from canonical coordinates | ||||
|  * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula | ||||
|  | @ -49,13 +58,9 @@ using SO3 = SO<3>; | |||
| template <> | ||||
| SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H); | ||||
| 
 | ||||
| // template<>
 | ||||
| // Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
 | ||||
| //   return sot::DexpFunctor(omega).dexp();
 | ||||
| // }
 | ||||
| 
 | ||||
| /// Derivative of Expmap
 | ||||
| //   static Matrix3 ExpmapDerivative(const Vector3& omega);
 | ||||
| template <> | ||||
| Matrix3 SO3::ExpmapDerivative(const Vector3& omega); | ||||
| 
 | ||||
| /**
 | ||||
|  * Log map at identity - returns the canonical coordinates | ||||
|  | @ -64,10 +69,9 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H); | |||
| template <> | ||||
| Vector3 SO3::Logmap(const SO3& R, ChartJacobian H); | ||||
| 
 | ||||
| /// Derivative of Logmap
 | ||||
| template <> | ||||
| Matrix3 SO3::AdjointMap() const { | ||||
|   return matrix_; | ||||
| } | ||||
| Matrix3 SO3::LogmapDerivative(const Vector3& omega); | ||||
| 
 | ||||
| // Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
 | ||||
| template <> | ||||
|  |  | |||
|  | @ -157,7 +157,7 @@ TEST(SO3, ExpmapDerivative) { | |||
|   EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| //******************************************************************************
 | ||||
| TEST(SO3, ExpmapDerivative2) { | ||||
|   const Vector3 theta(0.1, 0, 0.1); | ||||
|   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>( | ||||
|  | @ -168,7 +168,7 @@ TEST(SO3, ExpmapDerivative2) { | |||
|                      SO3::ExpmapDerivative(-theta))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| //******************************************************************************
 | ||||
| TEST(SO3, ExpmapDerivative3) { | ||||
|   const Vector3 theta(10, 20, 30); | ||||
|   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>( | ||||
|  | @ -179,7 +179,7 @@ TEST(SO3, ExpmapDerivative3) { | |||
|                      SO3::ExpmapDerivative(-theta))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| //******************************************************************************
 | ||||
| TEST(SO3, ExpmapDerivative4) { | ||||
|   // Iserles05an (Lie-group Methods) says:
 | ||||
|   // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
 | ||||
|  | @ -207,7 +207,7 @@ TEST(SO3, ExpmapDerivative4) { | |||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| //******************************************************************************
 | ||||
| TEST(SO3, ExpmapDerivative5) { | ||||
|   auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; | ||||
|   auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; | ||||
|  | @ -223,7 +223,7 @@ TEST(SO3, ExpmapDerivative5) { | |||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| //******************************************************************************
 | ||||
| TEST(SO3, ExpmapDerivative6) { | ||||
|   const Vector3 thetahat(0.1, 0, 0.1); | ||||
|   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>( | ||||
|  | @ -233,7 +233,7 @@ TEST(SO3, ExpmapDerivative6) { | |||
|   EXPECT(assert_equal(Jexpected, Jactual)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| //******************************************************************************
 | ||||
| TEST(SO3, LogmapDerivative) { | ||||
|   const Vector3 thetahat(0.1, 0, 0.1); | ||||
|   const SO3 R = SO3::Expmap(thetahat);  // some rotation
 | ||||
|  | @ -243,7 +243,7 @@ TEST(SO3, LogmapDerivative) { | |||
|   EXPECT(assert_equal(Jexpected, Jactual)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| //******************************************************************************
 | ||||
| TEST(SO3, JacobianLogmap) { | ||||
|   const Vector3 thetahat(0.1, 0, 0.1); | ||||
|   const SO3 R = SO3::Expmap(thetahat);  // some rotation
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue