Functioning "cols" method. Unfortunately, "rows" and "block" are not easy.

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));
 }
 
 //******************************************************************************