From bd342261e462c709f385972b0569bf8103ced3d1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 14:57:05 +0100 Subject: [PATCH] New OptionalJacobian header/cpp, moved unit test to base --- gtsam/base/Matrix.h | 73 -------------- gtsam/base/OptionalJacobian.h | 115 ++++++++++++++++++++++ gtsam/base/tests/testOptionalJacobian.cpp | 89 +++++++++++++++++ gtsam/geometry/tests/testCal3_S2.cpp | 38 ------- 4 files changed, 204 insertions(+), 111 deletions(-) create mode 100644 gtsam/base/OptionalJacobian.h create mode 100644 gtsam/base/tests/testOptionalJacobian.cpp diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 516ecd7b2..132bf79ad 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -534,79 +534,6 @@ Eigen::Matrix CayleyFixed(const Eigen::Matrix& A) { std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false); -/** - * FixedRef is an Eigen::Ref like class that can take be constructed using - * either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic - * 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. - */ -template -class FixedRef { - -public: - - /// Fixed size type - typedef Eigen::Matrix Fixed; - -private: - - bool empty_; ///< flag whether initialized or not - 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) { - new (&map_) Eigen::Map(data); - } - -public: - - /// Default constructor acts like boost::none - FixedRef() : - empty_(true), map_(NULL) { - } - - /// Default constructor acts like boost::none - FixedRef(boost::none_t none) : - empty_(true), map_(NULL) { - } - - /// Constructor that will usurp data of a fixed-size matrix - FixedRef(Fixed& fixed) : - empty_(false), map_(NULL) { - usurp(fixed.data()); - } - - /// Constructor that will resize a dynamic matrix (unless already correct) - FixedRef(Matrix& dynamic) : - empty_(false), map_(NULL) { - dynamic.resize(Rows, Cols); // no malloc if correct size - usurp(dynamic.data()); - } - - /// Constructor compatible with old-style derivatives - FixedRef(const boost::optional optional) : - empty_(!optional), map_(NULL) { - if (optional) { - optional->resize(Rows, Cols); - usurp(optional->data()); - } - } - - /// Return true is allocated, false if default constructor was used - operator bool() const { - return !empty_; - } - - /// De-reference, like boost optional - Eigen::Map& operator* () { - return map_; - } - - /// TODO: operator->() -}; - } // namespace gtsam #include diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h new file mode 100644 index 000000000..9b5615423 --- /dev/null +++ b/gtsam/base/OptionalJacobian.h @@ -0,0 +1,115 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file OptionalJacobian.h + * @brief Special class for optional Matrix arguments + * @author Frank Dellaert + * @author Natesh Srinivasan + * @date Nov 28, 2014 + */ + +#pragma once + +#include + +#ifndef OPTIONALJACOBIAN_NOBOOST +#include +#endif + +namespace gtsam { + +/** + * OptionalJacobian is an Eigen::Ref like class that can take be constructed using + * either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic + * 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. + */ +template +class OptionalJacobian { + +public: + + /// Fixed size type + typedef Eigen::Matrix Fixed; + +private: + + bool empty_; ///< flag whether initialized or not + 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) { + new (&map_) Eigen::Map(data); + } + +public: + + /// Default constructor acts like boost::none + OptionalJacobian() : + empty_(true), map_(NULL) { + } + + /// Constructor that will usurp data of a fixed-size matrix + OptionalJacobian(Fixed& fixed) : + empty_(false), map_(NULL) { + usurp(fixed.data()); + } + + /// Constructor that will usurp data of a fixed-size matrix, pointer version + OptionalJacobian(Fixed* fixedPtr) : + empty_(fixedPtr==NULL), map_(NULL) { + if (fixedPtr) + usurp(fixedPtr->data()); + } + + /// Constructor that will resize a dynamic matrix (unless already correct) + OptionalJacobian(Eigen::MatrixXd& dynamic) : + empty_(false), map_(NULL) { + dynamic.resize(Rows, Cols); // no malloc if correct size + usurp(dynamic.data()); + } + +#ifndef OPTIONALJACOBIAN_NOBOOST + + /// Constructor with boost::none just makes empty + OptionalJacobian(boost::none_t none) : + empty_(true), map_(NULL) { + } + + /// Constructor compatible with old-style derivatives + OptionalJacobian(const boost::optional optional) : + empty_(!optional), map_(NULL) { + if (optional) { + optional->resize(Rows, Cols); + usurp(optional->data()); + } + } + +#endif + + /// Return true is allocated, false if default constructor was used + operator bool() const { + return !empty_; + } + + /// De-reference, like boost optional + Eigen::Map& operator*() { + return map_; + } + + /// TODO: operator->() +}; + +} // namespace gtsam + diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp new file mode 100644 index 000000000..a03757e60 --- /dev/null +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -0,0 +1,89 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testOptionalJacobian.cpp + * @brief Unit test for OptionalJacobian + * @author Frank Dellaert + * @date Nov 28, 2014 + **/ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +TEST( OptionalJacobian, Constructors ) +{ + Matrix23 fixed; + Matrix dynamic; + OptionalJacobian<2,3> H1; + OptionalJacobian<2,3> H2(fixed); + OptionalJacobian<2,3> H3(&fixed); + OptionalJacobian<2,3> H4(dynamic); + OptionalJacobian<2,3> H5(boost::none); + boost::optional optional(dynamic); + OptionalJacobian<2,3> H6(optional); +} + +//****************************************************************************** +void test3(OptionalJacobian<2,3> H = OptionalJacobian<2,3>()) { + if (H) + *H = Matrix23::Zero(); +} + +TEST( OptionalJacobian, Ref2) { + + Matrix expected; + expected = Matrix23::Zero(); + + // Default argument does nothing + test3(); + + // Fixed size, no copy + Matrix23 fixed1; + fixed1.setOnes(); + test3(fixed1); + EXPECT(assert_equal(expected,fixed1)); + + // Fixed size, no copy, pointer style + Matrix23 fixed2; + fixed2.setOnes(); + test3(&fixed2); + EXPECT(assert_equal(expected,fixed2)); + + // Empty is no longer a sign we don't want a matrix, we want it resized + Matrix dynamic0; + test3(dynamic0); + EXPECT(assert_equal(expected,dynamic0)); + + // Dynamic wrong size + Matrix dynamic1(3,5); + dynamic1.setOnes(); + test3(dynamic1); + EXPECT(assert_equal(expected,dynamic0)); + + // Dynamic right size + Matrix dynamic2(2,5); + dynamic2.setOnes(); + test3(dynamic2); + EXPECT(assert_equal(dynamic2,dynamic0)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 4214d7850..aa06a2e29 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -104,44 +104,6 @@ TEST(Cal3_S2, between) { } -/* ************************************************************************* */ -void test3(FixedRef<2,3> H = FixedRef<2,3>()) { - if (H) - *H = Matrix23::Zero(); -} - -TEST( Cal3DS2, Ref2) { - - Matrix expected; - expected = Matrix23::Zero(); - - // Default argument does nothing - test3(); - - // Fixed size, no copy - Matrix23 fixedDcal; - fixedDcal.setOnes(); - test3(fixedDcal); - EXPECT(assert_equal(expected,fixedDcal)); - - // Empty is no longer a sign we don't want a matrix, we want it resized - Matrix dynamic0; - test3(dynamic0); - EXPECT(assert_equal(expected,dynamic0)); - - // Dynamic wrong size - Matrix dynamic1(3,5); - dynamic1.setOnes(); - test3(dynamic1); - EXPECT(assert_equal(expected,dynamic0)); - - // Dynamic right size - Matrix dynamic2(2,5); - dynamic2.setOnes(); - test3(dynamic2); - EXPECT(assert_equal(dynamic2,dynamic0)); -} - /* ************************************************************************* */ int main() { TestResult tr;