Hat and Vee in navigation
							parent
							
								
									e0c3be9ab7
								
							
						
					
					
						commit
						8cf3d04336
					
				|  | @ -270,6 +270,29 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { | |||
|   return J; | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| Matrix5 NavState::Hat(const Vector9& xi) { | ||||
|   Matrix5 X; | ||||
|   const double wx = xi(0), wy = xi(1), wz = xi(2); | ||||
|   const double px = xi(3), py = xi(4), pz = xi(5); | ||||
|   const double vx = xi(6), vy = xi(7), vz = xi(8); | ||||
|   X << 0., -wz, wy, px, vx, | ||||
|     wz, 0., -wx, py, vy, | ||||
|     -wy, wx, 0., pz, vz, | ||||
|     0., 0., 0., 0., 0., | ||||
|     0., 0., 0., 0., 0.; | ||||
|   return X; | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| Vector9 NavState::Vee(const Matrix5& Xi) { | ||||
|   Vector9 xi; | ||||
|   xi << Xi(2, 1), Xi(0, 2), Xi(1, 0), | ||||
|     Xi(0, 3), Xi(1, 3), Xi(2, 3), | ||||
|     Xi(0, 4), Xi(1, 4), Xi(2, 4); | ||||
|   return xi; | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, | ||||
|                                           ChartJacobian Hxi) { | ||||
|  |  | |||
|  | @ -56,18 +56,27 @@ public: | |||
|   NavState() : | ||||
|       t_(0, 0, 0), v_(Vector3::Zero()) { | ||||
|   } | ||||
| 
 | ||||
|   /// Construct from attitude, position, velocity
 | ||||
|   NavState(const Rot3& R, const Point3& t, const Velocity3& v) : | ||||
|       R_(R), t_(t), v_(v) { | ||||
|   } | ||||
| 
 | ||||
|   /// Construct from pose and velocity
 | ||||
|   NavState(const Pose3& pose, const Velocity3& v) : | ||||
|       R_(pose.rotation()), t_(pose.translation()), v_(v) { | ||||
|   } | ||||
| 
 | ||||
|   /// Construct from SO(3) and R^6
 | ||||
|   NavState(const Matrix3& R, const Vector6& tv) : | ||||
|       R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { | ||||
|   } | ||||
| 
 | ||||
|   /// Construct from Matrix5
 | ||||
|   NavState(const Matrix5& T) : | ||||
|     R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 3)), v_(T.block<3, 1>(0, 4)) { | ||||
|   } | ||||
| 
 | ||||
|   /// Named constructor with derivatives
 | ||||
|   static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v, | ||||
|                          OptionalJacobian<9, 3> H1 = {}, | ||||
|  | @ -154,10 +163,6 @@ public: | |||
|   /// Syntactic sugar
 | ||||
|   const Rot3& rotation() const { return attitude(); }; | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Lie Group
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   // Tangent space sugar.
 | ||||
|   // TODO(frank): move to private navstate namespace in cpp
 | ||||
|   static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) { | ||||
|  | @ -189,6 +194,12 @@ public: | |||
|       OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = | ||||
|           {}) const; | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Lie Group
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   using LieAlgebra = Matrix5; | ||||
| 
 | ||||
|   /**
 | ||||
|    * Exponential map at identity - create a NavState from canonical coordinates | ||||
|    * \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ | ||||
|  | @ -242,6 +253,12 @@ public: | |||
|     static Vector9 Local(const NavState& state, ChartJacobian Hstate = {}); | ||||
|   }; | ||||
| 
 | ||||
|   /// Hat maps from tangent vector to Lie algebra
 | ||||
|   static LieAlgebra Hat(const TangentVector& xi); | ||||
| 
 | ||||
|   /// Vee maps from Lie algebra to tangent vector
 | ||||
|   static TangentVector Vee(const LieAlgebra& X); | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Dynamics
 | ||||
|   /// @{
 | ||||
|  |  | |||
|  | @ -54,9 +54,24 @@ class NavState { | |||
|   gtsam::Vector velocity() const; | ||||
|   gtsam::Pose3 pose() const; | ||||
| 
 | ||||
|   // Manifold | ||||
|   gtsam::NavState retract(const gtsam::Vector& x) const; | ||||
|   gtsam::Vector localCoordinates(const gtsam::NavState& g) const; | ||||
| 
 | ||||
|   // Lie Group | ||||
|   static gtsam::NavState Expmap(gtsam::Vector v); | ||||
|   static gtsam::Vector Logmap(const gtsam::NavState& p); | ||||
|   static gtsam::NavState Expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H); | ||||
|   static gtsam::Vector Logmap(const gtsam::NavState& p, Eigen::Ref<Eigen::MatrixXd> H); | ||||
|   gtsam::NavState expmap(gtsam::Vector v); | ||||
|   gtsam::NavState expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2); | ||||
|   gtsam::Vector logmap(const gtsam::NavState& p); | ||||
|   gtsam::Vector logmap(const gtsam::NavState& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2); | ||||
|   gtsam::Matrix AdjointMap() const; | ||||
|   gtsam::Vector Adjoint(gtsam::Vector xi) const; | ||||
|   static gtsam::Matrix Hat(const gtsam::Vector& xi); | ||||
|   static gtsam::Vector Vee(const gtsam::Matrix& xi); | ||||
| 
 | ||||
|   // enabling serialization functionality | ||||
|   void serialize() const; | ||||
| }; | ||||
|  |  | |||
|  | @ -533,6 +533,63 @@ TEST(NavState, Adjoint_compose_full) { | |||
|   EXPECT(assert_equal(expected, actual, 1e-6)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(NavState, HatAndVee) { | ||||
|   // Create a few test vectors
 | ||||
|   Vector9 v1(1, 2, 3, 4, 5, 6, 7, 8, 9); | ||||
|   Vector9 v2(0.1, -0.5, 1.0, -1.0, 0.5, 2.0, 0.3, -0.2, 0.8); | ||||
|   Vector9 v3 = Vector9::Zero(); | ||||
| 
 | ||||
|   // Test that Vee(Hat(v)) == v for various inputs
 | ||||
|   EXPECT(assert_equal(v1, NavState::Vee(NavState::Hat(v1)))); | ||||
|   EXPECT(assert_equal(v2, NavState::Vee(NavState::Hat(v2)))); | ||||
|   EXPECT(assert_equal(v3, NavState::Vee(NavState::Hat(v3)))); | ||||
| 
 | ||||
|   // Check the structure of the Lie Algebra element
 | ||||
|   Matrix5 expected; | ||||
|   expected << 0, -3, 2, 4, 7, | ||||
|     3, 0, -1, 5, 8, | ||||
|     -2, 1, 0, 6, 9, | ||||
|     0, 0, 0, 0, 0, | ||||
|     0, 0, 0, 0, 0; | ||||
| 
 | ||||
|   EXPECT(assert_equal(expected, NavState::Hat(v1))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Checks correct exponential map (Expmap) with brute force matrix exponential
 | ||||
| TEST(NavState, BruteForceExpmap1) { | ||||
|   const Vector9 xi(0, 0, 0, 14, 24, 34, 15, 25, 35); | ||||
|   EXPECT(assert_equal(NavState::Expmap(xi), expm<NavState>(xi), 1e-6)); | ||||
| } | ||||
| 
 | ||||
| TEST(NavState, BruteForceExpmap2) { | ||||
|   const Vector9 xi(0.1, 0.2, 0.3, 0, 0, 0, 0, 0, 0); | ||||
|   EXPECT(assert_equal(NavState::Expmap(xi), expm<NavState>(xi), 1e-6)); | ||||
| } | ||||
| 
 | ||||
| TEST(NavState, BruteForceExpmap3) { | ||||
|   const Vector9 xi(0.1, 0.2, 0.3, 4, 5, 6, 7, 8, 9); | ||||
|   EXPECT(assert_equal(NavState::Expmap(xi), expm<NavState>(xi), 1e-6)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // assert that T*Hat(xi)*T^-1 is equal to Hat(Ad_T(xi))
 | ||||
| TEST(NavState, Adjoint_hat) | ||||
| { | ||||
|   Matrix5 expected = T.matrix() * NavState::Hat(screwNavState::xi) * T.matrix().inverse(); | ||||
|   Matrix5 xiprime = NavState::Hat(T.Adjoint(screwNavState::xi)); | ||||
|   EXPECT(assert_equal(expected, xiprime, 1e-6)); | ||||
| 
 | ||||
|   Matrix5 expected2 = T2.matrix() * NavState::Hat(screwNavState::xi) * T2.matrix().inverse(); | ||||
|   Matrix5 xiprime2 = NavState::Hat(T2.Adjoint(screwNavState::xi)); | ||||
|   EXPECT(assert_equal(expected2, xiprime2, 1e-6)); | ||||
| 
 | ||||
|   Matrix5 expected3 = T3.matrix() * NavState::Hat(screwNavState::xi) * T3.matrix().inverse(); | ||||
|   Matrix5 xiprime3 = NavState::Hat(T3.Adjoint(screwNavState::xi)); | ||||
|   EXPECT(assert_equal(expected3, xiprime3, 1e-6)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(NavState, Retract_LocalCoordinates) { | ||||
|   Vector9 d; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue