Fixed size matrix in project_to_camera

gtsam/base/Matrix.h

@@ -41,6 +41,10 @@ typedef Eigen::Matrix3d Matrix3;
 typedef Eigen::Matrix4d Matrix4;
 typedef Eigen::Matrix<double,6,6> Matrix6;
 
+typedef Eigen::Matrix<double,2,3> Matrix23;
+typedef Eigen::Matrix<double,2,5> Matrix25;
+typedef Eigen::Matrix<double,3,6> Matrix36;
+
 // Matrix expressions for accessing parts of matrices
 typedef Eigen::Block<Matrix> SubMatrix;
 typedef Eigen::Block<const Matrix> ConstSubMatrix;

gtsam/geometry/PinholeCamera.h

@@ -270,17 +270,15 @@ public:
    * @param Dpoint is the 2*3 Jacobian w.r.t. P
    */
   inline static Point2 project_to_camera(const Point3& P,
-      boost::optional<Matrix&> Dpoint = boost::none) {
+      boost::optional<Matrix23&> Dpoint = boost::none) {
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
     if (P.z() <= 0)
       throw CheiralityException();
 #endif
     double d = 1.0 / P.z();
     const double u = P.x() * d, v = P.y() * d;
-    if (Dpoint) {
-      Dpoint->resize(2,3);
+    if (Dpoint)
       *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
-    }
     return Point2(u, v);
   }
 
@@ -356,7 +354,7 @@ public:
     Dpc_pose.block(0, 0, 3, 3) = Dpc_rot;
 
     // camera to normalized image coordinate
-    Matrix Dpn_pc; // 2*3
+    Matrix23 Dpn_pc; // 2*3
     const Point2 pn = project_to_camera(pc, Dpn_pc);
 
     // uncalibration

gtsam/slam/EssentialMatrixFactor.h

@@ -174,12 +174,14 @@ public:
     } else {
 
       // Calculate derivatives. TODO if slow: optimize with Mathematica
-      //     3*2        3*3       3*3        2*3
-      Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2;
+      //     3*2        3*3       3*3
+      Matrix D_1T2_dir, DdP2_rot, DP2_point;
 
       Point3 _1T2 = E.direction().point3(D_1T2_dir);
       Point3 d1T2 = d * _1T2;
       Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
+
+      Matrix23 Dpn_dP2;
       pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
 
       if (DE) {