Cleaned up and tested dynamic version

gtsam/base/OptionalJacobian.h

@@ -11,7 +11,7 @@
 
 /**
  * @file    OptionalJacobian.h
- * @brief   Special class for optional Matrix arguments
+ * @brief   Special class for optional Jacobian arguments
  * @author  Frank Dellaert
  * @author  Natesh Srinivasan
  * @date    Nov 28, 2014
@@ -33,23 +33,24 @@ namespace gtsam {
  * matrix will be resized. Finally, there is a constructor that takes
  * boost::none, the default constructor acts like boost::none, and
  * boost::optional<Eigen::MatrixXd&> is also supported for backwards compatibility.
+ * Below this class, a dynamic version is also implemented.
  */
 template<int Rows, int Cols>
 class OptionalJacobian {
 
 public:
 
-  /// ::Fixed size type
+  /// ::Jacobian size type
-  typedef Eigen::Matrix<double, Rows, Cols> Matrix;
+  typedef Eigen::Matrix<double, Rows, Cols> Jacobian;
 
 private:
 
-  Eigen::Map<Matrix> map_; /// View on constructor argument, if given
+  Eigen::Map<Jacobian> map_; /// View on constructor argument, if given
 
   // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
   // uses "placement new" to make map_ usurp the memory of the fixed size matrix
-  void usurp(double* data, int rows, int cols) {
+  void usurp(double* data) {
-    new (&map_) Eigen::Map<Matrix>(data, rows, cols);
+    new (&map_) Eigen::Map<Jacobian>(data);
   }
 
 public:
@@ -60,24 +61,23 @@ public:
   }
 
   /// Constructor that will usurp data of a fixed-size matrix
-  OptionalJacobian(Matrix& fixed) :
+  OptionalJacobian(Jacobian& fixed) :
       map_(NULL) {
-    usurp(fixed.data(), fixed.rows(), fixed.cols());
+    usurp(fixed.data());
   }
 
   /// Constructor that will usurp data of a fixed-size matrix, pointer version
-  OptionalJacobian(Matrix* fixedPtr) :
+  OptionalJacobian(Jacobian* fixedPtr) :
       map_(NULL) {
     if (fixedPtr)
-      usurp(fixedPtr->data(), fixedPtr->rows(), fixedPtr->cols());
+      usurp(fixedPtr->data());
   }
 
-  // Disabled to deal with dynamic types.
   /// Constructor that will resize a dynamic matrix (unless already correct)
   OptionalJacobian(Eigen::MatrixXd& dynamic) :
       map_(NULL) {
     dynamic.resize(Rows, Cols); // no malloc if correct size
-    usurp(dynamic.data(), dynamic.rows(), dynamic.cols());
+    usurp(dynamic.data());
   }
 
 #ifndef OPTIONALJACOBIAN_NOBOOST
@@ -92,7 +92,7 @@ public:
       map_(NULL) {
     if (optional) {
       optional->resize(Rows, Cols);
-      usurp(optional->data(), optional->rows(), optional->cols());
+      usurp(optional->data());
     }
   }
 
@@ -104,91 +104,69 @@ public:
   }
 
   /// De-reference, like boost optional
-  Eigen::Map<Matrix>& operator*() {
+  Eigen::Map<Jacobian>& operator*() {
     return map_;
   }
 
   /// TODO: operator->()
-  Eigen::Map<Matrix>* operator->(){ return &map_; }
+  Eigen::Map<Jacobian>* operator->(){ return &map_; }
 };
 
 // The pure dynamic specialization of this is needed to support
-// variable-sized types.
+// variable-sized types. Note that this is designed to work like the
+// boost optional scheme from GTSAM 3.
 template<>
-class OptionalJacobian<-1, -1> {
+class OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic> {
 
 public:
 
-  /// Fixed size type
+  /// Jacobian size type
-  typedef Eigen::Matrix<double, -1, -1> Fixed;
+  typedef Eigen::MatrixXd Jacobian;
 
 private:
 
-  Eigen::Map<Fixed> map_; /// View on constructor argument, if given
+  Jacobian* pointer_; /// View on constructor argument, if given
-
-  // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
-  // uses "placement new" to make map_ usurp the memory of the fixed size matrix
-  void usurp(double* data, int rows, int cols) {
-    new (&map_) Eigen::Map<Fixed>(data, rows, cols);
-  }
 
 public:
 
   /// Default constructor acts like boost::none
   OptionalJacobian() :
-      map_(NULL, 0, 0) {
+    pointer_(NULL) {
   }
 
-  /// Constructor that will usurp data of a fixed-size matrix
+  /// Constructor that will resize a dynamic matrix (unless already correct)
-  OptionalJacobian(Fixed& fixed) :
+  OptionalJacobian(Eigen::MatrixXd& dynamic) :
-      map_(NULL, 0, 0) {
+      pointer_(&dynamic) {
-    usurp(fixed.data(), fixed.rows(), fixed.cols());
-  }
-
-  /// Constructor that will usurp data of a fixed-size matrix, pointer version
-  OptionalJacobian(Fixed* fixedPtr) :
-      map_(NULL, 0, 0) {
-    if (fixedPtr)
-      usurp(fixedPtr->data(), fixedPtr->rows(), fixedPtr->cols());
   }
 
 #ifndef OPTIONALJACOBIAN_NOBOOST
 
   /// Constructor with boost::none just makes empty
   OptionalJacobian(boost::none_t none) :
-      map_(NULL, 0, 0) {
+    pointer_(NULL) {
   }
 
   /// Constructor compatible with old-style derivatives
   OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
-      map_(NULL, 0, 0) {
+      pointer_(NULL) {
-    if (optional) {
+    if (optional) pointer_ = &(*optional);
-      usurp(optional->data(), optional->rows(), optional->cols());
-    }
   }
 
 #endif
 
   /// Return true is allocated, false if default constructor was used
   operator bool() const {
-    return map_.data();
+    return pointer_;
   }
 
   /// De-reference, like boost optional
-  Eigen::Map<Fixed>& operator*() {
+  Jacobian& operator*() {
-    return map_;
+    return *pointer_;
   }
 
   /// TODO: operator->()
-  Eigen::Map<Fixed>* operator->(){ return &map_; }
+  Jacobian* operator->(){ return pointer_; }
 };
 
-
-//// these make sure that no dynamic sized matrices are compiled.
-//template<int Cols>
-//class OptionalJacobian<-1, Cols> {};
-//template<int Rows>
-//class OptionalJacobian<Rows, -1> {};
-
 } // namespace gtsam
 

gtsam/base/tests/testOptionalJacobian.cpp

@@ -46,6 +46,18 @@ TEST( OptionalJacobian, Constructors ) {
   boost::optional<Matrix&> optional(dynamic);
   OptionalJacobian<2, 3> H6(optional);
   EXPECT(H6);
+
+  OptionalJacobian<-1, -1> H7;
+  EXPECT(!H7);
+
+  OptionalJacobian<-1, -1> H8(dynamic);
+  EXPECT(H8);
+
+  OptionalJacobian<-1, -1> H9(boost::none);
+  EXPECT(!H9);
+
+  OptionalJacobian<-1, -1> H10(optional);
+  EXPECT(H10);
 }
 
 //******************************************************************************
@@ -59,10 +71,9 @@ void testPtr(Matrix23* H = NULL) {
     *H = Matrix23::Zero();
 }
 
-TEST( OptionalJacobian, Ref2) {
+TEST( OptionalJacobian, Fixed) {
 
-  Matrix expected;
+  Matrix expected = Matrix23::Zero();
-  expected = Matrix23::Zero();
 
   // Default argument does nothing
   test();
@@ -88,13 +99,44 @@ TEST( OptionalJacobian, Ref2) {
   Matrix dynamic1(3, 5);
   dynamic1.setOnes();
   test(dynamic1);
-  EXPECT(assert_equal(expected,dynamic0));
+  EXPECT(assert_equal(expected,dynamic1));
 
   // Dynamic right size
   Matrix dynamic2(2, 5);
   dynamic2.setOnes();
   test(dynamic2);
-  EXPECT(assert_equal(dynamic2,dynamic0));
+  EXPECT(assert_equal(expected, dynamic2));
+}
+
+//******************************************************************************
+void test2(OptionalJacobian<-1,-1> H = boost::none) {
+  if (H)
+    *H = Matrix23::Zero(); // 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
+  Matrix dynamic0;
+  test2(dynamic0);
+  EXPECT(assert_equal(expected,dynamic0));
+
+  // Dynamic wrong size
+  Matrix dynamic1(3, 5);
+  dynamic1.setOnes();
+  test2(dynamic1);
+  EXPECT(assert_equal(expected,dynamic1));
+
+  // Dynamic right size
+  Matrix dynamic2(2, 5);
+  dynamic2.setOnes();
+  test2(dynamic2);
+  EXPECT(assert_equal(expected, dynamic2));
 }
 
 //******************************************************************************