Avoiding mallocs and working with fixed blocks where possible, makes a pretty noticeable difference in the inner linearize loop

gtsam/geometry/Cal3Bundler.cpp

@@ -78,17 +78,16 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
   // Derivatives make use of intermediate variables above
   if (Dcal) {
     double rx = r * x, ry = r * y;
-    Eigen::Matrix<double, 2, 3> D;
+    Dcal->resize(2, 3);
-    D << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry;
+    *Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry;
-    *Dcal = D;
   }
 
   if (Dp) {
     const double a = 2. * (k1_ + 2. * k2_ * r);
     const double axx = a * x * x, axy = a * x * y, ayy = a * y * y;
-    Eigen::Matrix<double, 2, 2> D;
+    Dp->resize(2,2);
-    D << g + axx, axy, axy, g + ayy;
+    *Dp << g + axx, axy, axy, g + ayy;
-    *Dp = f_ * D;
+    *Dp *= f_;
   }
 
   return Point2(u0_ + f_ * u, v0_ + f_ * v);

gtsam/geometry/PinholeCamera.h

@@ -271,6 +271,10 @@ public:
    */
   inline static Point2 project_to_camera(const Point3& P,
       boost::optional<Matrix&> Dpoint = boost::none) {
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+    if (P.z() <= 0)
+      throw CheiralityException();
+#endif
     if (Dpoint) {
       double d = 1.0 / P.z(), d2 = d * d;
       *Dpoint = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
@@ -300,40 +304,25 @@ public:
     // Transform to camera coordinates and check cheirality
     const Point3 pc = pose_.transform_to(pw);
 
-#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
-    if (pc.z() <= 0)
-      throw CheiralityException();
-#endif
-
     // Project to normalized image coordinates
     const Point2 pn = project_to_camera(pc);
 
     if (Dpose || Dpoint) {
-      // optimized version of derivatives, see CalibratedCamera.nb
       const double z = pc.z(), d = 1.0 / z;
-      const double u = pn.x(), v = pn.y();
-      Matrix Dpn_pose(2, 6), Dpn_point(2, 3);
-      if (Dpose) {
-        double uv = u * v, uu = u * u, vv = v * v;
-        Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
-      }
-      if (Dpoint) {
-        const Matrix R(pose_.rotation().matrix());
-        Dpn_point << //
-            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);
-        Dpn_point *= d;
-      }
 
       // uncalibration
-      Matrix Dpi_pn; // 2*2
+      Matrix Dpi_pn(2, 2);
       const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
 
       // chain the Jacobian matrices
-      if (Dpose)
+      if (Dpose) {
-        *Dpose = Dpi_pn * Dpn_pose;
+        Dpose->resize(2, 6);
-      if (Dpoint)
+        calculateDpose(pn, d, Dpi_pn, *Dpose);
-        *Dpoint = Dpi_pn * Dpn_point;
+      }
+      if (Dpoint) {
+        Dpoint->resize(2, 3);
+        calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
+      }
       return pi;
     } else
       return K_.uncalibrate(pn, Dcal);
@@ -391,27 +380,29 @@ public:
       boost::optional<Matrix&> Dcamera = boost::none,
       boost::optional<Matrix&> Dpoint = boost::none) const {
 
+    const Point3 pc = pose_.transform_to(pw);
+    const Point2 pn = project_to_camera(pc);
+
     if (!Dcamera && !Dpoint) {
-      const Point3 pc = pose_.transform_to(pw);
-
-#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
-    if (pc.z() <= 0)
-      throw CheiralityException();
-#endif
-      const Point2 pn = project_to_camera(pc);
       return K_.uncalibrate(pn);
-    }
+    } else {
+      const double z = pc.z(), d = 1.0 / z;
 
-    Matrix Dpose, Dp, Dcal;
+      // uncalibration
-    const Point2 pi = this->project(pw, Dpose, Dp, Dcal);
+      Matrix Dcal, Dpi_pn(2, 2);
-    if (Dcamera) {
+      const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
-      *Dcamera = Matrix(2, this->dim());
+
-      Dcamera->leftCols(pose().dim()) = Dpose; // Jacobian wrt pose3
+      if (Dcamera) {
-      Dcamera->rightCols(calibration().dim()) = Dcal; // Jacobian wrt calib
+        Dcamera->resize(2, this->dim());
+        calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>());
+        Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
+      }
+      if (Dpoint) {
+        Dpoint->resize(2, 3);
+        calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
+      }
+      return pi;
     }
-    if (Dpoint)
-      *Dpoint = Dp;
-    return pi;
   }
 
   /// backproject a 2-dimensional point to a 3-dimensional point at given depth
@@ -516,6 +507,50 @@ public:
 
 private:
 
+  /**
+   * Calculate Jacobian with respect to pose
+   * @param pn projection in normalized coordinates
+   * @param d disparity (inverse depth)
+   * @param Dpi_pn derivative of uncalibrate with respect to pn
+   * @param Dpose Output argument, can be matrix or block, assumed right size !
+   * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
+   */
+  template<typename Derived>
+  static void calculateDpose(const Point2& pn, double d, const Matrix& Dpi_pn,
+      Eigen::MatrixBase<Derived> const & Dpose) {
+    // optimized version of derivatives, see CalibratedCamera.nb
+    const double u = pn.x(), v = pn.y();
+    double uv = u * v, uu = u * u, vv = v * v;
+    Eigen::Matrix<double, 2, 6> Dpn_pose;
+    Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
+    assert(Dpose.rows()==2 && Dpose.cols()==6);
+    const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = //
+        Dpi_pn.block<2, 2>(0, 0) * Dpn_pose;
+  }
+
+  /**
+   * Calculate Jacobian with respect to point
+   * @param pn projection in normalized coordinates
+   * @param d disparity (inverse depth)
+   * @param Dpi_pn derivative of uncalibrate with respect to pn
+   * @param Dpoint Output argument, can be matrix or block, assumed right size !
+   * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
+   */
+  template<typename Derived>
+  static void calculateDpoint(const Point2& pn, double d, const Matrix& R,
+      const Matrix& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) {
+    // optimized version of derivatives, see CalibratedCamera.nb
+    const double u = pn.x(), v = pn.y();
+    Eigen::Matrix<double, 2, 3> Dpn_point;
+    Dpn_point << //
+        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);
+    Dpn_point *= d;
+    assert(Dpoint.rows()==2 && Dpoint.cols()==3);
+    const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = //
+        Dpi_pn.block<2, 2>(0, 0) * Dpn_point;
+  }
+
   /// @}
   /// @name Advanced Interface
   /// @{

gtsam/geometry/Rot3.cpp

@@ -101,10 +101,9 @@ Sphere2 Rot3::operator*(const Sphere2& p) const {
 // see doc/math.lyx, SO(3) section
 Point3 Rot3::unrotate(const Point3& p,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  const Matrix Rt(transpose());
+  Point3 q(transpose()*p.vector()); // q = Rt*p
-  Point3 q(Rt*p.vector()); // q = Rt*p
   if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
-  if (H2) *H2 = Rt;
+  if (H2) *H2 = transpose();
   return q;
 }
 

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -169,27 +169,27 @@ static Point2 project3(const Pose3& pose, const Point3& point, const Cal3_S2& ca
 }
 
 /* ************************************************************************* */
-static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) {
+TEST( PinholeCamera, Dproject)
-  Point3 point(point2D.x(), point2D.y(), 1.0);
-  return Camera(pose,cal).projectPointAtInfinity(point);
-}
-
-/* ************************************************************************* */
-TEST( PinholeCamera, Dproject_point_pose)
 {
   Matrix Dpose, Dpoint, Dcal;
   Point2 result = camera.project(point1, Dpose, Dpoint, Dcal);
   Matrix numerical_pose  = numericalDerivative31(project3, pose1, point1, K);
   Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K);
   Matrix numerical_cal   = numericalDerivative33(project3, pose1, point1, K);
-  CHECK(assert_equal(result, Point2(-100,  100) ));
+  CHECK(assert_equal(Point2(-100,  100), result));
-  CHECK(assert_equal(Dpose,  numerical_pose, 1e-7));
+  CHECK(assert_equal(numerical_pose,  Dpose,  1e-7));
-  CHECK(assert_equal(Dpoint, numerical_point,1e-7));
+  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
-  CHECK(assert_equal(Dcal,   numerical_cal,1e-7));
+  CHECK(assert_equal(numerical_cal,   Dcal,   1e-7));
 }
 
 /* ************************************************************************* */
-TEST( PinholeCamera, Dproject_point_pose_Infinity)
+static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) {
+  Point3 point(point2D.x(), point2D.y(), 1.0);
+  return Camera(pose,cal).projectPointAtInfinity(point);
+}
+
+/* ************************************************************************* */
+TEST( PinholeCamera, Dproject_Infinity)
 {
   Matrix Dpose, Dpoint, Dcal;
   Point2 point2D(-0.08,-0.08);
@@ -198,9 +198,26 @@ TEST( PinholeCamera, Dproject_point_pose_Infinity)
   Matrix numerical_pose  = numericalDerivative31(projectInfinity3, pose1, point2D, K);
   Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K);
   Matrix numerical_cal   = numericalDerivative33(projectInfinity3, pose1, point2D, K);
-  CHECK(assert_equal(Dpose,  numerical_pose, 1e-7));
+  CHECK(assert_equal(numerical_pose,  Dpose,  1e-7));
-  CHECK(assert_equal(Dpoint, numerical_point,1e-7));
+  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
-  CHECK(assert_equal(Dcal,   numerical_cal,1e-7));
+  CHECK(assert_equal(numerical_cal,   Dcal,   1e-7));
+}
+
+/* ************************************************************************* */
+static Point2 project4(const Camera& camera, const Point3& point) {
+  return camera.project2(point);
+}
+
+/* ************************************************************************* */
+TEST( PinholeCamera, Dproject2)
+{
+  Matrix Dcamera, Dpoint;
+  Point2 result = camera.project2(point1, Dcamera, Dpoint);
+  Matrix numerical_camera = numericalDerivative21(project4, camera, point1);
+  Matrix numerical_point  = numericalDerivative22(project4, camera, point1);
+  CHECK(assert_equal(result, Point2(-100,  100) ));
+  CHECK(assert_equal(numerical_camera, Dcamera, 1e-7));
+  CHECK(assert_equal(numerical_point,  Dpoint,  1e-7));
 }
 
 /* ************************************************************************* */