diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh
index f85e67dc1..55a8ac372 100644
--- a/.github/scripts/unix.sh
+++ b/.github/scripts/unix.sh
@@ -66,6 +66,8 @@ function configure()
       -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
       -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \
       -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
+      -DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \
+      -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \
       -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
       -DBOOST_ROOT=$BOOST_ROOT \
       -DBoost_NO_SYSTEM_PATHS=ON \
diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml
index 87a0430f8..d80a7f4ba 100644
--- a/.github/workflows/build-linux.yml
+++ b/.github/workflows/build-linux.yml
@@ -63,17 +63,17 @@ jobs:
 
           sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
           
-          echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)"
-          echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)"
+          echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV
+          echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV
           
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
-            echo "::set-env name=CC::gcc-${{ matrix.version }}"
-            echo "::set-env name=CXX::g++-${{ matrix.version }}"
+            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
+            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
             sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
-            echo "::set-env name=CC::clang-${{ matrix.version }}"
-            echo "::set-env name=CXX::clang++-${{ matrix.version }}"
+            echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
+            echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
           fi
       - name: Check Boost version
         if: runner.os == 'Linux'
diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml
index 363cd690f..69873980a 100644
--- a/.github/workflows/build-macos.yml
+++ b/.github/workflows/build-macos.yml
@@ -35,17 +35,19 @@ jobs:
       - name: Install (macOS)
         if: runner.os == 'macOS'
         run: |
-          brew install cmake ninja boost
+          brew tap ProfFan/robotics
+          brew install cmake ninja
+          brew install ProfFan/robotics/boost
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             brew install gcc@${{ matrix.version }}
-            echo "::set-env name=CC::gcc-${{ matrix.version }}"
-            echo "::set-env name=CXX::g++-${{ matrix.version }}"
+            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
+            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
             sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
-            echo "::set-env name=CC::clang"
-            echo "::set-env name=CXX::clang++"
+            echo "CC=clang" >> $GITHUB_ENV
+            echo "CXX=clang++" >> $GITHUB_ENV
           fi
       - name: Build and Test (macOS)
         if: runner.os == 'macOS'
         run: |
-          bash .github/scripts/unix.sh -t
\ No newline at end of file
+          bash .github/scripts/unix.sh -t
diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml
index fbec491f2..3f9a2e98a 100644
--- a/.github/workflows/build-python.yml
+++ b/.github/workflows/build-python.yml
@@ -19,7 +19,7 @@ jobs:
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
           ubuntu-18.04-gcc-5,
-          # ubuntu-18.04-gcc-9,  # TODO Disabled for now because of timeouts
+          ubuntu-18.04-gcc-9,
           ubuntu-18.04-clang-9,
           macOS-10.15-xcode-11.3.1,
           ubuntu-18.04-gcc-5-tbb,
@@ -33,11 +33,10 @@ jobs:
             compiler: gcc
             version: "5"
 
-          # TODO Disabled for now because of timeouts
-          # - name: ubuntu-18.04-gcc-9
-          #   os: ubuntu-18.04
-          #   compiler: gcc
-          #   version: "9"
+          - name: ubuntu-18.04-gcc-9
+            os: ubuntu-18.04
+            compiler: gcc
+            version: "9"
 
           - name: ubuntu-18.04-clang-9
             os: ubuntu-18.04
@@ -77,30 +76,32 @@ jobs:
           
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
-            echo "::set-env name=CC::gcc-${{ matrix.version }}"
-            echo "::set-env name=CXX::g++-${{ matrix.version }}"
+            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
+            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
             sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
-            echo "::set-env name=CC::clang-${{ matrix.version }}"
-            echo "::set-env name=CXX::clang++-${{ matrix.version }}"
+            echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
+            echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
           fi
       - name: Install (macOS)
         if: runner.os == 'macOS'
         run: |
-          brew install cmake ninja boost
+          brew tap ProfFan/robotics
+          brew install cmake ninja
+          brew install ProfFan/robotics/boost
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             brew install gcc@${{ matrix.version }}
-            echo "::set-env name=CC::gcc-${{ matrix.version }}"
-            echo "::set-env name=CXX::g++-${{ matrix.version }}"
+            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
+            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
             sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
-            echo "::set-env name=CC::clang"
-            echo "::set-env name=CXX::clang++"
+            echo "CC=clang" >> $GITHUB_ENV
+            echo "CXX=clang++" >> $GITHUB_ENV
           fi
       - name: Set GTSAM_WITH_TBB Flag
         if: matrix.flag == 'tbb'
         run: |
-          echo "::set-env name=GTSAM_WITH_TBB::ON"
+          echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV
           echo "GTSAM Uses TBB"
       - name: Build (Linux)
         if: runner.os == 'Linux'
@@ -109,4 +110,4 @@ jobs:
       - name: Build (macOS)
         if: runner.os == 'macOS'
         run: |
-          bash .github/scripts/python.sh
\ No newline at end of file
+          bash .github/scripts/python.sh
diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml
index c314acb16..532f917c1 100644
--- a/.github/workflows/build-special.yml
+++ b/.github/workflows/build-special.yml
@@ -24,6 +24,7 @@ jobs:
             ubuntu-gcc-deprecated,
             ubuntu-gcc-quaternions,
             ubuntu-gcc-tbb,
+            ubuntu-cayleymap,
           ]
 
         build_type: [Debug, Release]
@@ -47,6 +48,12 @@ jobs:
             version: "9"
             flag: tbb
 
+          - name: ubuntu-cayleymap
+            os: ubuntu-18.04
+            compiler: gcc
+            version: "9"
+            flag: cayley
+
     steps:
       - name: Checkout
         uses: actions/checkout@master
@@ -64,17 +71,17 @@ jobs:
 
           sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
 
-          echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)"
-          echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)"
+          echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV
+          echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV
 
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
-            echo "::set-env name=CC::gcc-${{ matrix.version }}"
-            echo "::set-env name=CXX::g++-${{ matrix.version }}"
+            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
+            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
             sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
-            echo "::set-env name=CC::clang-${{ matrix.version }}"
-            echo "::set-env name=CXX::clang++-${{ matrix.version }}"
+            echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
+            echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
           fi
 
       - name: Install (macOS)
@@ -83,32 +90,39 @@ jobs:
           brew install cmake ninja boost
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             brew install gcc@${{ matrix.version }}
-            echo "::set-env name=CC::gcc-${{ matrix.version }}"
-            echo "::set-env name=CXX::g++-${{ matrix.version }}"
+            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
+            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
             sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
-            echo "::set-env name=CC::clang"
-            echo "::set-env name=CXX::clang++"
+            echo "CC=clang" >> $GITHUB_ENV
+            echo "CXX=clang++" >> $GITHUB_ENV
             fi
 
       - name: Set Allow Deprecated Flag
         if: matrix.flag == 'deprecated'
         run: |
-          echo "::set-env name=GTSAM_ALLOW_DEPRECATED_SINCE_V41::ON"
+          echo "GTSAM_ALLOW_DEPRECATED_SINCE_V41=ON" >> $GITHUB_ENV
           echo "Allow deprecated since version 4.1"
 
       - name: Set Use Quaternions Flag
         if: matrix.flag == 'quaternions'
         run: |
-          echo "::set-env name=GTSAM_USE_QUATERNIONS::ON"
+          echo "GTSAM_USE_QUATERNIONS=ON" >> $GITHUB_ENV
           echo "Use Quaternions for rotations"
 
       - name: Set GTSAM_WITH_TBB Flag
         if: matrix.flag == 'tbb'
         run: |
-          echo "::set-env name=GTSAM_WITH_TBB::ON"
+          echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV
           echo "GTSAM Uses TBB"
 
+      - name: Use Cayley Transform for Rot3
+        if: matrix.flag == 'cayley'
+        run: |
+          echo "GTSAM_POSE3_EXPMAP=OFF" >> $GITHUB_ENV
+          echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV
+          echo "GTSAM Uses Cayley map for Rot3"
+
       - name: Build & Test
         run: |
           bash .github/scripts/unix.sh -t
diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml
index b3150a751..0a55de880 100644
--- a/.github/workflows/build-windows.yml
+++ b/.github/workflows/build-windows.yml
@@ -18,16 +18,19 @@ jobs:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          windows-2016-cl,
+          #TODO This build keeps timing out, need to understand why.
+          # windows-2016-cl,
           windows-2019-cl,
         ]
 
         build_type: [Debug, Release]
         build_unstable: [ON]
         include:
-          - name: windows-2016-cl
-            os: windows-2016
-            compiler: cl
+
+          #TODO This build keeps timing out, need to understand why.
+          # - name: windows-2016-cl
+          #   os: windows-2016
+          #   compiler: cl
 
           - name: windows-2019-cl
             os: windows-2019
@@ -50,17 +53,17 @@ jobs:
             # See: https://github.com/DaanDeMeyer/doctest/runs/231595515
             # See: https://github.community/t5/GitHub-Actions/Something-is-wrong-with-the-chocolatey-installed-version-of-gcc/td-p/32413
             scoop install gcc --global
-            echo "::set-env name=CC::gcc"
-            echo "::set-env name=CXX::g++"
+            echo "CC=gcc" >> $GITHUB_ENV
+            echo "CXX=g++" >> $GITHUB_ENV
           } elseif ("${{ matrix.compiler }}" -eq "clang") {
-            echo "::set-env name=CC::clang"
-            echo "::set-env name=CXX::clang++"
+            echo "CC=clang" >> $GITHUB_ENV
+            echo "CXX=clang++" >> $GITHUB_ENV
           } else {
-            echo "::set-env name=CC::${{ matrix.compiler }}"
-            echo "::set-env name=CXX::${{ matrix.compiler }}"
+            echo "CC=${{ matrix.compiler }}" >> $GITHUB_ENV
+            echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV
           }
           # Scoop modifies the PATH so we make the modified PATH global.
-          echo "::set-env name=PATH::$env:PATH"
+          echo "$env:PATH" >> $GITHUB_PATH
       - name: Build (Windows)
         if: runner.os == 'Windows'
         run: |
diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake
index 840d37427..3155161be 100644
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@@ -1,3 +1,5 @@
+include(CheckCXXCompilerFlag) # for check_cxx_compiler_flag()
+
 # Set cmake policy to recognize the AppleClang compiler
 # independently from the Clang compiler.
 if(POLICY CMP0025)
@@ -105,11 +107,14 @@ if(MSVC)
 else()
   # Common to all configurations, next for each configuration:
 
-  if (
-      ((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR
-      (CMAKE_CXX_COMPILER_ID MATCHES "GNU")
-     )
-    set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday
+  if (NOT MSVC)
+    check_cxx_compiler_flag(-Wsuggest-override COMPILER_HAS_WSUGGEST_OVERRIDE)
+    check_cxx_compiler_flag(-Wmissing COMPILER_HAS_WMISSING_OVERRIDE)
+    if (COMPILER_HAS_WSUGGEST_OVERRIDE)
+      set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday
+    elseif(COMPILER_HAS_WMISSING_OVERRIDE)
+      set(flag_override_ -Wmissing-override) # -Werror=missing-override: Add again someday
+    endif()
   endif()
 
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON
diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake
index 27d73bd86..85a529e49 100644
--- a/cmake/HandleGeneralOptions.cmake
+++ b/cmake/HandleGeneralOptions.cmake
@@ -31,6 +31,15 @@ if(NOT MSVC AND NOT XCODE_VERSION)
     option(GTSAM_BUILD_WITH_CCACHE           "Use ccache compiler cache" ON)
 endif()
 
+# Enable GTSAM_ROT3_EXPMAP if GTSAM_POSE3_EXPMAP is enabled, and vice versa.
+if(GTSAM_POSE3_EXPMAP)
+    message(STATUS "GTSAM_POSE3_EXPMAP=ON, enabling GTSAM_ROT3_EXPMAP as well")
+    set(GTSAM_ROT3_EXPMAP 1 CACHE BOOL "" FORCE)
+elseif(GTSAM_ROT3_EXPMAP)
+    message(STATUS "GTSAM_ROT3_EXPMAP=ON, enabling GTSAM_POSE3_EXPMAP as well")
+    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)
diff --git a/cmake/HandlePython.cmake b/cmake/HandlePython.cmake
index ac7401906..e5d55b451 100644
--- a/cmake/HandlePython.cmake
+++ b/cmake/HandlePython.cmake
@@ -1,4 +1,5 @@
-if(GTSAM_BUILD_PYTHON)
+# Set Python version if either Python or MATLAB wrapper is requested.
+if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
     if(${GTSAM_PYTHON_VERSION} STREQUAL "Default")
         # Get info about the Python3 interpreter
         # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3
@@ -14,7 +15,9 @@ if(GTSAM_BUILD_PYTHON)
                 "The version of Python to build the wrappers against."
                 FORCE)
     endif()
+endif()
 
+if(GTSAM_BUILD_PYTHON)
     if(GTSAM_UNSTABLE_BUILD_PYTHON)
         if (NOT GTSAM_BUILD_UNSTABLE)
             message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.")
diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in
index d5c969a8a..fd7f4e5f6 100644
--- a/doc/Doxyfile.in
+++ b/doc/Doxyfile.in
@@ -1188,7 +1188,7 @@ USE_MATHJAX            = YES
 # MathJax, but it is strongly recommended to install a local copy of MathJax 
 # before deployment.
 
-MATHJAX_RELPATH        = http://cdn.mathjax.org/mathjax/latest
+MATHJAX_RELPATH        = https://cdn.mathjax.org/mathjax/latest
 
 # The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension 
 # names that should be enabled during MathJax rendering.
diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h
index 1a31aa284..88dd619e5 100644
--- a/gtsam/base/TestableAssertions.h
+++ b/gtsam/base/TestableAssertions.h
@@ -23,6 +23,7 @@
 #include <boost/optional.hpp>
 #include <map>
 #include <iostream>
+#include <sstream>
 #include <vector>
 
 namespace gtsam {
@@ -349,4 +350,44 @@ bool assert_inequal(const V& expected, const V& actual, double tol = 1e-9) {
   return false;
 }
 
+/**
+ * Capture std out via cout stream and compare against string.
+ */
+template<class V>
+bool assert_stdout_equal(const std::string& expected, const V& actual) {
+  // Redirect output to buffer so we can compare
+  std::stringstream buffer;
+  // Save the original output stream so we can reset later
+  std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
+
+  // We test against actual std::cout for faithful reproduction
+  std::cout << actual;
+
+  // Get output string and reset stdout
+  std::string actual_ = buffer.str();
+  std::cout.rdbuf(old);
+
+  return assert_equal(expected, actual_);
+}
+
+/**
+ * Capture print function output and compare against string.
+ */
+template<class V>
+bool assert_print_equal(const std::string& expected, const V& actual) {
+  // Redirect output to buffer so we can compare
+  std::stringstream buffer;
+  // Save the original output stream so we can reset later
+  std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
+
+  // We test against actual std::cout for faithful reproduction
+  actual.print();
+
+  // Get output string and reset stdout
+  std::string actual_ = buffer.str();
+  std::cout.rdbuf(old);
+
+  return assert_equal(expected, actual_);
+}
+
 } // \namespace gtsam
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 96765f899..2e3fab002 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -98,6 +98,17 @@ public:
     return k2_;
   }
 
+  /// image center in x
+  inline double px() const {
+    return u0_;
+  }
+
+  /// image center in y
+  inline double py() const {
+    return v0_;
+  }
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
   /// get parameter u0
   inline double u0() const {
     return u0_;
@@ -107,6 +118,7 @@ public:
   inline double v0() const {
     return v0_;
   }
+#endif
 
 
   /**
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 2334312f6..abd74e063 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -526,7 +526,7 @@ namespace gtsam {
 
     /**
      * @brief Spherical Linear intERPolation between *this and other
-     * @param s a value between 0 and 1
+     * @param t a value between 0 and 1
      * @param other final point of iterpolation geodesic on manifold
      */
     Rot3 slerp(double t, const Rot3& other) const;
diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp
index 02e5b771f..07cc7302a 100644
--- a/gtsam/geometry/Rot3M.cpp
+++ b/gtsam/geometry/Rot3M.cpp
@@ -176,7 +176,17 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) {
   if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative");
   // Create a fixed-size matrix
   Matrix3 A = R.matrix();
-  // Mathematica closed form optimization (procrastination?) gone wild:
+
+  // Check if (A+I) is invertible. Same as checking for -1 eigenvalue.
+  if ((A + I_3x3).determinant() == 0.0) {
+    throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation");
+  }
+
+  // Mathematica closed form optimization.
+  // The following are the essential computations for the following algorithm
+  // 1. Compute the inverse of P = (A+I), using a closed-form formula since P is 3x3 
+  // 2. Compute the Cayley transform C = 2 * P^{-1} * (A-I)
+  // 3. C is skew-symmetric, so we pick out the computations corresponding only to x, y, and z.
   const double a = A(0, 0), b = A(0, 1), c = A(0, 2);
   const double d = A(1, 0), e = A(1, 1), f = A(1, 2);
   const double g = A(2, 0), h = A(2, 1), i = A(2, 2);
diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp
index ccecb09c3..55ea32e32 100644
--- a/gtsam/geometry/tests/testCal3_S2.cpp
+++ b/gtsam/geometry/tests/testCal3_S2.cpp
@@ -16,6 +16,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3_S2.h>
 
@@ -127,6 +128,16 @@ TEST(Cal3_S2, between) {
 
 }
 
+/* ************************************************************************* */
+TEST(Cal3_S2, Print) {
+  Cal3_S2 cal(5, 5, 5, 5, 5);
+  std::stringstream os;
+  os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px()
+     << ", py:" << cal.py() << "}";
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 9639ffbcf..6de2c0a33 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -18,6 +18,7 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/testLie.h>
 #include <gtsam/base/lieProxies.h>
+#include <gtsam/base/TestableAssertions.h>
 
 #include <boost/assign/std/vector.hpp> // for operator +=
 using namespace boost::assign;
@@ -906,9 +907,9 @@ TEST(Pose3 , ChartDerivatives) {
   Pose3 id;
   if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
     CHECK_CHART_DERIVATIVES(id,id);
-//    CHECK_CHART_DERIVATIVES(id,T2);
-//    CHECK_CHART_DERIVATIVES(T2,id);
-//    CHECK_CHART_DERIVATIVES(T2,T3);
+    CHECK_CHART_DERIVATIVES(id,T2);
+    CHECK_CHART_DERIVATIVES(T2,id);
+    CHECK_CHART_DERIVATIVES(T2,T3);
   }
 }
 
@@ -1028,32 +1029,13 @@ TEST(Pose3, Create) {
 }
 
 /* ************************************************************************* */
-TEST(Pose3, print) {
-  std::stringstream redirectStream;
-  std::streambuf* ssbuf = redirectStream.rdbuf();
-  std::streambuf* oldbuf  = std::cout.rdbuf();
-  // redirect cout to redirectStream
-  std::cout.rdbuf(ssbuf);
-
+TEST(Pose3, Print) {
   Pose3 pose(Rot3::identity(), Point3(1, 2, 3));
-  // output is captured to redirectStream
-  pose.print();
 
   // Generate the expected output
-  std::stringstream expected;
-  Point3 translation(1, 2, 3);
+  std::string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 1 2 3\n";
 
-  // Add expected rotation
-  expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
-  expected << "t: 1 2 3\n";
-
-  // reset cout to the original stream
-  std::cout.rdbuf(oldbuf);
-
-  // Get substring corresponding to translation part
-  std::string actual = redirectStream.str();
-
-  CHECK_EQUAL(expected.str(), actual);
+  EXPECT(assert_print_equal(expected, pose));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 880b1d4c7..1b4d976da 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -597,6 +597,7 @@ class Rot3 {
   Rot3(double R11, double R12, double R13,
       double R21, double R22, double R23,
       double R31, double R32, double R33);
+  Rot3(double w, double x, double y, double z);
 
   static gtsam::Rot3 Rx(double t);
   static gtsam::Rot3 Ry(double t);
@@ -980,8 +981,8 @@ class Cal3Bundler {
   double fy() const;
   double k1() const;
   double k2() const;
-  double u0() const;
-  double v0() const;
+  double px() const;
+  double py() const;
   Vector vector() const;
   Vector k() const;
   Matrix K() const;
@@ -2068,7 +2069,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::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::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}>
   void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
 
   // NonlinearFactorGraph
@@ -2157,12 +2158,13 @@ class Values {
   void insert(size_t j, const gtsam::SOn& P);
   void insert(size_t j, const gtsam::Rot3& rot3);
   void insert(size_t j, const gtsam::Pose3& pose3);
+  void insert(size_t j, const gtsam::Unit3& unit3);
   void insert(size_t j, const gtsam::Cal3_S2& cal3_s2);
   void insert(size_t j, const gtsam::Cal3DS2& cal3ds2);
   void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
   void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
   void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
-  // void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
+  void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(size_t j, const gtsam::NavState& nav_state);
 
@@ -2175,18 +2177,19 @@ class Values {
   void update(size_t j, const gtsam::SOn& P);
   void update(size_t j, const gtsam::Rot3& rot3);
   void update(size_t j, const gtsam::Pose3& pose3);
+  void update(size_t j, const gtsam::Unit3& unit3);
   void update(size_t j, const gtsam::Cal3_S2& cal3_s2);
   void update(size_t j, const gtsam::Cal3DS2& cal3ds2);
   void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
   void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
   void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
-  // void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
+  void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void update(size_t j, const gtsam::NavState& nav_state);
   void update(size_t j, Vector vector);
   void update(size_t j, Matrix matrix);
 
-  template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
+  template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
   T at(size_t j);
 
   /// version for double
@@ -2490,7 +2493,8 @@ 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, Vector, Matrix}>
+                     gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, 
+                     Vector, Matrix}>
   VALUE calculateEstimate(size_t key) const;
   gtsam::Values calculateBestEstimate() const;
   Matrix marginalCovariance(size_t key) const;
@@ -2528,7 +2532,7 @@ class NonlinearISAM {
 #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::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
+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>}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
   T prior() const;
@@ -2673,6 +2677,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
 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::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
 
 template<CALIBRATION = {gtsam::Cal3_S2}>
 virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
@@ -2759,21 +2764,36 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
 };
 
 #include <gtsam/slam/dataset.h>
+
 class SfmTrack {
-  Point3 point3() const;
+  SfmTrack();
+  SfmTrack(const gtsam::Point3& pt);
+  const Point3& point3() const;
+
+  double r;
+  double g;
+  double b;
+  // TODO Need to close wrap#10 to allow this to work.
+  // std::vector<pair<size_t, gtsam::Point2>> measurements;
+
   size_t number_measurements() const;
   pair<size_t, gtsam::Point2> measurement(size_t idx) const;
   pair<size_t, size_t> siftIndex(size_t idx) const;
+  void add_measurement(size_t idx, const gtsam::Point2& m);
 };
 
 class SfmData {
+  SfmData();
   size_t number_cameras() const;
   size_t number_tracks() const;
   gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
   gtsam::SfmTrack track(size_t idx) const;
+  void add_track(const gtsam::SfmTrack& t) ;
+  void add_camera(const gtsam::SfmCamera& cam);
 };
 
 gtsam::SfmData readBal(string filename);
+bool writeBAL(string filename, gtsam::SfmData& data);
 gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
 gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
 
diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h
index 5be195db3..469082f16 100644
--- a/gtsam/inference/Symbol.h
+++ b/gtsam/inference/Symbol.h
@@ -164,8 +164,16 @@ inline Key Y(std::uint64_t j) { return Symbol('y', j); }
 inline Key Z(std::uint64_t j) { return Symbol('z', j); }
 }
 
+/** Generates symbol shorthands with alternative names different than the
+ * one-letter predefined ones. */
+class SymbolGenerator {
+  const char c_;
+public:
+  SymbolGenerator(const char c) : c_(c) {}
+  Symbol operator()(const std::uint64_t j) const { return Symbol(c_, j); }
+};
+
 /// traits
 template<> struct traits<Symbol> : public Testable<Symbol> {};
 
 } // \ namespace gtsam
-
diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp
index a60258581..64674c36f 100644
--- a/gtsam/inference/tests/testKey.cpp
+++ b/gtsam/inference/tests/testKey.cpp
@@ -40,6 +40,25 @@ TEST(Key, KeySymbolConversion) {
   EXPECT(assert_equal(original, actual))
 }
 
+/* ************************************************************************* */
+TEST(Key, SymbolGenerator) {
+  const auto x1 = gtsam::symbol_shorthand::X(1);
+  const auto v1 = gtsam::symbol_shorthand::V(1);
+  const auto a1 = gtsam::symbol_shorthand::A(1);
+
+  const auto Z = gtsam::SymbolGenerator('x');
+  const auto DZ = gtsam::SymbolGenerator('v');
+  const auto DDZ = gtsam::SymbolGenerator('a');
+
+  const auto z1 = Z(1);
+  const auto dz1 = DZ(1);
+  const auto ddz1 = DDZ(1);
+
+  EXPECT(assert_equal(x1, z1));
+  EXPECT(assert_equal(v1, dz1));
+  EXPECT(assert_equal(a1, ddz1));
+}
+
 /* ************************************************************************* */
 template<int KeySize>
 Key KeyTestValue();
@@ -106,4 +125,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-
diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
index abf6b257a..a85f27425 100644
--- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
+++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
@@ -200,6 +200,10 @@ boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
     currentValues = system.advance(prevValues, alpha, direction);
     currentError = system.error(currentValues);
 
+    // User hook:
+    if (params.iterationHook)
+      params.iterationHook(iteration, prevError, currentError);
+
     // Maybe show output
     if (params.verbosity >= NonlinearOptimizerParams::ERROR)
       std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl;
diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp
index 9a9c487b6..0d7e9e17f 100644
--- a/gtsam/nonlinear/NonlinearOptimizer.cpp
+++ b/gtsam/nonlinear/NonlinearOptimizer.cpp
@@ -88,20 +88,28 @@ void NonlinearOptimizer::defaultOptimize() {
   }
 
   // Iterative loop
+  double newError = currentError; // used to avoid repeated calls to error()
   do {
     // Do next iteration
-    currentError = error(); // TODO(frank): don't do this twice at first !? Computed above!
+    currentError = newError;
     iterate();
     tictoc_finishedIteration();
 
+    // Update newError for either printouts or conditional-end checks:
+    newError = error();
+
+    // User hook:
+    if (params.iterationHook)
+      params.iterationHook(iterations(), currentError, newError);
+
     // Maybe show output
     if (params.verbosity >= NonlinearOptimizerParams::VALUES)
       values().print("newValues");
     if (params.verbosity >= NonlinearOptimizerParams::ERROR)
-      cout << "newError: " << error() << endl;
+      cout << "newError: " << newError << endl;
   } while (iterations() < params.maxIterations &&
            !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol,
-                             currentError, error(), params.verbosity) && std::isfinite(currentError));
+                             currentError, newError, params.verbosity) && std::isfinite(currentError));
 
   // Printing if verbose
   if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) {
diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h
index 9935f44ce..6fe369dd3 100644
--- a/gtsam/nonlinear/NonlinearOptimizer.h
+++ b/gtsam/nonlinear/NonlinearOptimizer.h
@@ -81,7 +81,7 @@ protected:
 
 public:
   /** A shared pointer to this class */
-  typedef boost::shared_ptr<const NonlinearOptimizer> shared_ptr;
+  using shared_ptr = boost::shared_ptr<const NonlinearOptimizer>;
 
   /// @name Standard interface
   /// @{
diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h
index 65fdd1c92..a7bc10a1f 100644
--- a/gtsam/nonlinear/NonlinearOptimizerParams.h
+++ b/gtsam/nonlinear/NonlinearOptimizerParams.h
@@ -38,21 +38,12 @@ public:
     SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR
   };
 
-  size_t maxIterations; ///< The maximum iterations to stop iterating (default 100)
-  double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5)
-  double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5)
-  double errorTol; ///< The maximum total error to stop iterating (default 0.0)
-  Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT)
-  Ordering::OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD)
-
-  NonlinearOptimizerParams() :
-      maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol(
-          0.0), verbosity(SILENT), orderingType(Ordering::COLAMD),
-          linearSolverType(MULTIFRONTAL_CHOLESKY) {}
-
-  virtual ~NonlinearOptimizerParams() {
-  }
-  virtual void print(const std::string& str = "") const;
+  size_t maxIterations = 100; ///< The maximum iterations to stop iterating (default 100)
+  double relativeErrorTol = 1e-5; ///< The maximum relative error decrease to stop iterating (default 1e-5)
+  double absoluteErrorTol = 1e-5; ///< The maximum absolute error decrease to stop iterating (default 1e-5)
+  double errorTol = 0.0; ///< The maximum total error to stop iterating (default 0.0)
+  Verbosity verbosity = SILENT; ///< The printing verbosity during optimization (default SILENT)
+  Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD)
 
   size_t getMaxIterations() const { return maxIterations; }
   double getRelativeErrorTol() const { return relativeErrorTol; }
@@ -71,6 +62,37 @@ public:
   static Verbosity verbosityTranslator(const std::string &s) ;
   static std::string verbosityTranslator(Verbosity value) ;
 
+  /** Type for an optional user-provided hook to be called after each
+   * internal optimizer iteration. See iterationHook below. */
+  using IterationHook = std::function<
+    void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>;
+
+  /** Optional user-provided iteration hook to be called after each
+   * optimization iteration (Default: none).
+   * Note that `IterationHook` is defined as a std::function<> with this
+   * signature:
+   * \code
+   *  void(size_t iteration, double errorBefore, double errorAfter)
+   * \endcode
+   * which allows binding by means of a reference to a regular function:
+   * \code
+   *  void foo(size_t iteration, double errorBefore, double errorAfter);
+   *  // ...
+   *  lmOpts.iterationHook = &foo;
+   * \endcode
+   * or to a C++11 lambda (preferred if you need to capture additional
+   * context variables, such that the optimizer object itself, the factor graph,
+   * etc.):
+   * \code
+   *  lmOpts.iterationHook = [&](size_t iter, double oldError, double newError)
+   *  {
+   *    // ...
+   *  };
+   * \endcode
+   * or to the result of a properly-formed `std::bind` call.
+   */
+  IterationHook iterationHook;
+
   /** See NonlinearOptimizerParams::linearSolverType */
   enum LinearSolverType {
     MULTIFRONTAL_CHOLESKY,
@@ -81,10 +103,16 @@ public:
     CHOLMOD, /* Experimental Flag */
   };
 
-  LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer
+  LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer
   boost::optional<Ordering> ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty)
   IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers.
 
+  NonlinearOptimizerParams() = default;
+  virtual ~NonlinearOptimizerParams() {
+  }
+
+  virtual void print(const std::string& str = "") const;
+
   inline bool isMultifrontal() const {
     return (linearSolverType == MULTIFRONTAL_CHOLESKY)
         || (linearSolverType == MULTIFRONTAL_QR);
diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
index 2f4f21286..1423b473e 100644
--- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
+++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
@@ -41,7 +41,7 @@ struct Cal3Bundler0 : public Cal3Bundler {
                double v0 = 0)
       : Cal3Bundler(f, k1, k2, u0, v0) {}
   Cal3Bundler0 retract(const Vector& d) const {
-    return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0());
+    return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), px(), py());
   }
   Vector3 localCoordinates(const Cal3Bundler0& T2) const {
     return T2.vector() - vector();
diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp
index 48ab73ad0..cb91320cf 100644
--- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp
+++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp
@@ -19,6 +19,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/FunctorizedFactor.h>
 #include <gtsam/nonlinear/factorTesting.h>
@@ -115,16 +116,6 @@ TEST(FunctorizedFactor, Print) {
   auto factor =
       MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
 
-  // redirect output to buffer so we can compare
-  stringstream buffer;
-  streambuf *old = cout.rdbuf(buffer.rdbuf());
-
-  factor.print();
-
-  // get output string and reset stdout
-  string actual = buffer.str();
-  cout.rdbuf(old);
-
   string expected =
       "  keys = { X0 }\n"
       "  noise model: unit (9) \n"
@@ -135,7 +126,7 @@ TEST(FunctorizedFactor, Print) {
       "]\n"
       "  noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
 
-  CHECK_EQUAL(expected, actual);
+  EXPECT(assert_print_equal(expected, factor));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index 09b358efb..88cfb666f 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -595,15 +595,7 @@ TEST(Values, Demangle) {
   values.insert(key1, v);
   string expected = "Values with 1 values:\nValue v1: (Eigen::Matrix<double, 1, 3, 1, 1, 3>)\n[\n	5, 6, 7\n]\n\n";
 
-  stringstream buffer;
-  streambuf * old = cout.rdbuf(buffer.rdbuf());
-
-  values.print();
-
-  string actual = buffer.str();
-  cout.rdbuf(old);
-
-  EXPECT(assert_equal(expected, actual));
+  EXPECT(assert_print_equal(expected, values));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp
index 4cd983ecd..913752d8a 100644
--- a/gtsam/sfm/MFAS.cpp
+++ b/gtsam/sfm/MFAS.cpp
@@ -121,8 +121,8 @@ MFAS::MFAS(const TranslationEdges& relativeTranslations,
   }
 }
 
-vector<Key> MFAS::computeOrdering() const {
-  vector<Key> ordering;  // Nodes in MFAS order (result).
+KeyVector MFAS::computeOrdering() const {
+  KeyVector ordering;  // Nodes in MFAS order (result).
 
   // A graph is an unordered map from keys to nodes. Each node contains a list
   // of its adjacent nodes. Create the graph from the edgeWeights.
@@ -140,7 +140,7 @@ vector<Key> MFAS::computeOrdering() const {
 
 map<MFAS::KeyPair, double> MFAS::computeOutlierWeights() const {
   // Find the ordering.
-  vector<Key> ordering = computeOrdering();
+  KeyVector ordering = computeOrdering();
 
   // Create a map from the node key to its position in the ordering. This makes
   // it easier to lookup positions of different nodes.
diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h
index 3b01122a9..decfbed0f 100644
--- a/gtsam/sfm/MFAS.h
+++ b/gtsam/sfm/MFAS.h
@@ -84,7 +84,7 @@ class MFAS {
    * @brief Computes the 1D MFAS ordering of nodes in the graph
    * @return orderedNodes: vector of nodes in the obtained order
    */
-  std::vector<Key> computeOrdering() const;
+  KeyVector computeOrdering() const;
 
   /**
    * @brief Computes the outlier weights of the graph. We define the outlier
diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp
index 362027d5d..a1f6dfa5f 100644
--- a/gtsam/sfm/tests/testMFAS.cpp
+++ b/gtsam/sfm/tests/testMFAS.cpp
@@ -25,7 +25,7 @@ using namespace gtsam;
 vector<MFAS::KeyPair> edges = {make_pair(3, 2), make_pair(0, 1), make_pair(3, 1),
                          make_pair(1, 2), make_pair(0, 2), make_pair(3, 0)};
 // nodes in the graph
-vector<Key> nodes = {Key(0), Key(1), Key(2), Key(3)};
+KeyVector nodes = {Key(0), Key(1), Key(2), Key(3)};
 // weights from projecting in direction-1 (bad direction, outlier accepted)
 vector<double> weights1 = {2, 1.5, 0.5, 0.25, 1, 0.75};
 // weights from projecting in direction-2 (good direction, outlier rejected)
@@ -47,10 +47,10 @@ map<MFAS::KeyPair, double> getEdgeWeights(const vector<MFAS::KeyPair> &edges,
 TEST(MFAS, OrderingWeights2) {
   MFAS mfas_obj(getEdgeWeights(edges, weights2));
 
-  vector<Key> ordered_nodes = mfas_obj.computeOrdering();
+  KeyVector ordered_nodes = mfas_obj.computeOrdering();
 
   // ground truth (expected) ordering in this example
-  vector<Key> gt_ordered_nodes = {0, 1, 3, 2};
+  KeyVector gt_ordered_nodes = {0, 1, 3, 2};
 
   // check if the expected ordering is obtained
   for (size_t i = 0; i < ordered_nodes.size(); i++) {
@@ -77,10 +77,10 @@ TEST(MFAS, OrderingWeights2) {
 TEST(MFAS, OrderingWeights1) {
   MFAS mfas_obj(getEdgeWeights(edges, weights1));
 
-  vector<Key> ordered_nodes = mfas_obj.computeOrdering();
+  KeyVector ordered_nodes = mfas_obj.computeOrdering();
 
   // "ground truth" expected ordering in this example
-  vector<Key> gt_ordered_nodes = {3, 0, 1, 2};
+  KeyVector gt_ordered_nodes = {3, 0, 1, 2};
 
   // check if the expected ordering is obtained
   for (size_t i = 0; i < ordered_nodes.size(); i++) {
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 270dbeb95..74e074c9e 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -1164,8 +1164,8 @@ bool writeBAL(const string &filename, SfmData &data) {
     for (size_t k = 0; k < track.number_measurements();
          k++) { // for each observation of the 3D point j
       size_t i = track.measurements[k].first; // camera id
-      double u0 = data.cameras[i].calibration().u0();
-      double v0 = data.cameras[i].calibration().v0();
+      double u0 = data.cameras[i].calibration().px();
+      double v0 = data.cameras[i].calibration().py();
 
       if (u0 != 0 || v0 != 0) {
         cout << "writeBAL has not been tested for calibration with nonzero "
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index a8fddcfe4..93bd2b2ee 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -211,16 +211,18 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
 /// A measurement with its camera index
 typedef std::pair<size_t, Point2> SfmMeasurement;
 
-/// SfmTrack
+/// Sift index for SfmTrack
 typedef std::pair<size_t, size_t> SiftIndex;
 
 /// Define the structure for the 3D points
 struct SfmTrack {
   SfmTrack(): p(0,0,0) {}
+  SfmTrack(const gtsam::Point3& pt) : p(pt) {}
   Point3 p; ///< 3D position of the point
   float r, g, b; ///< RGB color of the 3D point
   std::vector<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
   std::vector<SiftIndex> siftIndices;
+  
   /// Total number of measurements in this track
   size_t number_measurements() const {
     return measurements.size();
@@ -233,11 +235,17 @@ struct SfmTrack {
   SiftIndex siftIndex(size_t idx) const {
     return siftIndices[idx];
   }
-  Point3 point3() const {
+  /// Get 3D point
+  const Point3& point3() const {
     return p;
   }
+  /// Add measurement (camera_idx, Point2) to track
+  void add_measurement(size_t idx, const gtsam::Point2& m) {
+    measurements.emplace_back(idx, m);
+  }
 };
 
+
 /// Define the structure for the camera poses
 typedef PinholeCamera<Cal3Bundler> SfmCamera;
 
@@ -260,6 +268,14 @@ struct SfmData {
   SfmTrack track(size_t idx) const {
     return tracks[idx];
   }
+  /// Add a track to SfmData
+  void add_track(const SfmTrack& t)  {
+    tracks.push_back(t);
+  }
+  /// Add a camera to SfmData
+  void add_camera(const SfmCamera& cam){
+    cameras.push_back(cam);
+  }
 };
 
 /**
diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp
index b985eb374..937761f02 100644
--- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp
+++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp
@@ -361,8 +361,12 @@ TEST(Similarity3, AlignPose3) {
 
   vector<Pose3Pair> correspondences{bTa1, bTa2};
 
+  // Cayley transform cannot accommodate 180 degree rotations,
+  // hence we only test for Expmap
+#ifdef GTSAM_ROT3_EXPMAP
   Similarity3 actual_aSb = Similarity3::Align(correspondences);
   EXPECT(assert_equal(expected_aSb, actual_aSb));
+#endif
 }
 
 //******************************************************************************
diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md
index e05a0c7e1..2a2c9f2ef 100644
--- a/python/gtsam/examples/README.md
+++ b/python/gtsam/examples/README.md
@@ -39,7 +39,7 @@
 | SelfCalibrationExample                                |        |
 | SFMdata                                               |        |     
 | SFMExample_bal_COLAMD_METIS                           |        |
-| SFMExample_bal                                        |       |
+| SFMExample_bal                                        | :heavy_check_mark:      |
 | SFMExample                                            | :heavy_check_mark:      |
 | SFMExampleExpressions_bal                             |        |
 | SFMExampleExpressions                                 |        |
diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py
new file mode 100644
index 000000000..dfe8b523c
--- /dev/null
+++ b/python/gtsam/examples/SFMExample_bal.py
@@ -0,0 +1,118 @@
+"""
+  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
+
+  Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file
+  Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert)
+"""
+
+import argparse
+import logging
+import sys
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+import gtsam
+from gtsam import (
+    GeneralSFMFactorCal3Bundler,
+    PinholeCameraCal3Bundler,
+    PriorFactorPinholeCameraCal3Bundler,
+    readBal,
+    symbol_shorthand
+)
+
+C = symbol_shorthand.C
+P = symbol_shorthand.P
+
+
+logging.basicConfig(stream=sys.stdout, level=logging.DEBUG)
+
+def run(args):
+    """ Run LM optimization with BAL input data and report resulting error """
+    input_file = gtsam.findExampleDataFile(args.input_file)
+
+    # Load the SfM data from file
+    scene_data = readBal(input_file)
+    logging.info(f"read {scene_data.number_tracks()} tracks on {scene_data.number_cameras()} cameras\n")
+
+    # Create a factor graph
+    graph = gtsam.NonlinearFactorGraph()
+
+    # We share *one* noiseModel between all projection factors
+    noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
+
+    # Add measurements to the factor graph
+    j = 0
+    for t_idx in range(scene_data.number_tracks()):
+        track = scene_data.track(t_idx) # SfmTrack
+        # retrieve the SfmMeasurement objects
+        for m_idx in range(track.number_measurements()):
+            # i represents the camera index, and uv is the 2d measurement
+            i, uv = track.measurement(m_idx)
+            # note use of shorthand symbols C and P
+            graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j)))
+        j += 1
+
+    # Add a prior on pose x1. This indirectly specifies where the origin is.
+    graph.push_back(
+        gtsam.PriorFactorPinholeCameraCal3Bundler(
+            C(0), scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1)
+        )
+    )
+    # Also add a prior on the position of the first landmark to fix the scale
+    graph.push_back(
+        gtsam.PriorFactorPoint3(
+            P(0), scene_data.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
+        )
+    )
+
+    # Create initial estimate
+    initial = gtsam.Values()
+    
+    i = 0
+    # add each PinholeCameraCal3Bundler
+    for cam_idx in range(scene_data.number_cameras()):
+        camera = scene_data.camera(cam_idx)
+        initial.insert(C(i), camera)
+        i += 1
+
+    j = 0
+    # add each SfmTrack
+    for t_idx in range(scene_data.number_tracks()):
+        track = scene_data.track(t_idx)  
+        initial.insert(P(j), track.point3())
+        j += 1
+
+    # Optimize the graph and print results
+    try:
+        params = gtsam.LevenbergMarquardtParams()
+        params.setVerbosityLM("ERROR")
+        lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
+        result = lm.optimize()
+    except Exception as e:
+        logging.exception("LM Optimization failed")
+        return
+    # Error drops from ~2764.22 to ~0.046
+    logging.info(f"final error: {graph.error(result)}")
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        '-i',
+        '--input_file',
+        type=str,
+        default="dubrovnik-3-7-pre",
+        help='Read SFM data from the specified BAL file'
+        'The data format is described here: https://grail.cs.washington.edu/projects/bal/.'
+        'BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, '
+        'then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector'
+        'and (x,y,z) 3d point initializations.'
+    )
+    run(parser.parse_args())
+
diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py
new file mode 100644
index 000000000..9c965ddc5
--- /dev/null
+++ b/python/gtsam/tests/test_SfmData.py
@@ -0,0 +1,79 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for testing dataset access.
+Author: Frank Dellaert (Python: Sushmita Warrier)
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+from __future__ import print_function
+
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestSfmData(GtsamTestCase):
+    """Tests for SfmData and SfmTrack modules."""
+
+    def setUp(self):
+        """Initialize SfmData and SfmTrack"""
+        self.data = gtsam.SfmData()
+        # initialize SfmTrack with 3D point
+        self.tracks = gtsam.SfmTrack()
+
+    def test_tracks(self):
+        """Test functions in SfmTrack"""
+        # measurement is of format (camera_idx, imgPoint)
+        # create arbitrary camera indices for two cameras
+        i1, i2 = 4,5
+        # create arbitrary image measurements for cameras i1 and i2
+        uv_i1 = gtsam.Point2(12.6, 82)
+        # translating point uv_i1 along X-axis
+        uv_i2 = gtsam.Point2(24.88, 82)
+        # add measurements to the track
+        self.tracks.add_measurement(i1, uv_i1)
+        self.tracks.add_measurement(i2, uv_i2)
+        # Number of measurements in the track is 2
+        self.assertEqual(self.tracks.number_measurements(), 2)
+        # camera_idx in the first measurement of the track corresponds to i1
+        cam_idx, img_measurement = self.tracks.measurement(0)
+        self.assertEqual(cam_idx, i1)
+        np.testing.assert_array_almost_equal(
+            gtsam.Point3(0.,0.,0.), 
+            self.tracks.point3()
+        )
+
+
+    def test_data(self):
+        """Test functions in SfmData"""
+        # Create new track with 3 measurements
+        i1, i2, i3 = 3,5,6
+        uv_i1 = gtsam.Point2(21.23, 45.64)
+        # translating along X-axis
+        uv_i2 = gtsam.Point2(45.7, 45.64)
+        uv_i3 = gtsam.Point2(68.35, 45.64)
+        # add measurements and arbitrary point to the track
+        measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
+        pt = gtsam.Point3(1.0, 6.0, 2.0)
+        track2 = gtsam.SfmTrack(pt)
+        track2.add_measurement(i1, uv_i1)
+        track2.add_measurement(i2, uv_i2)
+        track2.add_measurement(i3, uv_i3)
+        self.data.add_track(self.tracks)
+        self.data.add_track(track2)
+        # Number of tracks in SfmData is 2
+        self.assertEqual(self.data.number_tracks(), 2)
+        # camera idx of first measurement of second track corresponds to i1
+        cam_idx, img_measurement = self.data.track(1).measurement(0)
+        self.assertEqual(cam_idx, i1)
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp
index dc19801a2..6415174d5 100644
--- a/tests/testNonlinearOptimizer.cpp
+++ b/tests/testNonlinearOptimizer.cpp
@@ -566,6 +566,58 @@ TEST( NonlinearOptimizer, logfile )
 //  EXPECT(actual.str()==expected.str());
 }
 
+/* ************************************************************************* */
+TEST( NonlinearOptimizer, iterationHook_LM )
+{
+  NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());
+
+  Point2 x0(3,3);
+  Values c0;
+  c0.insert(X(1), x0);
+
+  // Levenberg-Marquardt
+  LevenbergMarquardtParams lmParams;
+  size_t lastIterCalled = 0;
+  lmParams.iterationHook = [&](size_t iteration, double oldError, double newError)
+  {
+    // Tests:
+    lastIterCalled = iteration;
+    EXPECT(newError<oldError);
+    
+    // Example of evolution printout:
+    //std::cout << "iter: " << iteration << " error: " << oldError << " => " << newError <<"\n";
+  };
+  LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize();
+  
+  EXPECT(lastIterCalled>5);
+}
+/* ************************************************************************* */
+TEST( NonlinearOptimizer, iterationHook_CG )
+{
+  NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());
+
+  Point2 x0(3,3);
+  Values c0;
+  c0.insert(X(1), x0);
+
+  // Levenberg-Marquardt
+  NonlinearConjugateGradientOptimizer::Parameters cgParams;
+  size_t lastIterCalled = 0;
+  cgParams.iterationHook = [&](size_t iteration, double oldError, double newError)
+  {
+    // Tests:
+    lastIterCalled = iteration;
+    EXPECT(newError<oldError);
+    
+    // Example of evolution printout:
+    //std::cout << "iter: " << iteration << " error: " << oldError << " => " << newError <<"\n";
+  };
+  NonlinearConjugateGradientOptimizer(fg, c0, cgParams).optimize();
+  
+  EXPECT(lastIterCalled>5);
+}
+
+
 /* ************************************************************************* */
 //// Minimal traits example
 struct MyType : public Vector3 {
diff --git a/wrap/cmake/PybindWrap.cmake b/wrap/cmake/PybindWrap.cmake
index 85f956d50..ff69b5aed 100644
--- a/wrap/cmake/PybindWrap.cmake
+++ b/wrap/cmake/PybindWrap.cmake
@@ -146,7 +146,7 @@ function(install_python_scripts
       else()
         set(build_type_tag "")
       endif()
-      # Split up filename to strip trailing '/' in WRAP_CYTHON_INSTALL_PATH if
+      # Split up filename to strip trailing '/' in GTSAM_PY_INSTALL_PATH if
       # there is one
       get_filename_component(location "${dest_directory}" PATH)
       get_filename_component(name "${dest_directory}" NAME)