diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index 0d64d0184..b198643b0 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -25,13 +25,13 @@ namespace gtsam {
 
 /* ************************************************************************* */
 Cal3Bundler::Cal3Bundler() :
-    f_(1), k1_(0), k2_(0), u0_(0), v0_(0) {
+    f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) {
 }
 
 /* ************************************************************************* */
-Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) :
-    f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) {
-}
+Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0,
+                         double tol)
+    : f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {}
 
 /* ************************************************************************* */
 Matrix3 Cal3Bundler::K() const {
@@ -94,21 +94,24 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
+Point2 Cal3Bundler::calibrate(const Point2& pi,
+                              OptionalJacobian<2, 3> Dcal,
+                              OptionalJacobian<2, 2> Dp) const {
   // Copied from Cal3DS2 :-(
   // but specialized with k1,k2 non-zero only and fx=fy and s=0
-  const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_);
+  double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_;
+  const Point2 invKPi(x, y);
 
   // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
-  Point2 pn = invKPi;
+  Point2 pn(x, y);
 
   // 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)
+    if (distance2(uncalibrate(pn), pi) <= tol_)
       break;
-    const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y;
+    const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
     const double rr = xx + yy;
     const double g = (1 + k1_ * rr + k2_ * rr * rr);
     pn = invKPi / g;
@@ -118,6 +121,25 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
     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;
+
+  }
+
   return pn;
 }
 
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 4787f565b..96765f899 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -33,6 +33,7 @@ private:
   double f_; ///< focal length
   double k1_, k2_; ///< radial distortion
   double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
+  double tol_; ///< tolerance value when calibrating
 
 public:
 
@@ -51,8 +52,10 @@ public:
    *  @param k2 second radial distortion coefficient (quartic)
    *  @param u0 optional image center (default 0), considered a constant
    *  @param v0 optional image center (default 0), considered a constant
+   *  @param tol optional calibration tolerance value
    */
-  Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0);
+  Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
+              double tol = 1e-5);
 
   virtual ~Cal3Bundler() {}
 
@@ -117,8 +120,17 @@ public:
   Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
       OptionalJacobian<2, 2> Dp = boost::none) const;
 
-  /// Convert a pixel coordinate to ideal coordinate
-  Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
+  /**
+   * Convert a pixel coordinate to ideal coordinate xy
+   * @param p point in image coordinates
+   * @param tol optional tolerance threshold value for iterative minimization
+   * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in intrinsic coordinates
+   */
+  Point2 calibrate(const Point2& pi,
+                   OptionalJacobian<2, 3> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
   Matrix2 D2d_intrinsic(const Point2& p) const;
@@ -164,6 +176,7 @@ private:
     ar & BOOST_SERIALIZATION_NVP(k2_);
     ar & BOOST_SERIALIZATION_NVP(u0_);
     ar & BOOST_SERIALIZATION_NVP(v0_);
+    ar & BOOST_SERIALIZATION_NVP(tol_);
   }
 
   /// @}
diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp
index ce4ceee89..a565ac140 100644
--- a/gtsam/geometry/Point3.cpp
+++ b/gtsam/geometry/Point3.cpp
@@ -75,17 +75,16 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
   return p.x() * q.x() + p.y() * q.y() + p.z() * q.z();
 }
 
-Point3Pair mean(const std::vector<Point3Pair> &abPointPairs) {
+Point3Pair means(const std::vector<Point3Pair> &abPointPairs) {
   const size_t n = abPointPairs.size();
-  Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0);
+  if (n == 0) throw std::invalid_argument("Point3::mean input Point3Pair vector is empty");
+  Point3 aSum(0, 0, 0), bSum(0, 0, 0);
   for (const Point3Pair &abPair : abPointPairs) {
-    aCentroid += abPair.first;
-    bCentroid += abPair.second;
+    aSum += abPair.first;
+    bSum += abPair.second;
   }
   const double f = 1.0 / n;
-  aCentroid *= f;
-  bCentroid *= f;
-  return make_pair(aCentroid, bCentroid);
+  return {aSum * f, bSum * f};
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 7f58497e9..57188fc5e 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -62,13 +62,14 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
 /// mean
 template <class CONTAINER>
 GTSAM_EXPORT Point3 mean(const CONTAINER& points) {
+  if (points.size() == 0) throw std::invalid_argument("Point3::mean input container is empty");
   Point3 sum(0, 0, 0);
   sum = std::accumulate(points.begin(), points.end(), sum);
   return sum / points.size();
 }
 
-/// mean of Point3 pair 
-GTSAM_EXPORT Point3Pair mean(const std::vector<Point3Pair>& abPointPairs);
+/// Calculate the two means of a set of Point3 pairs
+GTSAM_EXPORT Point3Pair means(const std::vector<Point3Pair> &abPointPairs);
 
 template <typename A1, typename A2>
 struct Range;
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index ea822b796..22849d4f5 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -24,10 +24,11 @@
 #include <limits>
 #include <string>
 
-using namespace std;
-
 namespace gtsam {
 
+using std::vector;
+using Point3Pairs = vector<Point3Pair>;
+
 /** instantiate concept checks */
 GTSAM_CONCEPT_POSE_INST(Pose3);
 
@@ -212,18 +213,20 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThr
 #else
   // The closed-form formula in Barfoot14tro eq. (102)
   double phi = w.norm();
-  if (std::abs(phi)>nearZeroThreshold) {
-    const double sinPhi = sin(phi), cosPhi = cos(phi);
-    const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
+  const Matrix3 WVW = W * V * W;
+  if (std::abs(phi) > nearZeroThreshold) {
+    const double s = sin(phi), c = cos(phi);
+    const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi,
+                 phi5 = phi4 * phi;
     // Invert the sign of odd-order terms to have the right Jacobian
-    Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W)
-            + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W)
-            - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W);
-  }
-  else {
-    Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W)
-        - 1./24.*(W*W*V + V*W*W - 3*W*V*W)
-        + 1./120.*(W*V*W*W + W*W*V*W);
+    Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) +
+        (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) -
+        0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) *
+            (WVW * W + W * WVW);
+  } else {
+    Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) -
+        1. / 24. * (W * W * V + V * W * W - 3 * WVW) +
+        1. / 120. * (WVW * W + W * WVW);
   }
 #endif
 
@@ -381,39 +384,33 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself,
 }
 
 /* ************************************************************************* */
-boost::optional<Pose3> Pose3::Align(const std::vector<Point3Pair>& abPointPairs) {
+boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
   const size_t n = abPointPairs.size();
-  if (n < 3)
-    return boost::none;  // we need at least three pairs
+  if (n < 3) {
+    return boost::none; // we need at least three pairs
+  }
 
   // calculate centroids
-  Point3 aCentroid(0,0,0), bCentroid(0,0,0);
-  for(const Point3Pair& abPair: abPointPairs) {
-    aCentroid += abPair.first;
-    bCentroid += abPair.second;
-  }
-  double f = 1.0 / n;
-  aCentroid *= f;
-  bCentroid *= f;
+  const auto centroids = means(abPointPairs);
 
   // Add to form H matrix
   Matrix3 H = Z_3x3;
-  for(const Point3Pair& abPair: abPointPairs) {
-    Point3 da = abPair.first - aCentroid;
-    Point3 db = abPair.second - bCentroid;
+  for (const Point3Pair &abPair : abPointPairs) {
+    const Point3 da = abPair.first - centroids.first;
+    const Point3 db = abPair.second - centroids.second;
     H += da * db.transpose();
-    }
+  }
 
   // ClosestTo finds rotation matrix closest to H in Frobenius sense
-  Rot3 aRb = Rot3::ClosestTo(H);
-  Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid);
+  const Rot3 aRb = Rot3::ClosestTo(H);
+  const Point3 aTb = centroids.first - aRb * centroids.second;
   return Pose3(aRb, aTb);
 }
 
-boost::optional<Pose3> align(const vector<Point3Pair>& baPointPairs) {
-  vector<Point3Pair> abPointPairs;
-  for (const Point3Pair& baPair: baPointPairs) {
-    abPointPairs.push_back(make_pair(baPair.second, baPair.first));
+boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
+  Point3Pairs abPointPairs;
+  for (const Point3Pair &baPair : baPointPairs) {
+    abPointPairs.emplace_back(baPair.second, baPair.first);
   }
   return Pose3::Align(abPointPairs);
 }
diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp
index 8de049fa4..448600266 100644
--- a/gtsam/geometry/tests/testCal3Bundler.cpp
+++ b/gtsam/geometry/tests/testCal3Bundler.cpp
@@ -52,12 +52,14 @@ TEST( Cal3Bundler, calibrate )
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
   Point2 pn_hat = K.calibrate(pi);
-  CHECK( traits<Point2>::Equals(pn, pn_hat, 1e-5));
+  CHECK(traits<Point2>::Equals(pn, pn_hat, 1e-5));
 }
 
 /* ************************************************************************* */
 Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
 
+Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
+
 /* ************************************************************************* */
 TEST( Cal3Bundler, Duncalibrate)
 {
@@ -69,11 +71,20 @@ TEST( Cal3Bundler, Duncalibrate)
   Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
   CHECK(assert_equal(numerical1,Dcal,1e-7));
   CHECK(assert_equal(numerical2,Dp,1e-7));
-  CHECK(assert_equal(numerical1,K.D2d_calibration(p),1e-7));
-  CHECK(assert_equal(numerical2,K.D2d_intrinsic(p),1e-7));
-  Matrix Dcombined(2,5);
-  Dcombined << Dp, Dcal;
-  CHECK(assert_equal(Dcombined,K.D2d_intrinsic_calibration(p),1e-7));
+}
+
+/* ************************************************************************* */
+TEST( Cal3Bundler, Dcalibrate)
+{
+  Matrix Dcal, Dp;
+  Point2 pn(0.5, 0.5);
+  Point2 pi = K.uncalibrate(pn);
+  Point2 actual = K.calibrate(pi, Dcal, Dp);
+  CHECK(assert_equal(pn, actual, 1e-7));
+  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
+  CHECK(assert_equal(numerical1,Dcal,1e-5));
+  CHECK(assert_equal(numerical2,Dp,1e-5));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp
index a9b68bdec..ad6f4e921 100644
--- a/gtsam/geometry/tests/testPinholeCamera.cpp
+++ b/gtsam/geometry/tests/testPinholeCamera.cpp
@@ -336,6 +336,15 @@ TEST( PinholeCamera, range3) {
   EXPECT(assert_equal(Hexpected2, D2, 1e-7));
 }
 
+/* ************************************************************************* */
+TEST( PinholeCamera, Cal3Bundler) {
+  Cal3Bundler calibration;
+  Pose3 wTc;
+  PinholeCamera<Cal3Bundler> camera(wTc, calibration);
+  Point2 p(50, 100);
+  camera.backproject(p, 10);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index a481a8072..a655011a0 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -166,22 +166,22 @@ TEST (Point3, normalize) {
 
 //*************************************************************************
 TEST(Point3, mean) {
-  Point3 expected_a_mean(2, 2, 2);
+  Point3 expected(2, 2, 2);
   Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
   std::vector<Point3> a_points{a1, a2, a3};
-  Point3 actual_a_mean = mean(a_points);
-  EXPECT(assert_equal(expected_a_mean, actual_a_mean));
+  Point3 actual = mean(a_points);
+  EXPECT(assert_equal(expected, actual));
 }
 
 TEST(Point3, mean_pair) {
   Point3 a_mean(2, 2, 2), b_mean(-1, 1, 0);
-  Point3Pair expected_mean = std::make_pair(a_mean, b_mean);
+  Point3Pair expected = std::make_pair(a_mean, b_mean);
   Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
   Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0);
-  std::vector<Point3Pair> point_pairs{{a1,b1},{a2,b2},{a3,b3}};
-  Point3Pair actual_mean = mean(point_pairs);
-  EXPECT(assert_equal(expected_mean.first, actual_mean.first));
-  EXPECT(assert_equal(expected_mean.second, actual_mean.second));
+  std::vector<Point3Pair> point_pairs{{a1, b1}, {a2, b2}, {a3, b3}};
+  Point3Pair actual = means(point_pairs);
+  EXPECT(assert_equal(expected.first, actual.first));
+  EXPECT(assert_equal(expected.second, actual.second));
 }
 
 //*************************************************************************
diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index d9aa23893..e5f323d72 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -959,6 +959,7 @@ class Cal3Bundler {
   // Standard Constructors
   Cal3Bundler();
   Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
+  Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol);
 
   // Testable
   void print(string s) const;
@@ -971,7 +972,7 @@ class Cal3Bundler {
   Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
 
   // Action on Point2
-  gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
 
   // Standard Interface
@@ -1105,7 +1106,7 @@ 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::Cal3Bundler> PinholeCameraCal3Bundler;
+typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
 
 #include <gtsam/geometry/StereoCamera.h>
 class StereoCamera {
@@ -2462,6 +2463,8 @@ class ISAM2Result {
   size_t getVariablesRelinearized() const;
   size_t getVariablesReeliminated() const;
   size_t getCliques() const;
+  double getErrorBefore() const;
+  double getErrorAfter() const;
 };
 
 class ISAM2 {
@@ -2766,8 +2769,7 @@ class SfmTrack {
 class SfmData {
   size_t number_cameras() const;
   size_t number_tracks() const;
-  //TODO(Varun) Need to fix issue #237 first before this can work
-  // gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
+  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
   gtsam::SfmTrack track(size_t idx) const;
 };
 
diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h
index e45b17e4a..b249af5c5 100644
--- a/gtsam/nonlinear/ISAM2Result.h
+++ b/gtsam/nonlinear/ISAM2Result.h
@@ -176,6 +176,8 @@ struct ISAM2Result {
   size_t getVariablesRelinearized() const { return variablesRelinearized; }
   size_t getVariablesReeliminated() const { return variablesReeliminated; }
   size_t getCliques() const { return cliques; }
+  double getErrorBefore() const { return errorBefore ? *errorBefore : std::nan(""); }
+  double getErrorAfter() const { return errorAfter ? *errorAfter : std::nan(""); }
 };
 
 }  // namespace gtsam
diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp
index b2d7dc080..819c51fee 100644
--- a/gtsam_unstable/geometry/Similarity3.cpp
+++ b/gtsam_unstable/geometry/Similarity3.cpp
@@ -23,10 +23,14 @@
 
 namespace gtsam {
 
+using std::vector;
+using PointPairs = vector<Point3Pair>;
+
 namespace {
 /// Subtract centroids from point pairs.
-static std::vector<Point3Pair> subtractCentroids(const std::vector<Point3Pair>& abPointPairs, const Point3Pair& centroids) {
-  std::vector<Point3Pair> d_abPointPairs;
+static PointPairs subtractCentroids(const PointPairs &abPointPairs,
+                                    const Point3Pair &centroids) {
+  PointPairs d_abPointPairs;
   for (const Point3Pair& abPair : abPointPairs) {
     Point3 da = abPair.first - centroids.first;
     Point3 db = abPair.second - centroids.second;
@@ -36,7 +40,8 @@ static std::vector<Point3Pair> subtractCentroids(const std::vector<Point3Pair>&
 }
 
 /// Form inner products x and y and calculate scale.
-static const double calculateScale(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb) {
+static const double calculateScale(const PointPairs &d_abPointPairs,
+                                   const Rot3 &aRb) {
   double x = 0, y = 0;
   Point3 da, db;
   for (const Point3Pair& d_abPair : d_abPointPairs) {
@@ -50,7 +55,7 @@ static const double calculateScale(const std::vector<Point3Pair>& d_abPointPairs
 }
 
 /// Form outer product H.
-static Matrix3 calculateH(const std::vector<Point3Pair>& d_abPointPairs) {
+static Matrix3 calculateH(const PointPairs &d_abPointPairs) {
   Matrix3 H = Z_3x3;
   for (const Point3Pair& d_abPair : d_abPointPairs) {
     H += d_abPair.first * d_abPair.second.transpose();
@@ -59,7 +64,8 @@ static Matrix3 calculateH(const std::vector<Point3Pair>& d_abPointPairs) {
 }
 
 /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids.
-static Similarity3 align(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb, const Point3Pair& centroids) {
+static Similarity3 align(const PointPairs &d_abPointPairs, const Rot3 &aRb,
+                         const Point3Pair &centroids) {
   const double s = calculateScale(d_abPointPairs, aRb);
   const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
   return Similarity3(aRb, aTb, s);
@@ -67,8 +73,9 @@ static Similarity3 align(const std::vector<Point3Pair>& d_abPointPairs, const Ro
 
 /// This method estimates the similarity transform from point pairs, given a known or estimated rotation.
 // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
-static Similarity3 alignGivenR(const std::vector<Point3Pair>& abPointPairs, const Rot3& aRb) {
-  auto centroids = mean(abPointPairs);
+static Similarity3 alignGivenR(const PointPairs &abPointPairs,
+                               const Rot3 &aRb) {
+  auto centroids = means(abPointPairs);
   auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
   return align(d_abPointPairs, aRb, centroids);
 }
@@ -147,10 +154,12 @@ Point3 Similarity3::operator*(const Point3& p) const {
   return transformFrom(p);
 }
 
-Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
-  // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
-  if (abPointPairs.size() < 3) throw std::runtime_error("input should have at least 3 pairs of points");
-  auto centroids = mean(abPointPairs);
+Similarity3 Similarity3::Align(const PointPairs &abPointPairs) {
+  // Refer to Chapter 3 of
+  // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
+  if (abPointPairs.size() < 3)
+    throw std::runtime_error("input should have at least 3 pairs of points");
+  auto centroids = means(abPointPairs);
   auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
   Matrix3 H = calculateH(d_abPointPairs);
   // ClosestTo finds rotation matrix closest to H in Frobenius sense
@@ -158,17 +167,18 @@ Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
   return align(d_abPointPairs, aRb, centroids);
 }
 
-Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) {
+Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
   const size_t n = abPosePairs.size();
-  if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses");
+  if (n < 2)
+    throw std::runtime_error("input should have at least 2 pairs of poses");
 
   // calculate rotation
   vector<Rot3> rotations;
-  vector<Point3Pair> abPointPairs;
+  PointPairs abPointPairs;
   rotations.reserve(n);
   abPointPairs.reserve(n);
   Pose3 wTa, wTb;
-  for (const Pose3Pair& abPair : abPosePairs) {
+  for (const Pose3Pair &abPair : abPosePairs) {
     std::tie(wTa, wTb) = abPair;
     rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse()));
     abPointPairs.emplace_back(wTa.translation(), wTb.translation());
@@ -178,7 +188,7 @@ Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) {
   return alignGivenR(abPointPairs, aRb);
 }
 
-Matrix4 Similarity3::wedge(const Vector7& xi) {
+Matrix4 Similarity3::wedge(const Vector7 &xi) {
   // http://www.ethaneade.org/latex2html/lie/node29.html
   const auto w = xi.head<3>();
   const auto u = xi.segment<3>(3);
@@ -217,12 +227,13 @@ Matrix3 Similarity3::GetV(Vector3 w, double lambda) {
     W = 1.0 / 24.0 - theta2 / 720.0;
   }
   const double lambda2 = lambda * lambda, lambda3 = lambda2 * lambda;
+  const double expMinLambda = exp(-lambda);
   double A, alpha = 0.0, beta, mu;
   if (lambda2 > 1e-9) {
-    A = (1.0 - exp(-lambda)) / lambda;
+    A = (1.0 - expMinLambda) / lambda;
     alpha = 1.0 / (1.0 + theta2 / lambda2);
-    beta = (exp(-lambda) - 1 + lambda) / lambda2;
-    mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda)) / lambda3;
+    beta = (expMinLambda - 1 + lambda) / lambda2;
+    mu = (1 - lambda + (0.5 * lambda2) - expMinLambda) / lambda3;
   } else {
     A = 1.0 - lambda / 2.0 + lambda2 / 6.0;
     beta = 0.5 - lambda / 6.0 + lambda2 / 24.0 - lambda3 / 120.0;