diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index eb5d53eb8..dbb6c09eb 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -72,11 +72,13 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { } /* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const { +Point2 Cal3_S2::uncalibrate(const Point2& p, FixedRef<2, 5> Dcal, + FixedRef<2, 2> Dp) const { const double x = p.x(), y = p.y(); - if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; - if (Dp) *Dp << fx_, s_, 0.0, fy_; + if (Dcal) + *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; + if (Dp) + *Dp << fx_, s_, 0.0, fy_; return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 999a22853..693e1dca8 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -26,6 +26,68 @@ namespace gtsam { +/** + * Eigen::Ref like class that cane take either a fixed size or dynamic + * Eigen matrix. In the latter case, the dynamic matrix will be resized. + * Finally, the default constructor acts like boost::none. + */ +template +class FixedRef { + +public: + + /// Fixed size type + typedef Eigen::Matrix Fixed; + +private: + + bool empty_; + Eigen::Map map_; + + // Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html + // to make map_ usurp the memory of the fixed size matrix + void usurp(double* data) { + new (&map_) Eigen::Map(data); + } + +public: + + /// Defdault constructo acts like boost::none + FixedRef() : + 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 + FixedRef(Matrix& dynamic) : + empty_(false), map_(NULL) { + } + + /// Constructor compatible with old-style + 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_; + } +}; + /** * @brief The most common 5DOF 3D->2D calibration * @addtogroup geometry @@ -148,8 +210,8 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, FixedRef<2,5> Dcal = FixedRef<2,5>(), + FixedRef<2,2> Dp = FixedRef<2,2>()) const; /** * convert image coordinates uv to intrinsic coordinates xy diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index f22e7d065..d726f14f4 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -93,80 +93,6 @@ TEST( Cal3DS2, retract) CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); } -/* ************************************************************************* */ -template -struct FixedRef { - bool empty_; - typedef Eigen::Matrix Fixed; - Eigen::Map map_; - FixedRef() : - empty_(true), map_(NULL) { - } - FixedRef(Fixed& fixed) : - empty_(false), map_(NULL) { - // Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html - // to make map_ usurp the memory of the fixed size matrix - new (&map_) Eigen::Map(fixed.data()); - } - FixedRef(Matrix& dynamic) : - empty_(false), map_(NULL) { - dynamic.resize(Rows, Cols); - // Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html - // to make map_ usurp the memory of the fixed size matrix - new (&map_) Eigen::Map(dynamic.data()); - } - /// Return true is allocated, false if default constructor was used - operator bool() const { - return !empty_; - } - /// Assign a fixed matrix to this - void operator=(const Fixed& fixed) { - map_ << fixed; - } - /// Print - void print(const string& s) { - if (!empty_) - cout << s << map_ << endl; - } -}; - -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; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index aa06a2e29..4214d7850 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -104,6 +104,44 @@ 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;