With Natesh, trying out things

gtsam/geometry/Cal3_S2.cpp

@@ -80,12 +80,6 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
   return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
 }
 
-/* ************************************************************************* */
-Point2 Cal3_S2::uncalibrate(const Point2& p) const {
-  const double x = p.x(), y = p.y();
-  return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
-}
-
 /* ************************************************************************* */
 Point2 Cal3_S2::calibrate(const Point2& p) const {
   const double u = p.x(), v = p.y();

gtsam/geometry/Cal3_S2.h

@@ -141,13 +141,6 @@ public:
         1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0).finished();
   }
 
-  /**
-   * convert intrinsic coordinates xy to image coordinates uv
-   * @param p point in intrinsic coordinates
-   * @return point in image coordinates
-   */
-  Point2 uncalibrate(const Point2& p) const;
-
   /**
    * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
    * @param p point in intrinsic coordinates
@@ -155,8 +148,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::optional<Matrix2&> Dp) const;
+  Point2 uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal = boost::none,
+      boost::optional<Matrix2&> Dp = boost::none) const;
 
   /**
    * convert image coordinates uv to intrinsic coordinates xy

gtsam/geometry/PinholeCamera.h

@@ -42,7 +42,9 @@ private:
   Pose3 pose_;
   Calibration K_;
 
+  // Get dimension of calibration type at compile time
   static const int DimK = traits::dimension<Calibration>::value;
+  typedef Eigen::Matrix<double,2,DimK> JacobianK;
 
 public:
 
@@ -353,7 +355,7 @@ public:
       const Point3& pw, //
       boost::optional<Matrix&> Dpose,
       boost::optional<Matrix&> Dpoint,
-      boost::optional<Matrix25&> Dcal) const {
+      boost::optional<Matrix&> Dcal) const {
 
     // Transform to camera coordinates and check cheirality
     const Point3 pc = pose_.transform_to(pw);
@@ -379,7 +381,13 @@ public:
       }
       return pi;
     } else
-      return K_.uncalibrate(pn, Dcal, boost::none);
+      if (Dcal) {
+        JacobianK fixedDcal;
+        const Point2 pi = K_.uncalibrate(pn, fixedDcal);
+        *Dcal = fixedDcal;
+        return pi;
+      } else
+        return K_.uncalibrate(pn);
   }
 
   /** project a point at infinity from world coordinate to the image

gtsam/geometry/tests/testCal3DS2.cpp

@@ -20,6 +20,7 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3DS2.h>
 
+using namespace std;
 using namespace gtsam;
 
 GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2)
@@ -92,6 +93,122 @@ TEST( Cal3DS2, retract)
   CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
 }
 
+/* ************************************************************************* */
+// Test Eigen::Ref
+//TEST( Cal3DS2, Ref) {
+//  // In this case we don't want a copy
+//  Matrix25 fixedDcal;
+//  Eigen::Ref<Matrix25> test1(fixedDcal);
+//  cout << test1 << endl;
+//  test1.resize(2, 5);
+//  test1 = Matrix25::Zero();
+//  cout << test1 << endl;
+//
+//  // In this case we want dynaDcal to be correctly allocate and filled
+//  Matrix dynaDcal(2,5);
+//  Eigen::Ref<Matrix25> test2(dynaDcal);
+//  cout << test2.rows() << "," << test2.cols() << endl;
+//  test2.resize(2, 5);
+//  test2 = Matrix25::Zero();
+//  cout << test2 << endl;
+//}
+
+void test(Eigen::Ref<Matrix25> H) {
+  cout << "test" << endl;
+  cout << H.size() << endl;
+  cout << H.rows() << "," << H.cols() << endl;
+  if (H.size()) {
+    cout << "H before:\n" << H << endl;
+    H.resize(2, 5);
+    H = Matrix25::Zero();
+    cout << "H after:\n" << H << endl;
+  }
+}
+
+TEST( Cal3DS2, Ref) {
+
+  // In this case we don't want anything to happen
+  cout << "\nempty" << endl;
+  Matrix empty;
+  test(empty);
+  cout << "after" << empty << endl;
+
+  // In this case we don't want a copy, TODO: does it copy???
+  cout << "\nfixed" << endl;
+  Matrix25 fixedDcal;
+  fixedDcal.setOnes();
+
+  cout << fixedDcal << endl;
+  test(fixedDcal);
+  cout << "after" << fixedDcal << endl;
+
+  // In this case we want dynaDcal to be correctly allocate and filled
+  cout << "\ndynamic wrong size" << endl;
+  Matrix dynaDcal(8,5);
+  dynaDcal.setOnes();
+
+  cout << dynaDcal << endl;
+  test(dynaDcal);
+  cout << "after" << dynaDcal << endl;
+
+  // In this case we want dynaDcal to be correctly allocate and filled
+  cout << "\ndynamic right size" << endl;
+  Matrix dynamic2(2,5);
+  dynamic2.setOnes();
+
+  cout << dynamic2 << endl;
+  test(dynamic2);
+  cout << "after" << dynamic2 << endl;
+}
+
+void test2(Eigen::Ref<Matrix> H) {
+  cout << "test2" << endl;
+  cout << H.size() << endl;
+  cout << H.rows() << "," << H.cols() << endl;
+  if (H.size()) {
+    cout << "H before:\n" << H << endl;
+    H.resize(2, 5);
+    H = Matrix25::Zero();
+    cout << "H after:\n" << H << endl;
+  }
+}
+
+TEST( Cal3DS2, Ref2) {
+
+  // In this case we don't want anything to happen
+  cout << "\nempty" << endl;
+  Matrix empty;
+  test2(empty);
+  cout << "after" << empty << endl;
+
+  // In this case we don't want a copy, TODO: does it copy???
+  cout << "\nfixed" << endl;
+  Matrix25 fixedDcal;
+  fixedDcal.setOnes();
+
+  cout << fixedDcal << endl;
+  test2(fixedDcal);
+  cout << "after" << fixedDcal << endl;
+
+  // In this case we want dynaDcal to be correctly allocate and filled
+  cout << "\ndynamic wrong size" << endl;
+  Matrix dynaDcal(8,5);
+  dynaDcal.setOnes();
+
+  cout << dynaDcal << endl;
+  test2(dynaDcal);
+  cout << "after" << dynaDcal << endl;
+
+  // In this case we want dynaDcal to be correctly allocate and filled
+  cout << "\ndynamic right size" << endl;
+  Matrix dynamic2(2,5);
+  dynamic2.setOnes();
+
+  cout << dynamic2 << endl;
+  test2(dynamic2);
+  cout << "after" << dynamic2 << endl;
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */