diff --git a/README.md b/README.md
index 2632fbe2e..02ed5149c 100644
--- a/README.md
+++ b/README.md
@@ -62,10 +62,11 @@ If you are using the factor in academic work, please cite the publications above
 In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
 
 
-
 Additional Information
 ----------------------
 
+There is a [`GTSAM users Google group`](https://groups.google.com/forum/#!forum/gtsam-users) for general discussion.
+
 Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. A primer on GTSAM Expressions, 
 which support (superfast) automatic differentiation, 
 can be found on the [GTSAM wiki on BitBucket](https://bitbucket.org/gtborg/gtsam/wiki/Home).
diff --git a/gtsam.h b/gtsam.h
index a5e24715a..d0be06652 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -969,9 +969,10 @@ virtual class SimpleCamera {
 // Some typedefs for common camera types
 // PinholeCameraCal3_S2 is the same as SimpleCamera above
 typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
-typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
-typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
-typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+// due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
+//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
+//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
+//typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
 
 #include <gtsam/geometry/StereoCamera.h>
 class StereoCamera {
@@ -2337,7 +2338,8 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
   gtsam::Point2 measured() const;
 };
 typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
-typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
+// due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
+//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 
 template<CALIBRATION = {gtsam::Cal3_S2}>
 virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp
index 7bc5949cc..bc5279c7b 100644
--- a/gtsam/base/cholesky.cpp
+++ b/gtsam/base/cholesky.cpp
@@ -144,8 +144,9 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) {
   // Check last diagonal element - Eigen does not check it
   if (nFrontal >= 2) {
     int exp2, exp1;
-    (void)frexp(R(topleft + nFrontal - 2, topleft + nFrontal - 2), &exp2);
-    (void)frexp(R(topleft + nFrontal - 1, topleft + nFrontal - 1), &exp1);
+    // NOTE(gareth): R is already the size of A, so we don't need to add topleft here.
+    (void)frexp(R(nFrontal - 2, nFrontal - 2), &exp2);
+    (void)frexp(R(nFrontal - 1, nFrontal - 1), &exp1);
     return (exp2 - exp1 < underconstrainedExponentDifference);
   } else if (nFrontal == 1) {
     int exp1;
diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index dcb0425b7..2fa6e33c6 100644
--- a/gtsam/base/numericalDerivative.h
+++ b/gtsam/base/numericalDerivative.h
@@ -298,7 +298,7 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*
 
 /**
  * Compute numerical derivative in argument 1 of 4-argument function
- * @param h ternary function yielding m-vector
+ * @param h quartic function yielding m-vector
  * @param x1 n-dimensional first argument value
  * @param x2 second argument value
  * @param x3 third argument value
@@ -317,9 +317,15 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
   return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3, x4), x1, delta);
 }
 
+template<class Y, class X1, class X2, class X3, class X4>
+inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&),
+    const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
+  return numericalDerivative41<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
+}
+
 /**
  * Compute numerical derivative in argument 2 of 4-argument function
- * @param h ternary function yielding m-vector
+ * @param h quartic function yielding m-vector
  * @param x1 first argument value
  * @param x2 n-dimensional second argument value
  * @param x3 third argument value
@@ -338,9 +344,15 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
   return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4), x2, delta);
 }
 
+template<class Y, class X1, class X2, class X3, class X4>
+inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&),
+  const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
+  return numericalDerivative42<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
+}
+
 /**
  * Compute numerical derivative in argument 3 of 4-argument function
- * @param h ternary function yielding m-vector
+ * @param h quartic function yielding m-vector
  * @param x1 first argument value
  * @param x2 second argument value
  * @param x3 n-dimensional third argument value
@@ -359,9 +371,15 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
   return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1, x4), x3, delta);
 }
 
+template<class Y, class X1, class X2, class X3, class X4>
+inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&),
+    const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
+  return numericalDerivative43<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
+}
+
 /**
  * Compute numerical derivative in argument 4 of 4-argument function
- * @param h ternary function yielding m-vector
+ * @param h quartic function yielding m-vector
  * @param x1 first argument value
  * @param x2 second argument value
  * @param x3 third argument value
@@ -380,6 +398,152 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
   return numericalDerivative11<Y, X4>(boost::bind(h, x1, x2, x3, _1), x4, delta);
 }
 
+template<class Y, class X1, class X2, class X3, class X4>
+inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&),
+  const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
+  return numericalDerivative44<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
+}
+
+/**
+ * Compute numerical derivative in argument 1 of 5-argument function
+ * @param h quintic function yielding m-vector
+ * @param x1 n-dimensional first argument value
+ * @param x2 second argument value
+ * @param x3 third argument value
+ * @param x4 fourth argument value
+ * @param x5 fifth argument value
+ * @param delta increment for numerical derivative
+ * @return m*n Jacobian computed via central differencing
+ */
+template<class Y, class X1, class X2, class X3, class X4, class X5>
+typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
+    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
+      "Template argument Y must be a manifold type.");
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
+      "Template argument X1 must be a manifold type.");
+  return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3, x4, x5), x1, delta);
+}
+
+template<class Y, class X1, class X2, class X3, class X4, class X5>
+inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
+    const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
+  return numericalDerivative51<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
+}
+
+/**
+ * Compute numerical derivative in argument 2 of 5-argument function
+ * @param h quintic function yielding m-vector
+ * @param x1 n-dimensional first argument value
+ * @param x2 second argument value
+ * @param x3 third argument value
+ * @param x4 fourth argument value
+ * @param x5 fifth argument value
+ * @param delta increment for numerical derivative
+ * @return m*n Jacobian computed via central differencing
+ */
+template<class Y, class X1, class X2, class X3, class X4, class X5>
+typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
+    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
+      "Template argument Y must be a manifold type.");
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
+      "Template argument X1 must be a manifold type.");
+  return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4, x5), x2, delta);
+}
+
+template<class Y, class X1, class X2, class X3, class X4, class X5>
+inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
+    const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
+  return numericalDerivative52<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
+}
+
+/**
+ * Compute numerical derivative in argument 3 of 5-argument function
+ * @param h quintic function yielding m-vector
+ * @param x1 n-dimensional first argument value
+ * @param x2 second argument value
+ * @param x3 third argument value
+ * @param x4 fourth argument value
+ * @param x5 fifth argument value
+ * @param delta increment for numerical derivative
+ * @return m*n Jacobian computed via central differencing
+ */
+template<class Y, class X1, class X2, class X3, class X4, class X5>
+typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
+    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
+      "Template argument Y must be a manifold type.");
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
+      "Template argument X1 must be a manifold type.");
+  return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1, x4, x5), x3, delta);
+}
+
+template<class Y, class X1, class X2, class X3, class X4, class X5>
+inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
+    const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
+  return numericalDerivative53<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
+}
+
+/**
+ * Compute numerical derivative in argument 4 of 5-argument function
+ * @param h quintic function yielding m-vector
+ * @param x1 n-dimensional first argument value
+ * @param x2 second argument value
+ * @param x3 third argument value
+ * @param x4 fourth argument value
+ * @param x5 fifth argument value
+ * @param delta increment for numerical derivative
+ * @return m*n Jacobian computed via central differencing
+ */
+template<class Y, class X1, class X2, class X3, class X4, class X5>
+typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
+    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
+      "Template argument Y must be a manifold type.");
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
+      "Template argument X1 must be a manifold type.");
+  return numericalDerivative11<Y, X4>(boost::bind(h, x1, x2, x3, _1, x5), x4, delta);
+}
+
+template<class Y, class X1, class X2, class X3, class X4, class X5>
+inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
+    const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
+  return numericalDerivative54<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
+}
+
+/**
+ * Compute numerical derivative in argument 5 of 5-argument function
+ * @param h quintic function yielding m-vector
+ * @param x1 n-dimensional first argument value
+ * @param x2 second argument value
+ * @param x3 third argument value
+ * @param x4 fourth argument value
+ * @param x5 fifth argument value
+ * @param delta increment for numerical derivative
+ * @return m*n Jacobian computed via central differencing
+ */
+template<class Y, class X1, class X2, class X3, class X4, class X5>
+typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
+    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
+      "Template argument Y must be a manifold type.");
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
+      "Template argument X1 must be a manifold type.");
+  return numericalDerivative11<Y, X5>(boost::bind(h, x1, x2, x3, x4, _1), x5, delta);
+}
+
+template<class Y, class X1, class X2, class X3, class X4, class X5>
+inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
+    const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
+  return numericalDerivative55<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
+}
+
 /**
  * Compute numerical Hessian matrix.  Requires a single-argument Lie->scalar
  * function.  This is implemented simply as the derivative of the gradient.
diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h
index c55e5067b..4ef09beb1 100644
--- a/gtsam/base/serializationTestHelpers.h
+++ b/gtsam/base/serializationTestHelpers.h
@@ -24,6 +24,9 @@
 
 #include <gtsam/base/serialization.h>
 
+#include <boost/serialization/serialization.hpp>
+
+
 // whether to print the serialized text to stdout
 const bool verbose = false;
 
@@ -142,4 +145,3 @@ bool equalsDereferencedBinary(const T& input = T()) {
 
 } // \namespace serializationTestHelpers
 } // \namespace gtsam
-
diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp
index 1db28bcc8..d863eaba3 100644
--- a/gtsam/base/tests/testSerializationBase.cpp
+++ b/gtsam/base/tests/testSerializationBase.cpp
@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -11,7 +11,7 @@
 
 /**
  * @file testSerializationBase.cpp
- * @brief 
+ * @brief
  * @author Richard Roberts
  * @date Feb 7, 2012
  */
diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp
index 026becebe..3c9fb1563 100644
--- a/gtsam/geometry/CalibratedCamera.cpp
+++ b/gtsam/geometry/CalibratedCamera.cpp
@@ -142,8 +142,11 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
   Matrix23 Dpc_rot;
   Matrix2 Dpc_point;
   const Unit3 pc = pose().rotation().unrotate(pw, Dpose ? &Dpc_rot : 0,
-      Dpose ? &Dpc_point : 0);
-
+      Dpoint ? &Dpc_point : 0);
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+  if (pc.unitVector().z() <= 0)
+    throw CheiralityException();
+#endif
   // camera to normalized image coordinate
   Matrix2 Dpn_pc;
   const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0);
@@ -161,8 +164,12 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
   return pn;
 }
 /* ************************************************************************* */
-Point3 PinholeBase::backproject_from_camera(const Point2& p,
-    const double depth) {
+Point3 PinholeBase::BackprojectFromCamera(const Point2& p,
+    const double depth, OptionalJacobian<3, 2> Dpoint, OptionalJacobian<3, 1> Ddepth) {
+  if (Dpoint)
+    *Dpoint << depth, 0, 0, depth, 0, 0;
+  if (Ddepth)
+    *Ddepth << p.x(), p.y(), 1;
   return Point3(p.x() * depth, p.y() * depth, depth);
 }
 
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index f1fa509c1..f2bed032f 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -201,7 +201,9 @@ public:
       OptionalJacobian<2, 2> Dpoint = boost::none) const;
 
   /// backproject a 2-dimensional point to a 3-dimensional point at given depth
-  static Point3 backproject_from_camera(const Point2& p, const double depth);
+  static Point3 BackprojectFromCamera(const Point2& p, const double depth,
+                                      OptionalJacobian<3, 2> Dpoint = boost::none,
+                                      OptionalJacobian<3, 1> Ddepth = boost::none);
 
   /// @}
   /// @name Advanced interface
@@ -337,8 +339,28 @@ public:
       boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
 
   /// backproject a 2-dimensional point to a 3-dimensional point at given depth
-  Point3 backproject(const Point2& pn, double depth) const {
-    return pose().transform_from(backproject_from_camera(pn, depth));
+  Point3 backproject(const Point2& pn, double depth,
+                     OptionalJacobian<3, 6> Dresult_dpose = boost::none,
+                     OptionalJacobian<3, 2> Dresult_dp = boost::none,
+                     OptionalJacobian<3, 1> Dresult_ddepth = boost::none) const {
+
+    Matrix32 Dpoint_dpn;
+    Matrix31 Dpoint_ddepth;
+    const Point3 point = BackprojectFromCamera(pn, depth,
+                                                 Dresult_dp ? &Dpoint_dpn : 0,
+                                                 Dresult_ddepth ? &Dpoint_ddepth : 0);
+
+    Matrix33 Dresult_dpoint;
+    const Point3 result = pose().transform_from(point, Dresult_dpose,
+                                                       (Dresult_ddepth ||
+                                                        Dresult_dp) ? &Dresult_dpoint : 0);
+
+    if (Dresult_dp)
+      *Dresult_dp = Dresult_dpoint * Dpoint_dpn;
+    if (Dresult_ddepth)
+      *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;
+
+    return result;
   }
 
   /**
@@ -404,4 +426,3 @@ template <typename T>
 struct Range<CalibratedCamera, T> : HasRange<CalibratedCamera, T, double> {};
 
 }  // namespace gtsam
-
diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h
index 43ba78ea2..937eb0785 100644
--- a/gtsam/geometry/PinholePose.h
+++ b/gtsam/geometry/PinholePose.h
@@ -85,25 +85,9 @@ public:
     return pn;
   }
 
-  /** project a point from world coordinate to the image
-   *  @param pw is a point in the world coordinates
-   */
-  Point2 project(const Point3& pw) const {
-    const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates
-    return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates
-  }
-
-  /** project a point from world coordinate to the image
-   *  @param pw is a point at infinity in the world coordinates
-   */
-  Point2 project(const Unit3& pw) const {
-    const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame
-    const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates
-    return calibration().uncalibrate(pn);  // uncalibrate to pixel coordinates
-  }
 
   /** Templated projection of a point (possibly at infinity) from world coordinate to the image
-   *  @param pw is a 3D point or aUnit3 (point at infinity) in world coordinates
+   *  @param pw is a 3D point or a Unit3 (point at infinity) in world coordinates
    *  @param Dpose is the Jacobian w.r.t. pose3
    *  @param Dpoint is the Jacobian w.r.t. point3
    *  @param Dcal is the Jacobian w.r.t. calibration
@@ -131,25 +115,51 @@ public:
   }
 
   /// project a 3D point from world coordinates into the image
-  Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose,
+  Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
       OptionalJacobian<2, 3> Dpoint = boost::none,
       OptionalJacobian<2, DimK> Dcal = boost::none) const {
     return _project(pw, Dpose, Dpoint, Dcal);
   }
 
   /// project a point at infinity from world coordinates into the image
-  Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
+  Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
       OptionalJacobian<2, 2> Dpoint = boost::none,
       OptionalJacobian<2, DimK> Dcal = boost::none) const {
     return _project(pw, Dpose, Dpoint, Dcal);
   }
 
   /// backproject a 2-dimensional point to a 3-dimensional point at given depth
-  Point3 backproject(const Point2& p, double depth) const {
-    const Point2 pn = calibration().calibrate(p);
-    return pose().transform_from(backproject_from_camera(pn, depth));
+  Point3 backproject(const Point2& p, double depth,
+                     OptionalJacobian<3, 6> Dresult_dpose = boost::none,
+                     OptionalJacobian<3, 2> Dresult_dp = boost::none,
+                     OptionalJacobian<3, 1> Dresult_ddepth = boost::none,
+                     OptionalJacobian<3, DimK> Dresult_dcal = boost::none) const {
+    typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
+    Matrix2K Dpn_dcal;
+    Matrix22 Dpn_dp;
+    const Point2 pn = calibration().calibrate(p, Dresult_dcal ? &Dpn_dcal : 0,
+                                                 Dresult_dp ? &Dpn_dp : 0);
+    Matrix32 Dpoint_dpn;
+    Matrix31 Dpoint_ddepth;
+    const Point3 point = BackprojectFromCamera(pn, depth,
+                                               (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0,
+                                               Dresult_ddepth ? &Dpoint_ddepth : 0);
+    Matrix33 Dresult_dpoint;
+    const Point3 result = pose().transform_from(point, Dresult_dpose,
+                                                    (Dresult_ddepth ||
+                                                     Dresult_dp     ||
+                                                     Dresult_dcal) ? &Dresult_dpoint : 0);
+    if (Dresult_dcal)
+       *Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal;  // (3x3)*(3x2)*(2xDimK)
+    if (Dresult_dp)
+       *Dresult_dp =   Dresult_dpoint * Dpoint_dpn * Dpn_dp;    // (3x3)*(3x2)*(2x2)
+    if (Dresult_ddepth)
+       *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;        // (3x3)*(3x1)
+
+    return result;
   }
 
+
   /// backproject a 2-dimensional point to a 3-dimensional point at infinity
   Unit3 backprojectPointAtInfinity(const Point2& p) const {
     const Point2 pn = calibration().calibrate(p);
@@ -302,6 +312,11 @@ public:
       Base(v), K_(new CALIBRATION(K)) {
   }
 
+  // Init from Pose3 and calibration
+  PinholePose(const Pose3 &pose, const Vector &K) :
+      Base(pose), K_(new CALIBRATION(K)) {
+  }
+
   /// @}
   /// @name Testable
   /// @{
diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp
index e629eb3c6..33a88db1f 100644
--- a/gtsam/geometry/tests/testCalibratedCamera.cpp
+++ b/gtsam/geometry/tests/testCalibratedCamera.cpp
@@ -113,7 +113,7 @@ static Point2 Project2(const Unit3& point) {
   return PinholeBase::Project(point);
 }
 
-Unit3 pointAtInfinity(0, 0, 1000);
+Unit3 pointAtInfinity(0, 0, -1000);
 TEST( CalibratedCamera, DProjectInfinity) {
   Matrix test1;
   CalibratedCamera::Project(pointAtInfinity, test1);
@@ -127,6 +127,7 @@ static Point2 project2(const CalibratedCamera& camera, const Point3& point) {
   return camera.project(point);
 }
 
+
 TEST( CalibratedCamera, Dproject_point_pose)
 {
   Matrix Dpose, Dpoint;
@@ -173,7 +174,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity)
 // Add a test with more arbitrary rotation
 TEST( CalibratedCamera, Dproject_point_pose2_infinity)
 {
-  static const Pose3 pose(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
+  static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10));
   static const CalibratedCamera camera(pose);
   Matrix Dpose, Dpoint;
   camera.project2(pointAtInfinity, Dpose, Dpoint);
@@ -183,7 +184,48 @@ TEST( CalibratedCamera, Dproject_point_pose2_infinity)
   CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
 }
 
+static Point3 BackprojectFromCamera(const CalibratedCamera& camera, const Point2& point,
+                          const double& depth) {
+  return camera.BackprojectFromCamera(point, depth);
+}
+TEST( CalibratedCamera, DBackprojectFromCamera)
+{
+  static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10));
+  static const CalibratedCamera camera(pose);
+  static const double depth = 5.4;
+  static const Point2 point(10.1, 50.3);
+  Matrix Dpoint, Ddepth;
+  camera.BackprojectFromCamera(point, depth, Dpoint, Ddepth);
+  Matrix numerical_point = numericalDerivative32(BackprojectFromCamera, camera, point, depth);
+  Matrix numerical_depth  = numericalDerivative33(BackprojectFromCamera, camera, point, depth);
+  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+  CHECK(assert_equal(numerical_depth,  Ddepth, 1e-7));
+}
+
+
+static Point3 backproject(const Pose3& pose, const Point2& point, const double& depth) {
+  return CalibratedCamera(pose).backproject(point, depth);
+}
+TEST( PinholePose, Dbackproject)
+{
+  Matrix36 Dpose;
+  Matrix31 Ddepth;
+  Matrix32 Dpoint;
+  const Point2 point(-100, 100);
+  const double depth(10);
+  static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10));
+  static const CalibratedCamera camera(pose);
+  camera.backproject(point, depth, Dpose, Dpoint, Ddepth);
+  Matrix expectedDpose = numericalDerivative31(backproject, pose, point, depth);
+  Matrix expectedDpoint = numericalDerivative32(backproject, pose, point, depth);
+  Matrix expectedDdepth = numericalDerivative33(backproject, pose,point, depth);
+
+  EXPECT(assert_equal(expectedDpose,   Dpose,   1e-7));
+  EXPECT(assert_equal(expectedDpoint,  Dpoint,  1e-7));
+  EXPECT(assert_equal(expectedDdepth,  Ddepth,  1e-7));
+}
+
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
-
diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp
index ecbb92061..05d48f4cc 100644
--- a/gtsam/geometry/tests/testPinholePose.cpp
+++ b/gtsam/geometry/tests/testPinholePose.cpp
@@ -191,7 +191,7 @@ static Point2 project(const Pose3& pose, const Unit3& pointAtInfinity,
 /* ************************************************************************* */
 TEST( PinholePose, DprojectAtInfinity2)
 {
-  Unit3 pointAtInfinity(0,0,1000);
+  Unit3 pointAtInfinity(0,0,-1000);
   Matrix Dpose, Dpoint;
   Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint);
   Matrix expectedDcamera  = numericalDerivative31(project, pose, pointAtInfinity, K);
@@ -201,6 +201,32 @@ TEST( PinholePose, DprojectAtInfinity2)
   EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
 }
 
+
+static Point3 backproject(const Pose3& pose, const Cal3_S2& cal,
+                          const Point2& p, const double& depth) {
+  return Camera(pose, cal.vector()).backproject(p, depth);
+}
+
+TEST( PinholePose, Dbackproject)
+{
+  Matrix36 Dpose;
+  Matrix31 Ddepth;
+  Matrix32 Dpoint;
+  Matrix Dcal;
+  const Point2 point(-100, 100);
+  const double depth(10);
+  camera.backproject(point, depth, Dpose, Dpoint, Ddepth, Dcal);
+  Matrix expectedDpose = numericalDerivative41(backproject, pose, *K,  point, depth);
+  Matrix expectedDcal = numericalDerivative42(backproject, pose, *K,  point, depth);
+  Matrix expectedDpoint = numericalDerivative43(backproject, pose, *K, point, depth);
+  Matrix expectedDdepth = numericalDerivative44(backproject, pose, *K,  point, depth);
+
+  EXPECT(assert_equal(expectedDpose,   Dpose,   1e-7));
+  EXPECT(assert_equal(expectedDcal,    Dcal,    1e-7));
+  EXPECT(assert_equal(expectedDpoint,  Dpoint,  1e-7));
+  EXPECT(assert_equal(expectedDdepth,  Ddepth,  1e-7));
+}
+
 /* ************************************************************************* */
 static double range0(const Camera& camera, const Point3& point) {
   return camera.range(point);
diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h
index bf2cacc84..4471567fc 100644
--- a/gtsam/inference/ISAM-inst.h
+++ b/gtsam/inference/ISAM-inst.h
@@ -22,43 +22,48 @@
 
 namespace gtsam {
 
-  /* ************************************************************************* */
-  template<class BAYESTREE>
-  void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors, Cliques& orphans, const Eliminate& function)
-  {
-    // Remove the contaminated part of the Bayes tree
-    BayesNetType bn;
-    if (!this->empty()) {
-      const KeySet newFactorKeys = newFactors.keys();
-      this->removeTop(std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans);
-    }
-
-    // Add the removed top and the new factors
-    FactorGraphType factors;
-    factors += bn;
-    factors += newFactors;
-
-    // Add the orphaned subtrees
-    for(const sharedClique& orphan: orphans)
-      factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
-
-    // eliminate into a Bayes net
-    const VariableIndex varIndex(factors);
-    const KeySet newFactorKeys = newFactors.keys();
-    const Ordering constrainedOrdering =
-      Ordering::ColamdConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
-    Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex);
-    this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end());
-    this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end());
+/* ************************************************************************* */
+template<class BAYESTREE>
+void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
+    Cliques& orphans, const Eliminate& function) {
+  // Remove the contaminated part of the Bayes tree
+  BayesNetType bn;
+  const KeySet newFactorKeys = newFactors.keys();
+  if (!this->empty()) {
+    std::vector<Key> keyVector(newFactorKeys.begin(), newFactorKeys.end());
+    this->removeTop(keyVector, bn, orphans);
   }
 
-  /* ************************************************************************* */
-  template<class BAYESTREE>
-  void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors, const Eliminate& function)
-  {
-    Cliques orphans;
-    this->update_internal(newFactors, orphans, function);
-  }
+  // Add the removed top and the new factors
+  FactorGraphType factors;
+  factors += bn;
+  factors += newFactors;
+
+  // Add the orphaned subtrees
+  for (const sharedClique& orphan : orphans)
+    factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
+
+  // Get an ordering where the new keys are eliminated last
+  const VariableIndex index(factors);
+  const Ordering ordering = Ordering::ColamdConstrainedLast(index,
+      std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
+
+  // eliminate all factors (top, added, orphans) into a new Bayes tree
+  auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
+
+  // Re-add into Bayes tree data structures
+  this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(),
+      bayesTree->roots().end());
+  this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
+}
+
+/* ************************************************************************* */
+template<class BAYESTREE>
+void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors,
+    const Eliminate& function) {
+  Cliques orphans;
+  this->update_internal(newFactors, orphans, function);
+}
 
 }
 /// namespace gtsam
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index dc25026d2..ba51864f1 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -11,7 +11,7 @@
 /**
  * @file dataset.cpp
  * @date Jan 22, 2010
- * @author nikai, Luca Carlone
+ * @author Kai Ni, Luca Carlone, Frank Dellaert
  * @brief utility functions for loading datasets
  */
 
@@ -56,8 +56,10 @@ namespace gtsam {
 string findExampleDataFile(const string& name) {
   // Search source tree and installed location
   vector<string> rootsToSearch;
-  rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt
-  rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt
+
+  // Constants below are defined by CMake, see gtsam/gtsam/CMakeLists.txt
+  rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR);
+  rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR);
 
   // Search for filename as given, and with .graph and .txt extensions
   vector<string> namesToSearch;
@@ -75,12 +77,11 @@ string findExampleDataFile(const string& name) {
   }
 
   // If we did not return already, then we did not find the file
-  throw
-invalid_argument(
-    "gtsam::findExampleDataFile could not find a matching file in\n"
-    GTSAM_SOURCE_TREE_DATASET_DIR " or\n"
-    GTSAM_INSTALLED_DATASET_DIR " named\n" +
-    name + ", " + name + ".graph, or " + name + ".txt");
+  throw invalid_argument(
+      "gtsam::findExampleDataFile could not find a matching file in\n"
+      GTSAM_SOURCE_TREE_DATASET_DIR " or\n"
+      GTSAM_INSTALLED_DATASET_DIR " named\n" + name + ", " + name
+          + ".graph, or " + name + ".txt");
 }
 
 /* ************************************************************************* */
@@ -98,6 +99,7 @@ string createRewrittenFileName(const string& name) {
 
   return newpath.string();
 }
+
 /* ************************************************************************* */
 #endif
 
@@ -116,23 +118,20 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
   double v1, v2, v3, v4, v5, v6;
   is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
 
-   if (noiseFormat == NoiseFormatAUTO)
-   {
-     // Try to guess covariance matrix layout
-     if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0)
-     {
-       // NoiseFormatGRAPH
-       noiseFormat = NoiseFormatGRAPH;
-     }
-     else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0)
-     {
-       // NoiseFormatCOV
-       noiseFormat = NoiseFormatCOV;
-     }
-     else
-     {
-       throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format.");
-     }
+  if (noiseFormat == NoiseFormatAUTO) {
+    // Try to guess covariance matrix layout
+    if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0
+        && v6 == 0.0) {
+      // NoiseFormatGRAPH
+      noiseFormat = NoiseFormatGRAPH;
+    } else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0
+        && v6 != 0.0) {
+      // NoiseFormatCOV
+      noiseFormat = NoiseFormatCOV;
+    } else {
+      throw std::invalid_argument(
+          "load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format.");
+    }
   }
 
   // Read matrix and check that diagonal entries are non-zero
@@ -195,6 +194,32 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
   }
 }
 
+/* ************************************************************************* */
+boost::optional<IndexedPose> parseVertex(istream& is, const string& tag) {
+  if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
+    Key id;
+    double x, y, yaw;
+    is >> id >> x >> y >> yaw;
+    return IndexedPose(id, Pose2(x, y, yaw));
+  } else {
+    return boost::none;
+  }
+}
+
+/* ************************************************************************* */
+boost::optional<IndexedEdge> parseEdge(istream& is, const string& tag) {
+  if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
+      || (tag == "ODOMETRY")) {
+
+    Key id1, id2;
+    double x, y, yaw;
+    is >> id1 >> id2 >> x >> y >> yaw;
+    return IndexedEdge(pair<Key, Key>(id1, id2), Pose2(x, y, yaw));
+  } else {
+    return boost::none;
+  }
+}
+
 /* ************************************************************************* */
 GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
     bool addNoise, bool smart, NoiseFormat noiseFormat,
@@ -214,16 +239,15 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
     if (!(is >> tag))
       break;
 
-    if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
-      Key id;
-      double x, y, yaw;
-      is >> id >> x >> y >> yaw;
+    const auto indexed_pose = parseVertex(is, tag);
+    if (indexed_pose) {
+      Key id = indexed_pose->first;
 
       // optional filter
       if (maxID && id >= maxID)
         continue;
 
-      initial->insert(id, Pose2(x, y, yaw));
+      initial->insert(id, indexed_pose->second);
     }
     is.ignore(LINESIZE, '\n');
   }
@@ -251,13 +275,10 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
     if (!(is >> tag))
       break;
 
-    if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
-        || (tag == "ODOMETRY")) {
-
-      // Read transform
-      double x, y, yaw;
-      is >> id1 >> id2 >> x >> y >> yaw;
-      Pose2 l1Xl2(x, y, yaw);
+    auto between_pose = parseEdge(is, tag);
+    if (between_pose) {
+      std::tie(id1, id2) = between_pose->first;
+      Pose2& l1Xl2 = between_pose->second;
 
       // read noise model
       SharedNoiseModel modelInFile = readNoiseModel(is, smart, noiseFormat,
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index dc7804c2d..929ada2c1 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -33,6 +33,7 @@
 #include <string>
 #include <utility> // for pair
 #include <vector>
+#include <iosfwd>
 
 namespace gtsam {
 
@@ -71,6 +72,26 @@ enum KernelFunctionType {
   KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
 };
 
+/// Return type for auxiliary functions
+typedef std::pair<Key, Pose2> IndexedPose;
+typedef std::pair<std::pair<Key, Key>, Pose2> IndexedEdge;
+
+/**
+ * Parse TORO/G2O vertex "id x y yaw"
+ * @param is input stream
+ * @param tag string parsed from input stream, will only parse if vertex type
+ */
+GTSAM_EXPORT boost::optional<IndexedPose> parseVertex(std::istream& is,
+    const std::string& tag);
+
+/**
+ * Parse TORO/G2O edge "id1 id2 x y yaw"
+ * @param is input stream
+ * @param tag string parsed from input stream, will only parse if edge type
+ */
+GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is,
+    const std::string& tag);
+
 /// Return type for load functions
 typedef std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> GraphAndValues;
 
diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp
index b3e208b91..39c2d3722 100644
--- a/gtsam/slam/tests/testDataset.cpp
+++ b/gtsam/slam/tests/testDataset.cpp
@@ -26,6 +26,9 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <iostream>
+#include <sstream>
+
 using namespace gtsam::symbol_shorthand;
 using namespace std;
 using namespace gtsam;
@@ -39,6 +42,37 @@ TEST(dataSet, findExampleDataFile) {
   EXPECT(assert_equal(expected_end, actual_end));
 }
 
+/* ************************************************************************* */
+TEST( dataSet, parseVertex)
+{
+  const string str = "VERTEX2 1 2.000000 3.000000 4.000000";
+  istringstream is(str);
+  string tag;
+  EXPECT(is >> tag);
+  const auto actual = parseVertex(is, tag);
+  EXPECT(actual);
+  if (actual) {
+    EXPECT_LONGS_EQUAL(1, actual->first);
+    EXPECT(assert_equal(Pose2(2, 3, 4), actual->second));
+  }
+}
+
+/* ************************************************************************* */
+TEST( dataSet, parseEdge)
+{
+  const string str = "EDGE2 0 1 2.000000 3.000000 4.000000";
+  istringstream is(str);
+  string tag;
+  EXPECT(is >> tag);
+  const auto actual = parseEdge(is, tag);
+  EXPECT(actual);
+  if (actual) {
+    pair<Key, Key> expected(0, 1);
+    EXPECT(expected == actual->first);
+    EXPECT(assert_equal(Pose2(2, 3, 4), actual->second));
+  }
+}
+
 /* ************************************************************************* */
 TEST( dataSet, load2D)
 {
diff --git a/python/handwritten/geometry/PinholeBaseK.cpp b/python/handwritten/geometry/PinholeBaseK.cpp
index da98783e2..4153ecf06 100644
--- a/python/handwritten/geometry/PinholeBaseK.cpp
+++ b/python/handwritten/geometry/PinholeBaseK.cpp
@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -36,31 +36,38 @@ struct PinholeBaseKCal3_S2Callback : PinholeBaseKCal3_S2, wrapper<PinholeBaseKCa
   }
 };
 
-BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(project_overloads, PinholeBaseKCal3_S2::project, 2, 4)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(project_overloads, PinholeBaseKCal3_S2::project, 1, 4)
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, PinholeBaseKCal3_S2::range, 1, 3)
 
-// Function pointers to desambiguate project() calls
-Point2  (PinholeBaseKCal3_S2::*project1) (const Point3 &pw) const = &PinholeBaseKCal3_S2::project;
-Point2  (PinholeBaseKCal3_S2::*project2) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
-Point2  (PinholeBaseKCal3_S2::*project3) (const Unit3 &pw,  OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
+// Function pointers to disambiguate project() calls
+Point2  (PinholeBaseKCal3_S2::*project1) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
+Point2  (PinholeBaseKCal3_S2::*project2) (const Unit3 &pw,  OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
 
-// function pointers to desambiguate range() calls
+// function pointers to disambiguate range() calls
 double (PinholeBaseKCal3_S2::*range1) (const Point3 &point, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 3 > Dpoint) const = &PinholeBaseKCal3_S2::range;
 double (PinholeBaseKCal3_S2::*range2) (const Pose3 &pose,   OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dpose)  const = &PinholeBaseKCal3_S2::range;
 double (PinholeBaseKCal3_S2::*range3) (const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dother) const = &PinholeBaseKCal3_S2::range;
 
-void exportPinholeBaseK(){
+// wrap projectSafe in a function that returns None or a tuple
+// TODO(frank): find out how to return an ndarray instead
+object project_safe(const PinholeBaseKCal3_S2& camera, const gtsam::Point3& p) {
+  auto result = camera.projectSafe(p);
+  if (result.second)
+    return make_tuple(result.first.x(), result.first.y());
+  else
+    return object();
+}
 
+void exportPinholeBaseK() {
   class_<PinholeBaseKCal3_S2Callback, boost::noncopyable>("PinholeBaseKCal3_S2", no_init)
-    .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), return_value_policy<copy_const_reference>())
-    .def("project", project1)
-    .def("project", project2, project_overloads())
-    .def("project", project3, project_overloads())
-    .def("backproject", &PinholeBaseKCal3_S2::backproject)
-    .def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity)
-    .def("range", range1, range_overloads())
-    .def("range", range2, range_overloads())
-    .def("range", range3, range_overloads())
-  ;
-
-}
\ No newline at end of file
+      .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration),
+           return_value_policy<copy_const_reference>())
+      .def("project_safe", make_function(project_safe))
+      .def("project", project1, project_overloads())
+      .def("project", project2, project_overloads())
+      .def("backproject", &PinholeBaseKCal3_S2::backproject)
+      .def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity)
+      .def("range", range1, range_overloads())
+      .def("range", range2, range_overloads())
+      .def("range", range3, range_overloads());
+}
diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp
index 617a8cc1c..c06d10beb 100644
--- a/tests/testNonlinearISAM.cpp
+++ b/tests/testNonlinearISAM.cpp
@@ -7,6 +7,7 @@
 
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
+#include <gtsam/slam/dataset.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearISAM.h>
@@ -16,7 +17,10 @@
 #include <gtsam/linear/Sampler.h>
 #include <gtsam/geometry/Pose2.h>
 
+#include <iostream>
+#include <sstream>
 
+using namespace std;
 using namespace gtsam;
 
 const double tol=1e-5;
@@ -228,6 +232,93 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) {
   EXPECT(assert_equal(expected.at<Point2>(lm3), actualQR.at<Point2>(lm3), tol));
 }
 
+/* ************************************************************************* */
+TEST(testNonlinearISAM, loop_closures ) {
+  int relinearizeInterval = 100;
+  NonlinearISAM isam(relinearizeInterval);
+
+  // Create a Factor Graph and Values to hold the new data
+  NonlinearFactorGraph graph;
+  Values initialEstimate;
+
+  vector<string> lines;
+  lines.emplace_back("VERTEX2 0 0.000000 0.000000 0.000000");
+  lines.emplace_back("EDGE2 1 0 1.030390 0.011350 -0.012958");
+  lines.emplace_back("VERTEX2 1 1.030390 0.011350 -0.012958");
+  lines.emplace_back("EDGE2 2 1 1.013900 -0.058639 -0.013225");
+  lines.emplace_back("VERTEX2 2 2.043445 -0.060422 -0.026183");
+  lines.emplace_back("EDGE2 3 2 1.027650 -0.007456 0.004833");
+  lines.emplace_back("VERTEX2 3 3.070548 -0.094779 -0.021350");
+  lines.emplace_back("EDGE2 4 3 -0.012016 1.004360 1.566790");
+  lines.emplace_back("VERTEX2 4 3.079976 0.909609 1.545440");
+  lines.emplace_back("EDGE2 5 4 1.016030 0.014565 -0.016304");
+  lines.emplace_back("VERTEX2 5 3.091176 1.925681 1.529136");
+  lines.emplace_back("EDGE2 6 5 1.023890 0.006808 0.010981");
+  lines.emplace_back("VERTEX2 6 3.127018 2.948966 1.540117");
+  lines.emplace_back("EDGE2 7 6 0.957734 0.003159 0.010901");
+  lines.emplace_back("VERTEX2 7 3.153237 3.906347 1.551018");
+  lines.emplace_back("EDGE2 8 7 -1.023820 -0.013668 -3.093240");
+  lines.emplace_back("VERTEX2 8 3.146655 2.882457 -1.542222");
+  lines.emplace_back("EDGE2 9 8 1.023440 0.013984 -0.007802");
+  lines.emplace_back("EDGE2 9 5 0.033943 0.032439 -3.127400");
+  lines.emplace_back("VERTEX2 9 3.189873 1.859834 -1.550024");
+  lines.emplace_back("EDGE2 10 9 1.003350 0.022250 0.023491");
+  lines.emplace_back("EDGE2 10 3 0.044020 0.988477 -1.563530");
+  lines.emplace_back("VERTEX2 10 3.232959 0.857162 -1.526533");
+  lines.emplace_back("EDGE2 11 10 0.977245 0.019042 -0.028623");
+  lines.emplace_back("VERTEX2 11 3.295225 -0.118283 -1.555156");
+  lines.emplace_back("EDGE2 12 11 -0.996880 -0.025512 -3.126915");
+  lines.emplace_back("VERTEX2 12 3.254125 0.878076 1.601114");
+  lines.emplace_back("EDGE2 13 12 0.990646 0.018396 -0.016519");
+  lines.emplace_back("VERTEX2 13 3.205708 1.867709 1.584594");
+  lines.emplace_back("EDGE2 14 13 0.945873 0.008893 -0.002602");
+  lines.emplace_back("EDGE2 14 8 0.015808 0.021059 3.128310");
+  lines.emplace_back("VERTEX2 14 3.183765 2.813370 1.581993");
+  lines.emplace_back("EDGE2 15 14 1.000010 0.006428 0.028234");
+  lines.emplace_back("EDGE2 15 7 -0.014728 -0.001595 -0.019579");
+  lines.emplace_back("VERTEX2 15 3.166141 3.813245 1.610227");
+
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(3.0, 3.0, 0.5));
+
+  // Loop over the different poses, adding the observations to iSAM incrementally
+  for (const string& str : lines) {
+    // scan the tag
+    string tag;
+    istringstream is(str);
+    if (!(is >> tag))
+      break;
+
+    // Check if vertex
+    const auto indexedPose = parseVertex(is, tag);
+    if (indexedPose) {
+      Key id = indexedPose->first;
+      initialEstimate.insert(Symbol('x', id), indexedPose->second);
+      if (id == 0) {
+        noiseModel::Diagonal::shared_ptr priorNoise =
+            noiseModel::Diagonal::Sigmas(Vector3(0.001, 0.001, 0.001));
+        graph.emplace_shared<PriorFactor<Pose2> >(Symbol('x', id),
+            Pose2(0, 0, 0), priorNoise);
+      } else {
+        isam.update(graph, initialEstimate);
+
+        // Clear the factor graph and values for the next iteration
+        graph.resize(0);
+        initialEstimate.clear();
+      }
+    }
+
+    // check if edge
+    const auto betweenPose = parseEdge(is, tag);
+    if (betweenPose) {
+      Key id1, id2;
+      tie(id1, id2) = betweenPose->first;
+      graph.emplace_shared<BetweenFactor<Pose2> >(Symbol('x', id2),
+          Symbol('x', id1), betweenPose->second, model);
+    }
+  }
+  EXPECT_LONGS_EQUAL(16, isam.estimate().size())
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */