Inlined derivatives in Cal3DS2, gets rid of a lot of duplicated calculations.

.cproject

@@ -1645,6 +1645,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testCal3DS2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testCal3DS2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>

gtsam/geometry/Cal3DS2.cpp

@@ -49,23 +49,52 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
 Point2 Cal3DS2::uncalibrate(const Point2& p,
        boost::optional<Matrix&> H1,
        boost::optional<Matrix&> H2) const {
+
+  // parameters
+  const double fx = fx_, fy = fy_, s = s_;
+  const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
+
   //  rr = x^2 + y^2;
   //  g = (1 + k(1)*rr + k(2)*rr^2);
   //  dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
   //  pi(:,i) = g * pn(:,i) + dp;
   const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
   const double rr = xx + yy;
-  const double g = (1+k1_*rr+k2_*rr*rr) ;
+  const double r4 = rr * rr;
-  const double dx = 2*k3_*xy + k4_*(rr+2*xx) ;
+  const double g = 1. + k1 * rr + k2 * r4;
-  const double dy = 2*k4_*xy + k3_*(rr+2*yy) ;
+  const double dx = 2. * k3 * xy + k4 * (rr + 2. * xx);
+  const double dy = 2. * k4 * xy + k3 * (rr + 2. * yy);
 
-  const double x2 = g*x + dx ;
+  const double pnx = g*x + dx;
-  const double y2 = g*y + dy ;
+  const double pny = g*y + dy;
 
-  if (H1) *H1 = D2d_calibration(p) ;
+  // Inlined derivative for calibration
-  if (H2) *H2 = D2d_intrinsic(p) ;
+  if (H1) {
+    *H1 = Matrix_(2, 9, pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr,
+        fx * x * r4 + s * y * r4, fx * 2. * xy + s * (rr + 2. * yy),
+        fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0,
+        fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy));
+  }
+  // Inlined derivative for points
+  if (H2) {
+    const double dr_dx = 2. * x;
+    const double dr_dy = 2. * y;
+    const double dg_dx = k1 * dr_dx + k2 * 2. * rr * dr_dx;
+    const double dg_dy = k1 * dr_dy + k2 * 2. * rr * dr_dy;
 
-  return Point2(fx_* x2 + s_ * y2 + u0_, fy_ * y2 + v0_) ;
+    const double dDx_dx = 2. * k3 * y + k4 * (dr_dx + 4. * x);
+    const double dDx_dy = 2. * k3 * x + k4 * dr_dy;
+    const double dDy_dx = 2. * k4 * y + k3 * dr_dx;
+    const double dDy_dy = 2. * k4 * x + k3 * (dr_dy + 4. * y);
+
+    Matrix DK = Matrix_(2, 2, fx, s_, 0.0, fy);
+    Matrix DR = Matrix_(2, 2, g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy,
+        y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy);
+
+    *H2 = DK * DR;
+  }
+
+  return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_);
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/timePinholeCamera.cpp

@@ -19,7 +19,7 @@
 #include <iostream>
 
 #include <gtsam/geometry/PinholeCamera.h>
-#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3DS2.h>
 
 using namespace std;
 using namespace gtsam;
@@ -35,8 +35,8 @@ int main()
   ),
   Point3(0,0,0.5));
 
-  static const Cal3_S2 K(625, 625, 0, 0, 0);
+  static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
-  const PinholeCamera<Cal3_S2> camera(pose1,K);
+  const PinholeCamera<Cal3DS2> camera(pose1,K);
   const Point3 point1(-0.08,-0.08, 0.0);
 
   /**
@@ -50,6 +50,12 @@ int main()
   // And after collapse:
   //  8.71916e+06 calls/second
   //  0.11469 musecs/call
+  // Cal3DS2:
+  //  7.04176e+06 calls/second
+  //  0.14201 musecs/call
+  // After Cal3DS2 fix:
+  //  8.17595e+06 calls/second
+  //  0.12231 musecs/call
   {
     long timeLog = clock();
     for(int i = 0; i < n; i++)
@@ -66,6 +72,12 @@ int main()
   // And after collapse:
   //  380686 calls/second
   //  2.62684 musecs/call
+  // Cal3DS2:
+  //  230789 calls/second
+  //  4.33297 musecs/call
+  // After Cal3DS2 fix:
+  //  304354 calls/second
+  //  3.28565 musecs/call
   {
     Matrix Dpose, Dpoint;
     long timeLog = clock();
@@ -83,6 +95,12 @@ int main()
   // And after collapse:
   //  389135 calls/second
   //  2.5698 musecs/call
+  // Cal3DS2:
+  //  206933 calls/second
+  //  4.83248 musecs/call
+  // After Cal3DS2 fix:
+  //  289996 calls/second
+  //  3.44832 musecs/call
   {
     Matrix Dpose, Dpoint, Dcal;
     long timeLog = clock();