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

@@ -31,41 +31,70 @@ Cal3DS2::Cal3DS2(const Vector &v):
 Matrix Cal3DS2::K() const { return Matrix_(3,3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); }
 
 /* ************************************************************************* */
-Vector Cal3DS2::vector() const { return Vector_(9, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_) ; }
+Vector Cal3DS2::vector() const { return Vector_(9, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); }
 
 /* ************************************************************************* */
-void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K") ; gtsam::print(Vector(k()), s + ".k") ; }
+void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K"); gtsam::print(Vector(k()), s + ".k"); }
 
 /* ************************************************************************* */
 bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
   if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
       fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
       fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol)
-    return false ;
+    return false;
-  return true ;
+  return true;
 }
 
 /* ************************************************************************* */
 Point2 Cal3DS2::uncalibrate(const Point2& p,
        boost::optional<Matrix&> H1,
        boost::optional<Matrix&> H2) const {
-//  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 dx = 2*k3_*xy + k4_*(rr+2*xx) ;
-  const double dy = 2*k4_*xy + k3_*(rr+2*yy) ;
 
-  const double x2 = g*x + dx ;
+  // parameters
-  const double y2 = g*y + dy ;
+  const double fx = fx_, fy = fy_, s = s_;
+  const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
 
-  if (H1) *H1 = D2d_calibration(p) ;
+  //  rr = x^2 + y^2;
-  if (H2) *H2 = D2d_intrinsic(p) ;
+  //  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 r4 = rr * rr;
+  const double g = 1. + k1 * rr + k2 * r4;
+  const double dx = 2. * k3 * xy + k4 * (rr + 2. * xx);
+  const double dy = 2. * k4 * xy + k3 * (rr + 2. * yy);
 
-  return Point2(fx_* x2 + s_ * y2 + u0_, fy_ * y2 + v0_) ;
+  const double pnx = g*x + dx;
+  const double pny = g*y + dy;
+
+  // Inlined derivative for calibration
+  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;
+
+    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_);
 }
 
 /* ************************************************************************* */
@@ -82,14 +111,14 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
   // iterate until the uncalibrate is close to the actual pixel coordinate
   const int maxIterations = 10;
   int iteration;
-  for ( iteration = 0 ; iteration < maxIterations ; ++iteration ) {
+  for ( iteration = 0; iteration < maxIterations; ++iteration ) {
     if ( uncalibrate(pn).distance(pi) <= tol ) break;
-    const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y ;
+    const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y;
-    const double rr = xx + yy ;
+    const double rr = xx + yy;
-    const double g = (1+k1_*rr+k2_*rr*rr) ;
+    const double g = (1+k1_*rr+k2_*rr*rr);
-    const double dx = 2*k3_*xy + k4_*(rr+2*xx) ;
+    const double dx = 2*k3_*xy + k4_*(rr+2*xx);
-    const double dy = 2*k4_*xy + k3_*(rr+2*yy) ;
+    const double dy = 2*k4_*xy + k3_*(rr+2*yy);
-    pn = (invKPi - Point2(dx,dy))/g ;
+    pn = (invKPi - Point2(dx,dy))/g;
   }
 
   if ( iteration >= maxIterations )
@@ -100,41 +129,41 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
 
 /* ************************************************************************* */
 Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
-  //const double fx = fx_, fy = fy_, s = s_ ;
+  //const double fx = fx_, fy = fy_, s = s_;
   const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
-  //const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y ;
+  //const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y;
-  const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ;
+  const double x = p.x(), y = p.y(), xx = x*x, yy = y*y;
-  const double rr = xx + yy ;
+  const double rr = xx + yy;
-  const double dr_dx = 2*x ;
+  const double dr_dx = 2*x;
-  const double dr_dy = 2*y ;
+  const double dr_dy = 2*y;
-  const double r4 = rr*rr ;
+  const double r4 = rr*rr;
-  const double g = 1 + k1*rr + k2*r4 ;
+  const double g = 1 + k1*rr + k2*r4;
-  const double dg_dx = k1*dr_dx + k2*2*rr*dr_dx ;
+  const double dg_dx = k1*dr_dx + k2*2*rr*dr_dx;
-  const double dg_dy = k1*dr_dy + k2*2*rr*dr_dy ;
+  const double dg_dy = k1*dr_dy + k2*2*rr*dr_dy;
 
-  // Dx = 2*k3*xy + k4*(rr+2*xx) ;
+  // Dx = 2*k3*xy + k4*(rr+2*xx);
-  // Dy = 2*k4*xy + k3*(rr+2*yy) ;
+  // Dy = 2*k4*xy + k3*(rr+2*yy);
-  const double dDx_dx = 2*k3*y + k4*(dr_dx + 4*x) ;
+  const double dDx_dx = 2*k3*y + k4*(dr_dx + 4*x);
-  const double dDx_dy = 2*k3*x + k4*dr_dy ;
+  const double dDx_dy = 2*k3*x + k4*dr_dy;
-  const double dDy_dx = 2*k4*y + k3*dr_dx ;
+  const double dDy_dx = 2*k4*y + k3*dr_dx;
-  const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y) ;
+  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) ;
+             y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy);
 
   return DK * DR;
 }
 
 /* ************************************************************************* */
 Matrix Cal3DS2::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 x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y;
-  const double rr = xx + yy ;
+  const double rr = xx + yy;
-  const double r4 = rr*rr ;
+  const double r4 = rr*rr;
-  const double fx = fx_, fy = fy_, s = s_ ;
+  const double fx = fx_, fy = fy_, s = s_;
-  const double g = (1+k1_*rr+k2_*r4) ;
+  const double g = (1+k1_*rr+k2_*r4);
-  const double dx = 2*k3_*xy + k4_*(rr+2*xx) ;
+  const double dx = 2*k3_*xy + k4_*(rr+2*xx);
-  const double dy = 2*k4_*xy + k3_*(rr+2*yy) ;
+  const double dy = 2*k4_*xy + k3_*(rr+2*yy);
   const double pnx = g*x + dx;
   const double pny = g*y + dy;
 
@@ -144,7 +173,7 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
 }
 
 /* ************************************************************************* */
-Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d) ; }
+Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d); }
 
 /* ************************************************************************* */
 Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { return vector() - T2.vector(); }

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();