Merged in feature/OptionalJacobianBlocks (pull request #177)

New OptionalJacobian functionality

gtsam/base/OptionalJacobian.h

@@ -40,7 +40,8 @@ class OptionalJacobian {
 
 public:
 
-  /// ::Jacobian size type
+  /// Jacobian size type
+  /// TODO(frank): how to enforce RowMajor? Or better, make it work with any storage order?
   typedef Eigen::Matrix<double, Rows, Cols> Jacobian;
 
 private:
@@ -53,6 +54,14 @@ private:
     new (&map_) Eigen::Map<Jacobian>(data);
   }
 
+  // Private and very dangerous constructor straight from memory
+  OptionalJacobian(double* data) : map_(NULL) {
+    if (data) usurp(data);
+  }
+
+  template<int M, int N>
+  friend class OptionalJacobian;
+
 public:
 
   /// Default constructor acts like boost::none
@@ -98,6 +107,11 @@ public:
 
 #endif
 
+  /// Constructor that will usurp data of a block expression
+  /// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible
+  //  template <typename Derived, bool InnerPanel>
+  //  OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(NULL) { ?? }
+
   /// Return true is allocated, false if default constructor was used
   operator bool() const {
     return map_.data() != NULL;
@@ -108,8 +122,36 @@ public:
     return map_;
   }
 
-  /// TODO: operator->()
+  /// operator->()
-  Eigen::Map<Jacobian>* operator->(){ return &map_; }
+  Eigen::Map<Jacobian>* operator->() {
+    return &map_;
+  }
+
+  /// Access M*N sub-block if we are allocated, otherwise none
+  /// TODO(frank): this could work as is below if only constructor above worked
+  //  template <int M, int N>
+  //  OptionalJacobian<M, N> block(int startRow, int startCol) {
+  //    if (*this)
+  //      OptionalJacobian<M, N>(map_.block<M, N>(startRow, startCol));
+  //    else
+  //      return OptionalJacobian<M, N>();
+  //  }
+
+  /// Access Rows*N sub-block if we are allocated, otherwise return an empty OptionalJacobian
+  /// The use case is functions with arguments that are dissected, e.g. Pose3 into Rot3, Point3
+  /// TODO(frank): ideally, we'd like full block functionality, but see note above.
+  template <int N>
+  OptionalJacobian<Rows, N> cols(int startCol) {
+    if (*this)
+      return OptionalJacobian<Rows, N>(&map_(0,startCol));
+    else
+      return OptionalJacobian<Rows, N>();
+  }
+
+  /// Access M*Cols sub-block if we are allocated, otherwise return empty OptionalJacobian
+  /// The use case is functions that create their return value piecemeal by calling other functions
+  /// TODO(frank): Unfortunately we assume column-major storage order and hence this can't work
+  /// template <int M> OptionalJacobian<M, Cols> rows(int startRow) { ?? }
 };
 
 // The pure dynamic specialization of this is needed to support

gtsam/base/tests/testOptionalJacobian.cpp

@@ -61,20 +61,15 @@ TEST( OptionalJacobian, Constructors ) {
 }
 
 //******************************************************************************
+Matrix kTestMatrix = (Matrix23() << 11,12,13,21,22,23).finished();
+
 void test(OptionalJacobian<2, 3> H = boost::none) {
   if (H)
-    *H = Matrix23::Zero();
+    *H = kTestMatrix;
-}
-
-void testPtr(Matrix23* H = NULL) {
-  if (H)
-    *H = Matrix23::Zero();
 }
 
 TEST( OptionalJacobian, Fixed) {
 
-  Matrix expected = Matrix23::Zero();
-
   // Default argument does nothing
   test();
 
@@ -82,61 +77,83 @@ TEST( OptionalJacobian, Fixed) {
   Matrix23 fixed1;
   fixed1.setOnes();
   test(fixed1);
-  EXPECT(assert_equal(expected,fixed1));
+  EXPECT(assert_equal(kTestMatrix,fixed1));
 
   // Fixed size, no copy, pointer style
   Matrix23 fixed2;
   fixed2.setOnes();
   test(&fixed2);
-  EXPECT(assert_equal(expected,fixed2));
+  EXPECT(assert_equal(kTestMatrix,fixed2));
 
-  // Empty is no longer a sign we don't want a matrix, we want it resized
+  // Passing in an empty matrix means we want it resized
   Matrix dynamic0;
   test(dynamic0);
-  EXPECT(assert_equal(expected,dynamic0));
+  EXPECT(assert_equal(kTestMatrix,dynamic0));
 
   // Dynamic wrong size
   Matrix dynamic1(3, 5);
   dynamic1.setOnes();
   test(dynamic1);
-  EXPECT(assert_equal(expected,dynamic1));
+  EXPECT(assert_equal(kTestMatrix,dynamic1));
 
   // Dynamic right size
   Matrix dynamic2(2, 5);
   dynamic2.setOnes();
   test(dynamic2);
-  EXPECT(assert_equal(expected, dynamic2));
+  EXPECT(assert_equal(kTestMatrix, dynamic2));
 }
 
 //******************************************************************************
 void test2(OptionalJacobian<-1,-1> H = boost::none) {
   if (H)
-    *H = Matrix23::Zero(); // resizes
+    *H = kTestMatrix; // resizes
 }
 
 TEST( OptionalJacobian, Dynamic) {
 
-  Matrix expected = Matrix23::Zero();
-
   // Default argument does nothing
   test2();
 
-  // Empty is no longer a sign we don't want a matrix, we want it resized
+  // Passing in an empty matrix means we want it resized
   Matrix dynamic0;
   test2(dynamic0);
-  EXPECT(assert_equal(expected,dynamic0));
+  EXPECT(assert_equal(kTestMatrix,dynamic0));
 
   // Dynamic wrong size
   Matrix dynamic1(3, 5);
   dynamic1.setOnes();
   test2(dynamic1);
-  EXPECT(assert_equal(expected,dynamic1));
+  EXPECT(assert_equal(kTestMatrix,dynamic1));
 
   // Dynamic right size
   Matrix dynamic2(2, 5);
   dynamic2.setOnes();
   test2(dynamic2);
-  EXPECT(assert_equal(expected, dynamic2));
+  EXPECT(assert_equal(kTestMatrix, dynamic2));
+}
+
+//******************************************************************************
+void test3(double add, OptionalJacobian<2,1> H = boost::none) {
+  if (H) *H << add + 10, add + 20;
+}
+
+// This function calls the above function three times, one for each column
+void test4(OptionalJacobian<2, 3> H = boost::none) {
+  if (H) {
+    test3(1, H.cols<1>(0));
+    test3(2, H.cols<1>(1));
+    test3(3, H.cols<1>(2));
+  }
+}
+
+TEST(OptionalJacobian, Block) {
+  // Default argument does nothing
+  test4();
+
+  Matrix23 fixed;
+  fixed.setOnes();
+  test4(fixed);
+  EXPECT(assert_equal(kTestMatrix, fixed));
 }
 
 //******************************************************************************

gtsam/geometry/Pose2.cpp

@@ -201,95 +201,88 @@ Pose2 Pose2::inverse() const {
 /* ************************************************************************* */
 // see doc/math.lyx, SE(2) section
 Point2 Pose2::transform_to(const Point2& point,
-    OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
+    OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const {
-  Point2 d = point - t_;
+  OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0);
-  Point2 q = r_.unrotate(d);
+  OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2);
-  if (!H1 && !H2) return q;
+  const Point2 q = r_.unrotate(point - t_, Hrotation, Hpoint);
-  if (H1) *H1 <<
+  if (Htranslation) *Htranslation << -1.0, 0.0, 0.0, -1.0;
-      -1.0, 0.0,  q.y(),
-      0.0, -1.0, -q.x();
-  if (H2) *H2 << r_.transpose();
   return q;
 }
 
 /* ************************************************************************* */
 // see doc/math.lyx, SE(2) section
-Point2 Pose2::transform_from(const Point2& p,
+Point2 Pose2::transform_from(const Point2& point,
-    OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
+    OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const {
-  const Point2 q = r_ * p;
+  OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0);
-  if (H1 || H2) {
+  OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2);
-    const Matrix2 R = r_.matrix();
+  const Point2 q = r_.rotate(point, Hrotation, Hpoint);
-    Matrix21 Drotate1;
+  if (Htranslation) *Htranslation = Hpoint ? *Hpoint : r_.matrix();
-    Drotate1 <<  -q.y(), q.x();
-    if (H1) *H1 << R, Drotate1;
-    if (H2) *H2 = R; // R
-  }
   return q + t_;
 }
 
 /* ************************************************************************* */
 Rot2 Pose2::bearing(const Point2& point,
-    OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const {
+    OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 2> Hpoint) const {
   // make temporary matrices
-  Matrix23 D1; Matrix2 D2;
+  Matrix23 D_d_pose; Matrix2 D_d_point;
-  Point2 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); // uses pointer version
+  Point2 d = transform_to(point, Hpose ? &D_d_pose : 0, Hpoint ? &D_d_point : 0);
-  if (!H1 && !H2) return Rot2::relativeBearing(d);
+  if (!Hpose && !Hpoint) return Rot2::relativeBearing(d);
   Matrix12 D_result_d;
-  Rot2 result = Rot2::relativeBearing(d, D_result_d);
+  Rot2 result = Rot2::relativeBearing(d, Hpose || Hpoint ? &D_result_d : 0);
-  if (H1) *H1 = D_result_d * (D1);
+  if (Hpose) *Hpose = D_result_d * D_d_pose;
-  if (H2) *H2 = D_result_d * (D2);
+  if (Hpoint) *Hpoint = D_result_d * D_d_point;
   return result;
 }
 
 /* ************************************************************************* */
 Rot2 Pose2::bearing(const Pose2& pose,
-    OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const {
+    OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 3> Hother) const {
   Matrix12 D2;
-  Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0);
+  Rot2 result = bearing(pose.t(), Hpose, Hother ? &D2 : 0);
-  if (H2) {
+  if (Hother) {
     Matrix12 H2_ = D2 * pose.r().matrix();
-    *H2 << H2_, Z_1x1;
+    *Hother << H2_, Z_1x1;
   }
   return result;
 }
 /* ************************************************************************* */
 double Pose2::range(const Point2& point,
-    OptionalJacobian<1,3> H1, OptionalJacobian<1,2> H2) const {
+    OptionalJacobian<1,3> Hpose, OptionalJacobian<1,2> Hpoint) const {
   Point2 d = point - t_;
-  if (!H1 && !H2) return d.norm();
+  if (!Hpose && !Hpoint) return d.norm();
-  Matrix12 H;
+  Matrix12 D_r_d;
-  double r = d.norm(H);
+  double r = d.norm(D_r_d);
-  if (H1) {
+  if (Hpose) {
-      Matrix23 H1_;
+      Matrix23 D_d_pose;
-      H1_ << -r_.c(),  r_.s(),  0.0,
+      D_d_pose << -r_.c(),  r_.s(),  0.0,
-              -r_.s(), -r_.c(),  0.0;
+                  -r_.s(), -r_.c(),  0.0;
-      *H1 = H * H1_;
+      *Hpose = D_r_d * D_d_pose;
   }
-  if (H2) *H2 = H;
+  if (Hpoint) *Hpoint = D_r_d;
   return r;
 }
 
 /* ************************************************************************* */
 double Pose2::range(const Pose2& pose,
-    OptionalJacobian<1,3> H1,
+    OptionalJacobian<1,3> Hpose,
-    OptionalJacobian<1,3> H2) const {
+    OptionalJacobian<1,3> Hother) const {
   Point2 d = pose.t() - t_;
-  if (!H1 && !H2) return d.norm();
+  if (!Hpose && !Hother) return d.norm();
-  Matrix12 H;
+  Matrix12 D_r_d;
-  double r = d.norm(H);
+  double r = d.norm(D_r_d);
-  if (H1) {
+  if (Hpose) {
-      Matrix23 H1_;
+      Matrix23 D_d_pose;
-      H1_ <<
+      D_d_pose <<
       -r_.c(),  r_.s(),  0.0,
       -r_.s(), -r_.c(),  0.0;
-      *H1 = H * H1_;
+      *Hpose = D_r_d * D_d_pose;
   }
-  if (H2) {
+  if (Hother) {
-      Matrix23 H2_;
+      Matrix23 D_d_other;
-      H2_ <<
+      D_d_other <<
       pose.r_.c(), -pose.r_.s(),  0.0,
       pose.r_.s(),  pose.r_.c(),  0.0;
-      *H2 = H * H2_;
+      *Hother = D_r_d * D_d_other;
   }
   return r;
 }

gtsam/geometry/Pose3.cpp

@@ -193,10 +193,10 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
  *  The closed-form formula is similar to formula 102 in Barfoot14tro)
  */
 static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
-  Vector3 w(sub(xi, 0, 3));
+  const Vector3 w = xi.head<3>();
-  Vector3 v(sub(xi, 3, 6));
+  const Vector3 v = xi.tail<3>();
-  Matrix3 V = skewSymmetric(v);
+  const Matrix3 V = skewSymmetric(v);
-  Matrix3 W = skewSymmetric(w);
+  const Matrix3 W = skewSymmetric(w);
 
   Matrix3 Q;
 
@@ -215,8 +215,8 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
   // The closed-form formula in Barfoot14tro eq. (102)
   double phi = w.norm();
   if (fabs(phi)>1e-5) {
-    double sinPhi = sin(phi), cosPhi = cos(phi);
+    const double sinPhi = sin(phi), cosPhi = cos(phi);
-    double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
+    const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
     // Invert the sign of odd-order terms to have the right Jacobian
     Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W)
             + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W)
@@ -234,40 +234,37 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
 
 /* ************************************************************************* */
 Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
-  Vector3 w(sub(xi, 0, 3));
+  const Vector3 w = xi.head<3>();
-  Matrix3 Jw = Rot3::ExpmapDerivative(w);
+  const Matrix3 Jw = Rot3::ExpmapDerivative(w);
-  Matrix3 Q = computeQforExpmapDerivative(xi);
+  const Matrix3 Q = computeQforExpmapDerivative(xi);
-  Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q, Jw).finished();
+  Matrix6 J;
+  J << Jw, Z_3x3, Q, Jw;
   return J;
 }
 
 /* ************************************************************************* */
 Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
-  Vector6 xi = Logmap(pose);
+  const Vector6 xi = Logmap(pose);
-  Vector3 w(sub(xi, 0, 3));
+  const Vector3 w = xi.head<3>();
-  Matrix3 Jw = Rot3::LogmapDerivative(w);
+  const Matrix3 Jw = Rot3::LogmapDerivative(w);
-  Matrix3 Q = computeQforExpmapDerivative(xi);
+  const Matrix3 Q = computeQforExpmapDerivative(xi);
-  Matrix3 Q2 = -Jw*Q*Jw;
+  const Matrix3 Q2 = -Jw*Q*Jw;
-  Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q2, Jw).finished();
+  Matrix6 J;
+  J << Jw, Z_3x3, Q2, Jw;
   return J;
 }
 
 /* ************************************************************************* */
 const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const {
-  if (H) {
+  if (H) *H << Z_3x3, rotation().matrix();
-    *H << Z_3x3, rotation().matrix();
-  }
   return t_;
 }
 
 /* ************************************************************************* */
 Matrix4 Pose3::matrix() const {
-  const Matrix3 R = R_.matrix();
+  static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished();
-  const Vector3 T = t_.vector();
-  Matrix14 A14;
-  A14 << 0.0, 0.0, 0.0, 1.0;
   Matrix4 mat;
-  mat << R, T, A14;
+  mat << R_.matrix(), t_.vector(), A14;
   return mat;
 }
 
@@ -281,15 +278,17 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
 /* ************************************************************************* */
 Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
     OptionalJacobian<3,3> Dpoint) const {
+  // Only get matrix once, to avoid multiple allocations,
+  // as well as multiple conversions in the Quaternion case
+  const Matrix3 R = R_.matrix();
   if (Dpose) {
-    const Matrix3 R = R_.matrix();
+    Dpose->leftCols<3>() = R * skewSymmetric(-p.x(), -p.y(), -p.z());
-    Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
+    Dpose->rightCols<3>() = R;
-    (*Dpose) << DR, R;
   }
   if (Dpoint) {
-    *Dpoint = R_.matrix();
+    *Dpoint = R;
   }
-  return R_ * p + t_;
+  return Point3(R * p.vector()) + t_;
 }
 
 /* ************************************************************************* */