replace Eigen matrix type by gtsam matrix type

gtsam/geometry/Cal3DS2.cpp

@@ -50,8 +50,8 @@ Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const {
 /* ************************************************************************* */
 Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
     boost::optional<Matrix&> H2) const {
-  Eigen::Matrix<double, 2, 9> H1f;
+  Matrix29 H1f;
-  Eigen::Matrix<double, 2, 2> H2f;
+  Matrix2 H2f;
   Point2 u = Base::uncalibrate(p,H1f,H2f);
   if (H1)
     *H1 = H1f;

gtsam/geometry/Cal3DS2_Base.cpp

@@ -53,23 +53,23 @@ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
 }
 
 /* ************************************************************************* */
-static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
+static Matrix29 D2dcalibration(double x, double y, double xx,
     double yy, double xy, double rr, double r4, double pnx, double pny,
-    const Eigen::Matrix<double, 2, 2>& DK) {
+    const Matrix2& DK) {
-  Eigen::Matrix<double, 2, 5> DR1;
+  Matrix25 DR1;
   DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
-  Eigen::Matrix<double, 2, 4> DR2;
+  Matrix24 DR2;
   DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
          y * rr, y * r4, rr + 2 * yy, 2 * xy;
-  Eigen::Matrix<double, 2, 9> D;
+  Matrix29 D;
   D << DR1, DK * DR2;
   return D;
 }
 
 /* ************************************************************************* */
-static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
+static Matrix2 D2dintrinsic(double x, double y, double rr,
     double g, double k1, double k2, double p1, double p2,
-    const Eigen::Matrix<double, 2, 2>& DK) {
+    const Matrix2& DK) {
   const double drdx = 2. * x;
   const double drdy = 2. * y;
   const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
@@ -82,7 +82,7 @@ static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
   const double dDydx = 2. * p2 * y + p1 * drdx;
   const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
 
-  Eigen::Matrix<double, 2, 2> DR;
+  Matrix2 DR;
   DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
         y * dgdx + dDydx, g + y * dgdy + dDydy;
 
@@ -111,7 +111,7 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
   const double pnx = g * x + dx;
   const double pny = g * y + dy;
 
-  Eigen::Matrix<double, 2, 2> DK;
+  Matrix2 DK;
   if (H1 || H2) DK << fx_, s_, 0.0, fy_;
 
   // Derivative for calibration
@@ -163,7 +163,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
   const double rr = xx + yy;
   const double r4 = rr * rr;
   const double g = (1 + k1_ * rr + k2_ * r4);
-  Eigen::Matrix<double, 2, 2> DK;
+  Matrix2 DK;
   DK << fx_, s_, 0.0, fy_;
   return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
 }
@@ -178,7 +178,7 @@ Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const {
   const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
   const double pnx = g * x + dx;
   const double pny = g * y + dy;
-  Eigen::Matrix<double, 2, 2> DK;
+  Matrix2 DK;
   DK << fx_, s_, 0.0, fy_;
   return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
 }

gtsam/geometry/Cal3DS2_Base.h

@@ -115,8 +115,8 @@ public:
    * @return point in (distorted) image coordinates
    */
   Point2 uncalibrate(const Point2& p,
-       boost::optional<Eigen::Matrix<double, 2, 9>&> Dcal = boost::none,
+       boost::optional<Matrix29&> Dcal = boost::none,
-       boost::optional<Eigen::Matrix<double, 2, 2>&> Dp = boost::none) const ;
+       boost::optional<Matrix2&> Dp = boost::none) const ;
 
   /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
   Point2 calibrate(const Point2& p, const double tol=1e-5) const;

gtsam/geometry/Cal3Unified.cpp

@@ -71,17 +71,16 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
 
   // Part2: project NPlane point to pixel plane: use Cal3DS2
   Point2 m(x,y);
-  Eigen::Matrix<double, 2, 9> H1base;
+  Matrix29 H1base;
-  Eigen::Matrix<double, 2, 2> H2base;    // jacobians from Base class
+  Matrix2 H2base;    // jacobians from Base class
   Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
 
   // Inlined derivative for calibration
   if (H1) {
     // part1
-    Eigen::Matrix<double, 2, 1> DU;
+    Vector2 DU, DDS2U;
     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);
@@ -93,7 +92,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
     // part1
     const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
     const double mid = -(xi * xs*ys) * denom;
-    Eigen::Matrix<double, 2, 2> DU;
+    Matrix2 DU;
     DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
         mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;