The copy/paste from CalibratedCamera (can we not avoid this?) had the side effect of not benefiting from the collapsing derivatives there. I added this in Pinhole now, which should shave off quite a bit of time in the derivative calculation.

gtsam/geometry/PinholeCamera.h

@@ -313,35 +313,43 @@ public:
       boost::optional<Matrix&> Dpoint = boost::none,
       boost::optional<Matrix&> Dcal = boost::none) const {
 
-    if (!Dpose && !Dpoint && !Dcal) {
-      const Point3 pc = pose_.transform_to(pw);
-      if (pc.z() <= 0)
-        throw CheiralityException();
-      const Point2 pn = project_to_camera(pc);
-      return K_.uncalibrate(pn);
-    }
-
-    // world to camera coordinate
-    Matrix Dpc_pose /* 3*6 */, Dpc_point /* 3*3 */;
-    const Point3 pc = pose_.transform_to(pw, Dpc_pose, Dpc_point);
+    // Transform to camera coordinates and check cheirality
+    const Point3 pc = pose_.transform_to(pw);
     if (pc.z() <= 0)
       throw CheiralityException();
 
-    // camera to normalized image coordinate
-    Matrix Dpn_pc; // 2*3
-    const Point2 pn = project_to_camera(pc, Dpn_pc);
+    // Project to normalized image coordinates
+    const Point2 pn = project_to_camera(pc);
 
-    // uncalibration
-    Matrix Dpi_pn; // 2*2
-    const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
+    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;
+      }
 
-    // chain the Jacobian matrices
-    const Matrix Dpi_pc = Dpi_pn * Dpn_pc;
-    if (Dpose)
-      *Dpose = Dpi_pc * Dpc_pose;
-    if (Dpoint)
-      *Dpoint = Dpi_pc * Dpc_point;
-    return pi;
+      // uncalibration
+      Matrix Dpi_pn; // 2*2
+      const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
+
+      // chain the Jacobian matrices
+      if (Dpose)
+        *Dpose = Dpi_pn * Dpn_pose;
+      if (Dpoint)
+        *Dpoint = Dpi_pn * Dpn_point;
+      return pi;
+    } else
+      return K_.uncalibrate(pn, Dcal);
   }
 
   /** project a point at infinity from world coordinate to the image
@@ -531,4 +539,5 @@ private:
     ar & BOOST_SERIALIZATION_NVP(K_);
   }
   /// @}
-      };}
+      }
+      ;}

gtsam/geometry/tests/timePinholeCamera.cpp

@@ -45,8 +45,11 @@ int main()
    */
 
   // Oct 12 2013, iMac 3.06GHz Core i3
-  // 447577 calls/second
-  // 2.23425 musecs/call
+  //  6.78564e+06 calls/second
+  //  0.14737 musecs/call
+  // And after collapse:
+  //  8.71916e+06 calls/second
+  //  0.11469 musecs/call
   {
     long timeLog = clock();
     for(int i = 0; i < n; i++)
@@ -58,8 +61,11 @@ int main()
   }
 
   // Oct 12 2013, iMac 3.06GHz Core i3
-  // 35367.5 calls/second
-  // 28.2746 musecs/call
+  //  258265 calls/second
+  //  3.87199 musecs/call
+  // And after collapse:
+  //  380686 calls/second
+  //  2.62684 musecs/call
   {
     Matrix Dpose, Dpoint;
     long timeLog = clock();
@@ -72,8 +78,11 @@ int main()
   }
 
   // Oct 12 2013, iMac 3.06GHz Core i3
-  // 34325.7 calls/second
-  // 29.1327 musecs/call
+  //  249258 calls/second
+  //  4.0119 musecs/call
+  // And after collapse:
+  //  389135 calls/second
+  //  2.5698 musecs/call
   {
     Matrix Dpose, Dpoint, Dcal;
     long timeLog = clock();