diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 16b402f3a..ec276b2de 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -80,6 +80,22 @@ struct LieGroup { return Class::Logmap(between(g)); } + static Class Retract(const TangentVector& v) { + return Class::ChartAtOrigin::Retract(v); + } + + static TangentVector LocalCoordinates(const Class& g) { + return Class::ChartAtOrigin::Local(g); + } + + static Class Retract(const TangentVector& v, ChartJacobian H) { + return Class::ChartAtOrigin::Retract(v,H); + } + + static TangentVector LocalCoordinates(const Class& g, ChartJacobian H) { + return Class::ChartAtOrigin::Local(g,H); + } + Class retract(const TangentVector& v) const { return compose(Class::ChartAtOrigin::Retract(v)); } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 1587a5614..81a20445a 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -23,6 +23,8 @@ #include #include +#define GTSAM_POSE3_EXPMAP + using namespace std; namespace gtsam { @@ -36,6 +38,12 @@ Pose3::Pose3(const Pose2& pose2) : Point3(pose2.x(), pose2.y(), 0)) { } +/* ************************************************************************* */ +Pose3 Pose3::inverse() const { + Rot3 Rt = R_.inverse(); + return Pose3(Rt, Rt * (-t_)); +} + /* ************************************************************************* */ // Calculate Adjoint map // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) @@ -127,8 +135,7 @@ Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) { /* ************************************************************************* */ Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { - if (H) CONCEPT_NOT_IMPLEMENTED; - + if (H) *H = LogmapDerivative(p); Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); double t = w.norm(); if (t < 1e-10) { @@ -149,57 +156,25 @@ Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { } /* ************************************************************************* */ -Pose3 Pose3::retractFirstOrder(const Vector& xi) const { - Vector3 omega(sub(xi, 0, 3)); - Point3 v(sub(xi, 3, 6)); - Rot3 R = R_.retract(omega); // R is done exactly - Point3 t = t_ + R_ * v; // First order t approximation - return Pose3(R, t); +Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) { +#ifdef GTSAM_POSE3_EXPMAP + return Expmap(xi, H); +#else + if (H) CONCEPT_NOT_IMPLEMENTED; + return Pose3(Rot3::Retract(xi.head<3>()), Point3(xi.tail<3>())); +#endif } /* ************************************************************************* */ -// Different versions of retract -Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const { - if (mode == Pose3::EXPMAP) { - // Lie group exponential map, traces out geodesic - return compose(Expmap(xi)); - } else if (mode == Pose3::FIRST_ORDER) { - // First order - return retractFirstOrder(xi); - } else { - // Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently - // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation - assert(false); - exit(1); - } -} - -/* ************************************************************************* */ -// different versions of localCoordinates -Vector6 Pose3::localCoordinates(const Pose3& T, - Pose3::CoordinatesMode mode) const { - if (mode == Pose3::EXPMAP) { - // Lie group logarithm map, exact inverse of exponential map - return Logmap(between(T)); - } else if (mode == Pose3::FIRST_ORDER) { - // R is always done exactly in all three retract versions below - Vector3 omega = R_.localCoordinates(T.rotation()); - - // Incorrect version - // Independently computes the logmap of the translation and rotation - // Vector v = t_.localCoordinates(T.translation()); - - // Correct first order t inverse - Point3 d = R_.unrotate(T.translation() - t_); - - // TODO: correct second order t inverse - Vector6 local; - local << omega(0), omega(1), omega(2), d.x(), d.y(), d.z(); - return local; - } else { - assert(false); - exit(1); - } +Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { +#ifdef GTSAM_POSE3_EXPMAP + return Logmap(T, H); +#else + if (H) CONCEPT_NOT_IMPLEMENTED; + Vector6 xi; + xi << Rot3::Logmap(T.rotation()), T.translation().vector(); + return xi; +#endif } /* ************************************************************************* */ @@ -253,7 +228,8 @@ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { } /* ************************************************************************* */ -Matrix6 Pose3::LogmapDerivative(const Vector6& xi) { +Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { + Vector6 xi = Logmap(pose); Vector3 w(sub(xi, 0, 3)); Matrix3 Jw = Rot3::LogmapDerivative(w); Matrix3 Q = computeQforExpmapDerivative(xi); @@ -262,20 +238,6 @@ Matrix6 Pose3::LogmapDerivative(const Vector6& xi) { return J; } -/* ************************************************************************* */ -Pose3 Pose3::retract(const Vector& d, OptionalJacobian<6, 6> Hthis, - OptionalJacobian<6, 6> Hd, Pose3::CoordinatesMode mode) const { - if (Hthis || Hd) CONCEPT_NOT_IMPLEMENTED; - return retract(d, mode); -} - -/* ************************************************************************* */ -Vector6 Pose3::localCoordinates(const Pose3& T2, OptionalJacobian<6, 6> Horigin, - OptionalJacobian<6, 6> Hother, Pose3::CoordinatesMode mode) const { - if (Horigin || Hother) CONCEPT_NOT_IMPLEMENTED; - return localCoordinates(T2, mode); -} - /* ************************************************************************* */ Matrix4 Pose3::matrix() const { const Matrix3 R = R_.matrix(); @@ -326,31 +288,6 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, return q; } -/* ************************************************************************* */ -Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1, - OptionalJacobian<6,6> H2) const { - if (H1) *H1 = p2.inverse().AdjointMap(); - if (H2) *H2 = I_6x6; - return (*this) * p2; -} - -/* ************************************************************************* */ -Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const { - if (H1) *H1 = -AdjointMap(); - Rot3 Rt = R_.inverse(); - return Pose3(Rt, Rt * (-t_)); -} - -/* ************************************************************************* */ -// between = compose(p2,inverse(p1)); -Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1, - OptionalJacobian<6,6> H2) const { - Pose3 result = inverse() * p2; - if (H1) *H1 = -result.inverse().AdjointMap(); - if (H2) *H2 = I_6x6; - return result; -} - /* ************************************************************************* */ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) const { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index b707b1cab..20babaae8 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -19,12 +19,6 @@ #include -#ifndef GTSAM_POSE3_EXPMAP -#define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER -#else -#define POSE3_DEFAULT_COORDINATES_MODE Pose3::EXPMAP -#endif - #include #include #include @@ -39,7 +33,7 @@ class Pose2; * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Pose3{ +class GTSAM_EXPORT Pose3: public LieGroup { public: /** Pose Concept requirements */ @@ -48,13 +42,11 @@ public: private: - Rot3 R_; ///< Rotation gRp, between global and pose frame + Rot3 R_; ///< Rotation gRp, between global and pose frame Point3 t_; ///< Translation gTp, from global origin to pose frame origin public: - enum { dimension = 6 }; - /// @name Standard Constructors /// @{ @@ -77,7 +69,6 @@ public: R_(R), t_(t) { } - /** Construct from Pose2 */ explicit Pose3(const Pose2& pose2); @@ -107,251 +98,218 @@ public: } /// inverse transformation with derivatives - Pose3 inverse(OptionalJacobian<6, 6> H1 = boost::none) const; - - ///compose this transformation onto another (first *this and then p2) - Pose3 compose(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none, - OptionalJacobian<6, 6> H2 = boost::none) const; + Pose3 inverse() const; /// compose syntactic sugar Pose3 operator*(const Pose3& T) const { return Pose3(R_ * T.R_, t_ + R_ * T.t_); } - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - * as well as optionally the derivatives - */ - Pose3 between(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none, - OptionalJacobian<6, 6> H2 = boost::none) const; - /// @} - /// @name Manifold + /// @name Lie Group /// @{ - /** Enum to indicate which method should be used in Pose3::retract() and - * Pose3::localCoordinates() + /// 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 Vector& xi, OptionalJacobian<6, 6> H = boost::none); + + /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation + static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none); + + /** + * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame + * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) */ - enum CoordinatesMode { - EXPMAP, ///< The correct exponential map, computationally expensive. - FIRST_ORDER ///< A fast first-order approximation to the exponential map. + Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect + + /** + * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame + * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ + */ + Vector Adjoint(const Vector& xi_b) const { + return AdjointMap() * xi_b; + } /// FIXME Not tested - marked as incorrect + + /** + * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 + * [ad(w,v)] = [w^, zero3; v^, w^] + * Note that this is the matrix representation of the adjoint operator for se3 Lie algebra, + * aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3. + * + * Let \f$ \hat{\xi}_i \f$ be the se3 Lie algebra, and \f$ \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\f$ be its + * vector representation. + * We have the following relationship: + * \f$ [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \f$ + * + * We use this to compute the discrete version of the inverse right-trivialized tangent map, + * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. + * + */ + static Matrix6 adjointMap(const Vector6 &xi); + + /** + * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives + */ + static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, + OptionalJacobian<6, 6> = boost::none); + + /** + * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. + */ + static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, + OptionalJacobian<6, 6> H = boost::none); + + /// Derivative of Expmap + static Matrix6 ExpmapDerivative(const Vector6& xi); + + /// Derivative of Logmap + static Matrix6 LogmapDerivative(const Pose3& xi); + + // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP + struct ChartAtOrigin { + static Pose3 Retract(const Vector6& v, ChartJacobian H = boost::none); + static Vector6 Local(const Pose3& r, ChartJacobian H = boost::none); }; - /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes - static size_t Dim() { - return 6; - } - - /// Dimensionality of the tangent space = 6 DOF - size_t dim() const { - return 6; - } - - /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map - Pose3 retractFirstOrder(const Vector& d) const; - - /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose - Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = - POSE3_DEFAULT_COORDINATES_MODE) const; - - /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose - Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode = - POSE3_DEFAULT_COORDINATES_MODE) const; - - /// @} - - /// @name Lie Group - /// @{ - - /// 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 Vector& xi, OptionalJacobian<6, 6> H = boost::none); - - /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation - static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none); - - /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose - Pose3 retract(const Vector& d, OptionalJacobian<6, 6> Hthis, - OptionalJacobian<6, 6> Hd = boost::none, Pose3::CoordinatesMode mode = - POSE3_DEFAULT_COORDINATES_MODE) const; - - /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose - Vector6 localCoordinates(const Pose3& T2, OptionalJacobian<6, 6> Horigin, - OptionalJacobian<6, 6> Hother = boost::none, Pose3::CoordinatesMode mode = - POSE3_DEFAULT_COORDINATES_MODE) const; - - /** - * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame - * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) - */ - Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect - - /** - * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame - * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ - */ - Vector Adjoint(const Vector& xi_b) const {return AdjointMap()*xi_b; } /// FIXME Not tested - marked as incorrect - - /** - * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 - * [ad(w,v)] = [w^, zero3; v^, w^] - * Note that this is the matrix representation of the adjoint operator for se3 Lie algebra, - * aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3. - * - * Let \f$ \hat{\xi}_i \f$ be the se3 Lie algebra, and \f$ \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\f$ be its - * vector representation. - * We have the following relationship: - * \f$ [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \f$ - * - * We use this to compute the discrete version of the inverse right-trivialized tangent map, - * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. - * - */ - static Matrix6 adjointMap(const Vector6 &xi); - - /** - * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives - */ - static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian<6,6> = boost::none); - - /** - * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. - */ - static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none); - -public: - - /// Derivative of Expmap - static Matrix6 ExpmapDerivative(const Vector6& xi); - - /// Derivative of Logmap - static Matrix6 LogmapDerivative(const Vector6& xi); - - /** - * wedge 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(); - } - - /// @} - /// @name Group Action on Point3 - /// @{ - - /** - * @brief takes point in Pose coordinates and transforms it to world coordinates - * @param p point in Pose coordinates - * @param Dpose optional 3*6 Jacobian wrpt this pose - * @param Dpoint optional 3*3 Jacobian wrpt point - * @return point in world coordinates - */ - Point3 transform_from(const Point3& p, - OptionalJacobian<3,6> Dpose=boost::none, OptionalJacobian<3,3> Dpoint=boost::none) const; - - /** syntactic sugar for transform_from */ - inline Point3 operator*(const Point3& p) const { return transform_from(p); } - - /** - * @brief takes point in world coordinates and transforms it to Pose coordinates - * @param p point in world coordinates - * @param Dpose optional 3*6 Jacobian wrpt this pose - * @param Dpoint optional 3*3 Jacobian wrpt point - * @return point in Pose coordinates - */ - Point3 transform_to(const Point3& p, - OptionalJacobian<3,6> Dpose = boost::none, - OptionalJacobian<3,3> Dpoint = boost::none) const; - - /// @} - /// @name Standard Interface - /// @{ - - /// get rotation - const Rot3& rotation() const { return R_; } - - /// get translation - const Point3& translation() const { return t_; } - - /// get x - double x() const { return t_.x(); } - - /// get y - double y() const { return t_.y(); } - - /// get z - double z() const { return t_.z(); } - - /** convert to 4*4 matrix */ - Matrix4 matrix() const; - - /** receives a pose in world coordinates and transforms it to local coordinates */ - Pose3 transform_to(const Pose3& pose) const; - - /** - * Calculate range to a landmark - * @param point 3D location of landmark - * @return range (double) - */ - double range(const Point3& point, - OptionalJacobian<1,6> H1=boost::none, - OptionalJacobian<1,3> H2=boost::none) const; - - /** - * Calculate range to another pose - * @param pose Other SO(3) pose - * @return range (double) - */ - double range(const Pose3& pose, - OptionalJacobian<1,6> H1=boost::none, - OptionalJacobian<1,6> H2=boost::none) const; - - /// @} - /// @name Advanced Interface - /// @{ - - /** - * Return the start and end indices (inclusive) of the translation component of the - * exponential map parameterization - * @return a pair of [start, end] indices into the tangent space vector - */ - inline static std::pair translationInterval() { return std::make_pair(3, 5); } - - /** - * Return the start and end indices (inclusive) of the rotation component of the - * exponential map parameterization - * @return a pair of [start, end] indices into the tangent space vector - */ - static std::pair rotationInterval() { return std::make_pair(0, 2); } - - /// Output stream operator - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(R_); - ar & BOOST_SERIALIZATION_NVP(t_); - } - /// @} - - };// Pose3 class + using LieGroup::inverse; // version with derivative /** * wedge for Pose3: * @param xi 6-dim twist (omega,v) where - * omega = 3D angular velocity - * v = 3D velocity + * 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(); + } + + /// @} + /// @name Group Action on Point3 + /// @{ + + /** + * @brief takes point in Pose coordinates and transforms it to world coordinates + * @param p point in Pose coordinates + * @param Dpose optional 3*6 Jacobian wrpt this pose + * @param Dpoint optional 3*3 Jacobian wrpt point + * @return point in world coordinates + */ + Point3 transform_from(const Point3& p, OptionalJacobian<3, 6> Dpose = + boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; + + /** syntactic sugar for transform_from */ + inline Point3 operator*(const Point3& p) const { + return transform_from(p); + } + + /** + * @brief takes point in world coordinates and transforms it to Pose coordinates + * @param p point in world coordinates + * @param Dpose optional 3*6 Jacobian wrpt this pose + * @param Dpoint optional 3*3 Jacobian wrpt point + * @return point in Pose coordinates + */ + Point3 transform_to(const Point3& p, OptionalJacobian<3, 6> Dpose = + boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// get rotation + const Rot3& rotation() const { + return R_; + } + + /// get translation + const Point3& translation() const { + return t_; + } + + /// get x + double x() const { + return t_.x(); + } + + /// get y + double y() const { + return t_.y(); + } + + /// get z + double z() const { + return t_.z(); + } + + /** convert to 4*4 matrix */ + Matrix4 matrix() const; + + /** receives a pose in world coordinates and transforms it to local coordinates */ + Pose3 transform_to(const Pose3& pose) const; + + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @return range (double) + */ + double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none) const; + + /** + * Calculate range to another pose + * @param pose Other SO(3) pose + * @return range (double) + */ + double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none, + OptionalJacobian<1, 6> H2 = boost::none) const; + + /// @} + /// @name Advanced Interface + /// @{ + + /** + * Return the start and end indices (inclusive) of the translation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + inline static std::pair translationInterval() { + return std::make_pair(3, 5); + } + + /** + * Return the start and end indices (inclusive) of the rotation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + static std::pair rotationInterval() { + return std::make_pair(0, 2); + } + + /// Output stream operator + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, const Pose3& p); + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(t_); + } + /// @} + +}; +// 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 + */ template<> inline Matrix wedge(const Vector& xi) { return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); @@ -365,6 +323,7 @@ typedef std::pair Point3Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); template<> -struct traits_x : public internal::LieGroupTraits {}; +struct traits_x : public internal::LieGroupTraits { +}; } // namespace gtsam diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index ff4b830ad..702f6bf4c 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -16,9 +16,8 @@ #include #include +#include #include -#include -#include #include // for operator += using namespace boost::assign; @@ -29,6 +28,8 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +#define GTSAM_POSE3_EXPMAP + GTSAM_CONCEPT_TESTABLE_INST(Pose3) GTSAM_CONCEPT_LIE_INST(Pose3) @@ -57,25 +58,24 @@ TEST( Pose3, constructors) } /* ************************************************************************* */ +#ifndef GTSAM_POSE3_EXPMAP TEST( Pose3, retract_first_order) { Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::FIRST_ORDER),1e-2)); + EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2)); v(3)=0.2;v(4)=0.7;v(5)=-2; - EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::FIRST_ORDER),1e-2)); + EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2)); } - +#endif /* ************************************************************************* */ TEST( Pose3, retract_expmap) { - Pose3 id; - Vector v = zero(6); - v(0) = 0.3; - EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::EXPMAP),1e-2)); - v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; - EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::EXPMAP),1e-2)); + Vector v = zero(6); v(0) = 0.3; + Pose3 pose = Pose3::Expmap(v); + EXPECT(assert_equal(Pose3(R, Point3()), pose, 1e-2)); + EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2)); } /* ************************************************************************* */ @@ -463,43 +463,45 @@ TEST( Pose3, transformPose_to) } /* ************************************************************************* */ +#ifndef GTSAM_POSE3_EXPMAP TEST(Pose3, localCoordinates_first_order) { Vector d12 = repeat(6,0.1); - Pose3 t1 = T, t2 = t1.retract(d12, Pose3::FIRST_ORDER); - EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::FIRST_ORDER))); + Pose3 t1 = T, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); } - +#endif /* ************************************************************************* */ TEST(Pose3, localCoordinates_expmap) { Vector d12 = repeat(6,0.1); - Pose3 t1 = T, t2 = t1.retract(d12, Pose3::EXPMAP); - EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::EXPMAP))); + Pose3 t1 = T, t2 = t1.expmap(d12); + EXPECT(assert_equal(d12, t1.logmap(t2))); } /* ************************************************************************* */ +#ifndef GTSAM_POSE3_EXPMAP TEST(Pose3, manifold_first_order) { Pose3 t1 = T; Pose3 t2 = T3; Pose3 origin; - Vector d12 = t1.localCoordinates(t2, Pose3::FIRST_ORDER); - EXPECT(assert_equal(t2, t1.retract(d12, Pose3::FIRST_ORDER))); - Vector d21 = t2.localCoordinates(t1, Pose3::FIRST_ORDER); - EXPECT(assert_equal(t1, t2.retract(d21, Pose3::FIRST_ORDER))); + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); } - +#endif /* ************************************************************************* */ TEST(Pose3, manifold_expmap) { Pose3 t1 = T; Pose3 t2 = T3; Pose3 origin; - Vector d12 = t1.localCoordinates(t2, Pose3::EXPMAP); - EXPECT(assert_equal(t2, t1.retract(d12, Pose3::EXPMAP))); - Vector d21 = t2.localCoordinates(t1, Pose3::EXPMAP); - EXPECT(assert_equal(t1, t2.retract(d21, Pose3::EXPMAP))); + Vector d12 = t1.logmap(t2); + EXPECT(assert_equal(t2, t1.expmap(d12))); + Vector d21 = t2.logmap(t1); + EXPECT(assert_equal(t1, t2.expmap(d21))); // Check that log(t1,t2)=-log(t2,t1) EXPECT(assert_equal(d12,-d21)); @@ -672,22 +674,24 @@ TEST(Pose3, align_2) { } /* ************************************************************************* */ -/// exp(xi) exp(y) = exp(xi + dxi) -/// Hence, y = log (exp(-xi)*exp(xi+dxi)) -Vector6 xi = (Vector(6) << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0).finished(); - -Vector6 testExpmapDerivative(const Vector6& xi, const Vector6& dxi) { - return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi)); +TEST( Pose3, ExpmapDerivative1) { + Matrix6 actualH; + Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; + Pose3::Expmap(w,actualH); + Matrix6 expectedH = numericalDerivative21 >(&Pose3::Expmap, w, boost::none, 1e-2); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } -TEST( Pose3, ExpmapDerivative) { - Matrix actualDexpL = Pose3::ExpmapDerivative(xi); - Matrix expectedDexpL = numericalDerivative11( - boost::bind(testExpmapDerivative, xi, _1), zero(6), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); - - Matrix actualDexpInvL = Pose3::LogmapDerivative(xi); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); +/* ************************************************************************* */ +TEST( Pose3, LogmapDerivative1) { + Matrix6 actualH; + Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; + Pose3 p = Pose3::Expmap(w); + EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5)); + Matrix6 expectedH = numericalDerivative21 >(&Pose3::Logmap, p, boost::none, 1e-2); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } /* ************************************************************************* */ @@ -737,6 +741,13 @@ TEST( Pose3, stream) EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n\n[0, 0, 0]';\n"); } +//****************************************************************************** +TEST(Pose3 , Traits) { + check_group_invariants(T2,T3); + check_manifold_invariants(T2,T3); + CHECK_LIE_GROUP_DERIVATIVES(T2,T3); +} + /* ************************************************************************* */ int main(){ TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index b4f474467..a3e8453a0 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -7,9 +7,10 @@ #pragma once -#include -#include #include +#include +#include +#include namespace gtsam { @@ -29,7 +30,7 @@ class Reconstruction : public NoiseModelFactor3 { typedef NoiseModelFactor3 Base; public: Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) : - Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), gKey1, gKey, + Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey, xiKey), h_(h) { } virtual ~Reconstruction() {} @@ -46,7 +47,8 @@ public: boost::optional H3 = boost::none) const { Matrix D_gkxi_gk, D_gkxi_exphxi; - Pose3 gkxi = gk.compose(Pose3::Expmap(h_*xik), D_gkxi_gk, D_gkxi_exphxi); + Pose3 newPose = Pose3::Expmap(h_*xik); + Pose3 gkxi = gk.compose(newPose, D_gkxi_gk, D_gkxi_exphxi); Matrix D_hx_gk1, D_hx_gkxi; Pose3 hx = gkxi.between(gk1, D_hx_gkxi, D_hx_gk1); @@ -60,7 +62,7 @@ public: } if (H3) { - Matrix D_exphxi_xi = Pose3::LogmapDerivative(h_*xik)*h_; + Matrix D_exphxi_xi = Pose3::LogmapDerivative(newPose)*h_; Matrix D_hx_xi = D_hx_gkxi * D_gkxi_exphxi * D_exphxi_xi; *H3 = D_hx_xi; } @@ -92,7 +94,7 @@ public: DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, double h, const Matrix& Inertia, const Vector& Fu, double m, double mu = 1000.0) : - Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), xiKey1, xiKey_1, gKey), + Base(noiseModel::Constrained::All(6, std::abs(mu)), xiKey1, xiKey_1, gKey), h_(h), Inertia_(Inertia), Fu_(Fu), m_(m) { } virtual ~DiscreteEulerPoincareHelicopter() {}