diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp
index 66412b3fc..12b35c8a0 100644
--- a/gtsam/geometry/CalibratedCamera.cpp
+++ b/gtsam/geometry/CalibratedCamera.cpp
@@ -23,26 +23,27 @@ namespace gtsam {
 
 /* ************************************************************************* */
 CalibratedCamera::CalibratedCamera(const Pose3& pose) :
-        pose_(pose) {
+    pose_(pose) {
 }
 
 /* ************************************************************************* */
-CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {}
+CalibratedCamera::CalibratedCamera(const Vector &v) :
+    pose_(Pose3::Expmap(v)) {
+}
 
 /* ************************************************************************* */
 Point2 CalibratedCamera::project_to_camera(const Point3& P,
     boost::optional<Matrix&> H1) {
   if (H1) {
     double d = 1.0 / P.z(), d2 = d * d;
-    *H1 = Matrix_(2, 3,
-        d, 0.0, -P.x() * d2,
-        0.0, d, -P.y() * d2);
+    *H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
   }
   return Point2(P.x() / P.z(), P.y() / P.z());
 }
 
 /* ************************************************************************* */
-Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) {
+Point3 CalibratedCamera::backproject_from_camera(const Point2& p,
+    const double scale) {
   return Point3(p.x() * scale, p.y() * scale, scale);
 }
 
@@ -58,41 +59,39 @@ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
 
 /* ************************************************************************* */
 Point2 CalibratedCamera::project(const Point3& point,
-    boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H2) const {
+    boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const {
 
 #ifdef CALIBRATEDCAMERA_CHAIN_RULE
-  Point3 q = pose_.transform_to(point, H1, H2);
+  Point3 q = pose_.transform_to(point, Dpose, Dpoint);
 #else
   Point3 q = pose_.transform_to(point);
 #endif
   Point2 intrinsic = project_to_camera(q);
 
   // Check if point is in front of camera
-  if(q.z() <= 0)
+  if (q.z() <= 0)
     throw CheiralityException();
 
-  if (H1 || H2) {
+  if (Dpose || Dpoint) {
 #ifdef CALIBRATEDCAMERA_CHAIN_RULE
     // just implement chain rule
     Matrix H;
     project_to_camera(q,H);
-    if (H1) *H1 = H * (*H1);
-    if (H2) *H2 = H * (*H2);
+    if (Dpose) *Dpose = H * (*Dpose);
+    if (Dpoint) *Dpoint = H * (*Dpoint);
 #else
     // optimized version, see CalibratedCamera.nb
-    const double z = q.z(), d = 1.0/z;
-    const double u = intrinsic.x(), v = intrinsic.y(), uv = u*v;
-    if (H1) *H1 = Matrix_(2,6,
-              uv,-(1.+u*u), v, -d , 0., d*u,
-        (1.+v*v),      -uv,-u,  0.,-d , d*v
-        );
-    if (H2) {
+    const double z = q.z(), d = 1.0 / z;
+    const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
+    if (Dpose)
+      *Dpose = Matrix_(2, 6, uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
+          -uv, -u, 0., -d, d * v);
+    if (Dpoint) {
       const Matrix R(pose_.rotation().matrix());
-      *H2 = d * Matrix_(2,3,
-          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)
-        );
+      *Dpoint = d
+          * Matrix_(2, 3, 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));
     }
 #endif
   }
@@ -101,12 +100,12 @@ Point2 CalibratedCamera::project(const Point3& point,
 
 /* ************************************************************************* */
 CalibratedCamera CalibratedCamera::retract(const Vector& d) const {
-  return CalibratedCamera(pose().retract(d)) ;
+  return CalibratedCamera(pose().retract(d));
 }
 
 /* ************************************************************************* */
 Vector CalibratedCamera::localCoordinates(const CalibratedCamera& T2) const {
-  return pose().localCoordinates(T2.pose()) ;
+  return pose().localCoordinates(T2.pose());
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index 91dfad1e8..6e7b5a114 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -24,192 +24,202 @@
 
 namespace gtsam {
 
-  class GTSAM_EXPORT CheiralityException: public ThreadsafeException<CheiralityException> {
-  public:
-    CheiralityException() : ThreadsafeException<CheiralityException>("Cheirality Exception") {}
-  };
+class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
+    CheiralityException> {
+public:
+  CheiralityException() :
+      ThreadsafeException<CheiralityException>("Cheirality Exception") {
+  }
+};
+
+/**
+ * A Calibrated camera class [R|-R't], calibration K=I.
+ * If calibration is known, it is more computationally efficient
+ * to calibrate the measurements rather than try to predict in pixels.
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT CalibratedCamera: public DerivedValue<CalibratedCamera> {
+private:
+  Pose3 pose_; // 6DOF pose
+
+public:
+
+  /// @name Standard Constructors
+  /// @{
+
+  /// default constructor
+  CalibratedCamera() {
+  }
+
+  /// construct with pose
+  explicit CalibratedCamera(const Pose3& pose);
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+
+  /// construct from vector
+  explicit CalibratedCamera(const Vector &v);
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  virtual void print(const std::string& s = "") const {
+    pose_.print(s);
+  }
+
+  /// check equality to another camera
+  bool equals(const CalibratedCamera &camera, double tol = 1e-9) const {
+    return pose_.equals(camera.pose(), tol);
+  }
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /// destructor
+  virtual ~CalibratedCamera() {
+  }
+
+  /// return pose
+  inline const Pose3& pose() const {
+    return pose_;
+  }
+
+  /// compose the two camera poses: TODO Frank says this might not make sense
+  inline const CalibratedCamera compose(const CalibratedCamera &c,
+      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
+          boost::none) const {
+    return CalibratedCamera(pose_.compose(c.pose(), H1, H2));
+  }
+
+  /// between the two camera poses: TODO Frank says this might not make sense
+  inline const CalibratedCamera between(const CalibratedCamera& c,
+      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
+          boost::none) const {
+    return CalibratedCamera(pose_.between(c.pose(), H1, H2));
+  }
+
+  /// invert the camera pose: TODO Frank says this might not make sense
+  inline const CalibratedCamera inverse(boost::optional<Matrix&> H1 =
+      boost::none) const {
+    return CalibratedCamera(pose_.inverse(H1));
+  }
 
   /**
-   * A Calibrated camera class [R|-R't], calibration K=I.
-   * If calibration is known, it is more computationally efficient
-   * to calibrate the measurements rather than try to predict in pixels.
-   * @addtogroup geometry
-   * \nosubgrouping
+   * Create a level camera at the given 2D pose and height
+   * @param pose2 specifies the location and viewing direction
+   * @param height specifies the height of the camera (along the positive Z-axis)
+   * (theta 0 = looking in direction of positive X axis)
    */
-  class GTSAM_EXPORT CalibratedCamera : public DerivedValue<CalibratedCamera> {
-  private:
-    Pose3 pose_; // 6DOF pose
+  static CalibratedCamera Level(const Pose2& pose2, double height);
 
-  public:
+  /// @}
+  /// @name Manifold
+  /// @{
 
-    /// @name Standard Constructors
-    /// @{
+  /// move a cameras pose according to d
+  CalibratedCamera retract(const Vector& d) const;
 
-    /// default constructor
-    CalibratedCamera() {}
+  /// Return canonical coordinate
+  Vector localCoordinates(const CalibratedCamera& T2) const;
 
-    /// construct with pose
-    explicit CalibratedCamera(const Pose3& pose);
+  /// Lie group dimensionality
+  inline size_t dim() const {
+    return 6;
+  }
 
-    /// @}
-    /// @name Advanced Constructors
-    /// @{
+  /// Lie group dimensionality
+  inline static size_t Dim() {
+    return 6;
+  }
 
-    /// construct from vector
-    explicit CalibratedCamera(const Vector &v);
+  /* ************************************************************************* */
+  // measurement functions and derivatives
+  /* ************************************************************************* */
 
-    /// @}
-    /// @name Testable
-    /// @{
+  /// @}
+  /// @name Transformations and mesaurement functions
+  /// @{
+  /**
+   * This function receives the camera pose and the landmark location and
+   * returns the location the point is supposed to appear in the image
+   * @param point a 3D point to be projected
+   * @param Dpose the optionally computed Jacobian with respect to pose
+   * @param Dpoint the optionally computed Jacobian with respect to the 3D point
+   * @return the intrinsic coordinates of the projected point
+   */
+  Point2 project(const Point3& point,
+      boost::optional<Matrix&> Dpose = boost::none,
+      boost::optional<Matrix&> Dpoint = boost::none) const;
 
-    virtual void print(const std::string& s = "") const {
-      pose_.print(s);
-    }
+  /**
+   * projects a 3-dimensional point in camera coordinates into the
+   * camera and returns a 2-dimensional point, no calibration applied
+   * With optional 2by3 derivative
+   */
+  static Point2 project_to_camera(const Point3& cameraPoint,
+      boost::optional<Matrix&> H1 = boost::none);
 
-    /// check equality to another camera
-    bool equals (const CalibratedCamera &camera, double tol = 1e-9) const {
-      return pose_.equals(camera.pose(), tol) ;
-    }
+  /**
+   * backproject a 2-dimensional point to a 3-dimension point
+   */
+  static Point3 backproject_from_camera(const Point2& p, const double scale);
 
-    /// @}
-    /// @name Standard Interface
-    /// @{
+  /**
+   * Calculate range to a landmark
+   * @param point 3D location of landmark
+   * @param H1 optionally computed Jacobian with respect to pose
+   * @param H2 optionally computed Jacobian with respect to the 3D point
+   * @return range (double)
+   */
+  double range(const Point3& point, boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none) const {
+    return pose_.range(point, H1, H2);
+  }
 
-    /// destructor
-    virtual ~CalibratedCamera() {}
+  /**
+   * Calculate range to another pose
+   * @param pose Other SO(3) pose
+   * @param H1 optionally computed Jacobian with respect to pose
+   * @param H2 optionally computed Jacobian with respect to the 3D point
+   * @return range (double)
+   */
+  double range(const Pose3& pose, boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none) const {
+    return pose_.range(pose, H1, H2);
+  }
 
-    /// return pose
-    inline const Pose3& pose() const {  return pose_; }
-
-    /// compose the two camera poses: TODO Frank says this might not make sense
-    inline const CalibratedCamera compose(const CalibratedCamera &c,
-        boost::optional<Matrix&> H1=boost::none,
-        boost::optional<Matrix&> H2=boost::none) const {
-      return CalibratedCamera( pose_.compose(c.pose(), H1, H2) );
-    }
-
-    /// between the two camera poses: TODO Frank says this might not make sense
-    inline const CalibratedCamera between(const CalibratedCamera& c,
-        boost::optional<Matrix&> H1=boost::none,
-        boost::optional<Matrix&> H2=boost::none) const {
-      return CalibratedCamera( pose_.between(c.pose(), H1, H2) );
-    }
-
-    /// invert the camera pose: TODO Frank says this might not make sense
-    inline const CalibratedCamera inverse(boost::optional<Matrix&> H1=boost::none) const {
-      return CalibratedCamera( pose_.inverse(H1) );
-    }
-
-    /**
-     * Create a level camera at the given 2D pose and height
-     * @param pose2 specifies the location and viewing direction
-     * @param height specifies the height of the camera (along the positive Z-axis)
-     * (theta 0 = looking in direction of positive X axis)
-     */
-    static CalibratedCamera Level(const Pose2& pose2, double height);
-
-    /// @}
-    /// @name Manifold
-    /// @{
-
-    /// move a cameras pose according to d
-    CalibratedCamera retract(const Vector& d) const;
-
-    /// Return canonical coordinate
-    Vector localCoordinates(const CalibratedCamera& T2) const;
-
-    /// Lie group dimensionality
-    inline size_t dim() const { return 6 ; }
-
-    /// Lie group dimensionality
-    inline static size_t Dim() { return 6 ; }
-
-
-    /* ************************************************************************* */
-    // measurement functions and derivatives
-    /* ************************************************************************* */
-
-    /// @}
-    /// @name Transformations and mesaurement functions
-    /// @{
-
-    /**
-     * This function receives the camera pose and the landmark location and
-     * returns the location the point is supposed to appear in the image
-     * @param point a 3D point to be projected
-     * @param D_intrinsic_pose the optionally computed Jacobian with respect to pose
-     * @param D_intrinsic_point the optionally computed Jacobian with respect to the 3D point
-     * @return the intrinsic coordinates of the projected point
-     */
-    Point2 project(const Point3& point,
-          boost::optional<Matrix&> D_intrinsic_pose = boost::none,
-          boost::optional<Matrix&> D_intrinsic_point = boost::none) const;
-
-    /**
-     * projects a 3-dimensional point in camera coordinates into the
-     * camera and returns a 2-dimensional point, no calibration applied
-     * With optional 2by3 derivative
-     */
-    static Point2 project_to_camera(const Point3& cameraPoint,
-        boost::optional<Matrix&> H1 = boost::none);
-
-    /**
-     * backproject a 2-dimensional point to a 3-dimension point
-     */
-    static Point3 backproject_from_camera(const Point2& p, const double scale);
-
-    /**
-     * Calculate range to a landmark
-     * @param point 3D location of landmark
-     * @param H1 optionally computed Jacobian with respect to pose
-     * @param H2 optionally computed Jacobian with respect to the 3D point
-     * @return range (double)
-     */
-    double range(const Point3& point,
-        boost::optional<Matrix&> H1=boost::none,
-        boost::optional<Matrix&> H2=boost::none) const {
-      return pose_.range(point, H1, H2); }
-
-    /**
-     * Calculate range to another pose
-     * @param pose Other SO(3) pose
-     * @param H1 optionally computed Jacobian with respect to pose
-     * @param H2 optionally computed Jacobian with respect to the 3D point
-     * @return range (double)
-     */
-    double range(const Pose3& pose,
-        boost::optional<Matrix&> H1=boost::none,
-        boost::optional<Matrix&> H2=boost::none) const {
-      return pose_.range(pose, H1, H2); }
-
-    /**
-     * Calculate range to another camera
-     * @param camera Other camera
-     * @param H1 optionally computed Jacobian with respect to pose
-     * @param H2 optionally computed Jacobian with respect to the 3D point
-     * @return range (double)
-     */
-    double range(const CalibratedCamera& camera,
-        boost::optional<Matrix&> H1=boost::none,
-        boost::optional<Matrix&> H2=boost::none) const {
-      return pose_.range(camera.pose_, H1, H2); }
+  /**
+   * Calculate range to another camera
+   * @param camera Other camera
+   * @param H1 optionally computed Jacobian with respect to pose
+   * @param H2 optionally computed Jacobian with respect to the 3D point
+   * @return range (double)
+   */
+  double range(const CalibratedCamera& camera, boost::optional<Matrix&> H1 =
+      boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+    return pose_.range(camera.pose_, H1, H2);
+  }
 
 private:
 
-    /// @}
-    /// @name Advanced Interface
-    /// @{
+  /// @}
+  /// @name Advanced Interface
+  /// @{
 
-      /** Serialization function */
-      friend class boost::serialization::access;
-      template<class Archive>
-      void serialize(Archive & ar, const unsigned int version) {
-        ar & boost::serialization::make_nvp("CalibratedCamera",
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class Archive>
+  void serialize(Archive & ar, const unsigned int version) {
+    ar
+        & boost::serialization::make_nvp("CalibratedCamera",
             boost::serialization::base_object<Value>(*this));
-        ar & BOOST_SERIALIZATION_NVP(pose_);
-      }
+    ar & BOOST_SERIALIZATION_NVP(pose_);
+  }
 
-      /// @}
-  };
-}
+  /// @}
+      };}
 
diff --git a/gtsam/geometry/tests/timeCalibratedCamera.cpp b/gtsam/geometry/tests/timeCalibratedCamera.cpp
index 2a5ab9064..2e4670a23 100644
--- a/gtsam/geometry/tests/timeCalibratedCamera.cpp
+++ b/gtsam/geometry/tests/timeCalibratedCamera.cpp
@@ -37,43 +37,6 @@ int main()
   const CalibratedCamera camera(pose1);
   const Point3 point1(-0.08,-0.08, 0.0);
 
-  /**
-   * NOTE: because we only have combined derivative functions now,
-   * parts of this test are no longer useful.
-   */
-
-  // Aug 8, iMac 3.06GHz Core i3
-  //  378870 calls/second
-  //  2.63943 musecs/call
-  // AFTER collapse:
-  //  1.57399e+06 calls/second
-  //  0.63533 musecs/call
-//  {
-//    Matrix computed;
-//    long timeLog = clock();
-//    for(int i = 0; i < n; i++)
-//      computed = Dproject_pose(camera, point1);
-//    long timeLog2 = clock();
-//    double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
-//    cout << ((double)n/seconds) << " calls/second" << endl;
-//    cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
-//  }
-
-  // Aug 8, iMac 3.06GHz Core i3
-  // AFTER collapse:
-  //  1.55383e+06 calls/second
-  //  0.64357 musecs/call
-//  {
-//    Matrix computed;
-//    long timeLog = clock();
-//    for(int i = 0; i < n; i++)
-//      computed = Dproject_point(camera, point1);
-//    long timeLog2 = clock();
-//    double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
-//    cout << ((double)n/seconds) << " calls/second" << endl;
-//    cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
-//  }
-
   // Aug 8, iMac 3.06GHz Core i3
   //  371153 calls/second
   //  2.69431 musecs/call