diff --git a/.cproject b/.cproject
index 3de52ad94..292d4146f 100644
--- a/.cproject
+++ b/.cproject
@@ -1653,6 +1653,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testCal3Bundler.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testCal3Bundler.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>
diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index 212758f71..4af546fac 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -74,9 +74,9 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
 //  pi(:,i) = g * pn(:,i)
   const double x = p.x(), y = p.y();
   const double r = x * x + y * y;
-  const double r2 = r * r;
-  const double g = 1 + (k1_ + k2_ * r) * r;
-  const double fg = f_ * g;
+  const double g = 1. + (k1_ + k2_ * r) * r;
+  const double u = g * x, v = g * y;
+  const double f = f_;
 
   // semantic meaningful version
   //if (H1) *H1 = D2d_calibration(p);
@@ -84,26 +84,24 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
 
   // unrolled version, much faster
   if (Dcal || Dp) {
+    double rx = r * x, ry = r * y;
 
-    const double fx = f_ * x, fy = f_ * y;
     if (Dcal) {
-      *Dcal = Matrix_(2, 3, //
-          g * x, fx * r, fx * r2, //
-          g * y, fy * r, fy * r2);
+      Eigen::Matrix<double, 2, 3> D;
+      D << u, f * rx, f * r * rx, v, f * ry, f * r * ry;
+      *Dcal = D;
     }
 
     if (Dp) {
-      const double dr_dx = 2 * x;
-      const double dr_dy = 2 * y;
-      const double dg_dx = k1_ * dr_dx + k2_ * 2 * r * dr_dx;
-      const double dg_dy = k1_ * dr_dy + k2_ * 2 * r * dr_dy;
-      *Dp = Matrix_(2, 2, //
-          fg + fx * dg_dx, fx * dg_dy, //
-          fy * dg_dx, fg + fy * dg_dy);
+      const double dg_dx = 2. * k1_ * x + 4. * k2_ * rx;
+      const double dg_dy = 2. * k1_ * y + 4. * k2_ * ry;
+      Eigen::Matrix<double, 2, 2> D;
+      D << g + x * dg_dx, x * dg_dy, y * dg_dx, g + y * dg_dy;
+      *Dp = f * D;
     }
   }
 
-  return Point2(fg * x, fg * y);
+  return Point2(f * u, f * v);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp
index 67471e880..3580e728d 100644
--- a/gtsam/geometry/tests/testCal3Bundler.cpp
+++ b/gtsam/geometry/tests/testCal3Bundler.cpp
@@ -45,7 +45,9 @@ Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibra
 TEST( Cal3Bundler, Duncalibrate)
 {
   Matrix Dcal, Dp;
-  K.uncalibrate(p, Dcal, Dp);
+  Point2 actual = K.uncalibrate(p, Dcal, Dp);
+  Point2 expected(1182, 1773);
+  CHECK(assert_equal(expected,actual,1e-7));
   Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
   Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
   CHECK(assert_equal(numerical1,Dcal,1e-7));
diff --git a/gtsam/geometry/tests/timePinholeCamera.cpp b/gtsam/geometry/tests/timePinholeCamera.cpp
index fbe458829..abf375308 100644
--- a/gtsam/geometry/tests/timePinholeCamera.cpp
+++ b/gtsam/geometry/tests/timePinholeCamera.cpp
@@ -19,14 +19,14 @@
 #include <iostream>
 
 #include <gtsam/geometry/PinholeCamera.h>
-#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3Bundler.h>
 
 using namespace std;
 using namespace gtsam;
 
 int main()
 {
-  int n = 100000;
+  int n = 1000000;
 
   const Pose3 pose1(Matrix_(3,3,
       1., 0., 0.,
@@ -35,8 +35,10 @@ int main()
   ),
   Point3(0,0,0.5));
 
-  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<Cal3DS2> camera(pose1,K);
+//  static Cal3_S2 K(500, 100, 0.1, 320, 240);
+//  static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
+  static Cal3Bundler K(500, 1e-3, 2.0*1e-3);
+  const PinholeCamera<Cal3Bundler> camera(pose1,K);
   const Point3 point1(-0.08,-0.08, 0.0);
 
   /**
@@ -45,17 +47,12 @@ int main()
    */
 
   // Oct 12 2013, iMac 3.06GHz Core i3
-  //  6.78564e+06 calls/second
-  //  0.14737 musecs/call
-  // 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
+  // Original:          0.14737 musecs/call
+  // After collapse:    0.11469 musecs/call
+  // Cal3DS2:           0.14201 musecs/call
+  // After Cal3DS2 fix: 0.12231 musecs/call
+  // Cal3Bundler:       0.12000 musecs/call
+  // Cal3Bundler fix:   0.13864 musecs/call
   {
     long timeLog = clock();
     for(int i = 0; i < n; i++)
@@ -67,17 +64,12 @@ int main()
   }
 
   // Oct 12 2013, iMac 3.06GHz Core i3
-  //  258265 calls/second
-  //  3.87199 musecs/call
-  // 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
+  // Original:          3.87199 musecs/call
+  // After collapse:    2.62684 musecs/call
+  // Cal3DS2:           4.33297 musecs/call
+  // After Cal3DS2 fix: 3.28565 musecs/call
+  // Cal3Bundler:       2.65559 musecs/call
+  // Cal3Bundler fix:   2.18481 musecs/call
   {
     Matrix Dpose, Dpoint;
     long timeLog = clock();
@@ -90,17 +82,12 @@ int main()
   }
 
   // Oct 12 2013, iMac 3.06GHz Core i3
-  //  249258 calls/second
-  //  4.0119 musecs/call
-  // 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
+  // Original:          4.0119 musecs/call
+  // After collapse:    2.5698 musecs/call
+  // Cal3DS2:           4.83248 musecs/call
+  // After Cal3DS2 fix: 3.44832 musecs/call
+  // Cal3Bundler:       2.51124 musecs/call
+  // Cal3Bundler fix:   2.17292 musecs/call
   {
     Matrix Dpose, Dpoint, Dcal;
     long timeLog = clock();