Tightened what is needed for Lie, i.e., fewer versions of retract/localCoordinates

gtsam/base/Lie.h

@@ -53,26 +53,16 @@ struct LieGroup : Testable<T> {
   typedef OptionalJacobian<dimension, dimension> 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<T> {
   /// @name Lie Group
   /// @{
 
-  static TangentVector Logmap(const ManifoldType& m) {
+  static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm = boost::none) {
-    return ManifoldType::Logmap(m);
-  }
-
-  static ManifoldType Expmap(const TangentVector& v) {
-    return ManifoldType::Expmap(v);
-  }
-
-  static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm) {
     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);
   }
 

gtsam/geometry/Pose2.cpp

@@ -17,7 +17,10 @@
 #include <gtsam/geometry/concepts.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/concepts.h>
+
 #include <boost/foreach.hpp>
+
 #include <cmath>
 #include <iostream>
 #include <iomanip>
@@ -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)

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;
+  Vector localCoordinates(const Pose2& p2, OptionalJacobian<3, 3> Hthis =
-
+      boost::none, OptionalJacobian<3, 3> Hother = boost::none) 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;
 
   /// @}
   /// @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

gtsam/geometry/Pose3.cpp

@@ -17,6 +17,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/concepts.h>
+#include <gtsam/base/concepts.h>
 
 #include <boost/foreach.hpp>
 #include <iostream>
@@ -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();

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
+    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) const {
+        OptionalJacobian<6, 6> Hother = boost::none, Pose3::CoordinatesMode mode =
-      if (Horigin || Hother)
+            POSE3_DEFAULT_COORDINATES_MODE) const;
-        throw std::runtime_error("Pose3::localCoordinates derivatives not implemented");
-      return localCoordinates(T2);
-    }
 
     /**
      * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame

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_;

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 =
+    Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
-        boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
+        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_);
-    }
 
     /// @}
     /// @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) {
+    static Rot2 Expmap(const Vector& v, OptionalJacobian<1, 1> H = boost::none);
-      if (zero(v))
-        return (Rot2());
-      else
-        return Rot2::fromAngle(v(0));
-    }
 
     ///Log map at identity - return the canonical coordinates of this rotation
-    static inline Vector1 Logmap(const Rot2& r) {
+    static Vector1 Logmap(const Rot2& r, OptionalJacobian<1, 1> H = boost::none);
-      Vector1 v;
-      v << r.theta();
-      return v;
-    }
 
     /// Left-trivialized derivative of the exponential map
     static Matrix dexpL(const Vector& v) {

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);

gtsam/geometry/Rot3.h

@@ -21,8 +21,10 @@
 
 #pragma once
 
-#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
 #include <gtsam/geometry/Quaternion.h>
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/base/concepts.h>
+#include <gtsam/config.h> // 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 <gtsam/base/DerivedValue.h>
-#include <gtsam/base/Matrix.h>
-#include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Unit3.h>
-
 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);
 
+    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, Rot3::CoordinatesMode mode =
+        OptionalJacobian<3, 3> H2 = boost::none, Rot3::CoordinatesMode mode =
-          ROT3_DEFAULT_COORDINATES_MODE) const {
+            ROT3_DEFAULT_COORDINATES_MODE) const;
-    if (Horigin || H2)
-      throw std::runtime_error("Rot3::localCoordinates derivatives not implemented");
-    return localCoordinates(R2, mode);
-  }
 
     /**
      * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in

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<double>();
 

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_);
   }
 

gtsam/geometry/tests/testRot3.cpp

@@ -231,7 +231,7 @@ TEST( Rot3, rightJacobianExpMapSO3 )
   Vector3 thetahat; thetahat << 0.1, 0, 0;
 
   Matrix expectedJacobian = numericalDerivative11<Rot3, Vector3>(
-      boost::bind(&Rot3::Expmap, _1), thetahat);
+      boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
   Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat);
   CHECK(assert_equal(expectedJacobian, actualJacobian));
 }

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<dimension, dimension> Horigin,
+                         ChartJacobian Horigin,
-                         OptionalJacobian<dimension, dimension> Hv) const {
+                         ChartJacobian Hv) const {
-  if (Horigin) {
+  if (Horigin || Hv) CONCEPT_NOT_IMPLEMENTED;
-    CONCEPT_NOT_IMPLEMENTED;
-  }
-  if (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<dimension, dimension> Horigin,
+                                 ChartJacobian Horigin,
-                                 OptionalJacobian<dimension, dimension> Hp) const {
+                                 ChartJacobian Hp) const {
-  if (Horigin) {
+  if (Horigin || Hp) CONCEPT_NOT_IMPLEMENTED;
-    CONCEPT_NOT_IMPLEMENTED;
-  }
-  if (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<dimension,dimension> H1) const {
+PoseRTV PoseRTV::inverse(ChartJacobian H1) const {
   if (H1) *H1 = numericalDerivative11<PoseRTV,PoseRTV>(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,
+PoseRTV PoseRTV::compose(const PoseRTV& p, ChartJacobian H1,
-    OptionalJacobian<dimension,dimension> H1,
+    ChartJacobian H2) const {
-    OptionalJacobian<dimension,dimension> 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<dimension,dimension> H1,
+    ChartJacobian H1,
-    OptionalJacobian<dimension,dimension> H2) const {
+    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,
+PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
-    OptionalJacobian<dimension,dimension> Dglobal,
+    OptionalJacobian<9, 6> Dtrans) const {
-    OptionalJacobian<dimension,traits_x<Pose3>::dimension> Dtrans) const {
   // Note that we rotate the velocity
   Matrix DVr, DTt;
   Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt);

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<dimension, dimension> Horigin=boost::none, OptionalJacobian<dimension, dimension> Hv=boost::none) const;
+  PoseRTV retract(const Vector& v, ChartJacobian Horigin=boost::none, ChartJacobian Hv=boost::none) const;
-  Vector localCoordinates(const PoseRTV& p, OptionalJacobian<dimension, dimension> Horigin=boost::none,OptionalJacobian<dimension, dimension> Hp=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 PoseRTV Expmap(const Vector9& v, ChartJacobian H = boost::none);
-  static Vector9 Logmap(const PoseRTV& p);
+  static Vector9 Logmap(const PoseRTV& p, ChartJacobian H = boost::none);
 
   static PoseRTV identity() { return PoseRTV(); }
 
   /** Derivatives calculated numerically */
-  PoseRTV inverse(OptionalJacobian<dimension,dimension> H1=boost::none) const;
+  PoseRTV inverse(ChartJacobian H1=boost::none) const;
 
   /** Derivatives calculated numerically */
   PoseRTV compose(const PoseRTV& p,
-      OptionalJacobian<dimension,dimension> H1=boost::none,
+      ChartJacobian H1=boost::none,
-      OptionalJacobian<dimension,dimension> H2=boost::none) const;
+      ChartJacobian H2=boost::none) const;
 
   PoseRTV operator*(const PoseRTV& p) const { return compose(p); }
 
   /** Derivatives calculated numerically */
   PoseRTV between(const PoseRTV& p,
-                  OptionalJacobian<dimension,dimension> H1=boost::none,
+                  ChartJacobian H1=boost::none,
-                  OptionalJacobian<dimension,dimension> H2=boost::none) const;
+                  ChartJacobian H2=boost::none) const;
 
   // measurement functions
   /** Derivatives calculated numerically */
   double range(const PoseRTV& other,
-               OptionalJacobian<1,dimension> H1=boost::none,
+               OptionalJacobian<1,9> H1=boost::none,
-               OptionalJacobian<1,dimension> H2=boost::none) const;
+               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<dimension,dimension> Dglobal=boost::none,
+      ChartJacobian Dglobal = boost::none,
-                           OptionalJacobian<dimension,traits_x<Pose3>::dimension> Dtrans=boost::none) const;
+      OptionalJacobian<9, 6> Dtrans = boost::none) const;
 
   // Utility functions