commit
						6a765f2452
					
				|  | @ -171,21 +171,21 @@ namespace internal { | |||
| /// Assumes existence of: identity, dimension, localCoordinates, retract,
 | ||||
| /// and additionally Logmap, Expmap, compose, between, and inverse
 | ||||
| template<class Class> | ||||
| struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> { | ||||
|   typedef lie_group_tag structure_category; | ||||
| struct LieGroupTraits: public GetDimensionImpl<Class, Class::dimension> { | ||||
|   using structure_category = lie_group_tag; | ||||
| 
 | ||||
|   /// @name Group
 | ||||
|   /// @{
 | ||||
|   typedef multiplicative_group_tag group_flavor; | ||||
|   using group_flavor = multiplicative_group_tag; | ||||
|   static Class Identity() { return Class::Identity();} | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name Manifold
 | ||||
|   /// @{
 | ||||
|   typedef Class ManifoldType; | ||||
|   using ManifoldType = Class; | ||||
|   inline constexpr static auto dimension = Class::dimension; | ||||
|   typedef Eigen::Matrix<double, dimension, 1> TangentVector; | ||||
|   typedef OptionalJacobian<dimension, dimension> ChartJacobian; | ||||
|   using TangentVector = Eigen::Matrix<double, dimension, 1>; | ||||
|   using ChartJacobian = OptionalJacobian<dimension, dimension>; | ||||
| 
 | ||||
|   static TangentVector Local(const Class& origin, const Class& other, | ||||
|       ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { | ||||
|  | @ -225,10 +225,28 @@ struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> { | |||
|   /// @}
 | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
| /// Both LieGroupTraits and Testable
 | ||||
| template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {}; | ||||
| 
 | ||||
| } // \ namepsace internal
 | ||||
| /// Adds LieAlgebra, Hat, and Vie to LieGroupTraits
 | ||||
| template<class Class> struct MatrixLieGroupTraits: LieGroupTraits<Class> { | ||||
|   using LieAlgebra = typename Class::LieAlgebra; | ||||
|   using TangentVector = typename LieGroupTraits<Class>::TangentVector; | ||||
| 
 | ||||
|   static LieAlgebra Hat(const TangentVector& v) { | ||||
|     return Class::Hat(v); | ||||
|   } | ||||
| 
 | ||||
|   static TangentVector Vee(const LieAlgebra& X) { | ||||
|     return Class::Vee(X); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| /// Both LieGroupTraits and Testable
 | ||||
| template<class Class> struct MatrixLieGroup: MatrixLieGroupTraits<Class>, Testable<Class> {}; | ||||
| 
 | ||||
| } // \ namespace internal
 | ||||
| 
 | ||||
| /**
 | ||||
|  * These core global functions can be specialized by new Lie types | ||||
|  | @ -269,7 +287,7 @@ public: | |||
|         (std::is_base_of<lie_group_tag, structure_category_tag>::value), | ||||
|         "This type's trait does not assert it is a Lie group (or derived)"); | ||||
| 
 | ||||
|     // group opertations with Jacobians
 | ||||
|     // group operations with Jacobians
 | ||||
|     g = traits<T>::Compose(g, h, Hg, Hh); | ||||
|     g = traits<T>::Between(g, h, Hg, Hh); | ||||
|     g = traits<T>::Inverse(g, Hg); | ||||
|  | @ -286,6 +304,25 @@ private: | |||
|   ChartJacobian Hg, Hh; | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * Matrix Lie Group Concept | ||||
|  */ | ||||
| template<typename T> | ||||
| class IsMatrixLieGroup: public IsLieGroup<T> { | ||||
| public: | ||||
| typedef typename traits<T>::LieAlgebra LieAlgebra; | ||||
| typedef typename traits<T>::TangentVector TangentVector; | ||||
| 
 | ||||
|   BOOST_CONCEPT_USAGE(IsMatrixLieGroup) { | ||||
|     // hat and vee
 | ||||
|     X = traits<T>::Hat(xi); | ||||
|     xi = traits<T>::Vee(X); | ||||
|   } | ||||
| private: | ||||
|   LieAlgebra X; | ||||
|   TangentVector xi; | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  *  Three term approximation of the Baker-Campbell-Hausdorff formula | ||||
|  *  In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) | ||||
|  | @ -301,22 +338,21 @@ T BCH(const T& X, const T& Y) { | |||
|   return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, bracket(X, X_Y))); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Declaration of wedge (see Murray94book) used to convert | ||||
|  * from n exponential coordinates to n*n element of the Lie algebra | ||||
|  */ | ||||
| #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 | ||||
| /// @deprecated: use T::Hat
 | ||||
| template <class T> Matrix wedge(const Vector& x); | ||||
| #endif | ||||
| 
 | ||||
| /**
 | ||||
|  * Exponential map given exponential coordinates | ||||
|  * class T needs a wedge<> function and a constructor from Matrix | ||||
|  * class T needs a constructor from Matrix. | ||||
|  * @param x exponential coordinates, vector of size n | ||||
|  * @ return a T | ||||
|  */ | ||||
| template <class T> | ||||
| T expm(const Vector& x, int K=7) { | ||||
|   Matrix xhat = wedge<T>(x); | ||||
|   return T(expm(xhat,K)); | ||||
| T expm(const Vector& x, int K = 7) { | ||||
|   auto xhat = T::Hat(x); | ||||
|   return T(expm(xhat, K)); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  |  | |||
|  | @ -51,6 +51,8 @@ struct VectorSpaceImpl { | |||
|   /// @name Lie Group
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   typedef Eigen::Matrix<double, N, 1> LieAlgebra; | ||||
| 
 | ||||
|   static TangentVector Logmap(const Class& m, ChartJacobian Hm = {}) { | ||||
|     if (Hm) *Hm = Jacobian::Identity(); | ||||
|     return m.vector(); | ||||
|  | @ -80,6 +82,13 @@ struct VectorSpaceImpl { | |||
|     return -v; | ||||
|   } | ||||
| 
 | ||||
|   static LieAlgebra Hat(const TangentVector& v) { | ||||
|     return v; | ||||
|   } | ||||
| 
 | ||||
|   static TangentVector Vee(const LieAlgebra& X) { | ||||
|     return X; | ||||
|   } | ||||
|   /// @}
 | ||||
| }; | ||||
| 
 | ||||
|  | @ -411,6 +420,8 @@ struct DynamicTraits { | |||
| 
 | ||||
|   /// @name Lie Group
 | ||||
|   /// @{
 | ||||
|   using LieAlgebra = Dynamic; | ||||
|      | ||||
|   static TangentVector Logmap(const Dynamic& m, ChartJacobian H = {}) { | ||||
|     if (H) *H = Eye(m); | ||||
|     TangentVector result(GetDimension(m)); | ||||
|  | @ -441,6 +452,15 @@ struct DynamicTraits { | |||
|     if (H2) *H2 = Eye(v1); | ||||
|     return v2 - v1; | ||||
|   } | ||||
|    | ||||
|   static LieAlgebra Hat(const TangentVector& v) { | ||||
|     return v; | ||||
|   } | ||||
|    | ||||
|   static TangentVector Vee(const LieAlgebra& X) { | ||||
|     return X; | ||||
|   } | ||||
|    | ||||
|   /// @}
 | ||||
| 
 | ||||
| }; | ||||
|  |  | |||
|  | @ -203,6 +203,20 @@ Pose2 Pose2::inverse() const { | |||
|   return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Matrix3 Pose2::Hat(const Pose2::TangentVector& xi) { | ||||
|   Matrix3 X; | ||||
|   X << 0., -xi.z(), xi.x(), | ||||
|     xi.z(), 0., xi.y(), | ||||
|     0., 0., 0.; | ||||
|   return X; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Pose2::TangentVector Pose2::Vee(const Matrix3& X) { | ||||
|   return TangentVector(X(0, 2), X(1, 2), X(1,0)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // see doc/math.lyx, SE(2) section
 | ||||
| Point2 Pose2::transformTo(const Point2& point, | ||||
|  |  | |||
|  | @ -40,9 +40,12 @@ class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> { | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   /** Pose Concept requirements */ | ||||
|   typedef Rot2 Rotation; | ||||
|   typedef Point2 Translation; | ||||
|   /// Pose Concept requirements
 | ||||
|   using Rotation = Rot2; | ||||
|   using Translation = Point2; | ||||
| 
 | ||||
|   /// LieGroup Concept requirements
 | ||||
|   using LieAlgebra = Matrix3; | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|  | @ -183,21 +186,6 @@ public: | |||
|   static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);} | ||||
|   static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);} | ||||
| 
 | ||||
|   /**
 | ||||
|    * wedge for SE(2): | ||||
|    * @param xi 3-dim twist (v,omega) where | ||||
|    *  omega is angular velocity | ||||
|    *  v (vx,vy) = 2D velocity | ||||
|    * @return xihat, 3*3 element of Lie algebra that can be exponentiated | ||||
|    */ | ||||
|   static inline Matrix3 wedge(double vx, double vy, double w) { | ||||
|     Matrix3 m; | ||||
|     m << 0.,-w,  vx, | ||||
|          w,  0., vy, | ||||
|          0., 0.,  0.; | ||||
|     return m; | ||||
|   } | ||||
| 
 | ||||
|   /// Derivative of Expmap
 | ||||
|   static Matrix3 ExpmapDerivative(const Vector3& v); | ||||
| 
 | ||||
|  | @ -212,6 +200,12 @@ public: | |||
| 
 | ||||
|   using LieGroup<Pose2, 3>::inverse; // version with derivative
 | ||||
| 
 | ||||
|   /// Hat maps from tangent vector to Lie algebra
 | ||||
|   static Matrix3 Hat(const Vector3& xi); | ||||
| 
 | ||||
|   /// Vee maps from Lie algebra to tangent vector
 | ||||
|   static Vector3 Vee(const Matrix3& X); | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Group Action on Point2
 | ||||
|   /// @{
 | ||||
|  | @ -339,6 +333,16 @@ public: | |||
|   friend std::ostream &operator<<(std::ostream &os, const Pose2& p); | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name deprecated
 | ||||
|   /// @{
 | ||||
| 
 | ||||
| #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 | ||||
|   /// @deprecated: use Hat
 | ||||
|   static inline Matrix3 wedge(double vx, double vy, double w) { | ||||
|     return Hat(TangentVector(vx, vy, w)); | ||||
|   } | ||||
| #endif | ||||
|   /// @}
 | ||||
| 
 | ||||
|  private: | ||||
| 
 | ||||
|  | @ -357,22 +361,24 @@ public: | |||
|   GTSAM_MAKE_ALIGNED_OPERATOR_NEW | ||||
| }; // Pose2
 | ||||
| 
 | ||||
| /** specialization for pose2 wedge function (generic template in Lie.h) */ | ||||
| #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 | ||||
| /// @deprecated: use T::Hat
 | ||||
| template <> | ||||
| inline Matrix wedge<Pose2>(const Vector& xi) { | ||||
|   // NOTE(chris): Need eval() as workaround for Apple clang + avx2.
 | ||||
|   return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval(); | ||||
|   return Matrix(Pose2::Hat(xi)).eval(); | ||||
| } | ||||
| #endif | ||||
| 
 | ||||
| // Convenience typedef
 | ||||
| using Pose2Pair = std::pair<Pose2, Pose2>; | ||||
| using Pose2Pairs = std::vector<Pose2Pair>; | ||||
| 
 | ||||
| template <> | ||||
| struct traits<Pose2> : public internal::LieGroup<Pose2> {}; | ||||
| struct traits<Pose2> : public internal::MatrixLieGroup<Pose2> {}; | ||||
| 
 | ||||
| template <> | ||||
| struct traits<const Pose2> : public internal::LieGroup<Pose2> {}; | ||||
| struct traits<const Pose2> : public internal::MatrixLieGroup<Pose2> {}; | ||||
| 
 | ||||
| // bearing and range traits, used in RangeFactor
 | ||||
| template <typename T> | ||||
|  |  | |||
|  | @ -160,6 +160,21 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, | |||
|   return adT_xi * y; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Matrix4 Pose3::Hat(const Vector6& xi) { | ||||
|   Matrix4 X; | ||||
|   const double wx = xi(0), wy = xi(1), wz = xi(2), vx = xi(3), vy = xi(4), vz = xi(5); | ||||
|   X << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.; | ||||
|   return X; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Vector6 Pose3::Vee(const Matrix4& Xi) { | ||||
|   Vector6 xi; | ||||
|   xi << Xi(2, 1), Xi(0, 2), Xi(1, 0), Xi(0, 3), Xi(1, 3), Xi(2, 3); | ||||
|   return xi; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void Pose3::print(const std::string& s) const { | ||||
|   std::cout << (s.empty() ? s : s + " ") << *this << std::endl; | ||||
|  |  | |||
|  | @ -141,6 +141,8 @@ public: | |||
|   /// @name Lie Group
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   using LieAlgebra = Matrix4; | ||||
| 
 | ||||
|   /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
 | ||||
|   static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {}); | ||||
| 
 | ||||
|  | @ -230,16 +232,16 @@ public: | |||
|   using LieGroup<Pose3, 6>::inverse; // version with derivative
 | ||||
| 
 | ||||
|   /**
 | ||||
|    * wedge for Pose3: | ||||
|    * Hat for Pose3: | ||||
|    * @param xi 6-dim twist (omega,v) where | ||||
|    *  omega = (wx,wy,wz) 3D angular velocity | ||||
|    *  v (vx,vy,vz) = 3D velocity | ||||
|    * @return xihat, 4*4 element of Lie algebra that can be exponentiated | ||||
|    */ | ||||
|   static Matrix wedge(double wx, double wy, double wz, double vx, double vy, | ||||
|       double vz) { | ||||
|     return (Matrix(4, 4) << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.).finished(); | ||||
|   } | ||||
|   static Matrix4 Hat(const Vector6& xi); | ||||
| 
 | ||||
|   /// Vee maps from Lie algebra to tangent vector
 | ||||
|   static Vector6 Vee(const Matrix4& X); | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Group Action on Point3
 | ||||
|  | @ -394,6 +396,19 @@ public: | |||
|   GTSAM_EXPORT | ||||
|   friend std::ostream &operator<<(std::ostream &os, const Pose3& p); | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name deprecated
 | ||||
|   /// @{
 | ||||
| 
 | ||||
| #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 | ||||
|   /// @deprecated: use Hat
 | ||||
|   static inline LieAlgebra wedge(double wx, double wy, double wz, double vx, double vy, | ||||
|     double vz) { | ||||
|     return Hat(TangentVector(wx, wy, wz, vx, vy, vz)); | ||||
|   } | ||||
| #endif | ||||
|   /// @}
 | ||||
| 
 | ||||
|  private: | ||||
| #if GTSAM_ENABLE_BOOST_SERIALIZATION | ||||
|   /** Serialization function */ | ||||
|  | @ -414,17 +429,14 @@ public: | |||
| }; | ||||
| // Pose3 class
 | ||||
| 
 | ||||
| /**
 | ||||
|  * wedge for Pose3: | ||||
|  * @param xi 6-dim twist (omega,v) where | ||||
|  *  omega = 3D angular velocity | ||||
|  *  v = 3D velocity | ||||
|  * @return xihat, 4*4 element of Lie algebra that can be exponentiated | ||||
|  */ | ||||
| #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 | ||||
| /// @deprecated: use T::Hat
 | ||||
| template<> | ||||
| inline Matrix wedge<Pose3>(const Vector& xi) { | ||||
|   return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); | ||||
|   // NOTE(chris): Need eval() as workaround for Apple clang + avx2.
 | ||||
|   return Matrix(Pose3::Hat(xi)).eval(); | ||||
| } | ||||
| #endif | ||||
| 
 | ||||
| // Convenience typedef
 | ||||
| using Pose3Pair = std::pair<Pose3, Pose3>; | ||||
|  | @ -434,10 +446,10 @@ using Pose3Pairs = std::vector<std::pair<Pose3, Pose3> >; | |||
| typedef std::vector<Pose3> Pose3Vector; | ||||
| 
 | ||||
| template <> | ||||
| struct traits<Pose3> : public internal::LieGroup<Pose3> {}; | ||||
| struct traits<Pose3> : public internal::MatrixLieGroup<Pose3> {}; | ||||
| 
 | ||||
| template <> | ||||
| struct traits<const Pose3> : public internal::LieGroup<Pose3> {}; | ||||
| struct traits<const Pose3> : public internal::MatrixLieGroup<Pose3> {}; | ||||
| 
 | ||||
| // bearing and range traits, used in RangeFactor
 | ||||
| template <> | ||||
|  |  | |||
|  | @ -138,6 +138,16 @@ struct traits<QUATERNION_TYPE> { | |||
|     return omega; | ||||
|   } | ||||
| 
 | ||||
|   using LieAlgebra = Matrix3; | ||||
| 
 | ||||
|   static Matrix3 Hat(const Vector3& v) { | ||||
|     return SO3::Hat(v); | ||||
|   } | ||||
|    | ||||
|   static Vector3 Vee(const Matrix3& X) { | ||||
|     return SO3::Vee(X); | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Manifold traits
 | ||||
|   /// @{
 | ||||
|  |  | |||
|  | @ -81,6 +81,21 @@ Vector1 Rot2::Logmap(const Rot2& r, OptionalJacobian<1, 1> H) { | |||
|   v << r.theta(); | ||||
|   return v; | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
| Matrix2 Rot2::Hat(const Vector1& xi) { | ||||
|   Matrix2 X; | ||||
|   X << 0., -xi.x(), | ||||
|     xi.x(), 0.; | ||||
|   return X; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Vector1 Rot2::Vee(const Matrix2& X) { | ||||
|   TangentVector v; | ||||
|   v << X(1, 0); | ||||
|   return v; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Matrix2 Rot2::matrix() const { | ||||
|   Matrix2 rvalue_; | ||||
|  |  | |||
|  | @ -33,7 +33,6 @@ namespace gtsam { | |||
|    * \nosubgrouping | ||||
|    */ | ||||
|   class GTSAM_EXPORT Rot2 : public LieGroup<Rot2, 1> { | ||||
| 
 | ||||
|     /** we store cos(theta) and sin(theta) */ | ||||
|     double c_, s_; | ||||
| 
 | ||||
|  | @ -125,6 +124,8 @@ namespace gtsam { | |||
|     /// @name Lie Group
 | ||||
|     /// @{
 | ||||
| 
 | ||||
|     using LieAlgebra = Matrix2; | ||||
| 
 | ||||
|     /// Exponential map at identity - create a rotation from canonical coordinates
 | ||||
|     static Rot2 Expmap(const Vector1& v, ChartJacobian H = {}); | ||||
| 
 | ||||
|  | @ -156,6 +157,12 @@ namespace gtsam { | |||
| 
 | ||||
|     using LieGroup<Rot2, 1>::inverse; // version with derivative
 | ||||
| 
 | ||||
|     /// Hat maps from tangent vector to Lie algebra
 | ||||
|     static Matrix2 Hat(const Vector1& xi); | ||||
| 
 | ||||
|     /// Vee maps from Lie algebra to tangent vector
 | ||||
|     static Vector1 Vee(const Matrix2& X); | ||||
| 
 | ||||
|     /// @}
 | ||||
|     /// @name Group Action on Point2
 | ||||
|     /// @{
 | ||||
|  | @ -216,7 +223,9 @@ namespace gtsam { | |||
|     /** Find closest valid rotation matrix, given a 2x2 matrix */ | ||||
|     static Rot2 ClosestTo(const Matrix2& M); | ||||
| 
 | ||||
|   private: | ||||
|     /// @}
 | ||||
| 
 | ||||
|     private: | ||||
| #if GTSAM_ENABLE_BOOST_SERIALIZATION | ||||
|     /** Serialization function */ | ||||
|     friend class boost::serialization::access; | ||||
|  | @ -230,9 +239,9 @@ namespace gtsam { | |||
|   }; | ||||
| 
 | ||||
|   template<> | ||||
|   struct traits<Rot2> : public internal::LieGroup<Rot2> {}; | ||||
|   struct traits<Rot2> : public internal::MatrixLieGroup<Rot2> {}; | ||||
| 
 | ||||
|   template<> | ||||
|   struct traits<const Rot2> : public internal::LieGroup<Rot2> {}; | ||||
|   struct traits<const Rot2> : public internal::MatrixLieGroup<Rot2> {}; | ||||
| 
 | ||||
| } // gtsam
 | ||||
|  |  | |||
|  | @ -371,6 +371,8 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> { | |||
|     /// @name Lie Group
 | ||||
|     /// @{
 | ||||
| 
 | ||||
|     using LieAlgebra = Matrix3; | ||||
| 
 | ||||
|     /**
 | ||||
|      * Exponential map at identity - create a rotation from canonical coordinates | ||||
|      * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula | ||||
|  | @ -407,6 +409,12 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> { | |||
| 
 | ||||
|     using LieGroup<Rot3, 3>::inverse; // version with derivative
 | ||||
| 
 | ||||
|     /// Hat maps from tangent vector to Lie algebra
 | ||||
|     static inline Matrix3 Hat(const Vector3& xi) { return SO3::Hat(xi); } | ||||
| 
 | ||||
|     /// Vee maps from Lie algebra to tangent vector
 | ||||
|     static inline Vector3 Vee(const Matrix3& X) { return SO3::Vee(X); } | ||||
| 
 | ||||
|     /// @}
 | ||||
|     /// @name Group Action on Point3
 | ||||
|     /// @{
 | ||||
|  | @ -581,10 +589,10 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> { | |||
|       const Matrix3& A, OptionalJacobian<3, 9> H = {}); | ||||
| 
 | ||||
|   template<> | ||||
|   struct traits<Rot3> : public internal::LieGroup<Rot3> {}; | ||||
|   struct traits<Rot3> : public internal::MatrixLieGroup<Rot3> {}; | ||||
| 
 | ||||
|   template<> | ||||
|   struct traits<const Rot3> : public internal::LieGroup<Rot3> {}; | ||||
|   struct traits<const Rot3> : public internal::MatrixLieGroup<Rot3> {}; | ||||
|    | ||||
| }  // namespace gtsam
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -224,13 +224,13 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { | |||
| }  //  namespace so3
 | ||||
| 
 | ||||
| /*
 | ||||
|  * Define the traits. internal::LieGroup provides both Lie group and Testable | ||||
|  * Define the traits. internal::MatrixLieGroup provides both Lie group and Testable | ||||
|  */ | ||||
| 
 | ||||
| template <> | ||||
| struct traits<SO3> : public internal::LieGroup<SO3> {}; | ||||
| struct traits<SO3> : public internal::MatrixLieGroup<SO3> {}; | ||||
| 
 | ||||
| template <> | ||||
| struct traits<const SO3> : public internal::LieGroup<SO3> {}; | ||||
| struct traits<const SO3> : public internal::MatrixLieGroup<SO3> {}; | ||||
| 
 | ||||
| }  // end namespace gtsam
 | ||||
|  |  | |||
|  | @ -106,13 +106,13 @@ void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) { | |||
| #endif | ||||
| 
 | ||||
| /*
 | ||||
|  * Define the traits. internal::LieGroup provides both Lie group and Testable | ||||
|  * Define the traits. internal::MatrixLieGroup provides both Lie group and Testable | ||||
|  */ | ||||
| 
 | ||||
| template <> | ||||
| struct traits<SO4> : public internal::LieGroup<SO4> {}; | ||||
| struct traits<SO4> : public internal::MatrixLieGroup<SO4> {}; | ||||
| 
 | ||||
| template <> | ||||
| struct traits<const SO4> : public internal::LieGroup<SO4> {}; | ||||
| struct traits<const SO4> : public internal::MatrixLieGroup<SO4> {}; | ||||
| 
 | ||||
| }  // end namespace gtsam
 | ||||
|  |  | |||
|  | @ -59,6 +59,9 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> { | |||
|   using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>; | ||||
|   using MatrixDD = Eigen::Matrix<double, dimension, dimension>; | ||||
| 
 | ||||
|   /// LieGroup Concept requirements
 | ||||
|   using LieAlgebra = MatrixNN; | ||||
| 
 | ||||
|   GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true) | ||||
| 
 | ||||
|  protected: | ||||
|  | @ -393,14 +396,14 @@ void serialize( | |||
| #endif | ||||
| 
 | ||||
| /*
 | ||||
|  * Define the traits. internal::LieGroup provides both Lie group and Testable | ||||
|  * Define the traits. internal::MatrixLieGroup provides both Lie group and Testable | ||||
|  */ | ||||
| 
 | ||||
| template <int N> | ||||
| struct traits<SO<N>> : public internal::LieGroup<SO<N>> {}; | ||||
| struct traits<SO<N>> : public internal::MatrixLieGroup<SO<N>> {}; | ||||
| 
 | ||||
| template <int N> | ||||
| struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {}; | ||||
| struct traits<const SO<N>> : public internal::MatrixLieGroup<SO<N>> {}; | ||||
| 
 | ||||
| }  // namespace gtsam
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -222,6 +222,24 @@ Matrix4 Similarity2::AdjointMap() const { | |||
|   throw std::runtime_error("Similarity2::AdjointMap not implemented"); | ||||
| } | ||||
| 
 | ||||
| Matrix3 Similarity2::Hat(const Vector4 &xi) { | ||||
|   const auto w = xi[2]; | ||||
|   const auto u = xi.head<2>(); | ||||
|   const double lambda = xi[3]; | ||||
|   Matrix3 W; | ||||
|   W << 0, -w, u[0], | ||||
|        w,  0, u[1], | ||||
|        0,  0, -lambda; | ||||
|   return W; | ||||
| } | ||||
| 
 | ||||
| Vector4 Similarity2::Vee(const Matrix3 &Xi) { | ||||
|   Vector4 xi; | ||||
|   xi[2] = Xi(1, 0); | ||||
|   xi.head<2>() = Xi.topRightCorner<2, 1>(); | ||||
|   xi[3] = -Xi(2, 2); | ||||
|   return xi; | ||||
| } | ||||
| std::ostream& operator<<(std::ostream& os, const Similarity2& p) { | ||||
|   os << "[" << p.rotation().theta() << " " << p.translation().transpose() << " " | ||||
|      << p.scale() << "]\';"; | ||||
|  |  | |||
|  | @ -139,6 +139,8 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> { | |||
|   /// @name Lie Group
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   using LieAlgebra = Matrix3; | ||||
| 
 | ||||
|   /**
 | ||||
|    * Log map at the identity | ||||
|    * \f$ [t_x, t_y, \delta, \lambda] \f$ | ||||
|  | @ -167,6 +169,12 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> { | |||
| 
 | ||||
|   using LieGroup<Similarity2, 4>::inverse; | ||||
| 
 | ||||
|   /// Hat maps from tangent vector to Lie algebra
 | ||||
|   static Matrix3 Hat(const Vector4& xi); | ||||
| 
 | ||||
|   /// Vee maps from Lie algebra to tangent vector
 | ||||
|   static Vector4 Vee(const Matrix3& X); | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Standard interface
 | ||||
|   /// @{
 | ||||
|  | @ -193,9 +201,9 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> { | |||
| }; | ||||
| 
 | ||||
| template <> | ||||
| struct traits<Similarity2> : public internal::LieGroup<Similarity2> {}; | ||||
| struct traits<Similarity2> : public internal::MatrixLieGroup<Similarity2> {}; | ||||
| 
 | ||||
| template <> | ||||
| struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {}; | ||||
| struct traits<const Similarity2> : public internal::MatrixLieGroup<Similarity2> {}; | ||||
| 
 | ||||
| }  // namespace gtsam
 | ||||
|  |  | |||
|  | @ -192,7 +192,7 @@ Similarity3 Similarity3::Align(const Pose3Pairs &abPosePairs) { | |||
|   return internal::alignGivenR(abPointPairs, aRb_estimate); | ||||
| } | ||||
| 
 | ||||
| Matrix4 Similarity3::wedge(const Vector7 &xi) { | ||||
| Matrix4 Similarity3::Hat(const Vector7 &xi) { | ||||
|   // http://www.ethaneade.org/latex2html/lie/node29.html
 | ||||
|   const auto w = xi.head<3>(); | ||||
|   const auto u = xi.segment<3>(3); | ||||
|  | @ -202,6 +202,14 @@ Matrix4 Similarity3::wedge(const Vector7 &xi) { | |||
|   return W; | ||||
| } | ||||
| 
 | ||||
| Vector7 Similarity3::Vee(const Matrix4 &Xi) { | ||||
|   Vector7 xi; | ||||
|   xi.head<3>() = Rot3::Vee(Xi.topLeftCorner<3, 3>()); | ||||
|   xi.segment<3>(3) = Xi.topRightCorner<3, 1>(); | ||||
|   xi[6] = -Xi(3, 3); | ||||
|   return xi; | ||||
| } | ||||
| 
 | ||||
| Matrix7 Similarity3::AdjointMap() const { | ||||
|   // http://www.ethaneade.org/latex2html/lie/node30.html
 | ||||
|   const Matrix3 R = R_.matrix(); | ||||
|  |  | |||
|  | @ -139,6 +139,8 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> { | |||
|   /// @name Lie Group
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   using LieAlgebra = Matrix4; | ||||
| 
 | ||||
|   /** Log map at the identity
 | ||||
|    * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$ | ||||
|    */ | ||||
|  | @ -164,17 +166,19 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> { | |||
| 
 | ||||
|   using LieGroup<Similarity3, 7>::inverse; | ||||
| 
 | ||||
|   /**
 | ||||
|    * wedge for Similarity3: | ||||
|    * @param xi 7-dim twist (w,u,lambda) where | ||||
|    * @return 4*4 element of Lie algebra that can be exponentiated | ||||
|    * TODO(frank): rename to Hat, make part of traits | ||||
|    */ | ||||
|   static Matrix4 wedge(const Vector7& xi); | ||||
| 
 | ||||
|   /// Project from one tangent space to another
 | ||||
|   Matrix7 AdjointMap() const; | ||||
| 
 | ||||
|   /**
 | ||||
|    * Hat for Similarity3: | ||||
|    * @param xi 7-dim twist (w,u,lambda) where | ||||
|    * @return 4*4 element of Lie algebra that can be exponentiated | ||||
|    */ | ||||
|   static Matrix4 Hat(const Vector7& xi); | ||||
| 
 | ||||
|   /// Vee maps from Lie algebra to tangent vector
 | ||||
|   static Vector7 Vee(const Matrix4& X); | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Standard interface
 | ||||
|   /// @{
 | ||||
|  | @ -197,6 +201,17 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> { | |||
|   /// Dimensionality of tangent space = 7 DOF
 | ||||
|   inline size_t dim() const { return 7; } | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Deprecated
 | ||||
|   /// @{
 | ||||
| 
 | ||||
| #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 | ||||
|   /// @deprecated: use Similarity3::Hat
 | ||||
|     static Matrix4 wedge(const Vector7& xi) { | ||||
|       return Similarity3::Hat(xi); | ||||
|     } | ||||
| #endif | ||||
|    | ||||
|   /// @}
 | ||||
|   /// @name Helper functions
 | ||||
|   /// @{
 | ||||
|  | @ -220,15 +235,17 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> { | |||
|   /// @}
 | ||||
| }; | ||||
| 
 | ||||
| #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 | ||||
| /// @deprecated: use Similarity3::Hat
 | ||||
| template <> | ||||
| inline Matrix wedge<Similarity3>(const Vector& xi) { | ||||
|   return Similarity3::wedge(xi); | ||||
|   return Similarity3::Hat(xi); | ||||
| } | ||||
| #endif | ||||
| template <> | ||||
| struct traits<Similarity3> : public internal::MatrixLieGroup<Similarity3> {}; | ||||
| 
 | ||||
| template <> | ||||
| struct traits<Similarity3> : public internal::LieGroup<Similarity3> {}; | ||||
| 
 | ||||
| template <> | ||||
| struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {}; | ||||
| struct traits<const Similarity3> : public internal::MatrixLieGroup<Similarity3> {}; | ||||
| 
 | ||||
| }  // namespace gtsam
 | ||||
|  |  | |||
|  | @ -174,7 +174,10 @@ class Rot2 { | |||
|   // Lie Group | ||||
|   static gtsam::Rot2 Expmap(gtsam::Vector v); | ||||
|   static gtsam::Vector Logmap(const gtsam::Rot2& p); | ||||
|   gtsam::Rot2 expmap(gtsam::Vector v); | ||||
|   gtsam::Vector logmap(const gtsam::Rot2& p); | ||||
|   static gtsam::Matrix Hat(const gtsam::Vector& xi); | ||||
|   static gtsam::Vector Vee(const gtsam::Matrix& xi); | ||||
| 
 | ||||
|   // Group Action on Point2 | ||||
|   gtsam::Point2 rotate(const gtsam::Point2& point) const; | ||||
|  | @ -216,6 +219,14 @@ class SO3 { | |||
|   // Operator Overloads | ||||
|   gtsam::SO3 operator*(const gtsam::SO3& R) const; | ||||
| 
 | ||||
|   // Lie Group | ||||
|   static gtsam::SO3 Expmap(gtsam::Vector v); | ||||
|   static gtsam::Vector Logmap(const gtsam::SO3& p); | ||||
|   gtsam::SO3 expmap(gtsam::Vector v); | ||||
|   gtsam::Vector logmap(const gtsam::SO3& p); | ||||
|   static gtsam::Matrix Hat(const gtsam::Vector& xi); | ||||
|   static gtsam::Vector Vee(const gtsam::Matrix& xi); | ||||
| 
 | ||||
|   // Manifold | ||||
|   gtsam::SO3 retract(gtsam::Vector v) const; | ||||
|   gtsam::Vector localCoordinates(const gtsam::SO3& R) const; | ||||
|  | @ -246,6 +257,14 @@ class SO4 { | |||
|   // Operator Overloads | ||||
|   gtsam::SO4 operator*(const gtsam::SO4& Q) const; | ||||
| 
 | ||||
|   // Lie Group | ||||
|   static gtsam::SO4 Expmap(gtsam::Vector v); | ||||
|   static gtsam::Vector Logmap(const gtsam::SO4& p); | ||||
|   gtsam::SO4 expmap(gtsam::Vector v); | ||||
|   gtsam::Vector logmap(const gtsam::SO4& p); | ||||
|   static gtsam::Matrix Hat(const gtsam::Vector& xi); | ||||
|   static gtsam::Vector Vee(const gtsam::Matrix& xi); | ||||
| 
 | ||||
|   // Manifold | ||||
|   gtsam::SO4 retract(gtsam::Vector v) const; | ||||
|   gtsam::Vector localCoordinates(const gtsam::SO4& Q) const; | ||||
|  | @ -276,6 +295,14 @@ class SOn { | |||
|   // Operator Overloads | ||||
|   gtsam::SOn operator*(const gtsam::SOn& Q) const; | ||||
| 
 | ||||
|   // Lie Group | ||||
|   static gtsam::SOn Expmap(gtsam::Vector v); | ||||
|   static gtsam::Vector Logmap(const gtsam::SOn& p); | ||||
|   gtsam::SOn expmap(gtsam::Vector v); | ||||
|   gtsam::Vector logmap(const gtsam::SOn& p); | ||||
|   static gtsam::Matrix Hat(const gtsam::Vector& xi); | ||||
|   static gtsam::Vector Vee(const gtsam::Matrix& xi); | ||||
| 
 | ||||
|   // Manifold | ||||
|   gtsam::SOn retract(gtsam::Vector v) const; | ||||
|   gtsam::Vector localCoordinates(const gtsam::SOn& Q) const; | ||||
|  | @ -346,6 +373,14 @@ class Rot3 { | |||
|   gtsam::Rot3 retract(gtsam::Vector v) const; | ||||
|   gtsam::Vector localCoordinates(const gtsam::Rot3& p) const; | ||||
| 
 | ||||
|   // Lie group | ||||
|   static gtsam::Rot3 Expmap(gtsam::Vector v); | ||||
|   static gtsam::Vector Logmap(const gtsam::Rot3& p); | ||||
|   gtsam::Rot3 expmap(const gtsam::Vector& v); | ||||
|   gtsam::Vector logmap(const gtsam::Rot3& p); | ||||
|   static gtsam::Matrix Hat(const gtsam::Vector& xi); | ||||
|   static gtsam::Vector Vee(const gtsam::Matrix& xi); | ||||
| 
 | ||||
|   // Group Action on Point3 | ||||
|   gtsam::Point3 rotate(const gtsam::Point3& p) const; | ||||
|   gtsam::Point3 unrotate(const gtsam::Point3& p) const; | ||||
|  | @ -358,10 +393,6 @@ class Rot3 { | |||
|   gtsam::Unit3 unrotate(const gtsam::Unit3& p) const; | ||||
|    | ||||
|   // Standard Interface | ||||
|   static gtsam::Rot3 Expmap(gtsam::Vector v); | ||||
|   static gtsam::Vector Logmap(const gtsam::Rot3& p); | ||||
|   gtsam::Rot3 expmap(const gtsam::Vector& v); | ||||
|   gtsam::Vector logmap(const gtsam::Rot3& p); | ||||
|   gtsam::Matrix matrix() const; | ||||
|   gtsam::Matrix transpose() const; | ||||
|   gtsam::Point3 column(size_t index) const; | ||||
|  | @ -417,8 +448,12 @@ class Pose2 { | |||
|   // Lie Group | ||||
|   static gtsam::Pose2 Expmap(gtsam::Vector v); | ||||
|   static gtsam::Vector Logmap(const gtsam::Pose2& p); | ||||
|   static gtsam::Pose2 Expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H); | ||||
|   static gtsam::Vector Logmap(const gtsam::Pose2& p, Eigen::Ref<Eigen::MatrixXd> H); | ||||
|   gtsam::Pose2 expmap(gtsam::Vector v); | ||||
|   gtsam::Pose2 expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2); | ||||
|   gtsam::Vector logmap(const gtsam::Pose2& p); | ||||
|   gtsam::Vector logmap(const gtsam::Pose2& p, Eigen::Ref<Eigen::MatrixXd> H); | ||||
|   gtsam::Vector logmap(const gtsam::Pose2& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2); | ||||
|   static gtsam::Matrix ExpmapDerivative(gtsam::Vector v); | ||||
|   static gtsam::Matrix LogmapDerivative(const gtsam::Pose2& v); | ||||
|   gtsam::Matrix AdjointMap() const; | ||||
|  | @ -426,7 +461,8 @@ class Pose2 { | |||
|   static gtsam::Matrix adjointMap_(gtsam::Vector v); | ||||
|   static gtsam::Vector adjoint_(gtsam::Vector xi, gtsam::Vector y); | ||||
|   static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); | ||||
|   static gtsam::Matrix wedge(double vx, double vy, double w); | ||||
|   static gtsam::Matrix Hat(const gtsam::Vector& xi); | ||||
|   static gtsam::Vector Vee(const gtsam::Matrix& xi); | ||||
| 
 | ||||
|   // Group Actions on Point2 | ||||
|   gtsam::Point2 transformFrom(const gtsam::Point2& p) const; | ||||
|  | @ -496,11 +532,13 @@ class Pose3 { | |||
| 
 | ||||
|   // Lie Group | ||||
|   static gtsam::Pose3 Expmap(gtsam::Vector v); | ||||
|   static gtsam::Pose3 Expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> Hxi); | ||||
|   static gtsam::Vector Logmap(const gtsam::Pose3& pose); | ||||
|   static gtsam::Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hpose); | ||||
|   static gtsam::Vector Logmap(const gtsam::Pose3& p); | ||||
|   static gtsam::Pose3 Expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H); | ||||
|   static gtsam::Vector Logmap(const gtsam::Pose3& p, Eigen::Ref<Eigen::MatrixXd> H); | ||||
|   gtsam::Pose3 expmap(gtsam::Vector v); | ||||
|   gtsam::Vector logmap(const gtsam::Pose3& pose); | ||||
|   gtsam::Pose3 expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2); | ||||
|   gtsam::Vector logmap(const gtsam::Pose3& p); | ||||
|   gtsam::Vector logmap(const gtsam::Pose3& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2); | ||||
|   gtsam::Matrix AdjointMap() const; | ||||
|   gtsam::Vector Adjoint(gtsam::Vector xi) const; | ||||
|   gtsam::Vector Adjoint(gtsam::Vector xi, Eigen::Ref<Eigen::MatrixXd> H_this, | ||||
|  | @ -515,8 +553,8 @@ class Pose3 { | |||
|   static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y); | ||||
|   static gtsam::Matrix ExpmapDerivative(gtsam::Vector xi); | ||||
|   static gtsam::Matrix LogmapDerivative(const gtsam::Pose3& xi); | ||||
|   static gtsam::Matrix wedge(double wx, double wy, double wz, double vx, double vy, | ||||
|                       double vz); | ||||
|   static gtsam::Matrix Hat(const gtsam::Vector& xi); | ||||
|   static gtsam::Vector Vee(const gtsam::Matrix& xi); | ||||
| 
 | ||||
|   // Group Action on Point3 | ||||
|   gtsam::Point3 transformFrom(const gtsam::Point3& point) const; | ||||
|  | @ -1147,6 +1185,14 @@ class Similarity2 { | |||
|   static gtsam::Similarity2 Align(const gtsam::Point2Pairs& abPointPairs); | ||||
|   static gtsam::Similarity2 Align(const gtsam::Pose2Pairs& abPosePairs); | ||||
| 
 | ||||
|   // Lie group | ||||
|   static gtsam::Similarity2 Expmap(gtsam::Vector v); | ||||
|   static gtsam::Vector Logmap(const gtsam::Similarity2& p); | ||||
|   gtsam::Similarity2 expmap(const gtsam::Vector& v); | ||||
|   gtsam::Vector logmap(const gtsam::Similarity2& p); | ||||
|   static gtsam::Matrix Hat(const gtsam::Vector& xi); | ||||
|   static gtsam::Vector Vee(const gtsam::Matrix& xi); | ||||
| 
 | ||||
|   // Standard Interface | ||||
|   bool equals(const gtsam::Similarity2& sim, double tol) const; | ||||
|   void print(const std::string& s = "") const; | ||||
|  | @ -1171,6 +1217,14 @@ class Similarity3 { | |||
|   static gtsam::Similarity3 Align(const gtsam::Point3Pairs& abPointPairs); | ||||
|   static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs); | ||||
| 
 | ||||
|   // Lie group | ||||
|   static gtsam::Similarity3 Expmap(gtsam::Vector v); | ||||
|   static gtsam::Vector Logmap(const gtsam::Similarity3& p); | ||||
|   gtsam::Similarity3 expmap(const gtsam::Vector& v); | ||||
|   gtsam::Vector logmap(const gtsam::Similarity3& p); | ||||
|   static gtsam::Matrix Hat(const gtsam::Vector& xi); | ||||
|   static gtsam::Vector Vee(const gtsam::Matrix& xi); | ||||
| 
 | ||||
|   // Standard Interface | ||||
|   bool equals(const gtsam::Similarity3& sim, double tol) const; | ||||
|   void print(const std::string& s = "") const; | ||||
|  |  | |||
|  | @ -37,7 +37,7 @@ GTSAM_CONCEPT_LIE_INST(Pose2) | |||
| TEST(Pose2 , Concept) { | ||||
|   GTSAM_CONCEPT_ASSERT(IsGroup<Pose2 >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsManifold<Pose2 >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsLieGroup<Pose2 >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Pose2 >); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  | @ -141,6 +141,27 @@ TEST(Pose2, expmap0d) { | |||
|   EXPECT(assert_equal(expected, actual, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(Pose2, HatAndVee) { | ||||
|   // Create a few test vectors
 | ||||
|   Vector3 v1(1, 2, 3); | ||||
|   Vector3 v2(0.1, -0.5, 1.0); | ||||
|   Vector3 v3(0.0, 0.0, 0.0); | ||||
| 
 | ||||
|   // Test that Vee(Hat(v)) == v for various inputs
 | ||||
|   EXPECT(assert_equal(v1, Pose2::Vee(Pose2::Hat(v1)))); | ||||
|   EXPECT(assert_equal(v2, Pose2::Vee(Pose2::Hat(v2)))); | ||||
|   EXPECT(assert_equal(v3, Pose2::Vee(Pose2::Hat(v3)))); | ||||
| 
 | ||||
|   // Check the structure of the Lie Algebra element
 | ||||
|   Matrix3 expected; | ||||
|   expected << 0, -3, 1, | ||||
|     3, 0, 2, | ||||
|     0, 0, 0; | ||||
| 
 | ||||
|   EXPECT(assert_equal(expected, Pose2::Hat(v1))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // test case for screw motion in the plane
 | ||||
| namespace screwPose2 { | ||||
|  | @ -186,17 +207,16 @@ TEST(Pose2, Adjoint_full) { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
 | ||||
| // assert that T*Hat(xi)*T^-1 is equal to Hat(Ad_T(xi))
 | ||||
| TEST(Pose2, Adjoint_hat) { | ||||
|   Pose2 T(1, 2, 3); | ||||
|   auto hat = [](const Vector& xi) { return ::wedge<Pose2>(xi); }; | ||||
|   Matrix3 expected = T.matrix() * hat(screwPose2::xi) * T.matrix().inverse(); | ||||
|   Matrix3 xiprime = hat(T.Adjoint(screwPose2::xi)); | ||||
|   Matrix3 expected = T.matrix() * Pose2::Hat(screwPose2::xi) * T.matrix().inverse(); | ||||
|   Matrix3 xiprime = Pose2::Hat(T.Adjoint(screwPose2::xi)); | ||||
|   EXPECT(assert_equal(expected, xiprime, 1e-6)); | ||||
| 
 | ||||
|   Vector3 xi2(4, 5, 6); | ||||
|   Matrix3 expected2 = T.matrix() * hat(xi2) * T.matrix().inverse(); | ||||
|   Matrix3 xiprime2 = hat(T.Adjoint(xi2)); | ||||
|   Matrix3 expected2 = T.matrix() * Pose2::Hat(xi2) * T.matrix().inverse(); | ||||
|   Matrix3 xiprime2 = Pose2::Hat(T.Adjoint(xi2)); | ||||
|   EXPECT(assert_equal(expected2, xiprime2, 1e-6)); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -41,6 +41,13 @@ static const Pose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),P2); | |||
| static const Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3)); | ||||
| static const double tol=1e-5; | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(Pose3 , Concept) { | ||||
|   GTSAM_CONCEPT_ASSERT(IsGroup<Pose3 >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsManifold<Pose3 >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Pose3 >); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( Pose3, equals) | ||||
| { | ||||
|  | @ -221,20 +228,41 @@ TEST(Pose3, AdjointTranspose) | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
 | ||||
| TEST(Pose3, HatAndVee) { | ||||
|   // Create a few test vectors
 | ||||
|   Vector6 v1(1, 2, 3, 4, 5, 6); | ||||
|   Vector6 v2(0.1, -0.5, 1.0, -1.0, 0.5, 2.0); | ||||
|   Vector6 v3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); | ||||
| 
 | ||||
|   // Test that Vee(Hat(v)) == v for various inputs
 | ||||
|   EXPECT(assert_equal(v1, Pose3::Vee(Pose3::Hat(v1)))); | ||||
|   EXPECT(assert_equal(v2, Pose3::Vee(Pose3::Hat(v2)))); | ||||
|   EXPECT(assert_equal(v3, Pose3::Vee(Pose3::Hat(v3)))); | ||||
| 
 | ||||
|   // Check the structure of the Lie Algebra element
 | ||||
|   Matrix4 expected; | ||||
|   expected << 0, -3, 2, 4, | ||||
|     3, 0, -1, 5, | ||||
|     -2, 1, 0, 6, | ||||
|     0, 0, 0, 0; | ||||
| 
 | ||||
|   EXPECT(assert_equal(expected, Pose3::Hat(v1))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // assert that T*Hat(xi)*T^-1 is equal to Hat(Ad_T(xi))
 | ||||
| TEST(Pose3, Adjoint_hat) | ||||
| { | ||||
|   auto hat = [](const Vector& xi) { return ::wedge<Pose3>(xi); }; | ||||
|   Matrix4 expected = T.matrix() * hat(screwPose3::xi) * T.matrix().inverse(); | ||||
|   Matrix4 xiprime = hat(T.Adjoint(screwPose3::xi)); | ||||
|   Matrix4 expected = T.matrix() * Pose3::Hat(screwPose3::xi) * T.matrix().inverse(); | ||||
|   Matrix4 xiprime = Pose3::Hat(T.Adjoint(screwPose3::xi)); | ||||
|   EXPECT(assert_equal(expected, xiprime, 1e-6)); | ||||
| 
 | ||||
|   Matrix4 expected2 = T2.matrix() * hat(screwPose3::xi) * T2.matrix().inverse(); | ||||
|   Matrix4 xiprime2 = hat(T2.Adjoint(screwPose3::xi)); | ||||
|   Matrix4 expected2 = T2.matrix() * Pose3::Hat(screwPose3::xi) * T2.matrix().inverse(); | ||||
|   Matrix4 xiprime2 = Pose3::Hat(T2.Adjoint(screwPose3::xi)); | ||||
|   EXPECT(assert_equal(expected2, xiprime2, 1e-6)); | ||||
| 
 | ||||
|   Matrix4 expected3 = T3.matrix() * hat(screwPose3::xi) * T3.matrix().inverse(); | ||||
|   Matrix4 xiprime3 = hat(T3.Adjoint(screwPose3::xi)); | ||||
|   Matrix4 expected3 = T3.matrix() * Pose3::Hat(screwPose3::xi) * T3.matrix().inverse(); | ||||
|   Matrix4 xiprime3 = Pose3::Hat(T3.Adjoint(screwPose3::xi)); | ||||
|   EXPECT(assert_equal(expected3, xiprime3, 1e-6)); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -31,7 +31,7 @@ typedef traits<Q>::ChartJacobian QuaternionJacobian; | |||
| TEST(Quaternion , Concept) { | ||||
|   GTSAM_CONCEPT_ASSERT(IsGroup<Quaternion >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsManifold<Quaternion >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsLieGroup<Quaternion >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Quaternion >); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
|  |  | |||
|  | @ -103,6 +103,25 @@ TEST(Rot2, logmap) | |||
|   CHECK(assert_equal(expected, actual)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(Rot2, HatAndVee) { | ||||
|   // Create a few test vectors
 | ||||
|   Vector1 v1 = (Vector1() << 1).finished(); | ||||
|   Vector1 v2 = (Vector1() << 0.1).finished(); | ||||
|   Vector1 v3 = (Vector1() << 0.0).finished(); | ||||
| 
 | ||||
|   // Test that Vee(Hat(v)) == v for various inputs
 | ||||
|   EXPECT(assert_equal(v1, Rot2::Vee(Rot2::Hat(v1)))); | ||||
|   EXPECT(assert_equal(v2, Rot2::Vee(Rot2::Hat(v2)))); | ||||
|   EXPECT(assert_equal(v3, Rot2::Vee(Rot2::Hat(v3)))); | ||||
| 
 | ||||
|   // Check the structure of the Lie Algebra element
 | ||||
|   Matrix2 expected; | ||||
|   expected << 0., -1., 1., 0.; | ||||
| 
 | ||||
|   EXPECT(assert_equal(expected, Rot2::Hat(v1))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // rotate and derivatives
 | ||||
| inline Point2 rotate_(const Rot2 & R, const Point2& p) {return R.rotate(p);} | ||||
|  |  | |||
|  | @ -40,7 +40,7 @@ static double error = 1e-9, epsilon = 0.001; | |||
| TEST(Rot3 , Concept) { | ||||
|   GTSAM_CONCEPT_ASSERT(IsGroup<Rot3 >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsManifold<Rot3 >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsLieGroup<Rot3 >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Rot3 >); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  | @ -324,6 +324,34 @@ TEST(Rot3, manifold_expmap) | |||
|   CHECK(assert_equal(R5,R3*R2)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(Rot3, HatAndVee) { | ||||
|   // Create a few test vectors
 | ||||
|   Vector3 v1(1, 2, 3); | ||||
|   Vector3 v2(0.1, -0.5, 1.0); | ||||
|   Vector3 v3(0.0, 0.0, 0.0); | ||||
| 
 | ||||
|   // Test that Vee(Hat(v)) == v for various inputs
 | ||||
|   EXPECT(assert_equal(v1, Rot3::Vee(Rot3::Hat(v1)))); | ||||
|   EXPECT(assert_equal(v2, Rot3::Vee(Rot3::Hat(v2)))); | ||||
|   EXPECT(assert_equal(v3, Rot3::Vee(Rot3::Hat(v3)))); | ||||
| 
 | ||||
|   // Check the structure of the Lie Algebra element
 | ||||
|   Matrix3 expected; | ||||
|   expected << 0, -3, 2, | ||||
|     3, 0, -1, | ||||
|     -2, 1, 0; | ||||
| 
 | ||||
|   EXPECT(assert_equal(expected, Rot3::Hat(v1))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Checks correct exponential map (Expmap) with brute force matrix exponential
 | ||||
| TEST(Rot3, BruteForceExpmap) { | ||||
|   const Vector3 xi(0.1, 0.2, 0.3); | ||||
|   EXPECT(assert_equal(Rot3::Expmap(xi), expm<Rot3>(xi), 1e-6)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| class AngularVelocity : public Vector3 { | ||||
|  public: | ||||
|  |  | |||
|  | @ -38,7 +38,7 @@ TEST(SO3, Identity) { | |||
| TEST(SO3, Concept) { | ||||
|   GTSAM_CONCEPT_ASSERT(IsGroup<SO3>); | ||||
|   GTSAM_CONCEPT_ASSERT(IsManifold<SO3>); | ||||
|   GTSAM_CONCEPT_ASSERT(IsLieGroup<SO3>); | ||||
|   GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<SO3>); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
|  |  | |||
|  | @ -44,7 +44,7 @@ TEST(SO4, Identity) { | |||
| TEST(SO4, Concept) { | ||||
|   GTSAM_CONCEPT_ASSERT(IsGroup<SO4>); | ||||
|   GTSAM_CONCEPT_ASSERT(IsManifold<SO4>); | ||||
|   GTSAM_CONCEPT_ASSERT(IsLieGroup<SO4>); | ||||
|   GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<SO4>); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
|  |  | |||
|  | @ -86,7 +86,7 @@ TEST(SOn, SO5) { | |||
| TEST(SOn, Concept) { | ||||
|   GTSAM_CONCEPT_ASSERT(IsGroup<SOn>); | ||||
|   GTSAM_CONCEPT_ASSERT(IsManifold<SOn>); | ||||
|   GTSAM_CONCEPT_ASSERT(IsLieGroup<SOn>); | ||||
|   GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<SOn>); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
|  |  | |||
|  | @ -37,7 +37,7 @@ static const double s = 4; | |||
| TEST(Similarity2, Concepts) { | ||||
|   GTSAM_CONCEPT_ASSERT(IsGroup<Similarity2>); | ||||
|   GTSAM_CONCEPT_ASSERT(IsManifold<Similarity2>); | ||||
|   GTSAM_CONCEPT_ASSERT(IsLieGroup<Similarity2>); | ||||
|   GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Similarity2>); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
|  | @ -56,6 +56,34 @@ TEST(Similarity2, Getters) { | |||
|   EXPECT_DOUBLES_EQUAL(1.0, sim2_default.scale(), 1e-9); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(Similarity2, HatAndVee) { | ||||
|   // Create a few test vectors
 | ||||
|   Vector4 v1(1, 2, 3, 4); | ||||
|   Vector4 v2(0.1, -0.5, 1.0, -1.0); | ||||
|   Vector4 v3(0.0, 0.0, 0.0, 0.0); | ||||
| 
 | ||||
|   // Test that Vee(Hat(v)) == v for various inputs
 | ||||
|   EXPECT(assert_equal(v1, Similarity2::Vee(Similarity2::Hat(v1)))); | ||||
|   EXPECT(assert_equal(v2, Similarity2::Vee(Similarity2::Hat(v2)))); | ||||
|   EXPECT(assert_equal(v3, Similarity2::Vee(Similarity2::Hat(v3)))); | ||||
| 
 | ||||
|   // Check the structure of the Lie Algebra element
 | ||||
|   Matrix3 expected; | ||||
|   expected << 0, -3, 1, | ||||
|               3, 0, 2, | ||||
|               0, 0, -4; | ||||
| 
 | ||||
|   EXPECT(assert_equal(expected, Similarity2::Hat(v1))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Checks correct exponential map (Expmap) with brute force matrix exponential
 | ||||
| TEST_DISABLED(Similarity2, BruteForceExpmap) { | ||||
|   const Vector4 xi(0.1, 0.2, 0.3, 0.4); | ||||
|   EXPECT(assert_equal(Similarity2::Expmap(xi), expm<Similarity2>(xi), 1e-4)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|  |  | |||
|  | @ -56,7 +56,7 @@ const double degree = M_PI / 180; | |||
| TEST(Similarity3, Concepts) { | ||||
|   GTSAM_CONCEPT_ASSERT(IsGroup<Similarity3 >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsManifold<Similarity3 >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsLieGroup<Similarity3 >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Similarity3 >); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
|  | @ -80,14 +80,43 @@ TEST(Similarity3, Getters) { | |||
|   EXPECT_DOUBLES_EQUAL(7.0, sim3.scale(), 1e-9); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(Similarity3, HatAndVee) { | ||||
|   // Create a few test vectors
 | ||||
|   Vector7 v1(1, 2, 3, 4, 5, 6, 7); | ||||
|   Vector7 v2(0.1, -0.5, 1.0, -1.0, 0.5, 2.0, -0.3); | ||||
|   Vector7 v3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); | ||||
| 
 | ||||
|   // Test that Vee(Hat(v)) == v for various inputs
 | ||||
|   EXPECT(assert_equal(v1, Similarity3::Vee(Similarity3::Hat(v1)))); | ||||
|   EXPECT(assert_equal(v2, Similarity3::Vee(Similarity3::Hat(v2)))); | ||||
|   EXPECT(assert_equal(v3, Similarity3::Vee(Similarity3::Hat(v3)))); | ||||
| 
 | ||||
|   // Check the structure of the Lie Algebra element
 | ||||
|   Matrix4 expected; | ||||
|   expected << 0, -3, 2, 4, | ||||
|               3, 0, -1, 5, | ||||
|               -2, 1, 0, 6, | ||||
|               0, 0, 0, -7; | ||||
| 
 | ||||
|   EXPECT(assert_equal(expected, Similarity3::Hat(v1))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Checks correct exponential map (Expmap) with brute force matrix exponential
 | ||||
| TEST(Similarity3, BruteForceExpmap) { | ||||
|   const Vector7 xi(0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7); | ||||
|   EXPECT(assert_equal(Similarity3::Expmap(xi), expm<Similarity3>(xi), 1e-4)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(Similarity3, AdjointMap) { | ||||
|   const Matrix4 T = T2.matrix(); | ||||
|   // Check Ad with actual definition
 | ||||
|   Vector7 delta; | ||||
|   delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; | ||||
|   Matrix4 W = Similarity3::wedge(delta); | ||||
|   Matrix4 TW = Similarity3::wedge(T2.AdjointMap() * delta); | ||||
|   Matrix4 W = Similarity3::Hat(delta); | ||||
|   Matrix4 TW = Similarity3::Hat(T2.AdjointMap() * delta); | ||||
|   EXPECT(assert_equal(TW, Matrix4(T * W * T.inverse()), 1e-9)); | ||||
| } | ||||
| 
 | ||||
|  | @ -209,10 +238,6 @@ TEST(Similarity3, ExpLogMap) { | |||
|   Similarity3 expZero = Similarity3::Expmap(zeros); | ||||
|   Similarity3 ident = Similarity3::Identity(); | ||||
|   EXPECT(assert_equal(expZero, ident)); | ||||
| 
 | ||||
|   // Compare to matrix exponential, using expm in Lie.h
 | ||||
|   EXPECT( | ||||
|       assert_equal(expm<Similarity3>(delta), Similarity3::Expmap(delta), 1e-3)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
|  |  | |||
|  | @ -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 Matrix5 Hat(const Vector9& xi); | ||||
| 
 | ||||
|   /// Vee maps from Lie algebra to tangent vector
 | ||||
|   static Vector9 Vee(const Matrix5& X); | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Dynamics
 | ||||
|   /// @{
 | ||||
|  | @ -283,9 +300,9 @@ private: | |||
| 
 | ||||
| // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
 | ||||
| template <> | ||||
| struct traits<NavState> : public internal::LieGroup<NavState> {}; | ||||
| struct traits<NavState> : public internal::MatrixLieGroup<NavState> {}; | ||||
| 
 | ||||
| template <> | ||||
| struct traits<const NavState> : public internal::LieGroup<NavState> {}; | ||||
| struct traits<const NavState> : public internal::MatrixLieGroup<NavState> {}; | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
|  |  | |||
|  | @ -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; | ||||
| }; | ||||
|  |  | |||
|  | @ -52,6 +52,13 @@ static const NavState T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2); | |||
| static const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), | ||||
|                          Point3(1, 2, 3)); | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(NavState, Concept) { | ||||
|   GTSAM_CONCEPT_ASSERT(IsGroup<NavState >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsManifold<NavState >); | ||||
|   GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<NavState >); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(NavState, Constructor) { | ||||
|   std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create = | ||||
|  | @ -533,6 +540,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; | ||||
|  |  | |||
|  | @ -98,6 +98,7 @@ class TestDiscreteBayesNet(GtsamTestCase): | |||
|         self.assertFalse(bayesTree.empty()) | ||||
|         self.assertEqual(12, bayesTree.size()) | ||||
| 
 | ||||
|     @unittest.skip("Too Slow") | ||||
|     def test_discrete_bayes_tree_lookup(self): | ||||
|         """Check that we can have a multi-frontal lookup table.""" | ||||
|         # Make a small planning-like graph: 3 states, 2 actions | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue