From b698e30acbe21e3235c30814947240668402e331 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 24 Jun 2014 23:30:30 -0400
Subject: [PATCH] Small speedup in project (result of Visual SLAM slides :-))

---
 gtsam/geometry/PinholeCamera.h |  7 ++++---
 timing/timePinholeCamera.cpp   | 10 ++++++++++
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index 19eb87b5f..709b95181 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -275,11 +275,12 @@ public:
     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) {
-      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);
+      *Dpoint = (Matrix(2, 3) << d, 0.0, -u * d, 0.0, d, -v * d);
     }
-    return Point2(P.x() / P.z(), P.y() / P.z());
+    return Point2(u, v);
   }
 
   /// Project a point into the image and check depth
diff --git a/timing/timePinholeCamera.cpp b/timing/timePinholeCamera.cpp
index 66a8ca099..8e3e12e22 100644
--- a/timing/timePinholeCamera.cpp
+++ b/timing/timePinholeCamera.cpp
@@ -53,6 +53,10 @@ int main()
   // After Cal3DS2 fix: 0.12231 musecs/call
   // Cal3Bundler:       0.12000 musecs/call
   // Cal3Bundler fix:   0.14637 musecs/call
+  // June 24 2014, Macbook Pro 2.3GHz Core i7
+  // GTSAM 3.1:         0.04295 musecs/call
+  // After project fix: 0.04193 musecs/call
+
   {
     long timeLog = clock();
     for(int i = 0; i < n; i++)
@@ -70,6 +74,9 @@ int main()
   // After Cal3DS2 fix: 3.2857 musecs/call
   // Cal3Bundler:       2.6556 musecs/call
   // Cal3Bundler fix:   2.1613 musecs/call
+  // June 24 2014, Macbook Pro 2.3GHz Core i7
+  // GTSAM 3.1:         0.2322 musecs/call
+  // After project fix: 0.2094 musecs/call
   {
     Matrix Dpose, Dpoint;
     long timeLog = clock();
@@ -88,6 +95,9 @@ int main()
   // After Cal3DS2 fix: 3.4483 musecs/call
   // Cal3Bundler:       2.5112 musecs/call
   // Cal3Bundler fix:   2.0946 musecs/call
+  // June 24 2014, Macbook Pro 2.3GHz Core i7
+  // GTSAM 3.1:         0.2294 musecs/call
+  // After project fix: 0.2093 musecs/call
   {
     Matrix Dpose, Dpoint, Dcal;
     long timeLog = clock();