diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 5f550e69d..b2b1486a9 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -53,26 +53,16 @@ struct LieGroup : Testable { typedef OptionalJacobian ChartJacobian; static int GetDimension(const ManifoldType& m){ return m.dim(); } - static TangentVector Local(const ManifoldType& origin, - const ManifoldType& other) { - return origin.localCoordinates(other); - } - - static ManifoldType Retract(const ManifoldType& origin, - const TangentVector& v) { - return origin.retract(v); - } - static TangentVector Local(const ManifoldType& origin, const ManifoldType& other, - ChartJacobian Horigin, + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { return origin.localCoordinates(other, Horigin, Hother); } static ManifoldType Retract(const ManifoldType& origin, const TangentVector& v, - ChartJacobian Horigin, + ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { return origin.retract(v, Horigin, Hv); } @@ -82,19 +72,11 @@ struct LieGroup : Testable { /// @name Lie Group /// @{ - static TangentVector Logmap(const ManifoldType& m) { - return ManifoldType::Logmap(m); - } - - static ManifoldType Expmap(const TangentVector& v) { - return ManifoldType::Expmap(v); - } - - static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm) { + static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm = boost::none) { return ManifoldType::Logmap(m, Hm); } - static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv) { + static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { return ManifoldType::Expmap(v, Hv); } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index f32580ed2..2912d4f9b 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -17,7 +17,10 @@ #include #include #include +#include + #include + #include #include #include @@ -56,7 +59,8 @@ bool Pose2::equals(const Pose2& q, double tol) const { } /* ************************************************************************* */ -Pose2 Pose2::Expmap(const Vector& xi) { +Pose2 Pose2::Expmap(const Vector& xi, OptionalJacobian<3, 3> H) { + if (H) CONCEPT_NOT_IMPLEMENTED; assert(xi.size() == 3); Point2 v(xi(0),xi(1)); double w = xi(2); @@ -71,7 +75,8 @@ Pose2 Pose2::Expmap(const Vector& xi) { } /* ************************************************************************* */ -Vector3 Pose2::Logmap(const Pose2& p) { +Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) { + if (H) CONCEPT_NOT_IMPLEMENTED; const Rot2& R = p.r(); const Point2& t = p.t(); double w = R.theta(); @@ -87,7 +92,9 @@ Vector3 Pose2::Logmap(const Pose2& p) { } /* ************************************************************************* */ -Pose2 Pose2::retract(const Vector& v) const { +Pose2 Pose2::retract(const Vector& v, OptionalJacobian<3, 3> Hthis, + OptionalJacobian<3, 3> Hv) const { + if (Hthis || Hv) CONCEPT_NOT_IMPLEMENTED; #ifdef SLOW_BUT_CORRECT_EXPMAP return compose(Expmap(v)); #else @@ -97,7 +104,9 @@ Pose2 Pose2::retract(const Vector& v) const { } /* ************************************************************************* */ -Vector3 Pose2::localCoordinates(const Pose2& p2) const { +Vector Pose2::localCoordinates(const Pose2& p2, OptionalJacobian<3, 3> Hthis, + OptionalJacobian<3, 3> Hother) const { + if (Hthis || Hother) CONCEPT_NOT_IMPLEMENTED; #ifdef SLOW_BUT_CORRECT_EXPMAP return Logmap(between(p2)); #else @@ -106,15 +115,6 @@ Vector3 Pose2::localCoordinates(const Pose2& p2) const { #endif } -/// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose -Vector Pose2::localCoordinates(const Pose2& p2, OptionalJacobian<3, 3> Hthis, - OptionalJacobian<3, 3> Hother) const { - if (Hthis || Hother) - throw std::runtime_error( - "Pose2::localCoordinates derivatives not implemented"); - return localCoordinates(p2); -} - /* ************************************************************************* */ // Calculate Adjoint map // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index bef46dd10..78f80386f 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -136,23 +136,22 @@ public: inline size_t dim() const { return 3; } /// Retraction from R^3 \f$ [T_x,T_y,\theta] \f$ to Pose2 manifold neighborhood around current pose - Pose2 retract(const Vector& v) const; + Pose2 retract(const Vector& v, OptionalJacobian<3, 3> Hthis = + boost::none, OptionalJacobian<3, 3> Hv = boost::none) const; /// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose - Vector3 localCoordinates(const Pose2& p2) const; - - /// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose - Vector localCoordinates(const Pose2& p2, OptionalJacobian<3,3> Hthis, OptionalJacobian<3,3> Hother) const; + Vector localCoordinates(const Pose2& p2, OptionalJacobian<3, 3> Hthis = + boost::none, OptionalJacobian<3, 3> Hother = boost::none) const; /// @} /// @name Lie Group /// @{ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ - static Pose2 Expmap(const Vector& xi); + static Pose2 Expmap(const Vector& xi, OptionalJacobian<3, 3> H = boost::none); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation - static Vector3 Logmap(const Pose2& p); + static Vector3 Logmap(const Pose2& p, OptionalJacobian<3, 3> H = boost::none); /** * Calculate Adjoint map diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index e8ad50de0..3892ac5ce 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -124,7 +125,8 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ -Pose3 Pose3::Expmap(const Vector& xi) { +Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) { + if (H) CONCEPT_NOT_IMPLEMENTED; // get angular velocity omega and translational velocity v from twist xi Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); @@ -144,7 +146,9 @@ Pose3 Pose3::Expmap(const Vector& xi) { } /* ************************************************************************* */ -Vector6 Pose3::Logmap(const Pose3& p) { +Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { + if (H) CONCEPT_NOT_IMPLEMENTED; + Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); double t = w.norm(); if (t < 1e-10) { @@ -218,6 +222,20 @@ Vector6 Pose3::localCoordinates(const Pose3& T, } } +/* ************************************************************************* */ +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(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index db289fecb..fab8916db 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -155,7 +155,8 @@ public: 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; + Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode = + POSE3_DEFAULT_COORDINATES_MODE) const; /// @} @@ -163,18 +164,20 @@ public: /// @{ /// 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); + 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); + 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 - Vector6 localCoordinates(const Pose3& T2, OptionalJacobian<6,6> Horigin, - OptionalJacobian<6,6> Hother = boost::none) const { - if (Horigin || Hother) - throw std::runtime_error("Pose3::localCoordinates derivatives not implemented"); - return localCoordinates(T2); - } + 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 diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 754da3c4a..0baeb47db 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -59,6 +59,64 @@ Rot2& Rot2::normalize() { return *this; } +/* ************************************************************************* */ +Rot2 Rot2::compose(const Rot2& R, OptionalJacobian<1, 1> H1, + OptionalJacobian<1, 1> H2) const { + if (H1) + *H1 = I_1x1; + if (H2) + *H2 = I_1x1; + return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); +} + +/* ************************************************************************* */ +Rot2 Rot2::between(const Rot2& R, OptionalJacobian<1, 1> H1, + OptionalJacobian<1, 1> H2) const { + if (H1) + *H1 = -I_1x1; + if (H2) + *H2 = I_1x1; + return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); +} + +/* ************************************************************************* */ +Rot2 Rot2::retract(const Vector& v, OptionalJacobian<1, 1> H1, + OptionalJacobian<1, 1> H2) const { + if (H1) + *H1 = I_1x1; + if (H2) + *H2 = I_1x1; + return *this * Expmap(v); +} + +/* ************************************************************************* */ +Vector1 Rot2::localCoordinates(const Rot2& t2, OptionalJacobian<1, 1> H1, + OptionalJacobian<1, 1> H2) const { + if (H1) + *H1 = -I_1x1; + if (H2) + *H2 = I_1x1; + return Logmap(between(t2)); +} + +/* ************************************************************************* */ +Rot2 Rot2::Expmap(const Vector& v, OptionalJacobian<1, 1> H) { + if (H) + *H = I_1x1; + if (zero(v)) + return (Rot2()); + else + return Rot2::fromAngle(v(0)); +} + +/* ************************************************************************* */ +Vector1 Rot2::Logmap(const Rot2& r, OptionalJacobian<1, 1> H) { + if (H) + *H = I_1x1; + Vector1 v; + v << r.theta(); + return v; +} /* ************************************************************************* */ Matrix2 Rot2::matrix() const { Matrix2 rvalue_; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 3736b5bd3..e1c79657e 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -115,26 +115,18 @@ namespace gtsam { return Rot2(c_, -s_); } - /** Compose - make a new rotation by adding angles */ - inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 = - boost::none, OptionalJacobian<1,1> H2 = boost::none) const { - if (H1) *H1 = I_1x1; // set to 1x1 identity matrix - if (H2) *H2 = I_1x1; // set to 1x1 identity matrix - return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); - } - /** Compose - make a new rotation by adding angles */ Rot2 operator*(const Rot2& R) const { return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); } + /** Compose - make a new rotation by adding angles */ + Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 = + boost::none, OptionalJacobian<1,1> H2 = boost::none) const; + /** Between using the default implementation */ - inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 = - boost::none, OptionalJacobian<1,1> H2 = boost::none) const { - if (H1) *H1 = -I_1x1; // set to 1x1 identity matrix - if (H2) *H2 = I_1x1; // set to 1x1 identity matrix - return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); - } + Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 = + boost::none, OptionalJacobian<1,1> H2 = boost::none) const; /// @} /// @name Manifold @@ -151,29 +143,22 @@ namespace gtsam { } /// Updates a with tangent space delta - inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } + Rot2 retract(const Vector& v, OptionalJacobian<1, 1> H1 = boost::none, + OptionalJacobian<1, 1> H2 = boost::none) const; /// Returns inverse retraction - inline Vector1 localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } + Vector1 localCoordinates(const Rot2& t2, OptionalJacobian<1, 1> H1 = + boost::none, OptionalJacobian<1, 1> H2 = boost::none) const; /// @} /// @name Lie Group /// @{ ///Exponential map at identity - create a rotation from canonical coordinates - static Rot2 Expmap(const Vector& v) { - if (zero(v)) - return (Rot2()); - else - return Rot2::fromAngle(v(0)); - } + static Rot2 Expmap(const Vector& v, OptionalJacobian<1, 1> H = boost::none); ///Log map at identity - return the canonical coordinates of this rotation - static inline Vector1 Logmap(const Rot2& r) { - Vector1 v; - v << r.theta(); - return v; - } + static Vector1 Logmap(const Rot2& r, OptionalJacobian<1, 1> H = boost::none); /// Left-trivialized derivative of the exponential map static Matrix dexpL(const Vector& v) { diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 0b88a70c7..9f5127895 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -58,6 +58,20 @@ Rot3 Rot3::rodriguez(const Vector3& w) { return rodriguez(w/t, t); } +/* ************************************************************************* */ +Rot3 Rot3::retract(const Vector& omega, OptionalJacobian<3, 3> Hthis, + OptionalJacobian<3, 3> Hv, Rot3::CoordinatesMode mode) const { + if (Hthis || Hv) CONCEPT_NOT_IMPLEMENTED; + return retract(omega, mode); +} + +/* ************************************************************************* */ +Vector3 Rot3::localCoordinates(const Rot3& R2, OptionalJacobian<3, 3> Horigin, + OptionalJacobian<3, 3> H2, Rot3::CoordinatesMode mode) const { + if (Horigin || H2) CONCEPT_NOT_IMPLEMENTED; + return localCoordinates(R2, mode); +} + /* ************************************************************************* */ bool Rot3::equals(const Rot3 & R, double tol) const { return equal_with_abs_tol(matrix(), R.matrix(), tol); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index f30418da0..df08efa98 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -21,8 +21,10 @@ #pragma once -#include // Get GTSAM_USE_QUATERNIONS macro #include +#include +#include +#include // Get GTSAM_USE_QUATERNIONS macro // You can override the default coordinate mode using this flag #ifndef ROT3_DEFAULT_COORDINATES_MODE @@ -40,11 +42,6 @@ #endif #endif -#include -#include -#include -#include - namespace gtsam { /** @@ -285,7 +282,8 @@ namespace gtsam { * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula */ - static Rot3 Expmap(const Vector& v) { + static Rot3 Expmap(const Vector& v, OptionalJacobian<3, 3> H = boost::none) { + if (H) CONCEPT_NOT_IMPLEMENTED; if(zero(v)) return Rot3(); else return rodriguez(v); } @@ -293,7 +291,7 @@ namespace gtsam { /** * Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z] \f$ of this rotation */ - static Vector3 Logmap(const Rot3& R); + static Vector3 Logmap(const Rot3& R, OptionalJacobian<3, 3> H = boost::none); /// Left-trivialized derivative of the exponential map static Matrix3 dexpL(const Vector3& v); @@ -301,13 +299,13 @@ namespace gtsam { /// Left-trivialized derivative inverse of the exponential map static Matrix3 dexpInvL(const Vector3& v); - Vector3 localCoordinates(const Rot3& R2, OptionalJacobian<3, 3> Horigin, - OptionalJacobian<3, 3> H2, Rot3::CoordinatesMode mode = - ROT3_DEFAULT_COORDINATES_MODE) const { - if (Horigin || H2) - throw std::runtime_error("Rot3::localCoordinates derivatives not implemented"); - return localCoordinates(R2, mode); - } + Rot3 retract(const Vector& omega, OptionalJacobian<3, 3> Hthis, + OptionalJacobian<3, 3> Hv = boost::none, Rot3::CoordinatesMode mode = + ROT3_DEFAULT_COORDINATES_MODE) const; + + Vector3 localCoordinates(const Rot3& R2, OptionalJacobian<3, 3> Horigin, + OptionalJacobian<3, 3> H2 = boost::none, Rot3::CoordinatesMode mode = + ROT3_DEFAULT_COORDINATES_MODE) const; /** * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 23c54fd0a..8e39906ca 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -184,7 +184,9 @@ Point3 Rot3::rotate(const Point3& p, /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation -Vector3 Rot3::Logmap(const Rot3& R) { +Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) { + + if (H) CONCEPT_NOT_IMPLEMENTED; static const double PI = boost::math::constants::pi(); diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 6a02e01e4..c2042a7bb 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -134,7 +134,8 @@ namespace gtsam { } /* ************************************************************************* */ - Vector3 Rot3::Logmap(const Rot3& R) { + Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) { + if (H) CONCEPT_NOT_IMPLEMENTED; return QuaternionChart::Logmap(R.quaternion_); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 0d8432d51..b6fb6ab32 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -231,7 +231,7 @@ TEST( Rot3, rightJacobianExpMapSO3 ) Vector3 thetahat; thetahat << 0.1, 0, 0; Matrix expectedJacobian = numericalDerivative11( - boost::bind(&Rot3::Expmap, _1), thetahat); + boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); CHECK(assert_equal(expectedJacobian, actualJacobian)); } diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 74767ff87..818266d4c 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -57,14 +57,16 @@ void PoseRTV::print(const string& s) const { } /* ************************************************************************* */ -PoseRTV PoseRTV::Expmap(const Vector9& v) { +PoseRTV PoseRTV::Expmap(const Vector9& v, ChartJacobian H) { + if (H) CONCEPT_NOT_IMPLEMENTED; Pose3 newPose = Pose3::Expmap(v.head<6>()); Velocity3 newVel = Velocity3(v.tail<3>()); return PoseRTV(newPose, newVel); } /* ************************************************************************* */ -Vector9 PoseRTV::Logmap(const PoseRTV& p) { +Vector9 PoseRTV::Logmap(const PoseRTV& p, ChartJacobian H) { + if (H) CONCEPT_NOT_IMPLEMENTED; Vector6 Lx = Pose3::Logmap(p.Rt_); Vector3 Lv = p.v_.vector(); return (Vector9() << Lx, Lv).finished(); @@ -72,14 +74,9 @@ Vector9 PoseRTV::Logmap(const PoseRTV& p) { /* ************************************************************************* */ PoseRTV PoseRTV::retract(const Vector& v, - OptionalJacobian Horigin, - OptionalJacobian Hv) const { - if (Horigin) { - CONCEPT_NOT_IMPLEMENTED; - } - if (Hv) { - CONCEPT_NOT_IMPLEMENTED; - } + ChartJacobian Horigin, + ChartJacobian Hv) const { + if (Horigin || Hv) CONCEPT_NOT_IMPLEMENTED; assert(v.size() == 9); // First order approximation Pose3 newPose = Rt_.retract(sub(v, 0, 6)); @@ -89,14 +86,9 @@ PoseRTV PoseRTV::retract(const Vector& v, /* ************************************************************************* */ Vector PoseRTV::localCoordinates(const PoseRTV& p1, - OptionalJacobian Horigin, - OptionalJacobian Hp) const { - if (Horigin) { - CONCEPT_NOT_IMPLEMENTED; - } - if (Hp) { - CONCEPT_NOT_IMPLEMENTED; - } + ChartJacobian Horigin, + ChartJacobian Hp) const { + if (Horigin || Hp) CONCEPT_NOT_IMPLEMENTED; const Pose3& x0 = pose(), &x1 = p1.pose(); // First order approximation Vector6 poseLogmap = x0.localCoordinates(x1); @@ -106,16 +98,15 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1, /* ************************************************************************* */ PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); } -PoseRTV PoseRTV::inverse(OptionalJacobian H1) const { +PoseRTV PoseRTV::inverse(ChartJacobian H1) const { if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5); return PoseRTV(Rt_.inverse(), - v_); } /* ************************************************************************* */ PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); } -PoseRTV PoseRTV::compose(const PoseRTV& p, - OptionalJacobian H1, - OptionalJacobian H2) const { +PoseRTV PoseRTV::compose(const PoseRTV& p, ChartJacobian H1, + ChartJacobian H2) const { if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5); if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5); return PoseRTV(Rt_.compose(p.Rt_), v_+p.v_); @@ -124,8 +115,8 @@ PoseRTV PoseRTV::compose(const PoseRTV& p, /* ************************************************************************* */ PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); } PoseRTV PoseRTV::between(const PoseRTV& p, - OptionalJacobian H1, - OptionalJacobian H2) const { + ChartJacobian H1, + ChartJacobian H2) const { if (H1) *H1 = numericalDerivative21(between_, *this, p, 1e-5); if (H2) *H2 = numericalDerivative22(between_, *this, p, 1e-5); return inverse().compose(p); @@ -243,7 +234,7 @@ Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, doub /* ************************************************************************* */ double range_(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } double PoseRTV::range(const PoseRTV& other, - OptionalJacobian<1,dimension> H1, OptionalJacobian<1,dimension> H2) const { + OptionalJacobian<1,9> H1, OptionalJacobian<1,9> H2) const { if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5); if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5); return t().distance(other.t()); @@ -254,9 +245,8 @@ PoseRTV transformed_from_(const PoseRTV& global, const Pose3& transform) { return global.transformed_from(transform); } -PoseRTV PoseRTV::transformed_from(const Pose3& trans, - OptionalJacobian Dglobal, - OptionalJacobian::dimension> Dtrans) const { +PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, + OptionalJacobian<9, 6> Dtrans) const { // Note that we rotate the velocity Matrix DVr, DTt; Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt); diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index 439f2b9bb..e70f269ec 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -25,6 +25,8 @@ protected: Pose3 Rt_; Velocity3 v_; + typedef OptionalJacobian<9, 9> ChartJacobian; + public: enum { dimension = 9 }; @@ -80,39 +82,39 @@ public: * - v(3-5): Point3 * - v(6-8): Translational velocity */ - PoseRTV retract(const Vector& v, OptionalJacobian Horigin=boost::none, OptionalJacobian Hv=boost::none) const; - Vector localCoordinates(const PoseRTV& p, OptionalJacobian Horigin=boost::none,OptionalJacobian Hp=boost::none) const; + PoseRTV retract(const Vector& v, ChartJacobian Horigin=boost::none, ChartJacobian Hv=boost::none) const; + Vector localCoordinates(const PoseRTV& p, ChartJacobian Horigin=boost::none,ChartJacobian Hp=boost::none) const; // Lie TODO IS this a Lie group or just a Manifold???? /** * expmap/logmap are poor approximations that assume independence of components * Currently implemented using the poor retract/unretract approximations */ - static PoseRTV Expmap(const Vector9& v); - static Vector9 Logmap(const PoseRTV& p); + static PoseRTV Expmap(const Vector9& v, ChartJacobian H = boost::none); + static Vector9 Logmap(const PoseRTV& p, ChartJacobian H = boost::none); static PoseRTV identity() { return PoseRTV(); } /** Derivatives calculated numerically */ - PoseRTV inverse(OptionalJacobian H1=boost::none) const; + PoseRTV inverse(ChartJacobian H1=boost::none) const; /** Derivatives calculated numerically */ PoseRTV compose(const PoseRTV& p, - OptionalJacobian H1=boost::none, - OptionalJacobian H2=boost::none) const; + ChartJacobian H1=boost::none, + ChartJacobian H2=boost::none) const; PoseRTV operator*(const PoseRTV& p) const { return compose(p); } /** Derivatives calculated numerically */ PoseRTV between(const PoseRTV& p, - OptionalJacobian H1=boost::none, - OptionalJacobian H2=boost::none) const; + ChartJacobian H1=boost::none, + ChartJacobian H2=boost::none) const; // measurement functions /** Derivatives calculated numerically */ double range(const PoseRTV& other, - OptionalJacobian<1,dimension> H1=boost::none, - OptionalJacobian<1,dimension> H2=boost::none) const; + OptionalJacobian<1,9> H1=boost::none, + OptionalJacobian<1,9> H2=boost::none) const; // IMU-specific @@ -159,8 +161,8 @@ public: * Note: the transform jacobian convention is flipped */ PoseRTV transformed_from(const Pose3& trans, - OptionalJacobian Dglobal=boost::none, - OptionalJacobian::dimension> Dtrans=boost::none) const; + ChartJacobian Dglobal = boost::none, + OptionalJacobian<9, 6> Dtrans = boost::none) const; // Utility functions