diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index c0ac8cd7f..eb1c1bbcc 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/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 Jacobian; private: @@ -53,6 +54,14 @@ private: new (&map_) Eigen::Map(data); } + // Private and very dangerous constructor straight from memory + OptionalJacobian(double* data) : map_(NULL) { + if (data) usurp(data); + } + + template + 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 + // OptionalJacobian(Eigen::Block 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->() - Eigen::Map* operator->(){ return &map_; } + /// operator->() + Eigen::Map* 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 + // OptionalJacobian block(int startRow, int startCol) { + // if (*this) + // OptionalJacobian(map_.block(startRow, startCol)); + // else + // return OptionalJacobian(); + // } + + /// 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 + OptionalJacobian cols(int startCol) { + if (*this) + return OptionalJacobian(&map_(0,startCol)); + else + return OptionalJacobian(); + } + + /// 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 OptionalJacobian rows(int startRow) { ?? } }; // The pure dynamic specialization of this is needed to support diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp index 331fb49eb..de1c07dcf 100644 --- a/gtsam/base/tests/testOptionalJacobian.cpp +++ b/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(); -} - -void testPtr(Matrix23* H = NULL) { - if (H) - *H = Matrix23::Zero(); + *H = kTestMatrix; } 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)); } //******************************************************************************