From 6d05069ef7e01584c0f7a2b002b508bfe520f116 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 23 Dec 2014 14:54:42 +0100 Subject: [PATCH] Cleaned up and tested dynamic version --- gtsam/base/OptionalJacobian.h | 86 +++++++++-------------- gtsam/base/tests/testOptionalJacobian.cpp | 52 ++++++++++++-- 2 files changed, 79 insertions(+), 59 deletions(-) diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 81684b3e1..08b8151dc 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/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 is also supported for backwards compatibility. + * Below this class, a dynamic version is also implemented. */ template class OptionalJacobian { public: - /// ::Fixed size type - typedef Eigen::Matrix Matrix; + /// ::Jacobian size type + typedef Eigen::Matrix Jacobian; private: - Eigen::Map map_; /// View on constructor argument, if given + Eigen::Map 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) { - new (&map_) Eigen::Map(data, rows, cols); + void usurp(double* data) { + new (&map_) Eigen::Map(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& operator*() { + Eigen::Map& operator*() { return map_; } /// TODO: operator->() - Eigen::Map* operator->(){ return &map_; } + Eigen::Map* 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 { public: - /// Fixed size type - typedef Eigen::Matrix Fixed; + /// Jacobian size type + typedef Eigen::MatrixXd Jacobian; private: - Eigen::Map 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) { - new (&map_) Eigen::Map(data, rows, cols); - } + Jacobian* pointer_; /// View on constructor argument, if given public: /// Default constructor acts like boost::none OptionalJacobian() : - map_(NULL, 0, 0) { + pointer_(NULL) { } - /// Constructor that will usurp data of a fixed-size matrix - OptionalJacobian(Fixed& fixed) : - map_(NULL, 0, 0) { - 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()); + /// Constructor that will resize a dynamic matrix (unless already correct) + OptionalJacobian(Eigen::MatrixXd& dynamic) : + pointer_(&dynamic) { } #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 optional) : - map_(NULL, 0, 0) { - if (optional) { - usurp(optional->data(), optional->rows(), optional->cols()); - } + pointer_(NULL) { + if (optional) pointer_ = &(*optional); } #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& operator*() { - return map_; + Jacobian& operator*() { + return *pointer_; } /// TODO: operator->() - Eigen::Map* operator->(){ return &map_; } + Jacobian* operator->(){ return pointer_; } }; - -//// these make sure that no dynamic sized matrices are compiled. -//template -//class OptionalJacobian<-1, Cols> {}; -//template -//class OptionalJacobian {}; - } // namespace gtsam diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp index 6584c82d1..331fb49eb 100644 --- a/gtsam/base/tests/testOptionalJacobian.cpp +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -46,6 +46,18 @@ TEST( OptionalJacobian, Constructors ) { boost::optional 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; - expected = Matrix23::Zero(); + Matrix 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)); } //******************************************************************************