diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake
index 4c44d2cb3..b17618f49 100644
--- a/cmake/GtsamMatlabWrap.cmake
+++ b/cmake/GtsamMatlabWrap.cmake
@@ -1,51 +1,64 @@
+# Check / set dependent variables for MATLAB wrapper
+if(GTSAM_INSTALL_MATLAB_TOOLBOX)
+    find_package(Matlab COMPONENTS MEX_COMPILER REQUIRED)
+    if(NOT Matlab_MEX_COMPILER)
+        message(FATAL_ERROR "Cannot find MEX compiler binary. Please check your Matlab installation and ensure MEX in installed as well.")
+        endif()
+
+    if(GTSAM_BUILD_TYPE_POSTFIXES)
+        set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
+    endif()
+
+    if(NOT BUILD_SHARED_LIBS)
+        message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries.  If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
+    endif()
+endif()
+
 # Set up cache options
 option(GTSAM_MEX_BUILD_STATIC_MODULE "Build MATLAB wrapper statically (increases build time)" OFF)
 set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation")
 set(GTSAM_TOOLBOX_INSTALL_PATH "" CACHE PATH "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox")
 if(NOT GTSAM_TOOLBOX_INSTALL_PATH)
-	set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox")
+    set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox")
 endif()
 
 # GTSAM_MEX_BUILD_STATIC_MODULE is not for Windows - on Windows any static
 # are already compiled into the library by the linker
 if(GTSAM_MEX_BUILD_STATIC_MODULE AND WIN32)
-	message(FATAL_ERROR "GTSAM_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries.  To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).")
+    message(FATAL_ERROR "GTSAM_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries. To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).")
 endif()
 
-# Try to automatically configure mex path
-if(APPLE)
-	file(GLOB matlab_bin_directories "/Applications/MATLAB*/bin")
-	set(mex_program_name "mex")
-elseif(WIN32)
-	file(GLOB matlab_bin_directories "C:/Program Files*/MATLAB/*/bin")
-	set(mex_program_name "mex.bat")
-else()
-	file(GLOB matlab_bin_directories "/usr/local/MATLAB/*/bin")
-	set(mex_program_name "mex")
-endif()
+set(MEX_COMMAND ${Matlab_MEX_COMPILER} CACHE PATH "Path to MATLAB MEX compiler")
+set(MATLAB_ROOT ${Matlab_ROOT_DIR} CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
 
+# Try to automatically configure mex path from provided custom `bin` path.
 if(GTSAM_CUSTOM_MATLAB_PATH)
-	set(matlab_bin_directories ${GTSAM_CUSTOM_MATLAB_PATH})
-endif()
+    set(matlab_bin_directory ${GTSAM_CUSTOM_MATLAB_PATH})
 
-# Run find_program explicitly putting $PATH after our predefined program
-# directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents
-# finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is
-# on the system path.
-list(REVERSE matlab_bin_directories) # Reverse list so the highest version (sorted alphabetically) is preferred
-find_program(MEX_COMMAND ${mex_program_name}
-	PATHS ${matlab_bin_directories} ENV PATH
-	NO_DEFAULT_PATH)
-mark_as_advanced(FORCE MEX_COMMAND)
-# Now that we have mex, trace back to find the Matlab installation root
-get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
-get_filename_component(mex_path "${MEX_COMMAND}" PATH)
-if(mex_path MATCHES ".*/win64$")
-	get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE)
-else()
-	get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
+    if(WIN32)
+        set(mex_program_name "mex.bat")
+    else()
+        set(mex_program_name "mex")
+    endif()
+
+    # Run find_program explicitly putting $PATH after our predefined program
+    # directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents
+    # finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is
+    # on the system path.
+    find_program(MEX_COMMAND ${mex_program_name}
+       PATHS ${matlab_bin_directory} ENV PATH
+       NO_DEFAULT_PATH)
+
+    mark_as_advanced(FORCE MEX_COMMAND)
+    # Now that we have mex, trace back to find the Matlab installation root
+    get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
+    get_filename_component(mex_path "${MEX_COMMAND}" PATH)
+    if(mex_path MATCHES ".*/win64$")
+       get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE)
+    else()
+       get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
+    endif()
 endif()
-set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
 
 
 # User-friendly wrapping function.  Builds a mex module from the provided
diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake
index 85a529e49..ee86066a2 100644
--- a/cmake/HandleGeneralOptions.cmake
+++ b/cmake/HandleGeneralOptions.cmake
@@ -24,6 +24,7 @@ option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available"
 option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
 option(GTSAM_THROW_CHEIRALITY_EXCEPTION  "Throw exception when a triangulated point is behind a camera" ON)
 option(GTSAM_BUILD_PYTHON                "Enable/Disable building & installation of Python module with pybind11" OFF)
+option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab toolbox"  OFF)
 option(GTSAM_ALLOW_DEPRECATED_SINCE_V41  "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
 option(GTSAM_SUPPORT_NESTED_DISSECTION   "Support Metis-based nested dissection" ON)
 option(GTSAM_TANGENT_PREINTEGRATION      "Use new ImuFactor with integration on tangent space" ON)
@@ -40,16 +41,5 @@ elseif(GTSAM_ROT3_EXPMAP)
     set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE)
 endif()
 
-# Options relating to MATLAB wrapper
-# TODO: Check for matlab mex binary before handling building of binaries
-option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab toolbox"  OFF)
+# Set the default Python version. This is later updated in HandlePython.cmake.
 set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.")
-
-# Check / set dependent variables for MATLAB wrapper
-if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES)
-    set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
-endif()
-
-if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
-    message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries.  If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
-endif()
diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp
index a8a9715e0..793927d7e 100644
--- a/examples/ImuFactorsExample.cpp
+++ b/examples/ImuFactorsExample.cpp
@@ -125,7 +125,7 @@ int main(int argc, char* argv[]) {
   output_filename = var_map["output_filename"].as<string>();
   use_isam = var_map["use_isam"].as<bool>();
 
-  ISAM2* isam2;
+  ISAM2* isam2 = 0;
   if (use_isam) {
     printf("Using ISAM2\n");
     ISAM2Params parameters;
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 37c4a1f88..e0e037f52 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -199,7 +199,7 @@ if(WIN32)
 else()
   if("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
     # Suppress all warnings from 3rd party sources.
-    set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w")
+    set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w -Wno-everything")
   else()
     set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-Wno-error")
   endif()
diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h
new file mode 100644
index 000000000..d9e12f7d2
--- /dev/null
+++ b/gtsam/geometry/Cal3.h
@@ -0,0 +1,66 @@
+/* ----------------------------------------------------------------------------
+
+ * 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)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   Cal3.h
+ * @brief  Common code for all Calibration models.
+ * @author Varun Agrawal
+ */
+
+/**
+ * @addtogroup geometry
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Point2.h>
+
+namespace gtsam {
+
+/**
+ * Function which makes use of the Implicit Function Theorem to compute the
+ * Jacobians of `calibrate` using `uncalibrate`.
+ * This is useful when there are iterative operations in the `calibrate`
+ * function which make computing jacobians difficult.
+ * 
+ * Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can
+ * easily compute the Jacobians:
+ * df/pi = -I (pn and pi are independent args)
+ * Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
+ * Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
+ *
+ * @tparam Cal Calibration model.
+ * @tparam Dim The number of parameters in the calibration model.
+ * @param p Calibrated point.
+ * @param Dcal optional 2*p Jacobian wrpt `p` Cal3DS2 parameters.
+ * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates.
+ */
+template <typename Cal, size_t Dim>
+void calibrateJacobians(const Cal& calibration, const Point2& pn,
+                        OptionalJacobian<2, Dim> Dcal = boost::none,
+                        OptionalJacobian<2, 2> Dp = boost::none) {
+  if (Dcal || Dp) {
+    Eigen::Matrix<double, 2, Dim> H_uncal_K;
+    Matrix22 H_uncal_pn, H_uncal_pn_inv;
+
+    // Compute uncalibrate Jacobians
+    calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
+
+    H_uncal_pn_inv = H_uncal_pn.inverse();
+
+    if (Dp) *Dp = H_uncal_pn_inv;
+    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
+  }
+}
+
+//TODO(Varun) Make common base class for all calibration models.
+
+}  // \ namespace gtsam
diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index b198643b0..a73bfec52 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -121,24 +121,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
     throw std::runtime_error(
         "Cal3Bundler::calibrate fails to converge. need a better initialization");
 
-  // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
-  // Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians
-  // df/pi = -I (pn and pi are independent args)
-  // Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
-  // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
-  Matrix23 H_uncal_K;
-  Matrix22 H_uncal_pn, H_uncal_pn_inv;
-
-  if (Dcal || Dp) {
-    // Compute uncalibrate Jacobians
-    uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
-
-    H_uncal_pn_inv = H_uncal_pn.inverse();
-
-    if (Dp) *Dp = H_uncal_pn_inv;
-    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
-
-  }
+  calibrateJacobians<Cal3Bundler, dimension>(*this, pn, Dcal, Dp);
 
   return pn;
 }
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 2e3fab002..8c836b504 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index 7fd453d45..e66c3d124 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -44,8 +44,8 @@ public:
   Cal3DS2() : Base() {}
 
   Cal3DS2(double fx, double fy, double s, double u0, double v0,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
-        Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {}
+      double k1, double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) :
+        Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
 
   virtual ~Cal3DS2() {}
 
diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp
index 6c03883ce..d175259f2 100644
--- a/gtsam/geometry/Cal3DS2_Base.cpp
+++ b/gtsam/geometry/Cal3DS2_Base.cpp
@@ -13,6 +13,7 @@
  * @file Cal3DS2_Base.cpp
  * @date Feb 28, 2010
  * @author ydjian
+ * @author Varun Agrawal
  */
 
 #include <gtsam/base/Vector.h>
@@ -24,8 +25,16 @@
 namespace gtsam {
 
 /* ************************************************************************* */
-Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
-    fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
+Cal3DS2_Base::Cal3DS2_Base(const Vector& v)
+    : fx_(v(0)),
+      fy_(v(1)),
+      s_(v(2)),
+      u0_(v(3)),
+      v0_(v(4)),
+      k1_(v(5)),
+      k2_(v(6)),
+      p1_(v(7)),
+      p2_(v(8)) {}
 
 /* ************************************************************************* */
 Matrix3 Cal3DS2_Base::K() const {
@@ -94,9 +103,8 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
 }
 
 /* ************************************************************************* */
-Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
-    OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const {
-
+Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal,
+                                 OptionalJacobian<2, 2> Dp) 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)];
@@ -115,37 +123,44 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
   const double pny = g * y + dy;
 
   Matrix2 DK;
-  if (H1 || H2) DK << fx_, s_, 0.0, fy_;
+  if (Dcal || Dp) {
+    DK << fx_, s_, 0.0, fy_;
+  }
 
   // Derivative for calibration
-  if (H1)
-    *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
+  if (Dcal) {
+    *Dcal = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
+  }
 
   // Derivative for points
-  if (H2)
-    *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
+  if (Dp) {
+    *Dp = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
+  }
 
   // Regular uncalibrate after distortion
   return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
 }
 
 /* ************************************************************************* */
-Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
+Point2 Cal3DS2_Base::calibrate(const Point2& pi, OptionalJacobian<2, 9> Dcal,
+                               OptionalJacobian<2, 2> Dp) const {
   // Use the following fixed point iteration to invert the radial distortion.
   // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
 
-  const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
-                       (1 / fy_) * (pi.y() - v0_));
+  const Point2 invKPi((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
+                      (1 / fy_) * (pi.y() - v0_));
 
-  // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
+  // initialize by ignoring the distortion at all, might be problematic for
+  // pixels around boundary
   Point2 pn = invKPi;
 
   // iterate until the uncalibrate is close to the actual pixel coordinate
   const int maxIterations = 10;
   int iteration;
   for (iteration = 0; iteration < maxIterations; ++iteration) {
-    if (distance2(uncalibrate(pn), pi) <= tol) break;
-    const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
+    if (distance2(uncalibrate(pn), pi) <= tol_) break;
+    const double px = pn.x(), py = pn.y(), xy = px * py, xx = px * px,
+                 yy = py * py;
     const double rr = xx + yy;
     const double g = (1 + k1_ * rr + k2_ * rr * rr);
     const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
@@ -153,8 +168,11 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
     pn = (invKPi - Point2(dx, dy)) / g;
   }
 
-  if ( iteration >= maxIterations )
-    throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
+  if (iteration >= maxIterations)
+    throw std::runtime_error(
+        "Cal3DS2::calibrate fails to converge. need a better initialization");
+
+  calibrateJacobians<Cal3DS2_Base, dimension>(*this, pn, Dcal, Dp);
 
   return pn;
 }
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index a0ece8bdb..dbd6478e1 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -14,10 +14,12 @@
  * @brief Calibration of a camera with radial distortion
  * @date Feb 28, 2010
  * @author ydjian
+ * @author Varun Agrawal
  */
 
 #pragma once
 
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -39,22 +41,43 @@ namespace gtsam {
 class GTSAM_EXPORT Cal3DS2_Base {
 
 protected:
-
-  double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
-  double k1_, k2_ ; // radial 2nd-order and 4th-order
-  double p1_, p2_ ; // tangential distortion
+ double fx_, fy_, s_, u0_, v0_;  // focal length, skew and principal point
+ double k1_, k2_;                // radial 2nd-order and 4th-order
+ double p1_, p2_;                // tangential distortion
+ double tol_ = 1e-5;             // tolerance value when calibrating
 
 public:
 
+  enum { dimension = 9 };
+
   /// @name Standard Constructors
   /// @{
 
-  /// Default Constructor with only unit focal length
-  Cal3DS2_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
+ /// Default Constructor with only unit focal length
+ Cal3DS2_Base()
+     : fx_(1),
+       fy_(1),
+       s_(0),
+       u0_(0),
+       v0_(0),
+       k1_(0),
+       k2_(0),
+       p1_(0),
+       p2_(0),
+       tol_(1e-5) {}
 
-  Cal3DS2_Base(double fx, double fy, double s, double u0, double v0,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
-  fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
+ Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
+              double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
+     : fx_(fx),
+       fy_(fy),
+       s_(s),
+       u0_(u0),
+       v0_(v0),
+       k1_(k1),
+       k2_(k2),
+       p1_(p1),
+       p2_(p2),
+       tol_(tol) {}
 
   virtual ~Cal3DS2_Base() {}
 
@@ -72,7 +95,7 @@ public:
   virtual void print(const std::string& s = "") const;
 
   /// assert equality up to a tolerance
-  bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
+  bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const;
 
   /// @}
   /// @name Standard Interface
@@ -121,12 +144,12 @@ public:
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in (distorted) image coordinates
    */
-  Point2 uncalibrate(const Point2& p,
-       OptionalJacobian<2,9> Dcal = boost::none,
-       OptionalJacobian<2,2> Dp = boost::none) const ;
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
-  Point2 calibrate(const Point2& p, const double tol=1e-5) const;
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Derivative of uncalibrate wrpt intrinsic coordinates
   Matrix2 D2d_intrinsic(const Point2& p) const ;
@@ -164,6 +187,7 @@ private:
     ar & BOOST_SERIALIZATION_NVP(k2_);
     ar & BOOST_SERIALIZATION_NVP(p1_);
     ar & BOOST_SERIALIZATION_NVP(p2_);
+    ar & BOOST_SERIALIZATION_NVP(tol_);
   }
 
   /// @}
diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp
index f7794fafb..1ed1826ad 100644
--- a/gtsam/geometry/Cal3Fisheye.cpp
+++ b/gtsam/geometry/Cal3Fisheye.cpp
@@ -122,14 +122,15 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
 }
 
 /* ************************************************************************* */
-Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
+Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
+                              OptionalJacobian<2, 2> Dp) const {
   // initial gues just inverts the pinhole model
   const double u = uv.x(), v = uv.y();
   const double yd = (v - v0_) / fy_;
   const double xd = (u - s_ * yd - u0_) / fx_;
   Point2 pi(xd, yd);
 
-  // Perform newtons method, break when solution converges past tol,
+  // Perform newtons method, break when solution converges past tol_,
   // throw exception if max iterations are reached
   const int maxIterations = 10;
   int iteration;
@@ -140,7 +141,7 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
     const Point2 uv_hat = uncalibrate(pi, boost::none, jac);
 
     // Test convergence
-    if ((uv_hat - uv).norm() < tol) break;
+    if ((uv_hat - uv).norm() < tol_) break;
 
     // Newton's method update step
     pi = pi - jac.inverse() * (uv_hat - uv);
@@ -151,6 +152,8 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
         "Cal3Fisheye::calibrate fails to converge. need a better "
         "initialization");
 
+  calibrateJacobians<Cal3Fisheye, dimension>(*this, pi, Dcal, Dp);
+
   return pi;
 }
 
diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h
index e24fe074f..77e122f21 100644
--- a/gtsam/geometry/Cal3Fisheye.h
+++ b/gtsam/geometry/Cal3Fisheye.h
@@ -14,10 +14,12 @@
  * @brief Calibration of a fisheye camera
  * @date Apr 8, 2020
  * @author ghaggin
+ * @author Varun Agrawal
  */
 
 #pragma once
 
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 #include <string>
@@ -48,6 +50,7 @@ class GTSAM_EXPORT Cal3Fisheye {
  private:
   double fx_, fy_, s_, u0_, v0_;  // focal length, skew and principal point
   double k1_, k2_, k3_, k4_;      // fisheye distortion coefficients
+  double tol_ = 1e-5;             // tolerance value when calibrating
 
  public:
   enum { dimension = 9 };
@@ -59,11 +62,11 @@ class GTSAM_EXPORT Cal3Fisheye {
 
   /// Default Constructor with only unit focal length
   Cal3Fisheye()
-      : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {}
+      : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0), tol_(1e-5) {}
 
   Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
               const double v0, const double k1, const double k2,
-              const double k3, const double k4)
+              const double k3, const double k4, double tol = 1e-5)
       : fx_(fx),
         fy_(fy),
         s_(s),
@@ -72,7 +75,8 @@ class GTSAM_EXPORT Cal3Fisheye {
         k1_(k1),
         k2_(k2),
         k3_(k3),
-        k4_(k4) {}
+        k4_(k4),
+        tol_(tol) {}
 
   virtual ~Cal3Fisheye() {}
 
@@ -139,7 +143,8 @@ class GTSAM_EXPORT Cal3Fisheye {
 
   /// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
   /// y_i]
-  Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// @}
   /// @name Testable
diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp
index b1b9c3722..f4ce0ed75 100644
--- a/gtsam/geometry/Cal3Unified.cpp
+++ b/gtsam/geometry/Cal3Unified.cpp
@@ -13,6 +13,7 @@
  * @file Cal3Unified.cpp
  * @date Mar 8, 2014
  * @author Jing Dong
+ * @author Varun Agrawal
  */
 
 #include <gtsam/base/Vector.h>
@@ -54,8 +55,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
 /* ************************************************************************* */
 // todo: make a fixed sized jacobian version of this
 Point2 Cal3Unified::uncalibrate(const Point2& p,
-       OptionalJacobian<2,10> H1,
-       OptionalJacobian<2,2> H2) const {
+       OptionalJacobian<2,10> Dcal,
+       OptionalJacobian<2,2> Dp) const {
 
   // this part of code is modified from Cal3DS2,
   // since the second part of this model (after project to normalized plane)
@@ -78,16 +79,16 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
   Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
 
   // Inlined derivative for calibration
-  if (H1) {
+  if (Dcal) {
     // part1
     Vector2 DU;
     DU << -xs * sqrt_nx * xi_sqrt_nx2, //
         -ys * sqrt_nx * xi_sqrt_nx2;
-    *H1 << H1base, H2base * DU;
+    *Dcal << H1base, H2base * DU;
   }
 
   // Inlined derivative for points
-  if (H2) {
+  if (Dp) {
     // part1
     const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
     const double mid = -(xi * xs*ys) * denom;
@@ -95,20 +96,24 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
     DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
         mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
 
-    *H2 << H2base * DU;
+    *Dp << H2base * DU;
   }
 
   return puncalib;
 }
 
 /* ************************************************************************* */
-Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const {
-
+Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal,
+                              OptionalJacobian<2, 2> Dp) const {
   // calibrate point to Nplane use base class::calibrate()
-  Point2 pnplane = Base::calibrate(pi, tol);
+  Point2 pnplane = Base::calibrate(pi);
 
   // call nplane to space
-  return this->nPlaneToSpace(pnplane);
+  Point2 pn = this->nPlaneToSpace(pnplane);
+
+  calibrateJacobians<Cal3Unified, dimension>(*this, pn, Dcal, Dp);
+
+  return pn;
 }
 /* ************************************************************************* */
 Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index 381405d20..6fc37b0d1 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -14,6 +14,7 @@
  * @brief Unified Calibration Model, see Mei07icra for details
  * @date Mar 8, 2014
  * @author Jing Dong
+ * @author Varun Agrawal
  */
 
 /**
@@ -99,7 +100,8 @@ public:
       OptionalJacobian<2,2> Dp = boost::none) const ;
 
   /// Conver a pixel coordinate to ideal coordinate
-  Point2 calibrate(const Point2& p, const double tol=1e-5) const;
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Convert a 3D point to normalized unit plane
   Point2 spaceToNPlane(const Point2& p) const;
diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp
index 6134ae3d4..d1a5ed330 100644
--- a/gtsam/geometry/SimpleCamera.cpp
+++ b/gtsam/geometry/SimpleCamera.cpp
@@ -21,6 +21,7 @@
 
 namespace gtsam {
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
   SimpleCamera simpleCamera(const Matrix34& P) {
 
     // P = [A|a] = s K cRw [I|-T], with s the unknown scale
@@ -45,5 +46,6 @@ namespace gtsam {
     return SimpleCamera(Pose3(wRc, T),
         Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
   }
+#endif
 
 }
diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h
index 82f26aee2..aa00222c7 100644
--- a/gtsam/geometry/SimpleCamera.h
+++ b/gtsam/geometry/SimpleCamera.h
@@ -19,14 +19,22 @@
 #pragma once
 
 #include <gtsam/geometry/BearingRange.h>
-#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/PinholeCamera.h>
 
 namespace gtsam {
 
-  /// A simple camera class with a Cal3_S2 calibration
-typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
+  /// Convenient aliases for Pinhole camera classes with different calibrations.
+  /// Also needed as forward declarations in the wrapper.
+  using PinholeCameraCal3_S2 = gtsam::PinholeCamera<gtsam::Cal3_S2>;
+  using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
+  using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
+  using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
 /**
  * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x
  * Use PinholeCameraCal3_S2 instead
@@ -140,4 +148,6 @@ struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {};
 template <typename T>
 struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
 
+#endif
+
 }  // namespace gtsam
diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp
index 9317fb737..6bfbe3e46 100644
--- a/gtsam/geometry/tests/testCal3DFisheye.cpp
+++ b/gtsam/geometry/tests/testCal3DFisheye.cpp
@@ -181,6 +181,23 @@ TEST(Cal3Fisheye, calibrate3) {
   CHECK(assert_equal(xi_hat, xi));
 }
 
+Point2 calibrate_(const Cal3Fisheye& k, const Point2& pt) {
+  return k.calibrate(pt);
+}
+
+/* ************************************************************************* */
+TEST(Cal3Fisheye, Dcalibrate)
+{
+  Point2 p(0.5, 0.5);
+  Point2 pi = K.uncalibrate(p);
+  Matrix Dcal, Dp;
+  K.calibrate(pi, Dcal, Dp);
+  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
+  CHECK(assert_equal(numerical1,Dcal,1e-5));
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
+  CHECK(assert_equal(numerical2,Dp,1e-5));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;
diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp
index 416665d46..beed09883 100644
--- a/gtsam/geometry/tests/testCal3DS2.cpp
+++ b/gtsam/geometry/tests/testCal3DS2.cpp
@@ -74,12 +74,26 @@ TEST( Cal3DS2, Duncalibrate2)
   CHECK(assert_equal(numerical,separate,1e-5));
 }
 
-/* ************************************************************************* */
-TEST( Cal3DS2, assert_equal)
-{
-  CHECK(assert_equal(K,K,1e-5));
+Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
+  return k.calibrate(pt);
 }
 
+/* ************************************************************************* */
+TEST( Cal3DS2, Dcalibrate)
+{
+  Point2 pn(0.5, 0.5);
+  Point2 pi = K.uncalibrate(pn);
+  Matrix Dcal, Dp;
+  K.calibrate(pi, Dcal, Dp);
+  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi, 1e-7);
+  CHECK(assert_equal(numerical1, Dcal, 1e-5));
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi, 1e-7);
+  CHECK(assert_equal(numerical2, Dp, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
+
 /* ************************************************************************* */
 TEST( Cal3DS2, retract)
 {
diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp
index 2c5ffd7fb..8abb6fe04 100644
--- a/gtsam/geometry/tests/testCal3Unified.cpp
+++ b/gtsam/geometry/tests/testCal3Unified.cpp
@@ -82,6 +82,22 @@ TEST( Cal3Unified, Duncalibrate2)
   CHECK(assert_equal(numerical,computed,1e-6));
 }
 
+Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
+  return k.calibrate(pt);
+}
+
+/* ************************************************************************* */
+TEST( Cal3Unified, Dcalibrate)
+{
+  Point2 pi = K.uncalibrate(p);
+  Matrix Dcal, Dp;
+  K.calibrate(pi, Dcal, Dp);
+  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
+  CHECK(assert_equal(numerical1,Dcal,1e-5));
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
+  CHECK(assert_equal(numerical2,Dp,1e-5));
+}
+
 /* ************************************************************************* */
 TEST( Cal3Unified, assert_equal)
 {
diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp
index edf122d3c..18a25c553 100644
--- a/gtsam/geometry/tests/testSimpleCamera.cpp
+++ b/gtsam/geometry/tests/testSimpleCamera.cpp
@@ -26,6 +26,8 @@
 using namespace std;
 using namespace gtsam;
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
+
 static const Cal3_S2 K(625, 625, 0, 0, 0);
 
 static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()),
@@ -149,6 +151,8 @@ TEST( SimpleCamera, simpleCamera)
   CHECK(assert_equal(expected, actual,1e-1));
 }
 
+#endif
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index a8b1cf4f4..b7a49b9ac 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -329,7 +329,7 @@ virtual class Value {
 };
 
 #include <gtsam/base/GenericValue.h>
-template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
 virtual class GenericValue : gtsam::Value {
   void serializable() const;
 };
@@ -881,7 +881,7 @@ virtual class Cal3DS2_Base {
 
   // Action on Point2
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
-  gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -1059,54 +1059,13 @@ class PinholeCamera {
   void serialize() const;
 };
 
+// Forward declaration of PinholeCameraCalX is defined here.
 #include <gtsam/geometry/SimpleCamera.h>
-virtual class SimpleCamera {
-  // Standard Constructors and Named Constructors
-  SimpleCamera();
-  SimpleCamera(const gtsam::Pose3& pose);
-  SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K);
-  static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height);
-  static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
-  static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
-      const gtsam::Point3& upVector, const gtsam::Cal3_S2& K);
-  static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
-      const gtsam::Point3& upVector);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::SimpleCamera& camera, double tol) const;
-
-  // Standard Interface
-  gtsam::Pose3 pose() const;
-  gtsam::Cal3_S2 calibration() const;
-
-  // Manifold
-  gtsam::SimpleCamera retract(Vector d) const;
-  Vector localCoordinates(const gtsam::SimpleCamera& T2) const;
-  size_t dim() const;
-  static size_t Dim();
-
-  // Transformations and measurement functions
-  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
-  pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
-  gtsam::Point2 project(const gtsam::Point3& point);
-  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
-  double range(const gtsam::Point3& point);
-  double range(const gtsam::Pose3& pose);
-
-  // enabling serialization functionality
-  void serialize() const;
-
-};
-
-gtsam::SimpleCamera simpleCamera(const Matrix& P);
-
 // Some typedefs for common camera types
 // PinholeCameraCal3_S2 is the same as SimpleCamera above
 typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
-//TODO (Issue 237) 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::Cal3DS2> PinholeCameraCal3DS2;
+typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
 typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
 
 template<T = {gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3_S2}>
@@ -2091,7 +2050,7 @@ class NonlinearFactorGraph {
   gtsam::KeySet keys() const;
   gtsam::KeyVector keyVector() const;
 
-  template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}>
+  template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}>
   void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
 
   // NonlinearFactorGraph
@@ -2515,7 +2474,7 @@ class ISAM2 {
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
                      gtsam::Cal3Bundler, gtsam::EssentialMatrix,
-                     gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, 
+                     gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, 
                      Vector, Matrix}>
   VALUE calculateEstimate(size_t key) const;
   gtsam::Values calculateBestEstimate() const;
@@ -2549,12 +2508,11 @@ class NonlinearISAM {
 //*************************************************************************
 // Nonlinear factor types
 //*************************************************************************
-#include <gtsam/geometry/SimpleCamera.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/StereoPoint2.h>
 
 #include <gtsam/nonlinear/PriorFactor.h>
-template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
+template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
   T prior() const;
@@ -2578,7 +2536,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
 template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
                gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
                gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
-               gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2,
+               gtsam::PinholeCameraCal3_S2,
                gtsam::imuBias::ConstantBias}>
 virtual class NonlinearEquality : gtsam::NoiseModelFactor {
   // Constructor - forces exact evaluation
@@ -2697,8 +2655,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
   gtsam::Point2 measured() const;
 };
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
-//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
-//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
 
 template<CALIBRATION = {gtsam::Cal3_S2}>
diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp
index c6b3ca15f..1919d38be 100644
--- a/gtsam/linear/SubgraphBuilder.cpp
+++ b/gtsam/linear/SubgraphBuilder.cpp
@@ -383,7 +383,7 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const {
   const vector<size_t> tree = buildTree(gfg, forward_ordering, weights);
   if (tree.size() != n - 1) {
     throw std::runtime_error(
-        "SubgraphBuilder::operator() failure: tree.size() != n-1");
+        "SubgraphBuilder::operator() failure: tree.size() != n-1, might be caused by disconnected graph");
   }
 
   // Downweight the tree edges to zero.
diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h
index d52b4eb29..fad952232 100644
--- a/gtsam/navigation/ImuBias.h
+++ b/gtsam/navigation/ImuBias.h
@@ -29,8 +29,8 @@ namespace imuBias {
 
 class GTSAM_EXPORT ConstantBias {
 private:
-  Vector3 biasAcc_;
-  Vector3 biasGyro_;
+  Vector3 biasAcc_; ///< The units for stddev are σ = m/s² or m √Hz/s²
+  Vector3 biasGyro_; ///< The units for stddev are σ = rad/s or rad √Hz/s
 
 public:
   /// dimension of the variable - used to autodetect sizes
diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h
index 0e0559a32..e52d28e1e 100644
--- a/gtsam/navigation/PreintegratedRotation.h
+++ b/gtsam/navigation/PreintegratedRotation.h
@@ -29,7 +29,9 @@ namespace gtsam {
 /// Parameters for pre-integration:
 /// Usage: Create just a single Params and pass a shared pointer to the constructor
 struct GTSAM_EXPORT PreintegratedRotationParams {
-  Matrix3 gyroscopeCovariance;  ///< continuous-time "Covariance" of gyroscope measurements
+  /// Continuous-time "Covariance" of gyroscope measurements
+  /// The units for stddev are σ = rad/s/√Hz
+  Matrix3 gyroscopeCovariance;
   boost::optional<Vector3> omegaCoriolis;  ///< Coriolis constant
   boost::optional<Pose3> body_P_sensor;    ///< The pose of the sensor in the body frame
 
diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h
index ce1f0e734..960f67e24 100644
--- a/gtsam/navigation/PreintegrationParams.h
+++ b/gtsam/navigation/PreintegrationParams.h
@@ -24,7 +24,9 @@ namespace gtsam {
 /// Parameters for pre-integration:
 /// Usage: Create just a single Params and pass a shared pointer to the constructor
 struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
-  Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
+  /// Continuous-time "Covariance" of accelerometer
+  /// The units for stddev are σ = m/s²/√Hz
+  Matrix3 accelerometerCovariance;
   Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
   bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
   Vector3 n_gravity; ///< Gravity vector in nav frame
diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index d0f8a4790..6829e859b 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -338,19 +338,18 @@ namespace gtsam {
    }  // internal
 
    /* ************************************************************************* */
-   template<typename ValueType>
-   ValueType Values::at(Key j) const {
+   template <typename ValueType>
+   const ValueType Values::at(Key j) const {
      // Find the item
      KeyValueMap::const_iterator item = values_.find(j);
 
      // Throw exception if it does not exist
-     if(item == values_.end())
-       throw ValuesKeyDoesNotExist("at", j);
+     if (item == values_.end()) throw ValuesKeyDoesNotExist("at", j);
 
-    // Check the type and throw exception if incorrect
-    // h() split in two lines to avoid internal compiler error (MSVC2017)
-    auto h = internal::handle<ValueType>();
-    return h(j,item->second);
+     // Check the type and throw exception if incorrect
+     // h() split in two lines to avoid internal compiler error (MSVC2017)
+     auto h = internal::handle<ValueType>();
+     return h(j, item->second);
   }
 
   /* ************************************************************************* */
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index b49188b68..120c8839c 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -187,8 +187,8 @@ namespace gtsam {
      * Dynamic matrices/vectors can be retrieved as fixed-size, but not vice-versa.
      * @return The stored value
      */
-    template<typename ValueType>
-    ValueType at(Key j) const;
+    template <typename ValueType>
+    const ValueType at(Key j) const;
 
     /// version for double
     double atDouble(size_t key) const { return at<double>(key);}
diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp
index 1200c8ebb..172166116 100644
--- a/gtsam/sfm/tests/testShonanAveraging.cpp
+++ b/gtsam/sfm/tests/testShonanAveraging.cpp
@@ -91,6 +91,27 @@ TEST(ShonanAveraging3, checkOptimality) {
   EXPECT(!kShonan.checkOptimality(random));
 }
 
+/* ************************************************************************* */
+TEST(ShonanAveraging3, checkSubgraph) {
+  // Create parameter with solver set to SUBGRAPH
+  auto params = ShonanAveragingParameters3(
+      gtsam::LevenbergMarquardtParams::CeresDefaults(), "SUBGRAPH");
+  ShonanAveraging3::Measurements measurements;
+
+  // The toyExample.g2o has 5 vertices, from 0-4
+  // The edges are: 1-2, 2-3, 3-4, 3-1, 1-4, 0-1,
+  // which can build a connected graph
+  auto subgraphShonan = fromExampleName("toyExample.g2o", params);
+
+  // Create initial random estimation
+  Values initial;
+  initial = subgraphShonan.initializeRandomly(kRandomNumberGenerator);
+
+  // Run Shonan with SUBGRAPH solver
+  auto result = subgraphShonan.run(initial, 3, 3);
+  EXPECT_DOUBLES_EQUAL(1e-11, subgraphShonan.cost(result.first), 1e-4);
+}
+
 /* ************************************************************************* */
 TEST(ShonanAveraging3, tryOptimizingAt3) {
   const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp
index 8a661f2ef..88a94fd51 100644
--- a/gtsam_unstable/slam/serialization.cpp
+++ b/gtsam_unstable/slam/serialization.cpp
@@ -43,7 +43,6 @@ typedef PriorFactor<Pose3>                PriorFactorPose3;
 typedef PriorFactor<Cal3_S2>              PriorFactorCal3_S2;
 typedef PriorFactor<Cal3DS2>              PriorFactorCal3DS2;
 typedef PriorFactor<CalibratedCamera>     PriorFactorCalibratedCamera;
-typedef PriorFactor<SimpleCamera>         PriorFactorSimpleCamera;
 typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2;
 typedef PriorFactor<StereoCamera>         PriorFactorStereoCamera;
 
@@ -68,7 +67,6 @@ typedef NonlinearEquality<Pose3>                  NonlinearEqualityPose3;
 typedef NonlinearEquality<Cal3_S2>                NonlinearEqualityCal3_S2;
 typedef NonlinearEquality<Cal3DS2>                NonlinearEqualityCal3DS2;
 typedef NonlinearEquality<CalibratedCamera>       NonlinearEqualityCalibratedCamera;
-typedef NonlinearEquality<SimpleCamera>           NonlinearEqualitySimpleCamera;
 typedef NonlinearEquality<PinholeCameraCal3_S2>   NonlinearEqualityPinholeCameraCal3_S2;
 typedef NonlinearEquality<StereoCamera>           NonlinearEqualityStereoCamera;
 
@@ -77,10 +75,8 @@ typedef RangeFactor<Pose3, Point3>                              RangeFactor3D;
 typedef RangeFactor<Pose2, Pose2>                               RangeFactorPose2;
 typedef RangeFactor<Pose3, Pose3>                               RangeFactorPose3;
 typedef RangeFactor<CalibratedCamera, Point3>                   RangeFactorCalibratedCameraPoint;
-typedef RangeFactor<SimpleCamera, Point3>                       RangeFactorSimpleCameraPoint;
 typedef RangeFactor<PinholeCameraCal3_S2, Point3>               RangeFactorPinholeCameraCal3_S2Point;
 typedef RangeFactor<CalibratedCamera, CalibratedCamera>         RangeFactorCalibratedCamera;
-typedef RangeFactor<SimpleCamera, SimpleCamera>                 RangeFactorSimpleCamera;
 typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2;
 
 typedef BearingRangeFactor<Pose2, Point2>  BearingRangeFactor2D;
@@ -90,7 +86,7 @@ typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorC
 typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
 
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
-//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 
 typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
 
@@ -129,7 +125,6 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3_S2);
 GTSAM_VALUE_EXPORT(gtsam::Cal3DS2);
 GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo);
 GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera);
-GTSAM_VALUE_EXPORT(gtsam::SimpleCamera);
 GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2);
 GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
 
@@ -150,7 +145,6 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3");
 BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2");
 BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2");
 BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera");
-BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera");
 BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera");
 
 BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector");
@@ -174,7 +168,6 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3")
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera");
-BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera");
 
 BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D");
@@ -182,9 +175,7 @@ BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D");
 BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2");
 BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3");
 BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint");
-BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint");
 BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera");
-BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera");
 
 BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D");
 
@@ -192,12 +183,28 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectio
 BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2");
 
 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2");
-//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
+BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
 
 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2");
 
 BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D");
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
+
+typedef PriorFactor<SimpleCamera>               PriorFactorSimpleCamera;
+typedef NonlinearEquality<SimpleCamera>         NonlinearEqualitySimpleCamera;
+typedef RangeFactor<SimpleCamera, Point3>       RangeFactorSimpleCameraPoint;
+typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera;
+
+GTSAM_VALUE_EXPORT(gtsam::SimpleCamera);
+BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera");
+BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera");
+BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint");
+BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera");
+
+#endif
+
+
 /* ************************************************************************* */
 // Actual implementations of functions
 /* ************************************************************************* */
diff --git a/python/gtsam/tests/test_SimpleCamera.py b/python/gtsam/tests/test_SimpleCamera.py
index efdfec561..358eb1f48 100644
--- a/python/gtsam/tests/test_SimpleCamera.py
+++ b/python/gtsam/tests/test_SimpleCamera.py
@@ -14,11 +14,12 @@ import unittest
 import numpy as np
 
 import gtsam
-from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera
+from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2 as SimpleCamera
 from gtsam.utils.test_case import GtsamTestCase
 
 K = Cal3_S2(625, 625, 0, 0, 0)
 
+
 class TestSimpleCamera(GtsamTestCase):
 
     def test_constructor(self):
@@ -29,15 +30,15 @@ class TestSimpleCamera(GtsamTestCase):
 
     def test_level2(self):
         # Create a level camera, looking in Y-direction
-        pose2 = Pose2(0.4,0.3,math.pi/2.0)
+        pose2 = Pose2(0.4, 0.3, math.pi/2.0)
         camera = SimpleCamera.Level(K, pose2, 0.1)
 
         # expected
-        x = Point3(1,0,0)
-        y = Point3(0,0,-1)
-        z = Point3(0,1,0)
-        wRc = Rot3(x,y,z)
-        expected = Pose3(wRc,Point3(0.4,0.3,0.1))
+        x = Point3(1, 0, 0)
+        y = Point3(0, 0, -1)
+        z = Point3(0, 1, 0)
+        wRc = Rot3(x, y, z)
+        expected = Pose3(wRc, Point3(0.4, 0.3, 0.1))
         self.gtsamAssertEquals(camera.pose(), expected, 1e-9)
 
 
diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py
index b2c2ab879..7f48d03a3 100644
--- a/python/gtsam/utils/plot.py
+++ b/python/gtsam/utils/plot.py
@@ -36,18 +36,15 @@ def set_axes_equal(fignum):
     ax.set_zlim3d([origin[2] - radius, origin[2] + radius])
 
 
-def ellipsoid(xc, yc, zc, rx, ry, rz, n):
+def ellipsoid(rx, ry, rz, n):
     """
     Numpy equivalent of Matlab's ellipsoid function.
 
     Args:
-        xc (double): Center of ellipsoid in X-axis.
-        yc (double): Center of ellipsoid in Y-axis.
-        zc (double): Center of ellipsoid in Z-axis.
         rx (double): Radius of ellipsoid in X-axis.
         ry (double): Radius of ellipsoid in Y-axis.
         rz (double): Radius of ellipsoid in Z-axis.
-        n (int): The granularity of the ellipsoid plotted. 
+        n (int): The granularity of the ellipsoid plotted.
 
     Returns:
         tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot.
@@ -72,7 +69,8 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
     Args:
         axes (matplotlib.axes.Axes): Matplotlib axes.
         origin (gtsam.Point3): The origin in the world frame.
-        P (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse.
+        P (numpy.ndarray): The marginal covariance matrix of the 3D point
+            which will be represented as an ellipse.
         scale (float): Scaling factor of the radii of the covariance ellipse.
         n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses.
         alpha (float): Transparency value for the plotted surface in the range [0, 1].
@@ -85,7 +83,7 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
     rx, ry, rz = radii
 
     # generate data for "unrotated" ellipsoid
-    xc, yc, zc = ellipsoid(0, 0, 0, rx, ry, rz, n)
+    xc, yc, zc = ellipsoid(rx, ry, rz, n)
 
     # rotate data with orientation matrix U and center c
     data = np.kron(U[:, 0:1], xc) + np.kron(U[:, 1:2], yc) + \
@@ -106,7 +104,8 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
         axes (matplotlib.axes.Axes): Matplotlib axes.
         pose (gtsam.Pose2): The pose to be plotted.
         axis_length (float): The length of the camera axes.
-        covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+        covariance (numpy.ndarray): Marginal covariance matrix to plot
+            the uncertainty of the estimation.
     """
     # get rotation and translation (center)
     gRp = pose.rotation().matrix()  # rotation from pose to global
@@ -146,7 +145,8 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None,
         fignum (int): Integer representing the figure number to use for plotting.
         pose (gtsam.Pose2): The pose to be plotted.
         axis_length (float): The length of the camera axes.
-        covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+        covariance (numpy.ndarray): Marginal covariance matrix to plot
+            the uncertainty of the estimation.
         axis_labels (iterable[string]): List of axis labels to set.
     """
     # get figure object
@@ -215,7 +215,8 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None,
         fignum (int): Integer representing the figure number to use for plotting.
         values (gtsam.Values): Values dictionary consisting of points to be plotted.
         linespec (string): String representing formatting options for Matplotlib.
-        marginals (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+        marginals (numpy.ndarray): Marginal covariance matrix to plot the
+            uncertainty of the estimation.
         title (string): The title of the plot.
         axis_labels (iterable[string]): List of axis labels to set.
     """
@@ -238,6 +239,7 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None,
             continue
             # I guess it's not a Point3
 
+    fig = plt.figure(fignum)
     fig.suptitle(title)
     fig.canvas.set_window_title(title.lower())
 
diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp
index 84e521156..2e99aff71 100644
--- a/tests/testSerializationSLAM.cpp
+++ b/tests/testSerializationSLAM.cpp
@@ -89,10 +89,8 @@ typedef RangeFactor<Pose3, Point3>                              RangeFactor3D;
 typedef RangeFactor<Pose2, Pose2>                               RangeFactorPose2;
 typedef RangeFactor<Pose3, Pose3>                               RangeFactorPose3;
 typedef RangeFactor<CalibratedCamera, Point3>                   RangeFactorCalibratedCameraPoint;
-typedef RangeFactor<SimpleCamera, Point3>                       RangeFactorSimpleCameraPoint;
 typedef RangeFactor<PinholeCameraCal3_S2, Point3>               RangeFactorPinholeCameraCal3_S2Point;
 typedef RangeFactor<CalibratedCamera, CalibratedCamera>         RangeFactorCalibratedCamera;
-typedef RangeFactor<SimpleCamera, SimpleCamera>                 RangeFactorSimpleCamera;
 typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2;
 
 typedef BearingRangeFactor<Pose2, Point2>  BearingRangeFactor2D;
@@ -102,7 +100,7 @@ typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorC
 typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
 
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
-//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 
 typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
 
@@ -145,7 +143,6 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3_S2);
 GTSAM_VALUE_EXPORT(gtsam::Cal3DS2);
 GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo);
 GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera);
-GTSAM_VALUE_EXPORT(gtsam::SimpleCamera);
 GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2);
 GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
 
@@ -190,9 +187,9 @@ BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D");
 BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2");
 BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3");
 BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint");
-BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint");
+BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2Point, "gtsam::RangeFactorPinholeCameraCal3_S2Point");
 BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera");
-BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera");
+BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2, "gtsam::RangeFactorPinholeCameraCal3_S2");
 
 BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D");
 
@@ -200,7 +197,7 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectio
 BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2");
 
 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2");
-//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
+BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
 
 BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2");
 
@@ -352,9 +349,9 @@ TEST (testSerializationSLAM, factors) {
   RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1);
   RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1);
   RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1);
-  RangeFactorSimpleCameraPoint rangeFactorSimpleCameraPoint(a13, a05, 2.0, model1);
+  RangeFactorPinholeCameraCal3_S2Point rangeFactorPinholeCameraCal3_S2Point(a13, a05, 2.0, model1);
   RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1);
-  RangeFactorSimpleCamera rangeFactorSimpleCamera(a13, b13, 2.0, model1);
+  RangeFactorPinholeCameraCal3_S2 rangeFactorPinholeCameraCal3_S2(a13, b13, 2.0, model1);
 
   BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2);
 
@@ -405,9 +402,9 @@ TEST (testSerializationSLAM, factors) {
   graph += rangeFactorPose2;
   graph += rangeFactorPose3;
   graph += rangeFactorCalibratedCameraPoint;
-  graph += rangeFactorSimpleCameraPoint;
+  graph += rangeFactorPinholeCameraCal3_S2Point;
   graph += rangeFactorCalibratedCamera;
-  graph += rangeFactorSimpleCamera;
+  graph += rangeFactorPinholeCameraCal3_S2;
 
   graph += bearingRangeFactor2D;
 
@@ -463,9 +460,9 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsObj<RangeFactorPose2>(rangeFactorPose2));
   EXPECT(equalsObj<RangeFactorPose3>(rangeFactorPose3));
   EXPECT(equalsObj<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
-  EXPECT(equalsObj<RangeFactorSimpleCameraPoint>(rangeFactorSimpleCameraPoint));
+  EXPECT(equalsObj<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
   EXPECT(equalsObj<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
-  EXPECT(equalsObj<RangeFactorSimpleCamera>(rangeFactorSimpleCamera));
+  EXPECT(equalsObj<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
 
   EXPECT(equalsObj<BearingRangeFactor2D>(bearingRangeFactor2D));
 
@@ -521,9 +518,9 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsXML<RangeFactorPose2>(rangeFactorPose2));
   EXPECT(equalsXML<RangeFactorPose3>(rangeFactorPose3));
   EXPECT(equalsXML<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
-  EXPECT(equalsXML<RangeFactorSimpleCameraPoint>(rangeFactorSimpleCameraPoint));
+  EXPECT(equalsXML<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
   EXPECT(equalsXML<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
-  EXPECT(equalsXML<RangeFactorSimpleCamera>(rangeFactorSimpleCamera));
+  EXPECT(equalsXML<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
 
   EXPECT(equalsXML<BearingRangeFactor2D>(bearingRangeFactor2D));
 
@@ -579,9 +576,9 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsBinary<RangeFactorPose2>(rangeFactorPose2));
   EXPECT(equalsBinary<RangeFactorPose3>(rangeFactorPose3));
   EXPECT(equalsBinary<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
-  EXPECT(equalsBinary<RangeFactorSimpleCameraPoint>(rangeFactorSimpleCameraPoint));
+  EXPECT(equalsBinary<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
   EXPECT(equalsBinary<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
-  EXPECT(equalsBinary<RangeFactorSimpleCamera>(rangeFactorSimpleCamera));
+  EXPECT(equalsBinary<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
 
   EXPECT(equalsBinary<BearingRangeFactor2D>(bearingRangeFactor2D));