Moved and refined prototype, trouble with boost::none as argument

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<Matrix25&> Dcal,
+Point2 Cal3_S2::uncalibrate(const Point2& p, FixedRef<2, 5> Dcal,
-    boost::optional<Matrix2&> Dp) const {
+    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 (Dcal)
-  if (Dp) *Dp << fx_, s_, 0.0, fy_;
+    *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_);
 }
 

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<int Rows, int Cols>
+class FixedRef {
+
+public:
+
+  /// Fixed size type
+  typedef Eigen::Matrix<double, Rows, Cols> Fixed;
+
+private:
+
+  bool empty_;
+  Eigen::Map<Fixed> 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<Fixed>(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<Matrix&> 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<Fixed>& 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<Matrix25&> Dcal = boost::none,
+  Point2 uncalibrate(const Point2& p, FixedRef<2,5> Dcal = FixedRef<2,5>(),
-      boost::optional<Matrix2&> Dp = boost::none) const;
+      FixedRef<2,2> Dp = FixedRef<2,2>()) const;
 
   /**
    * convert image coordinates uv to intrinsic coordinates xy

gtsam/geometry/tests/testCal3DS2.cpp

@@ -93,80 +93,6 @@ TEST( Cal3DS2, retract)
   CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
 }
 
-/* ************************************************************************* */
-template<int Rows, int Cols>
-struct FixedRef {
-  bool empty_;
-  typedef Eigen::Matrix<double, Rows, Cols> Fixed;
-  Eigen::Map<Fixed> 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>(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<Fixed>(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); }
 /* ************************************************************************* */

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;