completed all calibration files

gtsam/geometry/Cal3Bundler.cpp

@@ -34,15 +34,17 @@ Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) :
 }
 
 /* ************************************************************************* */
-Matrix Cal3Bundler::K() const {
+Matrix3 Cal3Bundler::K() const {
   Matrix3 K;
   K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1;
   return K;
 }
 
 /* ************************************************************************* */
-Vector Cal3Bundler::k() const {
+Vector4 Cal3Bundler::k() const {
-  return (Vector(4) << k1_, k2_, 0, 0).finished();
+  Vector4 rvalue_;
+  rvalue_ << k1_, k2_, 0, 0;
+  return rvalue_;
 }
 
 /* ************************************************************************* */
@@ -120,25 +122,27 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
 }
 
 /* ************************************************************************* */
-Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
+Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const {
-  Matrix Dp;
+  Matrix2 Dp;
   uncalibrate(p, boost::none, Dp);
   return Dp;
 }
 
 /* ************************************************************************* */
-Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {
+Matrix23 Cal3Bundler::D2d_calibration(const Point2& p) const {
-  Matrix Dcal;
+  Matrix23 Dcal;
   uncalibrate(p, Dcal, boost::none);
   return Dcal;
 }
 
 /* ************************************************************************* */
-Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
+Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
-  Matrix Dcal, Dp;
+  Matrix23 Dcal;
+  Matrix2 Dp;
   uncalibrate(p, Dcal, Dp);
-  Matrix H(2, 5);
+  Matrix25 H;
-  H << Dp, Dcal;
+  H.block<2,2>(0,0) = Dp;
+  H.block<2,3>(0,2) = Dcal;
   return H;
 }
 

gtsam/geometry/Cal3Bundler.h

@@ -69,8 +69,8 @@ public:
   /// @name Standard Interface
   /// @{
 
-  Matrix K() const; ///< Standard 3*3 calibration matrix
+  Matrix3 K() const; ///< Standard 3*3 calibration matrix
-  Vector k() const; ///< Radial distortion parameters (4 of them, 2 0)
+  Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
 
   Vector3 vector() const;
 
@@ -120,13 +120,13 @@ public:
   Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
-  Matrix D2d_intrinsic(const Point2& p) const;
+  Matrix2 D2d_intrinsic(const Point2& p) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
-  Matrix D2d_calibration(const Point2& p) const;
+  Matrix23 D2d_calibration(const Point2& p) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
-  Matrix D2d_intrinsic_calibration(const Point2& p) const;
+  Matrix25 D2d_intrinsic_calibration(const Point2& p) const;
 
   /// @}
   /// @name Manifold

gtsam/geometry/Cal3DS2_Base.cpp

@@ -28,8 +28,10 @@ Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
     fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
 
 /* ************************************************************************* */
-Matrix Cal3DS2_Base::K() const {
+Matrix3 Cal3DS2_Base::K() const {
-  return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished();
+    Matrix3 K;
+    K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
+    return K;
 }
 
 /* ************************************************************************* */
@@ -39,7 +41,7 @@ Vector Cal3DS2_Base::vector() const {
 
 /* ************************************************************************* */
 void Cal3DS2_Base::print(const std::string& s_) const {
-  gtsam::print(K(), s_ + ".K");
+  gtsam::print((Matrix)K(), s_ + ".K");
   gtsam::print(Vector(k()), s_ + ".k");
 }
 
@@ -156,7 +158,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
 }
 
 /* ************************************************************************* */
-Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
+Matrix2 Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
   const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
   const double rr = xx + yy;
   const double r4 = rr * rr;
@@ -167,7 +169,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
 }
 
 /* ************************************************************************* */
-Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const {
+Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const {
   const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
   const double rr = xx + yy;
   const double r4 = rr * rr;

gtsam/geometry/Cal3DS2_Base.h

@@ -45,7 +45,7 @@ protected:
   double p1_, p2_ ; // tangential distortion
 
 public:
-  Matrix K() const ;
+  Matrix3 K() const ;
   Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
   Vector vector() const ;
 
@@ -121,10 +121,10 @@ public:
   Point2 calibrate(const Point2& p, const double tol=1e-5) const;
 
   /// Derivative of uncalibrate wrpt intrinsic coordinates
-  Matrix D2d_intrinsic(const Point2& p) const ;
+  Matrix2 D2d_intrinsic(const Point2& p) const ;
 
   /// Derivative of uncalibrate wrpt the calibration parameters
-  Matrix D2d_calibration(const Point2& p) const ;
+  Matrix29 D2d_calibration(const Point2& p) const ;
 
 private:
 

gtsam/geometry/Cal3Unified.cpp

@@ -52,8 +52,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
 /* ************************************************************************* */
 // todo: make a fixed sized jacobian version of this
 Point2 Cal3Unified::uncalibrate(const Point2& p,
-       boost::optional<Matrix&> H1,
+       OptionalJacobian<2,10> H1,
-       boost::optional<Matrix&> H2) const {
+       OptionalJacobian<2,2> H2) const {
 
   // this part of code is modified from Cal3DS2,
   // since the second part of this model (after project to normalized plane)
@@ -82,7 +82,6 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
     DU << -xs * sqrt_nx * xi_sqrt_nx2, //
         -ys * sqrt_nx * xi_sqrt_nx2;
 
-    H1->resize(2,10);
     H1->block<2,9>(0,0) = H1base;
     H1->block<2,1>(0,9) = H2base * DU;
   }

gtsam/geometry/Cal3Unified.h

@@ -96,8 +96,8 @@ public:
    * @return point in image coordinates
    */
   Point2 uncalibrate(const Point2& p,
-      boost::optional<Matrix&> Dcal = boost::none,
+      OptionalJacobian<2,10> Dcal = boost::none,
-      boost::optional<Matrix&> Dp = boost::none) const ;
+      OptionalJacobian<2,2> Dp = boost::none) const ;
 
   /// Conver a pixel coordinate to ideal coordinate
   Point2 calibrate(const Point2& p, const double tol=1e-5) const;

gtsam/geometry/Cal3_S2.cpp

@@ -53,7 +53,7 @@ Cal3_S2::Cal3_S2(const std::string &path) :
 
 /* ************************************************************************* */
 void Cal3_S2::print(const std::string& s) const {
-  gtsam::print(matrix(), s);
+  gtsam::print((Matrix)matrix(), s);
 }
 
 /* ************************************************************************* */

gtsam/geometry/Cal3_S2.h

@@ -125,20 +125,24 @@ public:
   }
 
   /// return calibration matrix K
-  Matrix K() const {
+  Matrix3 K() const {
-    return (Matrix(3, 3) <<  fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished();
+    Matrix3 K;
+    K <<  fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
+    return K;
   }
 
   /** @deprecated The following function has been deprecated, use K above */
-  Matrix matrix() const {
+  Matrix3 matrix() const {
     return K();
   }
 
   /// return inverted calibration matrix inv(K)
-  Matrix matrix_inverse() const {
+  Matrix3 matrix_inverse() const {
     const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
-    return (Matrix(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
+    Matrix3 K_inverse;
-        1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0).finished();
+    K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
+        1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0;
+    return K_inverse;
   }
 
   /**

gtsam/geometry/CalibratedCamera.cpp

@@ -33,10 +33,10 @@ CalibratedCamera::CalibratedCamera(const Vector &v) :
 
 /* ************************************************************************* */
 Point2 CalibratedCamera::project_to_camera(const Point3& P,
-    boost::optional<Matrix&> H1) {
+    OptionalJacobian<2,3> H1) {
   if (H1) {
     double d = 1.0 / P.z(), d2 = d * d;
-    *H1 = (Matrix(2, 3) <<  d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2).finished();
+    (*H1) <<  d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
   }
   return Point2(P.x() / P.z(), P.y() / P.z());
 }
@@ -59,10 +59,12 @@ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
 
 /* ************************************************************************* */
 Point2 CalibratedCamera::project(const Point3& point,
-    boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const {
+    OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const {
 
 #ifdef CALIBRATEDCAMERA_CHAIN_RULE
-  Point3 q = pose_.transform_to(point, Dpose, Dpoint);
+  Matrix36 Dpose_;
+  Matrix3 Dpoint_;
+  Point3 q = pose_.transform_to(point, Dpose_, Dpoint_);
 #else
   Point3 q = pose_.transform_to(point);
 #endif
@@ -75,23 +77,24 @@ Point2 CalibratedCamera::project(const Point3& point,
   if (Dpose || Dpoint) {
 #ifdef CALIBRATEDCAMERA_CHAIN_RULE
     // just implement chain rule
-    Matrix H;
+    Matrix23 H;
     project_to_camera(q,H);
-    if (Dpose) *Dpose = H * (*Dpose);
+    if (Dpose) *Dpose = H * (*Dpose_);
-    if (Dpoint) *Dpoint = H * (*Dpoint);
+    if (Dpoint) *Dpoint = H * (*Dpoint_);
 #else
     // optimized version, see CalibratedCamera.nb
     const double z = q.z(), d = 1.0 / z;
     const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
     if (Dpose)
-      *Dpose = (Matrix(2, 6) <<  uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
+      *Dpose <<  uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
-          -uv, -u, 0., -d, d * v).finished();
+          -uv, -u, 0., -d, d * v;
     if (Dpoint) {
-      const Matrix R(pose_.rotation().matrix());
+      const Matrix3 R(pose_.rotation().matrix());
-      *Dpoint = d
+      Matrix23 Dpoint_;
-          * (Matrix(2, 3) <<  R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
+      Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
               R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2),
-              R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2)).finished();
+              R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
+      *Dpoint = d * Dpoint_;
     }
 #endif
   }

gtsam/geometry/CalibratedCamera.h

@@ -90,20 +90,20 @@ public:
 
   /// compose the two camera poses: TODO Frank says this might not make sense
   inline const CalibratedCamera compose(const CalibratedCamera &c,
-      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
+      OptionalJacobian<6,6> H1=boost::none, OptionalJacobian<6,6> H2 =
           boost::none) const {
     return CalibratedCamera(pose_.compose(c.pose(), H1, H2));
   }
 
   /// between the two camera poses: TODO Frank says this might not make sense
   inline const CalibratedCamera between(const CalibratedCamera& c,
-      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
+      OptionalJacobian<6,6> H1 = boost::none, OptionalJacobian<6,6> H2 =
           boost::none) const {
     return CalibratedCamera(pose_.between(c.pose(), H1, H2));
   }
 
   /// invert the camera pose: TODO Frank says this might not make sense
-  inline const CalibratedCamera inverse(boost::optional<Matrix&> H1 =
+  inline const CalibratedCamera inverse(OptionalJacobian<6,6> H1 =
       boost::none) const {
     return CalibratedCamera(pose_.inverse(H1));
   }
@@ -152,8 +152,7 @@ public:
    * @return the intrinsic coordinates of the projected point
    */
   Point2 project(const Point3& point,
-      boost::optional<Matrix&> Dpose = boost::none,
+      OptionalJacobian<2,6> Dpose=boost::none, OptionalJacobian<2,3> Dpoint=boost::none) const;
-      boost::optional<Matrix&> Dpoint = boost::none) const;
 
   /**
    * projects a 3-dimensional point in camera coordinates into the
@@ -161,7 +160,7 @@ public:
    * With optional 2by3 derivative
    */
   static Point2 project_to_camera(const Point3& cameraPoint,
-      boost::optional<Matrix&> H1 = boost::none);
+      OptionalJacobian<2, 3> H1 = boost::none);
 
   /**
    * backproject a 2-dimensional point to a 3-dimension point
@@ -175,8 +174,8 @@ public:
    * @param H2 optionally computed Jacobian with respect to the 3D point
    * @return range (double)
    */
-  double range(const Point3& point, boost::optional<Matrix&> H1 = boost::none,
+  double range(const Point3& point, OptionalJacobian<1,6> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const {
+      OptionalJacobian<1,3> H2 = boost::none) const {
     return pose_.range(point, H1, H2);
   }
 
@@ -187,8 +186,8 @@ public:
    * @param H2 optionally computed Jacobian with respect to the 3D point
    * @return range (double)
    */
-  double range(const Pose3& pose, boost::optional<Matrix&> H1 = boost::none,
+  double range(const Pose3& pose, OptionalJacobian<1,6> H1=boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const {
+      OptionalJacobian<1,6> H2=boost::none) const {
     return pose_.range(pose, H1, H2);
   }
 
@@ -199,8 +198,8 @@ public:
    * @param H2 optionally computed Jacobian with respect to the 3D point
    * @return range (double)
    */
-  double range(const CalibratedCamera& camera, boost::optional<Matrix&> H1 =
+  double range(const CalibratedCamera& camera, OptionalJacobian<1,6> H1 =
-      boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+      boost::none, OptionalJacobian<1,6> H2 = boost::none) const {
     return pose_.range(camera.pose_, H1, H2);
   }