move changable size Jacobian matrix interface from Cal3DS2_Base to Cal3DS2 and Cal3Unified, fix fix size matrix interface issue of Cal3Unified

gtsam/geometry/Cal3DS2.cpp

@@ -47,6 +47,19 @@ Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const {
   return T2.vector() - vector();
 }
 
+/* ************************************************************************* */
+Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
+    boost::optional<Matrix&> H2) const {
+  Eigen::Matrix<double, 2, 9> H1f;
+  Eigen::Matrix<double, 2, 2> H2f;
+  Point2 u = Base::uncalibrate(p,H1f,H2f);
+  if (H1)
+    *H1 = H1f;
+  if (H2)
+    *H2 = H2f;
+  return u;
+}
+
 }
 /* ************************************************************************* */
 

gtsam/geometry/Cal3DS2.h

@@ -88,9 +88,27 @@ public:
   static size_t Dim() { return 9; }  //TODO: make a final dimension variable
 
   /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /**
+   * convert intrinsic coordinates xy to (distorted) image coordinates uv
+   * @param p point in intrinsic coordinates
+   * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in (distorted) image coordinates
+   */
+
+  Point2 uncalibrate(const Point2& p,
+      boost::optional<Matrix&> Dcal = boost::none,
+      boost::optional<Matrix&> Dp = boost::none) const ;
 
 private:
 
+  /// @}
+  /// @name Advanced Interface
+  /// @{
+
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
@@ -100,6 +118,8 @@ private:
         boost::serialization::base_object<Cal3DS2_Base>(*this));
   }
 
+  /// @}
+
 };
 
 // Define GTSAM traits

gtsam/geometry/Cal3DS2_Base.cpp

@@ -126,16 +126,6 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
   return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
 }
 
-Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H2) const {
-  Eigen::Matrix<double, 2, 9> H1f;
-  Eigen::Matrix<double, 2, 2> H2f;
-  Point2 u = uncalibrate(p,H1f,H2f);
-  *H1 = H1f;
-  *H2 = H2f;
-  return u;
-}
-
 
 /* ************************************************************************* */
 Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {

gtsam/geometry/Cal3DS2_Base.h

@@ -117,9 +117,6 @@ public:
   Point2 uncalibrate(const Point2& p,
        boost::optional<Eigen::Matrix<double, 2, 9>&> Dcal = boost::none,
        boost::optional<Eigen::Matrix<double, 2, 2>&> Dp = boost::none) const ;
-  Point2 uncalibrate(const Point2& p,
-      boost::optional<Matrix&> Dcal,
-      boost::optional<Matrix&> Dp) const ;
 
   /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
   Point2 calibrate(const Point2& p, const double tol=1e-5) const;
@@ -130,7 +127,6 @@ public:
   /// Derivative of uncalibrate wrpt the calibration parameters
   Matrix D2d_calibration(const Point2& p) const ;
 
-
 private:
 
   /// @}

gtsam/geometry/Cal3Unified.cpp

@@ -71,26 +71,31 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
 
   // Part2: project NPlane point to pixel plane: use Cal3DS2
   Point2 m(x,y);
-  Matrix H1base, H2base;    // jacobians from Base class
+  Eigen::Matrix<double, 2, 9> H1base;
+  Eigen::Matrix<double, 2, 2> H2base;    // jacobians from Base class
   Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
 
   // Inlined derivative for calibration
   if (H1) {
     // part1
-    Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2,
-        -ys * sqrt_nx * xi_sqrt_nx2);
-    Matrix DDS2U = H2base * DU;
+    Eigen::Matrix<double, 2, 1> DU;
+    DU << -xs * sqrt_nx * xi_sqrt_nx2, //
+        -ys * sqrt_nx * xi_sqrt_nx2;
+    Eigen::Matrix<double, 2, 1> DDS2U;
+    DDS2U = H2base * DU;
 
-    *H1 = collect(2, &H1base, &DDS2U);
+    //*H1 = collect(2, &H1base, &DDS2U);
+    *H1 = (Matrix(2,10) << H1base, DDS2U);
   }
+
   // Inlined derivative for points
   if (H2) {
     // part1
     const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
     const double mid = -(xi * xs*ys) * denom;
-    Matrix DU = (Matrix(2, 2) <<
-        (sqrt_nx + xi*(ys*ys + 1)) * denom, mid,
-        mid, (sqrt_nx + xi*(xs*xs + 1)) * denom);
+    Eigen::Matrix<double, 2, 2> DU;
+    DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
+        mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
 
     *H2 = H2base * DU;
   }