diff --git a/.github/workflows/trigger-python.yml b/.github/workflows/trigger-python.yml
new file mode 100644
index 000000000..8fad9e7ca
--- /dev/null
+++ b/.github/workflows/trigger-python.yml
@@ -0,0 +1,14 @@
+# This triggers Cython builds on `gtsam-manylinux-build`
+name: Trigger Python Builds
+on: push
+jobs:
+  triggerCython:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Repository Dispatch
+        uses: ProfFan/repository-dispatch@master
+        with:
+          token: ${{ secrets.PYTHON_CI_REPO_ACCESS_TOKEN }}
+          repository: borglab/gtsam-manylinux-build
+          event-type: cython-wrapper
+          client-payload: '{"ref": "${{ github.ref }}", "sha": "${{ github.sha }}"}'
diff --git a/.travis.python.sh b/.travis.python.sh
new file mode 100644
index 000000000..772311f38
--- /dev/null
+++ b/.travis.python.sh
@@ -0,0 +1,43 @@
+#!/bin/bash
+set -x -e
+
+if [ -z ${PYTHON_VERSION+x} ]; then
+    echo "Please provide the Python version to build against!"
+    exit 127
+fi
+
+PYTHON="python${PYTHON_VERSION}"
+
+if [[ $(uname) == "Darwin" ]]; then
+    brew install wget
+else
+    # Install a system package required by our library
+    sudo apt-get install wget libicu-dev python3-pip python3-setuptools
+fi
+
+CURRDIR=$(pwd)
+
+sudo $PYTHON -m pip install -r ./cython/requirements.txt
+
+mkdir $CURRDIR/build
+cd $CURRDIR/build
+
+cmake $CURRDIR -DCMAKE_BUILD_TYPE=Release \
+    -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_UNSTABLE=ON \
+    -DGTSAM_USE_QUATERNIONS=OFF \
+    -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
+    -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
+    -DGTSAM_INSTALL_CYTHON_TOOLBOX=ON \
+    -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
+    -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF \
+    -DCMAKE_INSTALL_PREFIX=$CURRDIR/../gtsam_install
+
+make -j$(nproc) install
+
+cd cython
+
+sudo $PYTHON setup.py install
+
+cd $CURRDIR/cython/gtsam/tests
+
+$PYTHON -m unittest discover
\ No newline at end of file
diff --git a/.travis.sh b/.travis.sh
index 3cec20f53..535a72f4b 100755
--- a/.travis.sh
+++ b/.travis.sh
@@ -1,7 +1,39 @@
 #!/bin/bash
 
+# install TBB with _debug.so files
+function install_tbb()
+{
+  TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download
+  TBB_VERSION=4.4.2
+  TBB_DIR=tbb44_20151115oss
+  TBB_SAVEPATH="/tmp/tbb.tgz"
+
+  if [ "$TRAVIS_OS_NAME" == "linux" ]; then
+    OS_SHORT="lin"
+    TBB_LIB_DIR="intel64/gcc4.4"
+    SUDO="sudo"
+
+  elif [ "$TRAVIS_OS_NAME" == "osx" ]; then
+    OS_SHORT="lin"
+    TBB_LIB_DIR=""
+    SUDO=""
+
+  fi
+
+  wget "${TBB_BASEURL}/${TBB_VERSION}/${TBB_DIR}_${OS_SHORT}.tgz" -O $TBB_SAVEPATH
+  tar -C /tmp -xf $TBB_SAVEPATH
+
+  TBBROOT=/tmp/$TBB_DIR
+  # Copy the needed files to the correct places.
+  # This works correctly for travis builds, instead of setting path variables.
+  # This is what Homebrew does to install TBB on Macs
+  $SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/
+  $SUDO cp -R $TBBROOT/include/ /usr/local/include/
+
+}
+
 # common tasks before either build or test
-function prepare ()
+function configure()
 {
   set -e   # Make sure any error makes the script to return an error code
   set -x   # echo
@@ -14,21 +46,27 @@ function prepare ()
   rm -fr $BUILD_DIR || true
   mkdir $BUILD_DIR && cd $BUILD_DIR
 
-  if [ -z "$CMAKE_BUILD_TYPE" ]; then
-    CMAKE_BUILD_TYPE=Debug
-  fi
-
-  if [ -z "$GTSAM_ALLOW_DEPRECATED_SINCE_V4" ]; then
-    GTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF
-  fi
+  install_tbb
 
   if [ ! -z "$GCC_VERSION" ]; then
-    sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-$GCC_VERSION 60 \
-                         --slave /usr/bin/g++ g++ /usr/bin/g++-$GCC_VERSION
-    sudo update-alternatives --set gcc /usr/bin/gcc-$GCC_VERSION
+    export CC=gcc-$GCC_VERSION
+    export CXX=g++-$GCC_VERSION
   fi
+
+  # GTSAM_BUILD_WITH_MARCH_NATIVE=OFF: to avoid crashes in builder VMs
+  cmake $SOURCE_DIR \
+      -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \
+      -DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
+      -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
+      -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
+      -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
+      -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
+      -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \
+      -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
+      -DCMAKE_VERBOSE_MAKEFILE=OFF
 }
 
+
 # common tasks after either build or test
 function finish ()
 {
@@ -41,17 +79,12 @@ function finish ()
 # compile the code with the intent of populating the cache
 function build ()
 {
-  prepare
+  export GTSAM_BUILD_EXAMPLES_ALWAYS=ON
+  export GTSAM_BUILD_TESTS=OFF
 
-  cmake $SOURCE_DIR \
-      -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \
-      -DGTSAM_BUILD_TESTS=OFF \
-      -DGTSAM_BUILD_UNSTABLE=$GTSAM_BUILD_UNSTABLE \
-      -DGTSAM_BUILD_EXAMPLES_ALWAYS=ON \
-      -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=$GTSAM_ALLOW_DEPRECATED_SINCE_V4
+  configure
 
-  # Actual build:
-  VERBOSE=1 make -j2
+  make -j2
 
   finish
 }
@@ -59,14 +92,10 @@ function build ()
 # run the tests
 function test ()
 {
-  prepare
+  export GTSAM_BUILD_EXAMPLES_ALWAYS=OFF
+  export GTSAM_BUILD_TESTS=ON
 
-  cmake $SOURCE_DIR \
-      -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \
-      -DGTSAM_BUILD_TESTS=ON \
-      -DGTSAM_BUILD_UNSTABLE=$GTSAM_BUILD_UNSTABLE \
-      -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-      -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF
+  configure
 
   # Actual build:
   make -j2 check
@@ -79,7 +108,7 @@ case $1 in
   -b)
     build
     ;;
-  -t)                      
+  -t)
     test
     ;;
 esac
diff --git a/.travis.yml b/.travis.yml
index 1e2d6760a..d8094ef4d 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -7,17 +7,19 @@ addons:
   apt:
     sources:
     - ubuntu-toolchain-r-test
+    - sourceline: 'deb http://apt.llvm.org/xenial/ llvm-toolchain-xenial-9 main'
+      key_url: 'https://apt.llvm.org/llvm-snapshot.gpg.key'
     packages:
-    - g++-8
-    - clang-3.8
-    - build-essential
-    - pkg-config
+    - g++-9
+    - clang-9
+    - build-essential pkg-config
     - cmake
-    - libpython-dev python-numpy
+    - python3-dev libpython-dev
+    - python3-numpy
     - libboost-all-dev
 
 # before_install:
-#  - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update          ; fi
+  # - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update; fi
 
 install:
   - if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache  ; fi
@@ -27,9 +29,16 @@ install:
 stages:
   - compile
   - test
+  - special
+
+env:
+  global:
+    - MAKEFLAGS="-j2"
+    - CCACHE_SLOPPINESS=pch_defines,time_macros
 
 # Compile stage without building examples/tests to populate the caches.
 jobs:
+# -------- STAGE 1: COMPILE -----------
   include:
 # on Mac, GCC
   - stage: compile
@@ -68,46 +77,65 @@ jobs:
   - stage: compile
     os: linux
     compiler: clang
-    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
+    env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
     script: bash .travis.sh -b
   - stage: compile
     os: linux
     compiler: clang
+    env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release
+    script: bash .travis.sh -b
+# on Linux, with deprecated ON to make sure that path still compiles/tests
+  - stage: special
+    os: linux
+    compiler: clang
+    env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON
+    script: bash .travis.sh -b
+# on Linux, with GTSAM_WITH_TBB on to make sure GTSAM still compiles/tests
+  - stage: special
+    os: linux
+    compiler: gcc
+    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=ON
+    script: bash .travis.sh -t
+# -------- STAGE 2: TESTS -----------
+# on Mac, GCC
+  - stage: test
+    os: osx
+    compiler: clang
     env: CMAKE_BUILD_TYPE=Release
-    script: bash .travis.sh -b
-# on Linux, with deprecated ON to make sure that path still compiles
-  - stage: compile
+    script: bash .travis.sh -t
+  - stage: test
+    os: osx
+    compiler: clang
+    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
+    script: bash .travis.sh -t
+  - stage: test
+    os: linux
+    compiler: gcc
+    env: CMAKE_BUILD_TYPE=Release
+    script: bash .travis.sh -t
+  - stage: test
+    os: linux
+    compiler: gcc
+    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
+    script: bash .travis.sh -t
+  - stage: test
     os: linux
     compiler: clang
-    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON
-    script: bash .travis.sh -b
-
-# Matrix configuration:
-os:
-  - osx
-  - linux
-compiler:
-  - gcc
-  - clang
-env:
-  global:
-    - MAKEFLAGS="-j2"
-    - CCACHE_SLOPPINESS=pch_defines,time_macros
-    - GTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF
-    - GTSAM_BUILD_UNSTABLE=ON
-  matrix:
-    - CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
-    - CMAKE_BUILD_TYPE=Release
-script:
-  - bash .travis.sh -t
-
-matrix:
-  exclude:
-    # Exclude g++ debug on Linux as it consistently times out
-    - os: linux
-      compiler: gcc
-      env : CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
-    # Exclude clang on Linux/clang in release until issue #57 is solved
-    - os: linux
-      compiler: clang
-      env : CMAKE_BUILD_TYPE=Release
+    env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release
+    script: bash .travis.sh -t
+# on Linux, with quaternions ON to make sure that path still compiles/tests
+  - stage: special
+    os: linux
+    compiler: clang
+    env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release GTSAM_BUILD_UNSTABLE=OFF GTSAM_USE_QUATERNIONS=ON
+    script: bash .travis.sh -t
+  - stage: special
+    os: linux
+    compiler: gcc
+    env: PYTHON_VERSION=3
+    script: bash .travis.python.sh
+  - stage: special
+    os: osx
+    compiler: clang
+    env: PYTHON_VERSION=3
+    script: bash .travis.python.sh
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1c9f0639a..edefbf2ea 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,7 @@ endif()
 # Set the version number for the library
 set (GTSAM_VERSION_MAJOR 4)
 set (GTSAM_VERSION_MINOR 0)
-set (GTSAM_VERSION_PATCH 2)
+set (GTSAM_VERSION_PATCH 3)
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
 
@@ -22,6 +22,7 @@ set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM
 
 set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
 include(GtsamMakeConfigFile)
+include(GNUInstallDirs)
 
 # Record the root dir for gtsam - needed during external builds, e.g., ROS
 set(GTSAM_SOURCE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR})
@@ -67,13 +68,13 @@ if(GTSAM_UNSTABLE_AVAILABLE)
 endif()
 option(BUILD_SHARED_LIBS                 "Build shared gtsam library, instead of static" ON)
 option(GTSAM_USE_QUATERNIONS             "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
-option(GTSAM_POSE3_EXPMAP 			 	 "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF)
-option(GTSAM_ROT3_EXPMAP 			 	 "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
+option(GTSAM_POSE3_EXPMAP 			 	 "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
+option(GTSAM_ROT3_EXPMAP 			 	 "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
 option(GTSAM_ENABLE_CONSISTENCY_CHECKS   "Enable/Disable expensive consistency checks"       OFF)
 option(GTSAM_WITH_TBB                    "Use Intel Threaded Building Blocks (TBB) if available" ON)
 option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available" OFF)
 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_THROW_CHEIRALITY_EXCEPTION  "Throw exception when a triangulated point is behind a camera" ON)
 option(GTSAM_ALLOW_DEPRECATED_SINCE_V4   "Allow use of methods/functions deprecated in GTSAM 4" ON)
 option(GTSAM_TYPEDEF_POINTS_TO_VECTORS   "Typedef Point2 and Point3 to Eigen::Vector equivalents" OFF)
 option(GTSAM_SUPPORT_NESTED_DISSECTION   "Support Metis-based nested dissection" ON)
@@ -88,7 +89,7 @@ if(NOT MSVC AND NOT XCODE_VERSION)
 
   # Set the GTSAM_BUILD_TAG variable.
   # If build type is Release, set to blank (""), else set to the build type.
-  if("${CMAKE_BUILD_TYPE_UPPER}" STREQUAL "RELEASE")
+  if(${CMAKE_BUILD_TYPE_UPPER} STREQUAL "RELEASE")
    set(GTSAM_BUILD_TAG "") # Don't create release mode tag on installed directory
   else()
    set(GTSAM_BUILD_TAG "${CMAKE_BUILD_TYPE}")
@@ -171,63 +172,45 @@ if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILE
   message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.")
 endif()
 
-# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
 option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
-
-# JLBC: This was once updated to target-based names (Boost::xxx), but it caused
-# problems with Boost versions newer than FindBoost.cmake was prepared to handle,
-# so we downgraded this to classic filenames-based variables, and manually adding
-# the target_include_directories(xxx ${Boost_INCLUDE_DIR})
+# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
 set(GTSAM_BOOST_LIBRARIES
-  optimized ${Boost_SERIALIZATION_LIBRARY_RELEASE}
-  optimized ${Boost_SYSTEM_LIBRARY_RELEASE}
-  optimized ${Boost_FILESYSTEM_LIBRARY_RELEASE}
-  optimized ${Boost_THREAD_LIBRARY_RELEASE}
-  optimized ${Boost_DATE_TIME_LIBRARY_RELEASE}
-  optimized ${Boost_REGEX_LIBRARY_RELEASE}
-  debug ${Boost_SERIALIZATION_LIBRARY_DEBUG}
-  debug ${Boost_SYSTEM_LIBRARY_DEBUG}
-  debug ${Boost_FILESYSTEM_LIBRARY_DEBUG}
-  debug ${Boost_THREAD_LIBRARY_DEBUG}
-  debug ${Boost_DATE_TIME_LIBRARY_DEBUG}
-  debug ${Boost_REGEX_LIBRARY_DEBUG}
+  Boost::serialization
+  Boost::system
+  Boost::filesystem
+  Boost::thread
+  Boost::date_time
+  Boost::regex
 )
-message(STATUS "GTSAM_BOOST_LIBRARIES: ${GTSAM_BOOST_LIBRARIES}")
 if (GTSAM_DISABLE_NEW_TIMERS)
     message("WARNING:  GTSAM timing instrumentation manually disabled")
     list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS)
 else()
     if(Boost_TIMER_LIBRARY)
-      list(APPEND GTSAM_BOOST_LIBRARIES
-        optimized ${Boost_TIMER_LIBRARY_RELEASE}
-        optimized ${Boost_CHRONO_LIBRARY_RELEASE}
-        debug ${Boost_TIMER_LIBRARY_DEBUG}
-        debug ${Boost_CHRONO_LIBRARY_DEBUG}
-        )
+      list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono)
     else()
       list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt
       message("WARNING:  GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.")
     endif()
 endif()
 
-
-if(NOT (${Boost_VERSION} LESS 105600))
-	message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
-	list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES)
-endif()
-
 ###############################################################################
 # Find TBB
 find_package(TBB 4.4 COMPONENTS tbb tbbmalloc)
 
 # Set up variables if we're using TBB
 if(TBB_FOUND AND GTSAM_WITH_TBB)
-	set(GTSAM_USE_TBB 1)  # This will go into config.h
-  # all definitions and link requisites will go via imported targets:
-  # tbb & tbbmalloc
-  list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
+    set(GTSAM_USE_TBB 1)  # This will go into config.h
+    if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020))
+        set(TBB_GREATER_EQUAL_2020 1)
+    else()
+        set(TBB_GREATER_EQUAL_2020 0)
+    endif()
+    # all definitions and link requisites will go via imported targets:
+    # tbb & tbbmalloc
+    list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
 else()
-	set(GTSAM_USE_TBB 0)  # This will go into config.h
+    set(GTSAM_USE_TBB 0)  # This will go into config.h
 endif()
 
 ###############################################################################
@@ -362,6 +345,11 @@ if (MSVC)
 	list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen
 endif()
 
+if (APPLE AND BUILD_SHARED_LIBS)
+	# Set the default install directory on macOS
+	set(CMAKE_INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib")
+endif()
+
 ###############################################################################
 # Global compile options
 
@@ -434,6 +422,10 @@ add_subdirectory(CppUnitLite)
 # Build wrap
 if (GTSAM_BUILD_WRAP)
     add_subdirectory(wrap)
+	# suppress warning of cython line being too long
+	if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+		set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation")
+	endif()
 endif(GTSAM_BUILD_WRAP)
 
 # Build GTSAM library
@@ -451,7 +443,7 @@ add_subdirectory(timing)
 # Build gtsam_unstable
 if (GTSAM_BUILD_UNSTABLE)
     add_subdirectory(gtsam_unstable)
-endif(GTSAM_BUILD_UNSTABLE)
+endif()
 
 # Matlab toolbox
 if (GTSAM_INSTALL_MATLAB_TOOLBOX)
@@ -462,12 +454,11 @@ endif()
 if (GTSAM_INSTALL_CYTHON_TOOLBOX)
   set(GTSAM_INSTALL_CYTHON_TOOLBOX 1)
   # Set up cache options
-  set(GTSAM_CYTHON_INSTALL_PATH "" CACHE PATH "Cython toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/cython")
-  if(NOT GTSAM_CYTHON_INSTALL_PATH)
-    set(GTSAM_CYTHON_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/cython")
-  endif()
+  # Cython install path appended with Build type (e.g. cython, cythonDebug, etc).
+  # This does not override custom values set from the command line
+  set(GTSAM_CYTHON_INSTALL_PATH "${PROJECT_BINARY_DIR}/cython${GTSAM_BUILD_TAG}" CACHE PATH "Cython toolbox destination, blank defaults to PROJECT_BINARY_DIR/cython<GTSAM_BUILD_TAG>")
   set(GTSAM_EIGENCY_INSTALL_PATH ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency)
-  add_subdirectory(cython)
+  add_subdirectory(cython ${GTSAM_CYTHON_INSTALL_PATH})
 else()
   set(GTSAM_INSTALL_CYTHON_TOOLBOX 0) # This will go into config.h
 endif()
@@ -545,48 +536,54 @@ endif()
 
 print_build_options_for_target(gtsam)
 
-message(STATUS "  Use System Eigen               : ${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
+message(STATUS "  Use System Eigen                : ${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
 
 if(GTSAM_USE_TBB)
-	message(STATUS "  Use Intel TBB                  : Yes")
+	message(STATUS "  Use Intel TBB                   : Yes")
 elseif(TBB_FOUND)
-	message(STATUS "  Use Intel TBB                  : TBB found but GTSAM_WITH_TBB is disabled")
+	message(STATUS "  Use Intel TBB                   : TBB found but GTSAM_WITH_TBB is disabled")
 else()
-	message(STATUS "  Use Intel TBB                  : TBB not found")
+	message(STATUS "  Use Intel TBB                   : TBB not found")
 endif()
 if(GTSAM_USE_EIGEN_MKL)
-	message(STATUS "  Eigen will use MKL             : Yes")
+	message(STATUS "  Eigen will use MKL              : Yes")
 elseif(MKL_FOUND)
-	message(STATUS "  Eigen will use MKL             : MKL found but GTSAM_WITH_EIGEN_MKL is disabled")
+	message(STATUS "  Eigen will use MKL              : MKL found but GTSAM_WITH_EIGEN_MKL is disabled")
 else()
-	message(STATUS "  Eigen will use MKL             : MKL not found")
+	message(STATUS "  Eigen will use MKL              : MKL not found")
 endif()
 if(GTSAM_USE_EIGEN_MKL_OPENMP)
-	message(STATUS "  Eigen will use MKL and OpenMP  : Yes")
+	message(STATUS "  Eigen will use MKL and OpenMP   : Yes")
 elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL)
-	message(STATUS "  Eigen will use MKL and OpenMP  : OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled")
+	message(STATUS "  Eigen will use MKL and OpenMP   : OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled")
 elseif(OPENMP_FOUND AND NOT MKL_FOUND)
-	message(STATUS "  Eigen will use MKL and OpenMP  : OpenMP found but MKL not found")
+	message(STATUS "  Eigen will use MKL and OpenMP   : OpenMP found but MKL not found")
 elseif(OPENMP_FOUND)
-	message(STATUS "  Eigen will use MKL and OpenMP  : OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled")
+	message(STATUS "  Eigen will use MKL and OpenMP   : OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled")
 else()
-	message(STATUS "  Eigen will use MKL and OpenMP  : OpenMP not found")
+	message(STATUS "  Eigen will use MKL and OpenMP   : OpenMP not found")
+endif()
+message(STATUS "  Default allocator               : ${GTSAM_DEFAULT_ALLOCATOR}")
+
+if(GTSAM_THROW_CHEIRALITY_EXCEPTION)
+	message(STATUS "  Cheirality exceptions enabled   : YES")
+else()
+	message(STATUS "  Cheirality exceptions enabled   : NO")
 endif()
-message(STATUS "  Default allocator              : ${GTSAM_DEFAULT_ALLOCATOR}")
 
 if(NOT MSVC AND NOT XCODE_VERSION)
 	if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
-		message(STATUS "  Build with ccache              : Yes")
+		message(STATUS "  Build with ccache               : Yes")
 	elseif(CCACHE_FOUND)
-		message(STATUS "  Build with ccache              : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
+		message(STATUS "  Build with ccache               : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
 	else()
-		message(STATUS "  Build with ccache              : No")
+		message(STATUS "  Build with ccache               : No")
 	endif()
 endif()
 
 message(STATUS "Packaging flags                                               ")
-message(STATUS "  CPack Source Generator         : ${CPACK_SOURCE_GENERATOR}")
-message(STATUS "  CPack Generator                : ${CPACK_GENERATOR}")
+message(STATUS "  CPack Source Generator          : ${CPACK_SOURCE_GENERATOR}")
+message(STATUS "  CPack Generator                 : ${CPACK_GENERATOR}")
 
 message(STATUS "GTSAM flags                                               ")
 print_config_flag(${GTSAM_USE_QUATERNIONS}             "Quaternions as default Rot3     ")
@@ -597,15 +594,19 @@ print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4}   "Deprecated in GTSAM 4 al
 print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS}   "Point3 is typedef to Vector3    ")
 print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION}   "Metis-based Nested Dissection   ")
 print_config_flag(${GTSAM_TANGENT_PREINTEGRATION}      "Use tangent-space preintegration")
-print_config_flag(${GTSAM_BUILD_WRAP}                  "Build Wrap                     ")
+print_config_flag(${GTSAM_BUILD_WRAP}                  "Build Wrap                      ")
 
 message(STATUS "MATLAB toolbox flags                                      ")
-print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX}      "Install matlab toolbox         ")
+print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX}      "Install MATLAB toolbox          ")
+if (${GTSAM_INSTALL_MATLAB_TOOLBOX})
+    message(STATUS "  MATLAB root                     : ${MATLAB_ROOT}")
+    message(STATUS "  MEX binary                      : ${MEX_COMMAND}")
+endif()
 
 message(STATUS "Cython toolbox flags                                      ")
-print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX}      "Install Cython toolbox         ")
+print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX}      "Install Cython toolbox          ")
 if(GTSAM_INSTALL_CYTHON_TOOLBOX)
-	message(STATUS "  Python version                 : ${GTSAM_PYTHON_VERSION}")
+	message(STATUS "  Python version                  : ${GTSAM_PYTHON_VERSION}")
 endif()
 message(STATUS "===============================================================")
 
diff --git a/CppUnitLite/CMakeLists.txt b/CppUnitLite/CMakeLists.txt
index 72651aca9..ab884ec1d 100644
--- a/CppUnitLite/CMakeLists.txt
+++ b/CppUnitLite/CMakeLists.txt
@@ -6,12 +6,12 @@ file(GLOB cppunitlite_src "*.cpp")
 add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers})
 list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
-target_include_directories(CppUnitLite PUBLIC ${Boost_INCLUDE_DIR}) # boost/lexical_cast.h
+target_link_libraries(CppUnitLite PUBLIC Boost::boost) # boost/lexical_cast.h
 
 gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure
 
 option(GTSAM_INSTALL_CPPUNITLITE "Enable/Disable installation of CppUnitLite library" ON)
 if (GTSAM_INSTALL_CPPUNITLITE)
-    install(FILES ${cppunitlite_headers} DESTINATION include/CppUnitLite)
-    install(TARGETS CppUnitLite EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
+    install(FILES ${cppunitlite_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/CppUnitLite)
+    install(TARGETS CppUnitLite EXPORT GTSAM-exports ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR})
 endif(GTSAM_INSTALL_CPPUNITLITE)
diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md
index a6cfee984..953357ede 100644
--- a/GTSAM-Concepts.md
+++ b/GTSAM-Concepts.md
@@ -72,9 +72,9 @@ A Lie group is both a manifold *and* a group. Hence, a LIE_GROUP type should imp
 However, we now also need to be able to evaluate the derivatives of compose and inverse. 
 Hence, we have the following extra valid static functions defined in the struct `gtsam::traits<T>`:
 
-* `r = traits<T>::Compose(p,q,Hq,Hp)`
+* `r = traits<T>::Compose(p,q,Hp,Hq)`
 * `q = traits<T>::Inverse(p,Hp)`
-* `r = traits<T>::Between(p,q,Hq,H2p)`
+* `r = traits<T>::Between(p,q,Hp,Hq)`
 
 where above the *H* arguments stand for optional Jacobian arguments. 
 That makes it possible to create factors implementing priors (PriorFactor) or relations between two instances of a Lie group type (BetweenFactor).
diff --git a/README.md b/README.md
index 381b56ba3..093e35f0f 100644
--- a/README.md
+++ b/README.md
@@ -1,17 +1,20 @@
-[![Build Status](https://travis-ci.com/borglab/gtsam.svg?branch=develop)](https://travis-ci.com/borglab/gtsam/)
-
-# README - Georgia Tech Smoothing and Mapping library
+# README - Georgia Tech Smoothing and Mapping Library
 
 ## What is GTSAM?
 
-GTSAM is a library of C++ classes that implement smoothing and
-mapping (SAM) in robotics and vision, using factor graphs and Bayes
-networks as the underlying computing paradigm rather than sparse
+GTSAM is a C++ library that implements smoothing and
+mapping (SAM) in robotics and vision, using Factor Graphs and Bayes
+Networks as the underlying computing paradigm rather than sparse
 matrices.
 
-On top of the C++ library, GTSAM includes a MATLAB interface (enable
-GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
-is under development.
+| Platform  | Build Status  |
+|:---------:|:-------------:|
+| gcc/clang | [![Build Status](https://travis-ci.com/borglab/gtsam.svg?branch=develop)](https://travis-ci.com/borglab/gtsam/) |
+| MSVC      | [![Build status](https://ci.appveyor.com/api/projects/status/3enllitj52jsxwfg/branch/develop?svg=true)](https://ci.appveyor.com/project/dellaert/gtsam) |
+
+
+On top of the C++ library, GTSAM includes [wrappers for MATLAB & Python](##Wrappers).
+
 
 ## Quickstart
 
@@ -41,9 +44,9 @@ Optional prerequisites - used automatically if findable by CMake:
 
 ## GTSAM 4 Compatibility
 
-GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
+GTSAM 4 introduces several new features, most notably Expressions and a Python toolbox. We also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
 
-Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect.
+Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 is deprecated, so please be aware that this might render functions using their default constructor incorrect.
 
 ## Wrappers
 
@@ -62,7 +65,7 @@ Our implementation improves on this using integration on the manifold, as detail
 
 If you are using the factor in academic work, please cite the publications above.
 
-In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
+In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in [this document](doc/ImuFactor.pdf), is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
 
 
 ## Additional Information
@@ -79,4 +82,4 @@ GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`L
 
 Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
 
-GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).
+GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS.md).
diff --git a/appveyor.yml b/appveyor.yml
new file mode 100644
index 000000000..3747354cf
--- /dev/null
+++ b/appveyor.yml
@@ -0,0 +1,33 @@
+# version format
+version: 4.0.3-{branch}-build{build}
+
+os: Visual Studio 2019
+
+clone_folder: c:\projects\gtsam
+
+platform: x64
+configuration: Release
+
+environment:
+  CTEST_OUTPUT_ON_FAILURE: 1
+  BOOST_ROOT: C:/Libraries/boost_1_71_0
+
+build_script:
+  - cd c:\projects\gtsam\build
+  # As of Dec 2019, not all unit tests build cleanly for MSVC, so we'll just
+  # check that parts of GTSAM build correctly:
+  #- cmake --build .
+  - cmake --build . --config Release --target gtsam
+  - cmake --build . --config Release --target gtsam_unstable
+  - cmake --build . --config Release --target wrap
+  #- cmake --build . --target check
+  - cmake --build . --config Release --target check.base
+  - cmake --build . --config Release --target check.base_unstable
+  - cmake --build . --config Release --target check.linear
+
+before_build:
+  - cd c:\projects\gtsam
+  - mkdir build
+  - cd build
+  # Disable examples to avoid AppVeyor timeout
+  - cmake -G "Visual Studio 16 2019"  .. -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF
diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt
index f2ca9933e..d612e2fae 100644
--- a/cmake/CMakeLists.txt
+++ b/cmake/CMakeLists.txt
@@ -20,6 +20,7 @@ install(FILES
   GtsamPythonWrap.cmake
   GtsamCythonWrap.cmake
   GtsamTesting.cmake
+  GtsamPrinting.cmake
   FindCython.cmake
   FindNumPy.cmake
   README.html
diff --git a/cmake/FindBoost.cmake b/cmake/FindBoost.cmake
new file mode 100644
index 000000000..5f7cb5ddc
--- /dev/null
+++ b/cmake/FindBoost.cmake
@@ -0,0 +1,2347 @@
+# Distributed under the OSI-approved BSD 3-Clause License.  See accompanying
+# file Copyright.txt or https://cmake.org/licensing for details.
+
+#[=======================================================================[.rst:
+FindBoost
+---------
+
+Find Boost include dirs and libraries
+
+Use this module by invoking find_package with the form::
+
+  find_package(Boost
+    [version] [EXACT]      # Minimum or EXACT version e.g. 1.67.0
+    [REQUIRED]             # Fail with error if Boost is not found
+    [COMPONENTS <libs>...] # Boost libraries by their canonical name
+                           # e.g. "date_time" for "libboost_date_time"
+    [OPTIONAL_COMPONENTS <libs>...]
+                           # Optional Boost libraries by their canonical name)
+    )                      # e.g. "date_time" for "libboost_date_time"
+
+This module finds headers and requested component libraries OR a CMake
+package configuration file provided by a "Boost CMake" build.  For the
+latter case skip to the "Boost CMake" section below.  For the former
+case results are reported in variables::
+
+  Boost_FOUND            - True if headers and requested libraries were found
+  Boost_INCLUDE_DIRS     - Boost include directories
+  Boost_LIBRARY_DIRS     - Link directories for Boost libraries
+  Boost_LIBRARIES        - Boost component libraries to be linked
+  Boost_<C>_FOUND        - True if component <C> was found (<C> is upper-case)
+  Boost_<C>_LIBRARY      - Libraries to link for component <C> (may include
+                           target_link_libraries debug/optimized keywords)
+  Boost_VERSION_MACRO    - BOOST_VERSION value from boost/version.hpp
+  Boost_VERSION_STRING   - Boost version number in x.y.z format
+  Boost_VERSION          - if CMP0093 NEW => same as Boost_VERSION_STRING
+                           if CMP0093 OLD or unset => same as Boost_VERSION_MACRO
+  Boost_LIB_VERSION      - Version string appended to library filenames
+  Boost_VERSION_MAJOR    - Boost major version number (X in X.y.z)
+                           alias: Boost_MAJOR_VERSION
+  Boost_VERSION_MINOR    - Boost minor version number (Y in x.Y.z)
+                           alias: Boost_MINOR_VERSION
+  Boost_VERSION_PATCH    - Boost subminor version number (Z in x.y.Z)
+                           alias: Boost_SUBMINOR_VERSION
+  Boost_VERSION_COUNT    - Amount of version components (3)
+  Boost_LIB_DIAGNOSTIC_DEFINITIONS (Windows)
+                         - Pass to add_definitions() to have diagnostic
+                           information about Boost's automatic linking
+                           displayed during compilation
+
+Note that Boost Python components require a Python version suffix
+(Boost 1.67 and later), e.g. ``python36`` or ``python27`` for the
+versions built against Python 3.6 and 2.7, respectively.  This also
+applies to additional components using Python including
+``mpi_python`` and ``numpy``.  Earlier Boost releases may use
+distribution-specific suffixes such as ``2``, ``3`` or ``2.7``.
+These may also be used as suffixes, but note that they are not
+portable.
+
+This module reads hints about search locations from variables::
+
+  BOOST_ROOT             - Preferred installation prefix
+   (or BOOSTROOT)
+  BOOST_INCLUDEDIR       - Preferred include directory e.g. <prefix>/include
+  BOOST_LIBRARYDIR       - Preferred library directory e.g. <prefix>/lib
+  Boost_NO_SYSTEM_PATHS  - Set to ON to disable searching in locations not
+                           specified by these hint variables. Default is OFF.
+  Boost_ADDITIONAL_VERSIONS
+                         - List of Boost versions not known to this module
+                           (Boost install locations may contain the version)
+
+and saves search results persistently in CMake cache entries::
+
+  Boost_INCLUDE_DIR         - Directory containing Boost headers
+  Boost_LIBRARY_DIR_RELEASE - Directory containing release Boost libraries
+  Boost_LIBRARY_DIR_DEBUG   - Directory containing debug Boost libraries
+  Boost_<C>_LIBRARY_DEBUG   - Component <C> library debug variant
+  Boost_<C>_LIBRARY_RELEASE - Component <C> library release variant
+
+The following :prop_tgt:`IMPORTED` targets are also defined::
+
+  Boost::headers                - Target for header-only dependencies
+                                  (Boost include directory)
+                                  alias: Boost::boost
+  Boost::<C>                    - Target for specific component dependency
+                                  (shared or static library); <C> is lower-
+                                  case
+  Boost::diagnostic_definitions - interface target to enable diagnostic
+                                  information about Boost's automatic linking
+                                  during compilation (adds BOOST_LIB_DIAGNOSTIC)
+  Boost::disable_autolinking    - interface target to disable automatic
+                                  linking with MSVC (adds BOOST_ALL_NO_LIB)
+  Boost::dynamic_linking        - interface target to enable dynamic linking
+                                  linking with MSVC (adds BOOST_ALL_DYN_LINK)
+
+Implicit dependencies such as ``Boost::filesystem`` requiring
+``Boost::system`` will be automatically detected and satisfied, even
+if system is not specified when using :command:`find_package` and if
+``Boost::system`` is not added to :command:`target_link_libraries`.  If using
+``Boost::thread``, then ``Threads::Threads`` will also be added automatically.
+
+It is important to note that the imported targets behave differently
+than variables created by this module: multiple calls to
+:command:`find_package(Boost)` in the same directory or sub-directories with
+different options (e.g. static or shared) will not override the
+values of the targets created by the first call.
+
+Users may set these hints or results as ``CACHE`` entries.  Projects
+should not read these entries directly but instead use the above
+result variables.  Note that some hint names start in upper-case
+"BOOST".  One may specify these as environment variables if they are
+not specified as CMake variables or cache entries.
+
+This module first searches for the ``Boost`` header files using the above
+hint variables (excluding ``BOOST_LIBRARYDIR``) and saves the result in
+``Boost_INCLUDE_DIR``.  Then it searches for requested component libraries
+using the above hints (excluding ``BOOST_INCLUDEDIR`` and
+``Boost_ADDITIONAL_VERSIONS``), "lib" directories near ``Boost_INCLUDE_DIR``,
+and the library name configuration settings below.  It saves the
+library directories in ``Boost_LIBRARY_DIR_DEBUG`` and
+``Boost_LIBRARY_DIR_RELEASE`` and individual library
+locations in ``Boost_<C>_LIBRARY_DEBUG`` and ``Boost_<C>_LIBRARY_RELEASE``.
+When one changes settings used by previous searches in the same build
+tree (excluding environment variables) this module discards previous
+search results affected by the changes and searches again.
+
+Boost libraries come in many variants encoded in their file name.
+Users or projects may tell this module which variant to find by
+setting variables::
+
+  Boost_USE_DEBUG_LIBS     - Set to ON or OFF to specify whether to search
+                             and use the debug libraries.  Default is ON.
+  Boost_USE_RELEASE_LIBS   - Set to ON or OFF to specify whether to search
+                             and use the release libraries.  Default is ON.
+  Boost_USE_MULTITHREADED  - Set to OFF to use the non-multithreaded
+                             libraries ('mt' tag).  Default is ON.
+  Boost_USE_STATIC_LIBS    - Set to ON to force the use of the static
+                             libraries.  Default is OFF.
+  Boost_USE_STATIC_RUNTIME - Set to ON or OFF to specify whether to use
+                             libraries linked statically to the C++ runtime
+                             ('s' tag).  Default is platform dependent.
+  Boost_USE_DEBUG_RUNTIME  - Set to ON or OFF to specify whether to use
+                             libraries linked to the MS debug C++ runtime
+                             ('g' tag).  Default is ON.
+  Boost_USE_DEBUG_PYTHON   - Set to ON to use libraries compiled with a
+                             debug Python build ('y' tag). Default is OFF.
+  Boost_USE_STLPORT        - Set to ON to use libraries compiled with
+                             STLPort ('p' tag).  Default is OFF.
+  Boost_USE_STLPORT_DEPRECATED_NATIVE_IOSTREAMS
+                           - Set to ON to use libraries compiled with
+                             STLPort deprecated "native iostreams"
+                             ('n' tag).  Default is OFF.
+  Boost_COMPILER           - Set to the compiler-specific library suffix
+                             (e.g. "-gcc43").  Default is auto-computed
+                             for the C++ compiler in use.  A list may be
+                             used if multiple compatible suffixes should
+                             be tested for, in decreasing order of
+                             preference.
+  Boost_ARCHITECTURE       - Set to the architecture-specific library suffix
+                             (e.g. "-x64").  Default is auto-computed for the
+                             C++ compiler in use.
+  Boost_THREADAPI          - Suffix for "thread" component library name,
+                             such as "pthread" or "win32".  Names with
+                             and without this suffix will both be tried.
+  Boost_NAMESPACE          - Alternate namespace used to build boost with
+                             e.g. if set to "myboost", will search for
+                             myboost_thread instead of boost_thread.
+
+Other variables one may set to control this module are::
+
+  Boost_DEBUG              - Set to ON to enable debug output from FindBoost.
+                             Please enable this before filing any bug report.
+  Boost_REALPATH           - Set to ON to resolve symlinks for discovered
+                             libraries to assist with packaging.  For example,
+                             the "system" component library may be resolved to
+                             "/usr/lib/libboost_system.so.1.67.0" instead of
+                             "/usr/lib/libboost_system.so".  This does not
+                             affect linking and should not be enabled unless
+                             the user needs this information.
+  Boost_LIBRARY_DIR        - Default value for Boost_LIBRARY_DIR_RELEASE and
+                             Boost_LIBRARY_DIR_DEBUG.
+
+On Visual Studio and Borland compilers Boost headers request automatic
+linking to corresponding libraries.  This requires matching libraries
+to be linked explicitly or available in the link library search path.
+In this case setting ``Boost_USE_STATIC_LIBS`` to ``OFF`` may not achieve
+dynamic linking.  Boost automatic linking typically requests static
+libraries with a few exceptions (such as ``Boost.Python``).  Use::
+
+  add_definitions(${Boost_LIB_DIAGNOSTIC_DEFINITIONS})
+
+to ask Boost to report information about automatic linking requests.
+
+Example to find Boost headers only::
+
+  find_package(Boost 1.36.0)
+  if(Boost_FOUND)
+    include_directories(${Boost_INCLUDE_DIRS})
+    add_executable(foo foo.cc)
+  endif()
+
+Example to find Boost libraries and use imported targets::
+
+  find_package(Boost 1.56 REQUIRED COMPONENTS
+               date_time filesystem iostreams)
+  add_executable(foo foo.cc)
+  target_link_libraries(foo Boost::date_time Boost::filesystem
+                            Boost::iostreams)
+
+Example to find Boost Python 3.6 libraries and use imported targets::
+
+  find_package(Boost 1.67 REQUIRED COMPONENTS
+               python36 numpy36)
+  add_executable(foo foo.cc)
+  target_link_libraries(foo Boost::python36 Boost::numpy36)
+
+Example to find Boost headers and some *static* (release only) libraries::
+
+  set(Boost_USE_STATIC_LIBS        ON)  # only find static libs
+  set(Boost_USE_DEBUG_LIBS         OFF) # ignore debug libs and
+  set(Boost_USE_RELEASE_LIBS       ON)  # only find release libs
+  set(Boost_USE_MULTITHREADED      ON)
+  set(Boost_USE_STATIC_RUNTIME    OFF)
+  find_package(Boost 1.66.0 COMPONENTS date_time filesystem system ...)
+  if(Boost_FOUND)
+    include_directories(${Boost_INCLUDE_DIRS})
+    add_executable(foo foo.cc)
+    target_link_libraries(foo ${Boost_LIBRARIES})
+  endif()
+
+Boost CMake
+^^^^^^^^^^^
+
+If Boost was built using the boost-cmake project or from Boost 1.70.0 on
+it provides a package configuration file for use with find_package's config mode.
+This module looks for the package configuration file called
+``BoostConfig.cmake`` or ``boost-config.cmake`` and stores the result in
+``CACHE`` entry "Boost_DIR".  If found, the package configuration file is loaded
+and this module returns with no further action.  See documentation of
+the Boost CMake package configuration for details on what it provides.
+
+Set ``Boost_NO_BOOST_CMAKE`` to ``ON``, to disable the search for boost-cmake.
+#]=======================================================================]
+
+# The FPHSA helper provides standard way of reporting final search results to
+# the user including the version and component checks.
+
+# Patch  for GTSAM:
+#include(${CMAKE_CURRENT_LIST_DIR}/FindPackageHandleStandardArgs.cmake)
+include(FindPackageHandleStandardArgs)
+
+# Save project's policies
+cmake_policy(PUSH)
+cmake_policy(SET CMP0057 NEW) # if IN_LIST
+
+function(_boost_get_existing_target component target_var)
+  set(names "${component}")
+  if(component MATCHES "^([a-z_]*)(python|numpy)([1-9])\\.?([0-9])?$")
+    # handle pythonXY and numpyXY versioned components and also python X.Y, mpi_python etc.
+    list(APPEND names
+      "${CMAKE_MATCH_1}${CMAKE_MATCH_2}" # python
+      "${CMAKE_MATCH_1}${CMAKE_MATCH_2}${CMAKE_MATCH_3}" # pythonX
+      "${CMAKE_MATCH_1}${CMAKE_MATCH_2}${CMAKE_MATCH_3}${CMAKE_MATCH_4}" #pythonXY
+    )
+  endif()
+  # https://github.com/boost-cmake/boost-cmake uses boost::file_system etc.
+  # So handle similar constructions of target names
+  string(TOLOWER "${component}" lower_component)
+  list(APPEND names "${lower_component}")
+  foreach(prefix Boost boost)
+    foreach(name IN LISTS names)
+      if(TARGET "${prefix}::${name}")
+        # The target may be an INTERFACE library that wraps around a single other
+        # target for compatibility.  Unwrap this layer so we can extract real info.
+        if("${name}" MATCHES "^(python|numpy|mpi_python)([1-9])([0-9])$")
+          set(name_nv "${CMAKE_MATCH_1}")
+          if(TARGET "${prefix}::${name_nv}")
+            get_property(type TARGET "${prefix}::${name}" PROPERTY TYPE)
+            if(type STREQUAL "INTERFACE_LIBRARY")
+              get_property(lib TARGET "${prefix}::${name}" PROPERTY INTERFACE_LINK_LIBRARIES)
+              if("${lib}" STREQUAL "${prefix}::${name_nv}")
+                set(${target_var} "${prefix}::${name_nv}" PARENT_SCOPE)
+                return()
+              endif()
+            endif()
+          endif()
+        endif()
+        set(${target_var} "${prefix}::${name}" PARENT_SCOPE)
+        return()
+      endif()
+    endforeach()
+  endforeach()
+  set(${target_var} "" PARENT_SCOPE)
+endfunction()
+
+function(_boost_get_canonical_target_name component target_var)
+  string(TOLOWER "${component}" component)
+  if(component MATCHES "^([a-z_]*)(python|numpy)([1-9])\\.?([0-9])?$")
+    # handle pythonXY and numpyXY versioned components and also python X.Y, mpi_python etc.
+    set(${target_var} "Boost::${CMAKE_MATCH_1}${CMAKE_MATCH_2}" PARENT_SCOPE)
+  else()
+    set(${target_var} "Boost::${component}" PARENT_SCOPE)
+  endif()
+endfunction()
+
+macro(_boost_set_in_parent_scope name value)
+  # Set a variable in parent scope and make it visibile in current scope
+  set(${name} "${value}" PARENT_SCOPE)
+  set(${name} "${value}")
+endmacro()
+
+macro(_boost_set_if_unset name value)
+  if(NOT ${name})
+    _boost_set_in_parent_scope(${name} "${value}")
+  endif()
+endmacro()
+
+macro(_boost_set_cache_if_unset name value)
+  if(NOT ${name})
+    set(${name} "${value}" CACHE STRING "" FORCE)
+  endif()
+endmacro()
+
+macro(_boost_append_include_dir target)
+  get_target_property(inc "${target}" INTERFACE_INCLUDE_DIRECTORIES)
+  if(inc)
+    list(APPEND include_dirs "${inc}")
+  endif()
+endmacro()
+
+function(_boost_set_legacy_variables_from_config)
+  # Set legacy variables for compatibility if not set
+  set(include_dirs "")
+  set(library_dirs "")
+  set(libraries "")
+  # Header targets Boost::headers or Boost::boost
+  foreach(comp headers boost)
+    _boost_get_existing_target(${comp} target)
+    if(target)
+      _boost_append_include_dir("${target}")
+    endif()
+  endforeach()
+  # Library targets
+  foreach(comp IN LISTS Boost_FIND_COMPONENTS)
+    string(TOUPPER ${comp} uppercomp)
+    # Overwrite if set
+    _boost_set_in_parent_scope(Boost_${uppercomp}_FOUND "${Boost_${comp}_FOUND}")
+    if(Boost_${comp}_FOUND)
+      _boost_get_existing_target(${comp} target)
+      if(NOT target)
+        if(Boost_DEBUG OR Boost_VERBOSE)
+          message(WARNING "Could not find imported target for required component '${comp}'. Legacy variables for this component might be missing. Refer to the documentation of your Boost installation for help on variables to use.")
+        endif()
+        continue()
+      endif()
+      _boost_append_include_dir("${target}")
+      _boost_set_if_unset(Boost_${uppercomp}_LIBRARY "${target}")
+      _boost_set_if_unset(Boost_${uppercomp}_LIBRARIES "${target}") # Very old legacy variable
+      list(APPEND libraries "${target}")
+      get_property(type TARGET "${target}" PROPERTY TYPE)
+      if(NOT type STREQUAL "INTERFACE_LIBRARY")
+        foreach(cfg RELEASE DEBUG)
+          get_target_property(lib ${target} IMPORTED_LOCATION_${cfg})
+          if(lib)
+            get_filename_component(lib_dir "${lib}" DIRECTORY)
+            list(APPEND library_dirs ${lib_dir})
+            _boost_set_cache_if_unset(Boost_${uppercomp}_LIBRARY_${cfg} "${lib}")
+          endif()
+        endforeach()
+      elseif(Boost_DEBUG OR Boost_VERBOSE)
+        # For projects using only the Boost::* targets this warning can be safely ignored.
+        message(WARNING "Imported target '${target}' for required component '${comp}' has no artifact. Legacy variables for this component might be missing. Refer to the documentation of your Boost installation for help on variables to use.")
+      endif()
+      _boost_get_canonical_target_name("${comp}" canonical_target)
+      if(NOT TARGET "${canonical_target}")
+        add_library("${canonical_target}" INTERFACE IMPORTED)
+        target_link_libraries("${canonical_target}" INTERFACE "${target}")
+      endif()
+    endif()
+  endforeach()
+  list(REMOVE_DUPLICATES include_dirs)
+  list(REMOVE_DUPLICATES library_dirs)
+  _boost_set_if_unset(Boost_INCLUDE_DIRS "${include_dirs}")
+  _boost_set_if_unset(Boost_LIBRARY_DIRS "${library_dirs}")
+  _boost_set_if_unset(Boost_LIBRARIES "${libraries}")
+  _boost_set_if_unset(Boost_VERSION_STRING "${Boost_VERSION_MAJOR}.${Boost_VERSION_MINOR}.${Boost_VERSION_PATCH}")
+  find_path(Boost_INCLUDE_DIR
+    NAMES boost/version.hpp boost/config.hpp
+    HINTS ${Boost_INCLUDE_DIRS}
+    NO_DEFAULT_PATH
+  )
+  if(NOT Boost_VERSION_MACRO OR NOT Boost_LIB_VERSION)
+    set(version_file ${Boost_INCLUDE_DIR}/boost/version.hpp)
+    if(EXISTS "${version_file}")
+      file(STRINGS "${version_file}" contents REGEX "#define BOOST_(LIB_)?VERSION ")
+      if(contents MATCHES "#define BOOST_VERSION ([0-9]+)")
+        _boost_set_if_unset(Boost_VERSION_MACRO "${CMAKE_MATCH_1}")
+      endif()
+      if(contents MATCHES "#define BOOST_LIB_VERSION \"([0-9_]+)\"")
+        _boost_set_if_unset(Boost_LIB_VERSION "${CMAKE_MATCH_1}")
+      endif()
+    endif()
+  endif()
+  _boost_set_if_unset(Boost_MAJOR_VERSION ${Boost_VERSION_MAJOR})
+  _boost_set_if_unset(Boost_MINOR_VERSION ${Boost_VERSION_MINOR})
+  _boost_set_if_unset(Boost_SUBMINOR_VERSION ${Boost_VERSION_PATCH})
+  if(WIN32)
+    _boost_set_if_unset(Boost_LIB_DIAGNOSTIC_DEFINITIONS "-DBOOST_LIB_DIAGNOSTIC")
+  endif()
+  if(NOT TARGET Boost::headers)
+    add_library(Boost::headers INTERFACE IMPORTED)
+    target_include_directories(Boost::headers INTERFACE ${Boost_INCLUDE_DIRS})
+  endif()
+  # Legacy targets w/o functionality as all handled by defined targets
+  foreach(lib diagnostic_definitions disable_autolinking dynamic_linking)
+    if(NOT TARGET Boost::${lib})
+      add_library(Boost::${lib} INTERFACE IMPORTED)
+    endif()
+  endforeach()
+  if(NOT TARGET Boost::boost)
+    add_library(Boost::boost INTERFACE IMPORTED)
+    target_link_libraries(Boost::boost INTERFACE Boost::headers)
+  endif()
+endfunction()
+
+#-------------------------------------------------------------------------------
+# Before we go searching, check whether a boost cmake package is available, unless
+# the user specifically asked NOT to search for one.
+#
+# If Boost_DIR is set, this behaves as any find_package call would. If not,
+# it looks at BOOST_ROOT and BOOSTROOT to find Boost.
+#
+if (NOT Boost_NO_BOOST_CMAKE)
+  # If Boost_DIR is not set, look for BOOSTROOT and BOOST_ROOT as alternatives,
+  # since these are more conventional for Boost.
+  if ("$ENV{Boost_DIR}" STREQUAL "")
+    if (NOT "$ENV{BOOST_ROOT}" STREQUAL "")
+      set(ENV{Boost_DIR} $ENV{BOOST_ROOT})
+    elseif (NOT "$ENV{BOOSTROOT}" STREQUAL "")
+      set(ENV{Boost_DIR} $ENV{BOOSTROOT})
+    endif()
+  endif()
+
+  # Do the same find_package call but look specifically for the CMake version.
+  # Note that args are passed in the Boost_FIND_xxxxx variables, so there is no
+  # need to delegate them to this find_package call.
+  find_package(Boost QUIET NO_MODULE)
+  mark_as_advanced(Boost_DIR)
+
+  # If we found a boost cmake package, then we're done. Print out what we found.
+  # Otherwise let the rest of the module try to find it.
+  if(Boost_FOUND)
+    # Convert component found variables to standard variables if required
+    # Necessary for legacy boost-cmake and 1.70 builtin BoostConfig
+    if(Boost_FIND_COMPONENTS)
+      foreach(_comp IN LISTS Boost_FIND_COMPONENTS)
+        if(DEFINED Boost_${_comp}_FOUND)
+          continue()
+        endif()
+        string(TOUPPER ${_comp} _uppercomp)
+        if(DEFINED Boost${_comp}_FOUND) # legacy boost-cmake project
+          set(Boost_${_comp}_FOUND ${Boost${_comp}_FOUND})
+        elseif(DEFINED Boost_${_uppercomp}_FOUND) # Boost 1.70
+          set(Boost_${_comp}_FOUND ${Boost_${_uppercomp}_FOUND})
+        endif()
+      endforeach()
+    endif()
+
+    find_package_handle_standard_args(Boost HANDLE_COMPONENTS CONFIG_MODE)
+    _boost_set_legacy_variables_from_config()
+
+    # Restore project's policies
+    cmake_policy(POP)
+    return()
+  endif()
+endif()
+
+
+#-------------------------------------------------------------------------------
+#  FindBoost functions & macros
+#
+
+#
+# Print debug text if Boost_DEBUG is set.
+# Call example:
+# _Boost_DEBUG_PRINT("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "debug message")
+#
+function(_Boost_DEBUG_PRINT file line text)
+  if(Boost_DEBUG)
+    message(STATUS "[ ${file}:${line} ] ${text}")
+  endif()
+endfunction()
+
+#
+# _Boost_DEBUG_PRINT_VAR(file line variable_name [ENVIRONMENT]
+#                        [SOURCE "short explanation of origin of var value"])
+#
+#   ENVIRONMENT - look up environment variable instead of CMake variable
+#
+# Print variable name and its value if Boost_DEBUG is set.
+# Call example:
+# _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" BOOST_ROOT)
+#
+function(_Boost_DEBUG_PRINT_VAR file line name)
+  if(Boost_DEBUG)
+    cmake_parse_arguments(_args "ENVIRONMENT" "SOURCE" "" ${ARGN})
+
+    unset(source)
+    if(_args_SOURCE)
+      set(source " (${_args_SOURCE})")
+    endif()
+
+    if(_args_ENVIRONMENT)
+      if(DEFINED ENV{${name}})
+        set(value "\"$ENV{${name}}\"")
+      else()
+        set(value "<unset>")
+      endif()
+      set(_name "ENV{${name}}")
+    else()
+      if(DEFINED "${name}")
+        set(value "\"${${name}}\"")
+      else()
+        set(value "<unset>")
+      endif()
+      set(_name "${name}")
+    endif()
+
+    _Boost_DEBUG_PRINT("${file}" "${line}" "${_name} = ${value}${source}")
+  endif()
+endfunction()
+
+############################################
+#
+# Check the existence of the libraries.
+#
+############################################
+# This macro was taken directly from the FindQt4.cmake file that is included
+# with the CMake distribution. This is NOT my work. All work was done by the
+# original authors of the FindQt4.cmake file. Only minor modifications were
+# made to remove references to Qt and make this file more generally applicable
+# And ELSE/ENDIF pairs were removed for readability.
+#########################################################################
+
+macro(_Boost_ADJUST_LIB_VARS basename)
+  if(Boost_INCLUDE_DIR )
+    if(Boost_${basename}_LIBRARY_DEBUG AND Boost_${basename}_LIBRARY_RELEASE)
+      # if the generator is multi-config or if CMAKE_BUILD_TYPE is set for
+      # single-config generators, set optimized and debug libraries
+      get_property(_isMultiConfig GLOBAL PROPERTY GENERATOR_IS_MULTI_CONFIG)
+      if(_isMultiConfig OR CMAKE_BUILD_TYPE)
+        set(Boost_${basename}_LIBRARY optimized ${Boost_${basename}_LIBRARY_RELEASE} debug ${Boost_${basename}_LIBRARY_DEBUG})
+      else()
+        # For single-config generators where CMAKE_BUILD_TYPE has no value,
+        # just use the release libraries
+        set(Boost_${basename}_LIBRARY ${Boost_${basename}_LIBRARY_RELEASE} )
+      endif()
+      # FIXME: This probably should be set for both cases
+      set(Boost_${basename}_LIBRARIES optimized ${Boost_${basename}_LIBRARY_RELEASE} debug ${Boost_${basename}_LIBRARY_DEBUG})
+    endif()
+
+    # if only the release version was found, set the debug variable also to the release version
+    if(Boost_${basename}_LIBRARY_RELEASE AND NOT Boost_${basename}_LIBRARY_DEBUG)
+      set(Boost_${basename}_LIBRARY_DEBUG ${Boost_${basename}_LIBRARY_RELEASE})
+      set(Boost_${basename}_LIBRARY       ${Boost_${basename}_LIBRARY_RELEASE})
+      set(Boost_${basename}_LIBRARIES     ${Boost_${basename}_LIBRARY_RELEASE})
+    endif()
+
+    # if only the debug version was found, set the release variable also to the debug version
+    if(Boost_${basename}_LIBRARY_DEBUG AND NOT Boost_${basename}_LIBRARY_RELEASE)
+      set(Boost_${basename}_LIBRARY_RELEASE ${Boost_${basename}_LIBRARY_DEBUG})
+      set(Boost_${basename}_LIBRARY         ${Boost_${basename}_LIBRARY_DEBUG})
+      set(Boost_${basename}_LIBRARIES       ${Boost_${basename}_LIBRARY_DEBUG})
+    endif()
+
+    # If the debug & release library ends up being the same, omit the keywords
+    if("${Boost_${basename}_LIBRARY_RELEASE}" STREQUAL "${Boost_${basename}_LIBRARY_DEBUG}")
+      set(Boost_${basename}_LIBRARY   ${Boost_${basename}_LIBRARY_RELEASE} )
+      set(Boost_${basename}_LIBRARIES ${Boost_${basename}_LIBRARY_RELEASE} )
+    endif()
+
+    if(Boost_${basename}_LIBRARY AND Boost_${basename}_HEADER)
+      set(Boost_${basename}_FOUND ON)
+      if("x${basename}" STREQUAL "xTHREAD" AND NOT TARGET Threads::Threads)
+        string(APPEND Boost_ERROR_REASON_THREAD " (missing dependency: Threads)")
+        set(Boost_THREAD_FOUND OFF)
+      endif()
+    endif()
+
+  endif()
+  # Make variables changeable to the advanced user
+  mark_as_advanced(
+      Boost_${basename}_LIBRARY_RELEASE
+      Boost_${basename}_LIBRARY_DEBUG
+  )
+endmacro()
+
+# Detect changes in used variables.
+# Compares the current variable value with the last one.
+# In short form:
+# v != v_LAST                      -> CHANGED = 1
+# v is defined, v_LAST not         -> CHANGED = 1
+# v is not defined, but v_LAST is  -> CHANGED = 1
+# otherwise                        -> CHANGED = 0
+# CHANGED is returned in variable named ${changed_var}
+macro(_Boost_CHANGE_DETECT changed_var)
+  set(${changed_var} 0)
+  foreach(v ${ARGN})
+    if(DEFINED _Boost_COMPONENTS_SEARCHED)
+      if(${v})
+        if(_${v}_LAST)
+          string(COMPARE NOTEQUAL "${${v}}" "${_${v}_LAST}" _${v}_CHANGED)
+        else()
+          set(_${v}_CHANGED 1)
+        endif()
+      elseif(_${v}_LAST)
+        set(_${v}_CHANGED 1)
+      endif()
+      if(_${v}_CHANGED)
+        set(${changed_var} 1)
+      endif()
+    else()
+      set(_${v}_CHANGED 0)
+    endif()
+  endforeach()
+endmacro()
+
+#
+# Find the given library (var).
+# Use 'build_type' to support different lib paths for RELEASE or DEBUG builds
+#
+macro(_Boost_FIND_LIBRARY var build_type)
+
+  find_library(${var} ${ARGN})
+
+  if(${var})
+    # If this is the first library found then save Boost_LIBRARY_DIR_[RELEASE,DEBUG].
+    if(NOT Boost_LIBRARY_DIR_${build_type})
+      get_filename_component(_dir "${${var}}" PATH)
+      set(Boost_LIBRARY_DIR_${build_type} "${_dir}" CACHE PATH "Boost library directory ${build_type}" FORCE)
+    endif()
+  elseif(_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT)
+    # Try component-specific hints but do not save Boost_LIBRARY_DIR_[RELEASE,DEBUG].
+    find_library(${var} HINTS ${_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT} ${ARGN})
+  endif()
+
+  # If Boost_LIBRARY_DIR_[RELEASE,DEBUG] is known then search only there.
+  if(Boost_LIBRARY_DIR_${build_type})
+    set(_boost_LIBRARY_SEARCH_DIRS_${build_type} ${Boost_LIBRARY_DIR_${build_type}} NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+    _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}"
+                           "Boost_LIBRARY_DIR_${build_type}")
+    _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}"
+                           "_boost_LIBRARY_SEARCH_DIRS_${build_type}")
+  endif()
+endmacro()
+
+#-------------------------------------------------------------------------------
+
+# Convert CMAKE_CXX_COMPILER_VERSION to boost compiler suffix version.
+function(_Boost_COMPILER_DUMPVERSION _OUTPUT_VERSION _OUTPUT_VERSION_MAJOR _OUTPUT_VERSION_MINOR)
+  string(REGEX REPLACE "([0-9]+)\\.([0-9]+)(\\.[0-9]+)?" "\\1"
+    _boost_COMPILER_VERSION_MAJOR "${CMAKE_CXX_COMPILER_VERSION}")
+  string(REGEX REPLACE "([0-9]+)\\.([0-9]+)(\\.[0-9]+)?" "\\2"
+    _boost_COMPILER_VERSION_MINOR "${CMAKE_CXX_COMPILER_VERSION}")
+
+  set(_boost_COMPILER_VERSION "${_boost_COMPILER_VERSION_MAJOR}${_boost_COMPILER_VERSION_MINOR}")
+
+  set(${_OUTPUT_VERSION} ${_boost_COMPILER_VERSION} PARENT_SCOPE)
+  set(${_OUTPUT_VERSION_MAJOR} ${_boost_COMPILER_VERSION_MAJOR} PARENT_SCOPE)
+  set(${_OUTPUT_VERSION_MINOR} ${_boost_COMPILER_VERSION_MINOR} PARENT_SCOPE)
+endfunction()
+
+#
+# Take a list of libraries with "thread" in it
+# and prepend duplicates with "thread_${Boost_THREADAPI}"
+# at the front of the list
+#
+function(_Boost_PREPEND_LIST_WITH_THREADAPI _output)
+  set(_orig_libnames ${ARGN})
+  string(REPLACE "thread" "thread_${Boost_THREADAPI}" _threadapi_libnames "${_orig_libnames}")
+  set(${_output} ${_threadapi_libnames} ${_orig_libnames} PARENT_SCOPE)
+endfunction()
+
+#
+# If a library is found, replace its cache entry with its REALPATH
+#
+function(_Boost_SWAP_WITH_REALPATH _library _docstring)
+  if(${_library})
+    get_filename_component(_boost_filepathreal ${${_library}} REALPATH)
+    unset(${_library} CACHE)
+    set(${_library} ${_boost_filepathreal} CACHE FILEPATH "${_docstring}")
+  endif()
+endfunction()
+
+function(_Boost_CHECK_SPELLING _var)
+  if(${_var})
+    string(TOUPPER ${_var} _var_UC)
+    message(FATAL_ERROR "ERROR: ${_var} is not the correct spelling.  The proper spelling is ${_var_UC}.")
+  endif()
+endfunction()
+
+# Guesses Boost's compiler prefix used in built library names
+# Returns the guess by setting the variable pointed to by _ret
+function(_Boost_GUESS_COMPILER_PREFIX _ret)
+  if("x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xIntel")
+    if(WIN32)
+      set (_boost_COMPILER "-iw")
+    else()
+      set (_boost_COMPILER "-il")
+    endif()
+  elseif (GHSMULTI)
+    set(_boost_COMPILER "-ghs")
+  elseif("x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xMSVC" OR "x${CMAKE_CXX_SIMULATE_ID}" STREQUAL "xMSVC")
+    if(MSVC_TOOLSET_VERSION GREATER_EQUAL 150)
+      # Not yet known.
+      set(_boost_COMPILER "")
+    elseif(MSVC_TOOLSET_VERSION GREATER_EQUAL 140)
+      # MSVC toolset 14.x versions are forward compatible.
+      set(_boost_COMPILER "")
+      foreach(v 9 8 7 6 5 4 3 2 1 0)
+        if(MSVC_TOOLSET_VERSION GREATER_EQUAL 14${v})
+          list(APPEND _boost_COMPILER "-vc14${v}")
+        endif()
+      endforeach()
+    elseif(MSVC_TOOLSET_VERSION GREATER_EQUAL 80)
+      set(_boost_COMPILER "-vc${MSVC_TOOLSET_VERSION}")
+    elseif(NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 13.10)
+      set(_boost_COMPILER "-vc71")
+    elseif(NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 13) # Good luck!
+      set(_boost_COMPILER "-vc7") # yes, this is correct
+    else() # VS 6.0 Good luck!
+      set(_boost_COMPILER "-vc6") # yes, this is correct
+    endif()
+
+    if("x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xClang")
+      string(REPLACE "." ";" VERSION_LIST "${CMAKE_CXX_COMPILER_VERSION}")
+      list(GET VERSION_LIST 0 CLANG_VERSION_MAJOR)
+      set(_boost_COMPILER "-clangw${CLANG_VERSION_MAJOR};${_boost_COMPILER}")
+    endif()
+  elseif (BORLAND)
+    set(_boost_COMPILER "-bcb")
+  elseif(CMAKE_CXX_COMPILER_ID STREQUAL "SunPro")
+    set(_boost_COMPILER "-sw")
+  elseif(CMAKE_CXX_COMPILER_ID STREQUAL "XL")
+    set(_boost_COMPILER "-xlc")
+  elseif (MINGW)
+    if(Boost_VERSION_STRING VERSION_LESS 1.34)
+        set(_boost_COMPILER "-mgw") # no GCC version encoding prior to 1.34
+    else()
+      _Boost_COMPILER_DUMPVERSION(_boost_COMPILER_VERSION _boost_COMPILER_VERSION_MAJOR _boost_COMPILER_VERSION_MINOR)
+      set(_boost_COMPILER "-mgw${_boost_COMPILER_VERSION}")
+    endif()
+  elseif (UNIX)
+    _Boost_COMPILER_DUMPVERSION(_boost_COMPILER_VERSION _boost_COMPILER_VERSION_MAJOR _boost_COMPILER_VERSION_MINOR)
+    if(NOT Boost_VERSION_STRING VERSION_LESS 1.69.0)
+      # From GCC 5 and clang 4, versioning changes and minor becomes patch.
+      # For those compilers, patch is exclude from compiler tag in Boost 1.69+ library naming.
+      if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND _boost_COMPILER_VERSION_MAJOR VERSION_GREATER 4)
+        set(_boost_COMPILER_VERSION "${_boost_COMPILER_VERSION_MAJOR}")
+      elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND _boost_COMPILER_VERSION_MAJOR VERSION_GREATER 3)
+        set(_boost_COMPILER_VERSION "${_boost_COMPILER_VERSION_MAJOR}")
+      endif()
+    endif()
+
+    if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+      if(Boost_VERSION_STRING VERSION_LESS 1.34)
+        set(_boost_COMPILER "-gcc") # no GCC version encoding prior to 1.34
+      else()
+        # Determine which version of GCC we have.
+        if(APPLE)
+          if(Boost_VERSION_STRING VERSION_LESS 1.36.0)
+            # In Boost <= 1.35.0, there is no mangled compiler name for
+            # the macOS/Darwin version of GCC.
+            set(_boost_COMPILER "")
+          else()
+            # In Boost 1.36.0 and newer, the mangled compiler name used
+            # on macOS/Darwin is "xgcc".
+            set(_boost_COMPILER "-xgcc${_boost_COMPILER_VERSION}")
+          endif()
+        else()
+          set(_boost_COMPILER "-gcc${_boost_COMPILER_VERSION}")
+        endif()
+      endif()
+    elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+      # TODO: Find out any Boost version constraints vs clang support.
+      set(_boost_COMPILER "-clang${_boost_COMPILER_VERSION}")
+    endif()
+  else()
+    set(_boost_COMPILER "")
+  endif()
+  _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}"
+                         "_boost_COMPILER" SOURCE "guessed")
+  set(${_ret} ${_boost_COMPILER} PARENT_SCOPE)
+endfunction()
+
+#
+# Get component dependencies.  Requires the dependencies to have been
+# defined for the Boost release version.
+#
+# component - the component to check
+# _ret - list of library dependencies
+#
+function(_Boost_COMPONENT_DEPENDENCIES component _ret)
+  # Note: to add a new Boost release, run
+  #
+  #   % cmake -DBOOST_DIR=/path/to/boost/source -P Utilities/Scripts/BoostScanDeps.cmake
+  #
+  # The output may be added in a new block below.  If it's the same as
+  # the previous release, simply update the version range of the block
+  # for the previous release.  Also check if any new components have
+  # been added, and add any new components to
+  # _Boost_COMPONENT_HEADERS.
+  #
+  # This information was originally generated by running
+  # BoostScanDeps.cmake against every boost release to date supported
+  # by FindBoost:
+  #
+  #   % for version in /path/to/boost/sources/*
+  #     do
+  #       cmake -DBOOST_DIR=$version -P Utilities/Scripts/BoostScanDeps.cmake
+  #     done
+  #
+  # The output was then updated by search and replace with these regexes:
+  #
+  # - Strip message(STATUS) prefix dashes
+  #   s;^-- ;;
+  # - Indent
+  #   s;^set(;    set(;;
+  # - Add conditionals
+  #   s;Scanning /path/to/boost/sources/boost_\(.*\)_\(.*\)_\(.*);  elseif(NOT Boost_VERSION_STRING VERSION_LESS \1\.\2\.\3 AND Boost_VERSION_STRING VERSION_LESS xxxx);
+  #
+  # This results in the logic seen below, but will require the xxxx
+  # replacing with the following Boost release version (or the next
+  # minor version to be released, e.g. 1.59 was the latest at the time
+  # of writing, making 1.60 the next. Identical consecutive releases
+  # were then merged together by updating the end range of the first
+  # block and removing the following redundant blocks.
+  #
+  # Running the script against all historical releases should be
+  # required only if the BoostScanDeps.cmake script logic is changed.
+  # The addition of a new release should only require it to be run
+  # against the new release.
+
+  # Handle Python version suffixes
+  if(component MATCHES "^(python|mpi_python|numpy)([0-9][0-9]?|[0-9]\\.[0-9])\$")
+    set(component "${CMAKE_MATCH_1}")
+    set(component_python_version "${CMAKE_MATCH_2}")
+  endif()
+
+  set(_Boost_IMPORTED_TARGETS TRUE)
+  if(Boost_VERSION_STRING AND Boost_VERSION_STRING VERSION_LESS 1.33.0)
+    message(WARNING "Imported targets and dependency information not available for Boost version ${Boost_VERSION_STRING} (all versions older than 1.33)")
+    set(_Boost_IMPORTED_TARGETS FALSE)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.33.0 AND Boost_VERSION_STRING VERSION_LESS 1.35.0)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex thread)
+    set(_Boost_REGEX_DEPENDENCIES thread)
+    set(_Boost_WAVE_DEPENDENCIES filesystem thread)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.35.0 AND Boost_VERSION_STRING VERSION_LESS 1.36.0)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system thread)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.36.0 AND Boost_VERSION_STRING VERSION_LESS 1.38.0)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system thread)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.38.0 AND Boost_VERSION_STRING VERSION_LESS 1.43.0)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_THREAD_DEPENDENCIES date_time)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system thread date_time)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.43.0 AND Boost_VERSION_STRING VERSION_LESS 1.44.0)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l random)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_THREAD_DEPENDENCIES date_time)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system thread date_time)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.44.0 AND Boost_VERSION_STRING VERSION_LESS 1.45.0)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l random serialization)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_THREAD_DEPENDENCIES date_time)
+    set(_Boost_WAVE_DEPENDENCIES serialization filesystem system thread date_time)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.45.0 AND Boost_VERSION_STRING VERSION_LESS 1.47.0)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l random)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_THREAD_DEPENDENCIES date_time)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread date_time)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.47.0 AND Boost_VERSION_STRING VERSION_LESS 1.48.0)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l random)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_THREAD_DEPENDENCIES date_time)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread date_time)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.48.0 AND Boost_VERSION_STRING VERSION_LESS 1.50.0)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l random)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_THREAD_DEPENDENCIES date_time)
+    set(_Boost_TIMER_DEPENDENCIES chrono system)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread date_time)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.50.0 AND Boost_VERSION_STRING VERSION_LESS 1.53.0)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l regex random)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_THREAD_DEPENDENCIES chrono system date_time)
+    set(_Boost_TIMER_DEPENDENCIES chrono system)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.53.0 AND Boost_VERSION_STRING VERSION_LESS 1.54.0)
+    set(_Boost_ATOMIC_DEPENDENCIES thread chrono system date_time)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l regex random)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic)
+    set(_Boost_TIMER_DEPENDENCIES chrono system)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.54.0 AND Boost_VERSION_STRING VERSION_LESS 1.55.0)
+    set(_Boost_ATOMIC_DEPENDENCIES thread chrono system date_time)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_LOG_DEPENDENCIES log_setup date_time system filesystem thread regex chrono)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l regex random)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic)
+    set(_Boost_TIMER_DEPENDENCIES chrono system)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.55.0 AND Boost_VERSION_STRING VERSION_LESS 1.56.0)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_COROUTINE_DEPENDENCIES context system)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_LOG_DEPENDENCIES log_setup date_time system filesystem thread regex chrono)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l regex random)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic)
+    set(_Boost_TIMER_DEPENDENCIES chrono system)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.56.0 AND Boost_VERSION_STRING VERSION_LESS 1.59.0)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_COROUTINE_DEPENDENCIES context system)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_LOG_DEPENDENCIES log_setup date_time system filesystem thread regex chrono)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_RANDOM_DEPENDENCIES system)
+    set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic)
+    set(_Boost_TIMER_DEPENDENCIES chrono system)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.59.0 AND Boost_VERSION_STRING VERSION_LESS 1.60.0)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_COROUTINE_DEPENDENCIES context system)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_LOG_DEPENDENCIES log_setup date_time system filesystem thread regex chrono atomic)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_RANDOM_DEPENDENCIES system)
+    set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic)
+    set(_Boost_TIMER_DEPENDENCIES chrono system)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.60.0 AND Boost_VERSION_STRING VERSION_LESS 1.61.0)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_COROUTINE_DEPENDENCIES context system)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_LOG_DEPENDENCIES date_time log_setup system filesystem thread regex chrono atomic)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_RANDOM_DEPENDENCIES system)
+    set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic)
+    set(_Boost_TIMER_DEPENDENCIES chrono system)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.61.0 AND Boost_VERSION_STRING VERSION_LESS 1.62.0)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_CONTEXT_DEPENDENCIES thread chrono system date_time)
+    set(_Boost_COROUTINE_DEPENDENCIES context system)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_LOG_DEPENDENCIES date_time log_setup system filesystem thread regex chrono atomic)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_RANDOM_DEPENDENCIES system)
+    set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.62.0 AND Boost_VERSION_STRING VERSION_LESS 1.63.0)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_CONTEXT_DEPENDENCIES thread chrono system date_time)
+    set(_Boost_COROUTINE_DEPENDENCIES context system)
+    set(_Boost_FIBER_DEPENDENCIES context thread chrono system date_time)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_LOG_DEPENDENCIES date_time log_setup system filesystem thread regex chrono atomic)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_RANDOM_DEPENDENCIES system)
+    set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.63.0 AND Boost_VERSION_STRING VERSION_LESS 1.65.0)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_CONTEXT_DEPENDENCIES thread chrono system date_time)
+    set(_Boost_COROUTINE_DEPENDENCIES context system)
+    set(_Boost_COROUTINE2_DEPENDENCIES context fiber thread chrono system date_time)
+    set(_Boost_FIBER_DEPENDENCIES context thread chrono system date_time)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_LOG_DEPENDENCIES date_time log_setup system filesystem thread regex chrono atomic)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_RANDOM_DEPENDENCIES system)
+    set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.65.0 AND Boost_VERSION_STRING VERSION_LESS 1.67.0)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_CONTEXT_DEPENDENCIES thread chrono system date_time)
+    set(_Boost_COROUTINE_DEPENDENCIES context system)
+    set(_Boost_FIBER_DEPENDENCIES context thread chrono system date_time)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_LOG_DEPENDENCIES date_time log_setup system filesystem thread regex chrono atomic)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_NUMPY_DEPENDENCIES python${component_python_version})
+    set(_Boost_RANDOM_DEPENDENCIES system)
+    set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic)
+    set(_Boost_TIMER_DEPENDENCIES chrono system)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.67.0 AND Boost_VERSION_STRING VERSION_LESS 1.68.0)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_CONTEXT_DEPENDENCIES thread chrono system date_time)
+    set(_Boost_COROUTINE_DEPENDENCIES context system)
+    set(_Boost_FIBER_DEPENDENCIES context thread chrono system date_time)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_LOG_DEPENDENCIES date_time log_setup system filesystem thread regex chrono atomic)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_NUMPY_DEPENDENCIES python${component_python_version})
+    set(_Boost_RANDOM_DEPENDENCIES system)
+    set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic)
+    set(_Boost_TIMER_DEPENDENCIES chrono system)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.68.0 AND Boost_VERSION_STRING VERSION_LESS 1.69.0)
+    set(_Boost_CHRONO_DEPENDENCIES system)
+    set(_Boost_CONTEXT_DEPENDENCIES thread chrono system date_time)
+    set(_Boost_CONTRACT_DEPENDENCIES thread chrono system date_time)
+    set(_Boost_COROUTINE_DEPENDENCIES context system)
+    set(_Boost_FIBER_DEPENDENCIES context thread chrono system date_time)
+    set(_Boost_FILESYSTEM_DEPENDENCIES system)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_LOG_DEPENDENCIES date_time log_setup system filesystem thread regex chrono atomic)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_NUMPY_DEPENDENCIES python${component_python_version})
+    set(_Boost_RANDOM_DEPENDENCIES system)
+    set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic)
+    set(_Boost_TIMER_DEPENDENCIES chrono system)
+    set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.69.0 AND Boost_VERSION_STRING VERSION_LESS 1.70.0)
+    set(_Boost_CONTRACT_DEPENDENCIES thread chrono date_time)
+    set(_Boost_COROUTINE_DEPENDENCIES context)
+    set(_Boost_FIBER_DEPENDENCIES context)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_LOG_DEPENDENCIES date_time log_setup filesystem thread regex chrono atomic)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_NUMPY_DEPENDENCIES python${component_python_version})
+    set(_Boost_THREAD_DEPENDENCIES chrono date_time atomic)
+    set(_Boost_TIMER_DEPENDENCIES chrono system)
+    set(_Boost_WAVE_DEPENDENCIES filesystem serialization thread chrono date_time atomic)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+  elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.70.0)
+    set(_Boost_CONTRACT_DEPENDENCIES thread chrono date_time)
+    set(_Boost_COROUTINE_DEPENDENCIES context)
+    set(_Boost_FIBER_DEPENDENCIES context)
+    set(_Boost_IOSTREAMS_DEPENDENCIES regex)
+    set(_Boost_LOG_DEPENDENCIES date_time log_setup filesystem thread regex chrono atomic)
+    set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic)
+    set(_Boost_MPI_DEPENDENCIES serialization)
+    set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization)
+    set(_Boost_NUMPY_DEPENDENCIES python${component_python_version})
+    set(_Boost_THREAD_DEPENDENCIES chrono date_time atomic)
+    set(_Boost_TIMER_DEPENDENCIES chrono)
+    set(_Boost_WAVE_DEPENDENCIES filesystem serialization thread chrono date_time atomic)
+    set(_Boost_WSERIALIZATION_DEPENDENCIES serialization)
+    if(NOT Boost_VERSION_STRING VERSION_LESS 1.72.0)
+      message(WARNING "New Boost version may have incorrect or missing dependencies and imported targets")
+    endif()
+  endif()
+
+  string(TOUPPER ${component} uppercomponent)
+  set(${_ret} ${_Boost_${uppercomponent}_DEPENDENCIES} PARENT_SCOPE)
+  set(_Boost_IMPORTED_TARGETS ${_Boost_IMPORTED_TARGETS} PARENT_SCOPE)
+
+  string(REGEX REPLACE ";" " " _boost_DEPS_STRING "${_Boost_${uppercomponent}_DEPENDENCIES}")
+  if (NOT _boost_DEPS_STRING)
+    set(_boost_DEPS_STRING "(none)")
+  endif()
+  # message(STATUS "Dependencies for Boost::${component}: ${_boost_DEPS_STRING}")
+endfunction()
+
+#
+# Get component headers.  This is the primary header (or headers) for
+# a given component, and is used to check that the headers are present
+# as well as the library itself as an extra sanity check of the build
+# environment.
+#
+# component - the component to check
+# _hdrs
+#
+function(_Boost_COMPONENT_HEADERS component _hdrs)
+  # Handle Python version suffixes
+  if(component MATCHES "^(python|mpi_python|numpy)([0-9][0-9]?|[0-9]\\.[0-9])\$")
+    set(component "${CMAKE_MATCH_1}")
+    set(component_python_version "${CMAKE_MATCH_2}")
+  endif()
+
+  # Note: new boost components will require adding here.  The header
+  # must be present in all versions of Boost providing a library.
+  set(_Boost_ATOMIC_HEADERS              "boost/atomic.hpp")
+  set(_Boost_CHRONO_HEADERS              "boost/chrono.hpp")
+  set(_Boost_CONTAINER_HEADERS           "boost/container/container_fwd.hpp")
+  set(_Boost_CONTRACT_HEADERS            "boost/contract.hpp")
+  if(Boost_VERSION_STRING VERSION_LESS 1.61.0)
+    set(_Boost_CONTEXT_HEADERS           "boost/context/all.hpp")
+  else()
+    set(_Boost_CONTEXT_HEADERS           "boost/context/detail/fcontext.hpp")
+  endif()
+  set(_Boost_COROUTINE_HEADERS           "boost/coroutine/all.hpp")
+  set(_Boost_DATE_TIME_HEADERS           "boost/date_time/date.hpp")
+  set(_Boost_EXCEPTION_HEADERS           "boost/exception/exception.hpp")
+  set(_Boost_FIBER_HEADERS               "boost/fiber/all.hpp")
+  set(_Boost_FILESYSTEM_HEADERS          "boost/filesystem/path.hpp")
+  set(_Boost_GRAPH_HEADERS               "boost/graph/adjacency_list.hpp")
+  set(_Boost_GRAPH_PARALLEL_HEADERS      "boost/graph/adjacency_list.hpp")
+  set(_Boost_IOSTREAMS_HEADERS           "boost/iostreams/stream.hpp")
+  set(_Boost_LOCALE_HEADERS              "boost/locale.hpp")
+  set(_Boost_LOG_HEADERS                 "boost/log/core.hpp")
+  set(_Boost_LOG_SETUP_HEADERS           "boost/log/detail/setup_config.hpp")
+  set(_Boost_MATH_HEADERS                "boost/math_fwd.hpp")
+  set(_Boost_MATH_C99_HEADERS            "boost/math/tr1.hpp")
+  set(_Boost_MATH_C99F_HEADERS           "boost/math/tr1.hpp")
+  set(_Boost_MATH_C99L_HEADERS           "boost/math/tr1.hpp")
+  set(_Boost_MATH_TR1_HEADERS            "boost/math/tr1.hpp")
+  set(_Boost_MATH_TR1F_HEADERS           "boost/math/tr1.hpp")
+  set(_Boost_MATH_TR1L_HEADERS           "boost/math/tr1.hpp")
+  set(_Boost_MPI_HEADERS                 "boost/mpi.hpp")
+  set(_Boost_MPI_PYTHON_HEADERS          "boost/mpi/python/config.hpp")
+  set(_Boost_NUMPY_HEADERS               "boost/python/numpy.hpp")
+  set(_Boost_PRG_EXEC_MONITOR_HEADERS    "boost/test/prg_exec_monitor.hpp")
+  set(_Boost_PROGRAM_OPTIONS_HEADERS     "boost/program_options.hpp")
+  set(_Boost_PYTHON_HEADERS              "boost/python.hpp")
+  set(_Boost_RANDOM_HEADERS              "boost/random.hpp")
+  set(_Boost_REGEX_HEADERS               "boost/regex.hpp")
+  set(_Boost_SERIALIZATION_HEADERS       "boost/serialization/serialization.hpp")
+  set(_Boost_SIGNALS_HEADERS             "boost/signals.hpp")
+  set(_Boost_STACKTRACE_ADDR2LINE_HEADERS "boost/stacktrace.hpp")
+  set(_Boost_STACKTRACE_BACKTRACE_HEADERS "boost/stacktrace.hpp")
+  set(_Boost_STACKTRACE_BASIC_HEADERS    "boost/stacktrace.hpp")
+  set(_Boost_STACKTRACE_NOOP_HEADERS     "boost/stacktrace.hpp")
+  set(_Boost_STACKTRACE_WINDBG_CACHED_HEADERS "boost/stacktrace.hpp")
+  set(_Boost_STACKTRACE_WINDBG_HEADERS   "boost/stacktrace.hpp")
+  set(_Boost_SYSTEM_HEADERS              "boost/system/config.hpp")
+  set(_Boost_TEST_EXEC_MONITOR_HEADERS   "boost/test/test_exec_monitor.hpp")
+  set(_Boost_THREAD_HEADERS              "boost/thread.hpp")
+  set(_Boost_TIMER_HEADERS               "boost/timer.hpp")
+  set(_Boost_TYPE_ERASURE_HEADERS        "boost/type_erasure/config.hpp")
+  set(_Boost_UNIT_TEST_FRAMEWORK_HEADERS "boost/test/framework.hpp")
+  set(_Boost_WAVE_HEADERS                "boost/wave.hpp")
+  set(_Boost_WSERIALIZATION_HEADERS      "boost/archive/text_wiarchive.hpp")
+  if(WIN32)
+    set(_Boost_BZIP2_HEADERS             "boost/iostreams/filter/bzip2.hpp")
+    set(_Boost_ZLIB_HEADERS              "boost/iostreams/filter/zlib.hpp")
+  endif()
+
+  string(TOUPPER ${component} uppercomponent)
+  set(${_hdrs} ${_Boost_${uppercomponent}_HEADERS} PARENT_SCOPE)
+
+  string(REGEX REPLACE ";" " " _boost_HDRS_STRING "${_Boost_${uppercomponent}_HEADERS}")
+  if (NOT _boost_HDRS_STRING)
+    set(_boost_HDRS_STRING "(none)")
+  endif()
+  # message(STATUS "Headers for Boost::${component}: ${_boost_HDRS_STRING}")
+endfunction()
+
+#
+# Determine if any missing dependencies require adding to the component list.
+#
+# Sets _Boost_${COMPONENT}_DEPENDENCIES for each required component,
+# plus _Boost_IMPORTED_TARGETS (TRUE if imported targets should be
+# defined; FALSE if dependency information is unavailable).
+#
+# componentvar - the component list variable name
+# extravar - the indirect dependency list variable name
+#
+#
+function(_Boost_MISSING_DEPENDENCIES componentvar extravar)
+  # _boost_unprocessed_components - list of components requiring processing
+  # _boost_processed_components - components already processed (or currently being processed)
+  # _boost_new_components - new components discovered for future processing
+  #
+  list(APPEND _boost_unprocessed_components ${${componentvar}})
+
+  while(_boost_unprocessed_components)
+    list(APPEND _boost_processed_components ${_boost_unprocessed_components})
+    foreach(component ${_boost_unprocessed_components})
+      string(TOUPPER ${component} uppercomponent)
+      set(${_ret} ${_Boost_${uppercomponent}_DEPENDENCIES} PARENT_SCOPE)
+      _Boost_COMPONENT_DEPENDENCIES("${component}" _Boost_${uppercomponent}_DEPENDENCIES)
+      set(_Boost_${uppercomponent}_DEPENDENCIES ${_Boost_${uppercomponent}_DEPENDENCIES} PARENT_SCOPE)
+      set(_Boost_IMPORTED_TARGETS ${_Boost_IMPORTED_TARGETS} PARENT_SCOPE)
+      foreach(componentdep ${_Boost_${uppercomponent}_DEPENDENCIES})
+        if (NOT ("${componentdep}" IN_LIST _boost_processed_components OR "${componentdep}" IN_LIST _boost_new_components))
+          list(APPEND _boost_new_components ${componentdep})
+        endif()
+      endforeach()
+    endforeach()
+    set(_boost_unprocessed_components ${_boost_new_components})
+    unset(_boost_new_components)
+  endwhile()
+  set(_boost_extra_components ${_boost_processed_components})
+  if(_boost_extra_components AND ${componentvar})
+    list(REMOVE_ITEM _boost_extra_components ${${componentvar}})
+  endif()
+  set(${componentvar} ${_boost_processed_components} PARENT_SCOPE)
+  set(${extravar} ${_boost_extra_components} PARENT_SCOPE)
+endfunction()
+
+#
+# Some boost libraries may require particular set of compler features.
+# The very first one was `boost::fiber` introduced in Boost 1.62.
+# One can check required compiler features of it in
+# - `${Boost_ROOT}/libs/fiber/build/Jamfile.v2`;
+# - `${Boost_ROOT}/libs/context/build/Jamfile.v2`.
+#
+# TODO (Re)Check compiler features on (every?) release ???
+# One may use the following command to get the files to check:
+#
+#   $ find . -name Jamfile.v2 | grep build | xargs grep -l cxx1
+#
+function(_Boost_COMPILER_FEATURES component _ret)
+  # Boost >= 1.62
+  if(NOT Boost_VERSION_STRING VERSION_LESS 1.62.0)
+    set(_Boost_FIBER_COMPILER_FEATURES
+        cxx_alias_templates
+        cxx_auto_type
+        cxx_constexpr
+        cxx_defaulted_functions
+        cxx_final
+        cxx_lambdas
+        cxx_noexcept
+        cxx_nullptr
+        cxx_rvalue_references
+        cxx_thread_local
+        cxx_variadic_templates
+    )
+    # Compiler feature for `context` same as for `fiber`.
+    set(_Boost_CONTEXT_COMPILER_FEATURES ${_Boost_FIBER_COMPILER_FEATURES})
+  endif()
+
+  # Boost Contract library available in >= 1.67
+  if(NOT Boost_VERSION_STRING VERSION_LESS 1.67.0)
+    # From `libs/contract/build/boost_contract_build.jam`
+    set(_Boost_CONTRACT_COMPILER_FEATURES
+        cxx_lambdas
+        cxx_variadic_templates
+    )
+  endif()
+
+  string(TOUPPER ${component} uppercomponent)
+  set(${_ret} ${_Boost_${uppercomponent}_COMPILER_FEATURES} PARENT_SCOPE)
+endfunction()
+
+#
+# Update library search directory hint variable with paths used by prebuilt boost binaries.
+#
+# Prebuilt windows binaries (https://sourceforge.net/projects/boost/files/boost-binaries/)
+# have library directories named using MSVC compiler version and architecture.
+# This function would append corresponding directories if MSVC is a current compiler,
+# so having `BOOST_ROOT` would be enough to specify to find everything.
+#
+function(_Boost_UPDATE_WINDOWS_LIBRARY_SEARCH_DIRS_WITH_PREBUILT_PATHS componentlibvar basedir)
+  if("x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xMSVC")
+    if(CMAKE_SIZEOF_VOID_P EQUAL 8)
+      set(_arch_suffix 64)
+    else()
+      set(_arch_suffix 32)
+    endif()
+    if(MSVC_TOOLSET_VERSION GREATER_EQUAL 150)
+      # Not yet known.
+    elseif(MSVC_TOOLSET_VERSION GREATER_EQUAL 140)
+      # MSVC toolset 14.x versions are forward compatible.
+      foreach(v 9 8 7 6 5 4 3 2 1 0)
+        if(MSVC_TOOLSET_VERSION GREATER_EQUAL 14${v})
+          list(APPEND ${componentlibvar} ${basedir}/lib${_arch_suffix}-msvc-14.${v})
+        endif()
+      endforeach()
+    elseif(MSVC_TOOLSET_VERSION GREATER_EQUAL 80)
+      math(EXPR _toolset_major_version "${MSVC_TOOLSET_VERSION} / 10")
+      list(APPEND ${componentlibvar} ${basedir}/lib${_arch_suffix}-msvc-${_toolset_major_version}.0)
+    endif()
+    set(${componentlibvar} ${${componentlibvar}} PARENT_SCOPE)
+  endif()
+endfunction()
+
+#
+# End functions/macros
+#
+#-------------------------------------------------------------------------------
+
+#-------------------------------------------------------------------------------
+# main.
+#-------------------------------------------------------------------------------
+
+
+# If the user sets Boost_LIBRARY_DIR, use it as the default for both
+# configurations.
+if(NOT Boost_LIBRARY_DIR_RELEASE AND Boost_LIBRARY_DIR)
+  set(Boost_LIBRARY_DIR_RELEASE "${Boost_LIBRARY_DIR}")
+endif()
+if(NOT Boost_LIBRARY_DIR_DEBUG AND Boost_LIBRARY_DIR)
+  set(Boost_LIBRARY_DIR_DEBUG   "${Boost_LIBRARY_DIR}")
+endif()
+
+if(NOT DEFINED Boost_USE_DEBUG_LIBS)
+  set(Boost_USE_DEBUG_LIBS TRUE)
+endif()
+if(NOT DEFINED Boost_USE_RELEASE_LIBS)
+  set(Boost_USE_RELEASE_LIBS TRUE)
+endif()
+if(NOT DEFINED Boost_USE_MULTITHREADED)
+  set(Boost_USE_MULTITHREADED TRUE)
+endif()
+if(NOT DEFINED Boost_USE_DEBUG_RUNTIME)
+  set(Boost_USE_DEBUG_RUNTIME TRUE)
+endif()
+
+# Check the version of Boost against the requested version.
+if(Boost_FIND_VERSION AND NOT Boost_FIND_VERSION_MINOR)
+  message(SEND_ERROR "When requesting a specific version of Boost, you must provide at least the major and minor version numbers, e.g., 1.34")
+endif()
+
+if(Boost_FIND_VERSION_EXACT)
+  # The version may appear in a directory with or without the patch
+  # level, even when the patch level is non-zero.
+  set(_boost_TEST_VERSIONS
+    "${Boost_FIND_VERSION_MAJOR}.${Boost_FIND_VERSION_MINOR}.${Boost_FIND_VERSION_PATCH}"
+    "${Boost_FIND_VERSION_MAJOR}.${Boost_FIND_VERSION_MINOR}")
+else()
+  # The user has not requested an exact version.  Among known
+  # versions, find those that are acceptable to the user request.
+  #
+  # Note: When adding a new Boost release, also update the dependency
+  # information in _Boost_COMPONENT_DEPENDENCIES and
+  # _Boost_COMPONENT_HEADERS.  See the instructions at the top of
+  # _Boost_COMPONENT_DEPENDENCIES.
+  set(_Boost_KNOWN_VERSIONS ${Boost_ADDITIONAL_VERSIONS}
+    "1.71.0" "1.71" "1.70.0" "1.70" "1.69.0" "1.69"
+    "1.68.0" "1.68" "1.67.0" "1.67" "1.66.0" "1.66" "1.65.1" "1.65.0" "1.65"
+    "1.64.0" "1.64" "1.63.0" "1.63" "1.62.0" "1.62" "1.61.0" "1.61" "1.60.0" "1.60"
+    "1.59.0" "1.59" "1.58.0" "1.58" "1.57.0" "1.57" "1.56.0" "1.56" "1.55.0" "1.55"
+    "1.54.0" "1.54" "1.53.0" "1.53" "1.52.0" "1.52" "1.51.0" "1.51"
+    "1.50.0" "1.50" "1.49.0" "1.49" "1.48.0" "1.48" "1.47.0" "1.47" "1.46.1"
+    "1.46.0" "1.46" "1.45.0" "1.45" "1.44.0" "1.44" "1.43.0" "1.43" "1.42.0" "1.42"
+    "1.41.0" "1.41" "1.40.0" "1.40" "1.39.0" "1.39" "1.38.0" "1.38" "1.37.0" "1.37"
+    "1.36.1" "1.36.0" "1.36" "1.35.1" "1.35.0" "1.35" "1.34.1" "1.34.0"
+    "1.34" "1.33.1" "1.33.0" "1.33")
+
+  set(_boost_TEST_VERSIONS)
+  if(Boost_FIND_VERSION)
+    set(_Boost_FIND_VERSION_SHORT "${Boost_FIND_VERSION_MAJOR}.${Boost_FIND_VERSION_MINOR}")
+    # Select acceptable versions.
+    foreach(version ${_Boost_KNOWN_VERSIONS})
+      if(NOT "${version}" VERSION_LESS "${Boost_FIND_VERSION}")
+        # This version is high enough.
+        list(APPEND _boost_TEST_VERSIONS "${version}")
+      elseif("${version}.99" VERSION_EQUAL "${_Boost_FIND_VERSION_SHORT}.99")
+        # This version is a short-form for the requested version with
+        # the patch level dropped.
+        list(APPEND _boost_TEST_VERSIONS "${version}")
+      endif()
+    endforeach()
+  else()
+    # Any version is acceptable.
+    set(_boost_TEST_VERSIONS "${_Boost_KNOWN_VERSIONS}")
+  endif()
+endif()
+
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_TEST_VERSIONS")
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_USE_MULTITHREADED")
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_USE_STATIC_LIBS")
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_USE_STATIC_RUNTIME")
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_ADDITIONAL_VERSIONS")
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_NO_SYSTEM_PATHS")
+
+# Supply Boost_LIB_DIAGNOSTIC_DEFINITIONS as a convenience target. It
+# will only contain any interface definitions on WIN32, but is created
+# on all platforms to keep end user code free from platform dependent
+# code.  Also provide convenience targets to disable autolinking and
+# enable dynamic linking.
+if(NOT TARGET Boost::diagnostic_definitions)
+  add_library(Boost::diagnostic_definitions INTERFACE IMPORTED)
+  add_library(Boost::disable_autolinking INTERFACE IMPORTED)
+  add_library(Boost::dynamic_linking INTERFACE IMPORTED)
+  set_target_properties(Boost::dynamic_linking PROPERTIES
+    INTERFACE_COMPILE_DEFINITIONS "BOOST_ALL_DYN_LINK")
+endif()
+if(WIN32)
+  # In windows, automatic linking is performed, so you do not have
+  # to specify the libraries.  If you are linking to a dynamic
+  # runtime, then you can choose to link to either a static or a
+  # dynamic Boost library, the default is to do a static link.  You
+  # can alter this for a specific library "whatever" by defining
+  # BOOST_WHATEVER_DYN_LINK to force Boost library "whatever" to be
+  # linked dynamically.  Alternatively you can force all Boost
+  # libraries to dynamic link by defining BOOST_ALL_DYN_LINK.
+
+  # This feature can be disabled for Boost library "whatever" by
+  # defining BOOST_WHATEVER_NO_LIB, or for all of Boost by defining
+  # BOOST_ALL_NO_LIB.
+
+  # If you want to observe which libraries are being linked against
+  # then defining BOOST_LIB_DIAGNOSTIC will cause the auto-linking
+  # code to emit a #pragma message each time a library is selected
+  # for linking.
+  set(Boost_LIB_DIAGNOSTIC_DEFINITIONS "-DBOOST_LIB_DIAGNOSTIC")
+  set_target_properties(Boost::diagnostic_definitions PROPERTIES
+    INTERFACE_COMPILE_DEFINITIONS "BOOST_LIB_DIAGNOSTIC")
+  set_target_properties(Boost::disable_autolinking PROPERTIES
+    INTERFACE_COMPILE_DEFINITIONS "BOOST_ALL_NO_LIB")
+endif()
+
+# Patch  for GTSAM:
+if (POLICY CMP0074)
+	cmake_policy(GET CMP0074 _Boost_CMP0074)
+endif()
+
+if(NOT "x${_Boost_CMP0074}x" STREQUAL "xNEWx")
+  _Boost_CHECK_SPELLING(Boost_ROOT)
+endif()
+unset(_Boost_CMP0074)
+_Boost_CHECK_SPELLING(Boost_LIBRARYDIR)
+_Boost_CHECK_SPELLING(Boost_INCLUDEDIR)
+
+# Collect environment variable inputs as hints.  Do not consider changes.
+foreach(v BOOSTROOT BOOST_ROOT BOOST_INCLUDEDIR BOOST_LIBRARYDIR)
+  set(_env $ENV{${v}})
+  if(_env)
+    file(TO_CMAKE_PATH "${_env}" _ENV_${v})
+  else()
+    set(_ENV_${v} "")
+  endif()
+endforeach()
+if(NOT _ENV_BOOST_ROOT AND _ENV_BOOSTROOT)
+  set(_ENV_BOOST_ROOT "${_ENV_BOOSTROOT}")
+endif()
+
+# Collect inputs and cached results.  Detect changes since the last run.
+if(NOT BOOST_ROOT AND BOOSTROOT)
+  set(BOOST_ROOT "${BOOSTROOT}")
+endif()
+set(_Boost_VARS_DIR
+  BOOST_ROOT
+  Boost_NO_SYSTEM_PATHS
+  )
+
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "BOOST_ROOT")
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "BOOST_ROOT" ENVIRONMENT)
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "BOOST_INCLUDEDIR")
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "BOOST_INCLUDEDIR" ENVIRONMENT)
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "BOOST_LIBRARYDIR")
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "BOOST_LIBRARYDIR" ENVIRONMENT)
+
+# ------------------------------------------------------------------------
+#  Search for Boost include DIR
+# ------------------------------------------------------------------------
+
+set(_Boost_VARS_INC BOOST_INCLUDEDIR Boost_INCLUDE_DIR Boost_ADDITIONAL_VERSIONS)
+_Boost_CHANGE_DETECT(_Boost_CHANGE_INCDIR ${_Boost_VARS_DIR} ${_Boost_VARS_INC})
+# Clear Boost_INCLUDE_DIR if it did not change but other input affecting the
+# location did.  We will find a new one based on the new inputs.
+if(_Boost_CHANGE_INCDIR AND NOT _Boost_INCLUDE_DIR_CHANGED)
+  unset(Boost_INCLUDE_DIR CACHE)
+endif()
+
+if(NOT Boost_INCLUDE_DIR)
+  set(_boost_INCLUDE_SEARCH_DIRS "")
+  if(BOOST_INCLUDEDIR)
+    list(APPEND _boost_INCLUDE_SEARCH_DIRS ${BOOST_INCLUDEDIR})
+  elseif(_ENV_BOOST_INCLUDEDIR)
+    list(APPEND _boost_INCLUDE_SEARCH_DIRS ${_ENV_BOOST_INCLUDEDIR})
+  endif()
+
+  if( BOOST_ROOT )
+    list(APPEND _boost_INCLUDE_SEARCH_DIRS ${BOOST_ROOT}/include ${BOOST_ROOT})
+  elseif( _ENV_BOOST_ROOT )
+    list(APPEND _boost_INCLUDE_SEARCH_DIRS ${_ENV_BOOST_ROOT}/include ${_ENV_BOOST_ROOT})
+  endif()
+
+  if( Boost_NO_SYSTEM_PATHS)
+    list(APPEND _boost_INCLUDE_SEARCH_DIRS NO_CMAKE_SYSTEM_PATH NO_SYSTEM_ENVIRONMENT_PATH)
+  else()
+    if("x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xMSVC")
+      foreach(ver ${_boost_TEST_VERSIONS})
+        string(REPLACE "." "_" ver "${ver}")
+        list(APPEND _boost_INCLUDE_SEARCH_DIRS PATHS "C:/local/boost_${ver}")
+      endforeach()
+    endif()
+    list(APPEND _boost_INCLUDE_SEARCH_DIRS PATHS
+      C:/boost/include
+      C:/boost
+      /sw/local/include
+      )
+  endif()
+
+  # Try to find Boost by stepping backwards through the Boost versions
+  # we know about.
+  # Build a list of path suffixes for each version.
+  set(_boost_PATH_SUFFIXES)
+  foreach(_boost_VER ${_boost_TEST_VERSIONS})
+    # Add in a path suffix, based on the required version, ideally
+    # we could read this from version.hpp, but for that to work we'd
+    # need to know the include dir already
+    set(_boost_BOOSTIFIED_VERSION)
+
+    # Transform 1.35 => 1_35 and 1.36.0 => 1_36_0
+    if(_boost_VER MATCHES "([0-9]+)\\.([0-9]+)\\.([0-9]+)")
+        set(_boost_BOOSTIFIED_VERSION
+          "${CMAKE_MATCH_1}_${CMAKE_MATCH_2}_${CMAKE_MATCH_3}")
+    elseif(_boost_VER MATCHES "([0-9]+)\\.([0-9]+)")
+        set(_boost_BOOSTIFIED_VERSION
+          "${CMAKE_MATCH_1}_${CMAKE_MATCH_2}")
+    endif()
+
+    list(APPEND _boost_PATH_SUFFIXES
+      "boost-${_boost_BOOSTIFIED_VERSION}"
+      "boost_${_boost_BOOSTIFIED_VERSION}"
+      "boost/boost-${_boost_BOOSTIFIED_VERSION}"
+      "boost/boost_${_boost_BOOSTIFIED_VERSION}"
+      )
+
+  endforeach()
+
+  _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_INCLUDE_SEARCH_DIRS")
+  _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_PATH_SUFFIXES")
+
+  # Look for a standard boost header file.
+  find_path(Boost_INCLUDE_DIR
+    NAMES         boost/config.hpp
+    HINTS         ${_boost_INCLUDE_SEARCH_DIRS}
+    PATH_SUFFIXES ${_boost_PATH_SUFFIXES}
+    )
+endif()
+
+# ------------------------------------------------------------------------
+#  Extract version information from version.hpp
+# ------------------------------------------------------------------------
+
+if(Boost_INCLUDE_DIR)
+  _Boost_DEBUG_PRINT("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}"
+                     "location of version.hpp: ${Boost_INCLUDE_DIR}/boost/version.hpp")
+
+  # Extract Boost_VERSION_MACRO and Boost_LIB_VERSION from version.hpp
+  set(Boost_VERSION_MACRO 0)
+  set(Boost_LIB_VERSION "")
+  file(STRINGS "${Boost_INCLUDE_DIR}/boost/version.hpp" _boost_VERSION_HPP_CONTENTS REGEX "#define BOOST_(LIB_)?VERSION ")
+  if("${_boost_VERSION_HPP_CONTENTS}" MATCHES "#define BOOST_VERSION ([0-9]+)")
+    set(Boost_VERSION_MACRO "${CMAKE_MATCH_1}")
+  endif()
+  if("${_boost_VERSION_HPP_CONTENTS}" MATCHES "#define BOOST_LIB_VERSION \"([0-9_]+)\"")
+    set(Boost_LIB_VERSION "${CMAKE_MATCH_1}")
+  endif()
+  unset(_boost_VERSION_HPP_CONTENTS)
+
+  # Calculate version components
+  math(EXPR Boost_VERSION_MAJOR "${Boost_VERSION_MACRO} / 100000")
+  math(EXPR Boost_VERSION_MINOR "${Boost_VERSION_MACRO} / 100 % 1000")
+  math(EXPR Boost_VERSION_PATCH "${Boost_VERSION_MACRO} % 100")
+  set(Boost_VERSION_COUNT 3)
+
+  # Define alias variables for backwards compat.
+  set(Boost_MAJOR_VERSION ${Boost_VERSION_MAJOR})
+  set(Boost_MINOR_VERSION ${Boost_VERSION_MINOR})
+  set(Boost_SUBMINOR_VERSION ${Boost_VERSION_PATCH})
+
+  # Define Boost version in x.y.z format
+  set(Boost_VERSION_STRING "${Boost_VERSION_MAJOR}.${Boost_VERSION_MINOR}.${Boost_VERSION_PATCH}")
+
+  # Define final Boost_VERSION
+  if (POLICY CMP0093)
+    cmake_policy(GET CMP0093 _Boost_CMP0093
+   	  PARENT_SCOPE # undocumented, do not use outside of CMake
+    )
+  endif()
+
+  if("x${_Boost_CMP0093}x" STREQUAL "xNEWx")
+    set(Boost_VERSION ${Boost_VERSION_STRING})
+  else()
+    set(Boost_VERSION ${Boost_VERSION_MACRO})
+  endif()
+  unset(_Boost_CMP0093)
+
+  _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_VERSION")
+  _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_VERSION_STRING")
+  _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_VERSION_MACRO")
+  _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_VERSION_MAJOR")
+  _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_VERSION_MINOR")
+  _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_VERSION_PATCH")
+  _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_VERSION_COUNT")
+endif()
+
+# ------------------------------------------------------------------------
+#  Prefix initialization
+# ------------------------------------------------------------------------
+
+set(Boost_LIB_PREFIX "")
+if ( (GHSMULTI AND Boost_USE_STATIC_LIBS) OR
+    (WIN32 AND Boost_USE_STATIC_LIBS AND NOT CYGWIN) )
+  set(Boost_LIB_PREFIX "lib")
+endif()
+
+if ( NOT Boost_NAMESPACE )
+  set(Boost_NAMESPACE "boost")
+endif()
+
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_LIB_PREFIX")
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_NAMESPACE")
+
+# ------------------------------------------------------------------------
+#  Suffix initialization and compiler suffix detection.
+# ------------------------------------------------------------------------
+
+set(_Boost_VARS_NAME
+  Boost_NAMESPACE
+  Boost_COMPILER
+  Boost_THREADAPI
+  Boost_USE_DEBUG_PYTHON
+  Boost_USE_MULTITHREADED
+  Boost_USE_STATIC_LIBS
+  Boost_USE_STATIC_RUNTIME
+  Boost_USE_STLPORT
+  Boost_USE_STLPORT_DEPRECATED_NATIVE_IOSTREAMS
+  )
+_Boost_CHANGE_DETECT(_Boost_CHANGE_LIBNAME ${_Boost_VARS_NAME})
+
+# Setting some more suffixes for the library
+if (Boost_COMPILER)
+  set(_boost_COMPILER ${Boost_COMPILER})
+  _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}"
+                         "_boost_COMPILER" SOURCE "user-specified via Boost_COMPILER")
+else()
+  # Attempt to guess the compiler suffix
+  # NOTE: this is not perfect yet, if you experience any issues
+  # please report them and use the Boost_COMPILER variable
+  # to work around the problems.
+  _Boost_GUESS_COMPILER_PREFIX(_boost_COMPILER)
+endif()
+
+set (_boost_MULTITHREADED "-mt")
+if( NOT Boost_USE_MULTITHREADED )
+  set (_boost_MULTITHREADED "")
+endif()
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_MULTITHREADED")
+
+#======================
+# Systematically build up the Boost ABI tag for the 'tagged' and 'versioned' layouts
+# http://boost.org/doc/libs/1_66_0/more/getting_started/windows.html#library-naming
+# http://boost.org/doc/libs/1_66_0/boost/config/auto_link.hpp
+# http://boost.org/doc/libs/1_66_0/tools/build/src/tools/common.jam
+# http://boost.org/doc/libs/1_66_0/boostcpp.jam
+set( _boost_RELEASE_ABI_TAG "-")
+set( _boost_DEBUG_ABI_TAG   "-")
+# Key       Use this library when:
+#  s        linking statically to the C++ standard library and
+#           compiler runtime support libraries.
+if(Boost_USE_STATIC_RUNTIME)
+  set( _boost_RELEASE_ABI_TAG "${_boost_RELEASE_ABI_TAG}s")
+  set( _boost_DEBUG_ABI_TAG   "${_boost_DEBUG_ABI_TAG}s")
+endif()
+#  g        using debug versions of the standard and runtime
+#           support libraries
+if(WIN32 AND Boost_USE_DEBUG_RUNTIME)
+  if("x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xMSVC"
+          OR "x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xClang"
+          OR "x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xIntel")
+    string(APPEND _boost_DEBUG_ABI_TAG "g")
+  endif()
+endif()
+#  y        using special debug build of python
+if(Boost_USE_DEBUG_PYTHON)
+  string(APPEND _boost_DEBUG_ABI_TAG "y")
+endif()
+#  d        using a debug version of your code
+string(APPEND _boost_DEBUG_ABI_TAG "d")
+#  p        using the STLport standard library rather than the
+#           default one supplied with your compiler
+if(Boost_USE_STLPORT)
+  string(APPEND _boost_RELEASE_ABI_TAG "p")
+  string(APPEND _boost_DEBUG_ABI_TAG "p")
+endif()
+#  n        using the STLport deprecated "native iostreams" feature
+#           removed from the documentation in 1.43.0 but still present in
+#           boost/config/auto_link.hpp
+if(Boost_USE_STLPORT_DEPRECATED_NATIVE_IOSTREAMS)
+  string(APPEND _boost_RELEASE_ABI_TAG "n")
+  string(APPEND _boost_DEBUG_ABI_TAG "n")
+endif()
+
+#  -x86     Architecture and address model tag
+#           First character is the architecture, then word-size, either 32 or 64
+#           Only used in 'versioned' layout, added in Boost 1.66.0
+if(DEFINED Boost_ARCHITECTURE)
+  set(_boost_ARCHITECTURE_TAG "${Boost_ARCHITECTURE}")
+  _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}"
+                         "_boost_ARCHITECTURE_TAG" SOURCE "user-specified via Boost_ARCHITECTURE")
+else()
+  set(_boost_ARCHITECTURE_TAG "")
+  # {CMAKE_CXX_COMPILER_ARCHITECTURE_ID} is not currently set for all compilers
+  if(NOT "x${CMAKE_CXX_COMPILER_ARCHITECTURE_ID}" STREQUAL "x" AND NOT Boost_VERSION_STRING VERSION_LESS 1.66.0)
+    string(APPEND _boost_ARCHITECTURE_TAG "-")
+    # This needs to be kept in-sync with the section of CMakePlatformId.h.in
+    # inside 'defined(_WIN32) && defined(_MSC_VER)'
+    if(CMAKE_CXX_COMPILER_ARCHITECTURE_ID STREQUAL "IA64")
+      string(APPEND _boost_ARCHITECTURE_TAG "i")
+    elseif(CMAKE_CXX_COMPILER_ARCHITECTURE_ID STREQUAL "X86"
+              OR CMAKE_CXX_COMPILER_ARCHITECTURE_ID STREQUAL "x64")
+      string(APPEND _boost_ARCHITECTURE_TAG "x")
+    elseif(CMAKE_CXX_COMPILER_ARCHITECTURE_ID MATCHES "^ARM")
+      string(APPEND _boost_ARCHITECTURE_TAG "a")
+    elseif(CMAKE_CXX_COMPILER_ARCHITECTURE_ID STREQUAL "MIPS")
+      string(APPEND _boost_ARCHITECTURE_TAG "m")
+    endif()
+
+    if(CMAKE_SIZEOF_VOID_P EQUAL 8)
+      string(APPEND _boost_ARCHITECTURE_TAG "64")
+    else()
+      string(APPEND _boost_ARCHITECTURE_TAG "32")
+    endif()
+  endif()
+  _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}"
+                         "_boost_ARCHITECTURE_TAG" SOURCE "detected")
+endif()
+
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_RELEASE_ABI_TAG")
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_DEBUG_ABI_TAG")
+
+# ------------------------------------------------------------------------
+#  Begin finding boost libraries
+# ------------------------------------------------------------------------
+
+set(_Boost_VARS_LIB "")
+foreach(c DEBUG RELEASE)
+  set(_Boost_VARS_LIB_${c} BOOST_LIBRARYDIR Boost_LIBRARY_DIR_${c})
+  list(APPEND _Boost_VARS_LIB ${_Boost_VARS_LIB_${c}})
+  _Boost_CHANGE_DETECT(_Boost_CHANGE_LIBDIR_${c} ${_Boost_VARS_DIR} ${_Boost_VARS_LIB_${c}} Boost_INCLUDE_DIR)
+  # Clear Boost_LIBRARY_DIR_${c} if it did not change but other input affecting the
+  # location did.  We will find a new one based on the new inputs.
+  if(_Boost_CHANGE_LIBDIR_${c} AND NOT _Boost_LIBRARY_DIR_${c}_CHANGED)
+    unset(Boost_LIBRARY_DIR_${c} CACHE)
+  endif()
+
+  # If Boost_LIBRARY_DIR_[RELEASE,DEBUG] is set, prefer its value.
+  if(Boost_LIBRARY_DIR_${c})
+    set(_boost_LIBRARY_SEARCH_DIRS_${c} ${Boost_LIBRARY_DIR_${c}} NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+  else()
+    set(_boost_LIBRARY_SEARCH_DIRS_${c} "")
+    if(BOOST_LIBRARYDIR)
+      list(APPEND _boost_LIBRARY_SEARCH_DIRS_${c} ${BOOST_LIBRARYDIR})
+    elseif(_ENV_BOOST_LIBRARYDIR)
+      list(APPEND _boost_LIBRARY_SEARCH_DIRS_${c} ${_ENV_BOOST_LIBRARYDIR})
+    endif()
+
+    if(BOOST_ROOT)
+      list(APPEND _boost_LIBRARY_SEARCH_DIRS_${c} ${BOOST_ROOT}/lib ${BOOST_ROOT}/stage/lib)
+      _Boost_UPDATE_WINDOWS_LIBRARY_SEARCH_DIRS_WITH_PREBUILT_PATHS(_boost_LIBRARY_SEARCH_DIRS_${c} "${BOOST_ROOT}")
+    elseif(_ENV_BOOST_ROOT)
+      list(APPEND _boost_LIBRARY_SEARCH_DIRS_${c} ${_ENV_BOOST_ROOT}/lib ${_ENV_BOOST_ROOT}/stage/lib)
+      _Boost_UPDATE_WINDOWS_LIBRARY_SEARCH_DIRS_WITH_PREBUILT_PATHS(_boost_LIBRARY_SEARCH_DIRS_${c} "${_ENV_BOOST_ROOT}")
+    endif()
+
+    list(APPEND _boost_LIBRARY_SEARCH_DIRS_${c}
+      ${Boost_INCLUDE_DIR}/lib
+      ${Boost_INCLUDE_DIR}/../lib
+      ${Boost_INCLUDE_DIR}/stage/lib
+      )
+    _Boost_UPDATE_WINDOWS_LIBRARY_SEARCH_DIRS_WITH_PREBUILT_PATHS(_boost_LIBRARY_SEARCH_DIRS_${c} "${Boost_INCLUDE_DIR}/..")
+    _Boost_UPDATE_WINDOWS_LIBRARY_SEARCH_DIRS_WITH_PREBUILT_PATHS(_boost_LIBRARY_SEARCH_DIRS_${c} "${Boost_INCLUDE_DIR}")
+    if( Boost_NO_SYSTEM_PATHS )
+      list(APPEND _boost_LIBRARY_SEARCH_DIRS_${c} NO_CMAKE_SYSTEM_PATH NO_SYSTEM_ENVIRONMENT_PATH)
+    else()
+      foreach(ver ${_boost_TEST_VERSIONS})
+        string(REPLACE "." "_" ver "${ver}")
+        _Boost_UPDATE_WINDOWS_LIBRARY_SEARCH_DIRS_WITH_PREBUILT_PATHS(_boost_LIBRARY_SEARCH_DIRS_${c} "C:/local/boost_${ver}")
+      endforeach()
+      _Boost_UPDATE_WINDOWS_LIBRARY_SEARCH_DIRS_WITH_PREBUILT_PATHS(_boost_LIBRARY_SEARCH_DIRS_${c} "C:/boost")
+      list(APPEND _boost_LIBRARY_SEARCH_DIRS_${c} PATHS
+        C:/boost/lib
+        C:/boost
+        /sw/local/lib
+        )
+    endif()
+  endif()
+endforeach()
+
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_LIBRARY_SEARCH_DIRS_RELEASE")
+_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_LIBRARY_SEARCH_DIRS_DEBUG")
+
+# Support preference of static libs by adjusting CMAKE_FIND_LIBRARY_SUFFIXES
+if( Boost_USE_STATIC_LIBS )
+  set( _boost_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES ${CMAKE_FIND_LIBRARY_SUFFIXES})
+  if(WIN32)
+    list(INSERT CMAKE_FIND_LIBRARY_SUFFIXES 0 .lib .a)
+  else()
+    set(CMAKE_FIND_LIBRARY_SUFFIXES .a)
+  endif()
+endif()
+
+# We want to use the tag inline below without risking double dashes
+if(_boost_RELEASE_ABI_TAG)
+  if(${_boost_RELEASE_ABI_TAG} STREQUAL "-")
+    set(_boost_RELEASE_ABI_TAG "")
+  endif()
+endif()
+if(_boost_DEBUG_ABI_TAG)
+  if(${_boost_DEBUG_ABI_TAG} STREQUAL "-")
+    set(_boost_DEBUG_ABI_TAG "")
+  endif()
+endif()
+
+# The previous behavior of FindBoost when Boost_USE_STATIC_LIBS was enabled
+# on WIN32 was to:
+#  1. Search for static libs compiled against a SHARED C++ standard runtime library (use if found)
+#  2. Search for static libs compiled against a STATIC C++ standard runtime library (use if found)
+# We maintain this behavior since changing it could break people's builds.
+# To disable the ambiguous behavior, the user need only
+# set Boost_USE_STATIC_RUNTIME either ON or OFF.
+set(_boost_STATIC_RUNTIME_WORKAROUND false)
+if(WIN32 AND Boost_USE_STATIC_LIBS)
+  if(NOT DEFINED Boost_USE_STATIC_RUNTIME)
+    set(_boost_STATIC_RUNTIME_WORKAROUND TRUE)
+  endif()
+endif()
+
+# On versions < 1.35, remove the System library from the considered list
+# since it wasn't added until 1.35.
+if(Boost_VERSION_STRING AND Boost_FIND_COMPONENTS)
+  if(Boost_VERSION_STRING VERSION_LESS 1.35.0)
+    list(REMOVE_ITEM Boost_FIND_COMPONENTS system)
+  endif()
+endif()
+
+# Additional components may be required via component dependencies.
+# Add any missing components to the list.
+_Boost_MISSING_DEPENDENCIES(Boost_FIND_COMPONENTS _Boost_EXTRA_FIND_COMPONENTS)
+
+# If thread is required, get the thread libs as a dependency
+if("thread" IN_LIST Boost_FIND_COMPONENTS)
+  if(Boost_FIND_QUIETLY)
+    set(_Boost_find_quiet QUIET)
+  else()
+    set(_Boost_find_quiet "")
+  endif()
+  find_package(Threads ${_Boost_find_quiet})
+  unset(_Boost_find_quiet)
+endif()
+
+# If the user changed any of our control inputs flush previous results.
+if(_Boost_CHANGE_LIBDIR_DEBUG OR _Boost_CHANGE_LIBDIR_RELEASE OR _Boost_CHANGE_LIBNAME)
+  foreach(COMPONENT ${_Boost_COMPONENTS_SEARCHED})
+    string(TOUPPER ${COMPONENT} UPPERCOMPONENT)
+    foreach(c DEBUG RELEASE)
+      set(_var Boost_${UPPERCOMPONENT}_LIBRARY_${c})
+      unset(${_var} CACHE)
+      set(${_var} "${_var}-NOTFOUND")
+    endforeach()
+  endforeach()
+  set(_Boost_COMPONENTS_SEARCHED "")
+endif()
+
+foreach(COMPONENT ${Boost_FIND_COMPONENTS})
+  string(TOUPPER ${COMPONENT} UPPERCOMPONENT)
+
+  set( _boost_docstring_release "Boost ${COMPONENT} library (release)")
+  set( _boost_docstring_debug   "Boost ${COMPONENT} library (debug)")
+
+  # Compute component-specific hints.
+  set(_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT "")
+  if(${COMPONENT} STREQUAL "mpi" OR ${COMPONENT} STREQUAL "mpi_python" OR
+     ${COMPONENT} STREQUAL "graph_parallel")
+    foreach(lib ${MPI_CXX_LIBRARIES} ${MPI_C_LIBRARIES})
+      if(IS_ABSOLUTE "${lib}")
+        get_filename_component(libdir "${lib}" PATH)
+        string(REPLACE "\\" "/" libdir "${libdir}")
+        list(APPEND _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT ${libdir})
+      endif()
+    endforeach()
+  endif()
+
+  # Handle Python version suffixes
+  unset(COMPONENT_PYTHON_VERSION_MAJOR)
+  unset(COMPONENT_PYTHON_VERSION_MINOR)
+  if(${COMPONENT} MATCHES "^(python|mpi_python|numpy)([0-9])\$")
+    set(COMPONENT_UNVERSIONED "${CMAKE_MATCH_1}")
+    set(COMPONENT_PYTHON_VERSION_MAJOR "${CMAKE_MATCH_2}")
+  elseif(${COMPONENT} MATCHES "^(python|mpi_python|numpy)([0-9])\\.?([0-9])\$")
+    set(COMPONENT_UNVERSIONED "${CMAKE_MATCH_1}")
+    set(COMPONENT_PYTHON_VERSION_MAJOR "${CMAKE_MATCH_2}")
+    set(COMPONENT_PYTHON_VERSION_MINOR "${CMAKE_MATCH_3}")
+  endif()
+
+  unset(_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME)
+  if (COMPONENT_PYTHON_VERSION_MINOR)
+    # Boost >= 1.67
+    list(APPEND _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME "${COMPONENT_UNVERSIONED}${COMPONENT_PYTHON_VERSION_MAJOR}${COMPONENT_PYTHON_VERSION_MINOR}")
+    # Debian/Ubuntu (Some versions omit the 2 and/or 3 from the suffix)
+    list(APPEND _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME "${COMPONENT_UNVERSIONED}${COMPONENT_PYTHON_VERSION_MAJOR}-py${COMPONENT_PYTHON_VERSION_MAJOR}${COMPONENT_PYTHON_VERSION_MINOR}")
+    list(APPEND _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME "${COMPONENT_UNVERSIONED}-py${COMPONENT_PYTHON_VERSION_MAJOR}${COMPONENT_PYTHON_VERSION_MINOR}")
+    # Gentoo
+    list(APPEND _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME "${COMPONENT_UNVERSIONED}-${COMPONENT_PYTHON_VERSION_MAJOR}.${COMPONENT_PYTHON_VERSION_MINOR}")
+    # RPMs
+    list(APPEND _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME "${COMPONENT_UNVERSIONED}-${COMPONENT_PYTHON_VERSION_MAJOR}${COMPONENT_PYTHON_VERSION_MINOR}")
+  endif()
+  if (COMPONENT_PYTHON_VERSION_MAJOR AND NOT COMPONENT_PYTHON_VERSION_MINOR)
+    # Boost < 1.67
+    list(APPEND _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME "${COMPONENT_UNVERSIONED}${COMPONENT_PYTHON_VERSION_MAJOR}")
+  endif()
+
+  # Consolidate and report component-specific hints.
+  if(_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME)
+    list(REMOVE_DUPLICATES _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME)
+    _Boost_DEBUG_PRINT("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}"
+      "Component-specific library search names for ${COMPONENT_NAME}: ${_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME}")
+  endif()
+  if(_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT)
+    list(REMOVE_DUPLICATES _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT)
+    _Boost_DEBUG_PRINT("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}"
+      "Component-specific library search paths for ${COMPONENT}: ${_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT}")
+  endif()
+
+  #
+  # Find headers
+  #
+  _Boost_COMPONENT_HEADERS("${COMPONENT}" Boost_${UPPERCOMPONENT}_HEADER_NAME)
+  # Look for a standard boost header file.
+  if(Boost_${UPPERCOMPONENT}_HEADER_NAME)
+    if(EXISTS "${Boost_INCLUDE_DIR}/${Boost_${UPPERCOMPONENT}_HEADER_NAME}")
+      set(Boost_${UPPERCOMPONENT}_HEADER ON)
+    else()
+      set(Boost_${UPPERCOMPONENT}_HEADER OFF)
+    endif()
+  else()
+    set(Boost_${UPPERCOMPONENT}_HEADER ON)
+    message(WARNING "No header defined for ${COMPONENT}; skipping header check")
+  endif()
+
+  #
+  # Find RELEASE libraries
+  #
+  unset(_boost_RELEASE_NAMES)
+  foreach(component IN LISTS _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME COMPONENT)
+    foreach(compiler IN LISTS _boost_COMPILER)
+      list(APPEND _boost_RELEASE_NAMES
+        ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_RELEASE_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION}
+        ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_RELEASE_ABI_TAG}${_boost_ARCHITECTURE_TAG}
+        ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_RELEASE_ABI_TAG} )
+    endforeach()
+    list(APPEND _boost_RELEASE_NAMES
+      ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_RELEASE_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION}
+      ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_RELEASE_ABI_TAG}${_boost_ARCHITECTURE_TAG}
+      ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_RELEASE_ABI_TAG}
+      ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}
+      ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component} )
+    if(_boost_STATIC_RUNTIME_WORKAROUND)
+      set(_boost_RELEASE_STATIC_ABI_TAG "-s${_boost_RELEASE_ABI_TAG}")
+      foreach(compiler IN LISTS _boost_COMPILER)
+        list(APPEND _boost_RELEASE_NAMES
+          ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_RELEASE_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION}
+          ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_RELEASE_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG}
+          ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_RELEASE_STATIC_ABI_TAG} )
+      endforeach()
+      list(APPEND _boost_RELEASE_NAMES
+        ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_RELEASE_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION}
+        ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_RELEASE_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG}
+        ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_RELEASE_STATIC_ABI_TAG} )
+    endif()
+  endforeach()
+  if(Boost_THREADAPI AND ${COMPONENT} STREQUAL "thread")
+    _Boost_PREPEND_LIST_WITH_THREADAPI(_boost_RELEASE_NAMES ${_boost_RELEASE_NAMES})
+  endif()
+  _Boost_DEBUG_PRINT("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}"
+                     "Searching for ${UPPERCOMPONENT}_LIBRARY_RELEASE: ${_boost_RELEASE_NAMES}")
+
+  # if Boost_LIBRARY_DIR_RELEASE is not defined,
+  # but Boost_LIBRARY_DIR_DEBUG is, look there first for RELEASE libs
+  if(NOT Boost_LIBRARY_DIR_RELEASE AND Boost_LIBRARY_DIR_DEBUG)
+    list(INSERT _boost_LIBRARY_SEARCH_DIRS_RELEASE 0 ${Boost_LIBRARY_DIR_DEBUG})
+  endif()
+
+  # Avoid passing backslashes to _Boost_FIND_LIBRARY due to macro re-parsing.
+  string(REPLACE "\\" "/" _boost_LIBRARY_SEARCH_DIRS_tmp "${_boost_LIBRARY_SEARCH_DIRS_RELEASE}")
+
+  if(Boost_USE_RELEASE_LIBS)
+    _Boost_FIND_LIBRARY(Boost_${UPPERCOMPONENT}_LIBRARY_RELEASE RELEASE
+      NAMES ${_boost_RELEASE_NAMES}
+      HINTS ${_boost_LIBRARY_SEARCH_DIRS_tmp}
+      NAMES_PER_DIR
+      DOC "${_boost_docstring_release}"
+      )
+  endif()
+
+  #
+  # Find DEBUG libraries
+  #
+  unset(_boost_DEBUG_NAMES)
+  foreach(component IN LISTS _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME COMPONENT)
+    foreach(compiler IN LISTS _boost_COMPILER)
+      list(APPEND _boost_DEBUG_NAMES
+        ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_DEBUG_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION}
+        ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_DEBUG_ABI_TAG}${_boost_ARCHITECTURE_TAG}
+        ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_DEBUG_ABI_TAG} )
+    endforeach()
+    list(APPEND _boost_DEBUG_NAMES
+      ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_DEBUG_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION}
+      ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_DEBUG_ABI_TAG}${_boost_ARCHITECTURE_TAG}
+      ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_DEBUG_ABI_TAG}
+      ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}
+      ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component} )
+    if(_boost_STATIC_RUNTIME_WORKAROUND)
+      set(_boost_DEBUG_STATIC_ABI_TAG "-s${_boost_DEBUG_ABI_TAG}")
+      foreach(compiler IN LISTS _boost_COMPILER)
+        list(APPEND _boost_DEBUG_NAMES
+          ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_DEBUG_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION}
+          ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_DEBUG_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG}
+          ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_DEBUG_STATIC_ABI_TAG} )
+      endforeach()
+      list(APPEND _boost_DEBUG_NAMES
+        ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_DEBUG_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION}
+        ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_DEBUG_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG}
+        ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_DEBUG_STATIC_ABI_TAG} )
+    endif()
+  endforeach()
+  if(Boost_THREADAPI AND ${COMPONENT} STREQUAL "thread")
+     _Boost_PREPEND_LIST_WITH_THREADAPI(_boost_DEBUG_NAMES ${_boost_DEBUG_NAMES})
+  endif()
+  _Boost_DEBUG_PRINT("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}"
+                     "Searching for ${UPPERCOMPONENT}_LIBRARY_DEBUG: ${_boost_DEBUG_NAMES}")
+
+  # if Boost_LIBRARY_DIR_DEBUG is not defined,
+  # but Boost_LIBRARY_DIR_RELEASE is, look there first for DEBUG libs
+  if(NOT Boost_LIBRARY_DIR_DEBUG AND Boost_LIBRARY_DIR_RELEASE)
+    list(INSERT _boost_LIBRARY_SEARCH_DIRS_DEBUG 0 ${Boost_LIBRARY_DIR_RELEASE})
+  endif()
+
+  # Avoid passing backslashes to _Boost_FIND_LIBRARY due to macro re-parsing.
+  string(REPLACE "\\" "/" _boost_LIBRARY_SEARCH_DIRS_tmp "${_boost_LIBRARY_SEARCH_DIRS_DEBUG}")
+
+  if(Boost_USE_DEBUG_LIBS)
+    _Boost_FIND_LIBRARY(Boost_${UPPERCOMPONENT}_LIBRARY_DEBUG DEBUG
+      NAMES ${_boost_DEBUG_NAMES}
+      HINTS ${_boost_LIBRARY_SEARCH_DIRS_tmp}
+      NAMES_PER_DIR
+      DOC "${_boost_docstring_debug}"
+      )
+  endif ()
+
+  if(Boost_REALPATH)
+    _Boost_SWAP_WITH_REALPATH(Boost_${UPPERCOMPONENT}_LIBRARY_RELEASE "${_boost_docstring_release}")
+    _Boost_SWAP_WITH_REALPATH(Boost_${UPPERCOMPONENT}_LIBRARY_DEBUG   "${_boost_docstring_debug}"  )
+  endif()
+
+  _Boost_ADJUST_LIB_VARS(${UPPERCOMPONENT})
+
+  # Check if component requires some compiler features
+  _Boost_COMPILER_FEATURES(${COMPONENT} _Boost_${UPPERCOMPONENT}_COMPILER_FEATURES)
+
+endforeach()
+
+# Restore the original find library ordering
+if( Boost_USE_STATIC_LIBS )
+  set(CMAKE_FIND_LIBRARY_SUFFIXES ${_boost_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES})
+endif()
+
+# ------------------------------------------------------------------------
+#  End finding boost libraries
+# ------------------------------------------------------------------------
+
+set(Boost_INCLUDE_DIRS ${Boost_INCLUDE_DIR})
+set(Boost_LIBRARY_DIRS)
+if(Boost_LIBRARY_DIR_RELEASE)
+  list(APPEND Boost_LIBRARY_DIRS ${Boost_LIBRARY_DIR_RELEASE})
+endif()
+if(Boost_LIBRARY_DIR_DEBUG)
+  list(APPEND Boost_LIBRARY_DIRS ${Boost_LIBRARY_DIR_DEBUG})
+endif()
+if(Boost_LIBRARY_DIRS)
+  list(REMOVE_DUPLICATES Boost_LIBRARY_DIRS)
+endif()
+
+# ------------------------------------------------------------------------
+#  Call FPHSA helper, see https://cmake.org/cmake/help/latest/module/FindPackageHandleStandardArgs.html
+# ------------------------------------------------------------------------
+
+# Define aliases as needed by the component handler in the FPHSA helper below
+foreach(_comp IN LISTS Boost_FIND_COMPONENTS)
+  string(TOUPPER ${_comp} _uppercomp)
+  if(DEFINED Boost_${_uppercomp}_FOUND)
+    set(Boost_${_comp}_FOUND ${Boost_${_uppercomp}_FOUND})
+  endif()
+endforeach()
+
+find_package_handle_standard_args(Boost
+  REQUIRED_VARS Boost_INCLUDE_DIR
+  VERSION_VAR Boost_VERSION_STRING
+  HANDLE_COMPONENTS)
+
+if(Boost_FOUND)
+  if( NOT Boost_LIBRARY_DIRS )
+    # Compatibility Code for backwards compatibility with CMake
+    # 2.4's FindBoost module.
+
+    # Look for the boost library path.
+    # Note that the user may not have installed any libraries
+    # so it is quite possible the Boost_LIBRARY_DIRS may not exist.
+    set(_boost_LIB_DIR ${Boost_INCLUDE_DIR})
+
+    if("${_boost_LIB_DIR}" MATCHES "boost-[0-9]+")
+      get_filename_component(_boost_LIB_DIR ${_boost_LIB_DIR} PATH)
+    endif()
+
+    if("${_boost_LIB_DIR}" MATCHES "/include$")
+      # Strip off the trailing "/include" in the path.
+      get_filename_component(_boost_LIB_DIR ${_boost_LIB_DIR} PATH)
+    endif()
+
+    if(EXISTS "${_boost_LIB_DIR}/lib")
+      string(APPEND _boost_LIB_DIR /lib)
+    elseif(EXISTS "${_boost_LIB_DIR}/stage/lib")
+      string(APPEND _boost_LIB_DIR "/stage/lib")
+    else()
+      set(_boost_LIB_DIR "")
+    endif()
+
+    if(_boost_LIB_DIR AND EXISTS "${_boost_LIB_DIR}")
+      set(Boost_LIBRARY_DIRS ${_boost_LIB_DIR})
+    endif()
+
+  endif()
+else()
+  # Boost headers were not found so no components were found.
+  foreach(COMPONENT ${Boost_FIND_COMPONENTS})
+    string(TOUPPER ${COMPONENT} UPPERCOMPONENT)
+    set(Boost_${UPPERCOMPONENT}_FOUND 0)
+  endforeach()
+endif()
+
+# ------------------------------------------------------------------------
+#  Add imported targets
+# ------------------------------------------------------------------------
+
+if(Boost_FOUND)
+  # The builtin CMake package in Boost 1.70+ introduces a new name
+  # for the header-only lib, let's provide the same UI in module mode
+  if(NOT TARGET Boost::headers)
+    add_library(Boost::headers INTERFACE IMPORTED)
+    if(Boost_INCLUDE_DIRS)
+      set_target_properties(Boost::headers PROPERTIES
+        INTERFACE_INCLUDE_DIRECTORIES "${Boost_INCLUDE_DIRS}")
+    endif()
+  endif()
+
+  # Define the old target name for header-only libraries for backwards
+  # compat.
+  if(NOT TARGET Boost::boost)
+    add_library(Boost::boost INTERFACE IMPORTED)
+    set_target_properties(Boost::boost
+      PROPERTIES INTERFACE_LINK_LIBRARIES Boost::headers)
+  endif()
+
+  foreach(COMPONENT ${Boost_FIND_COMPONENTS})
+    if(_Boost_IMPORTED_TARGETS AND NOT TARGET Boost::${COMPONENT})
+      string(TOUPPER ${COMPONENT} UPPERCOMPONENT)
+      if(Boost_${UPPERCOMPONENT}_FOUND)
+        if(Boost_USE_STATIC_LIBS)
+          add_library(Boost::${COMPONENT} STATIC IMPORTED)
+        else()
+          # Even if Boost_USE_STATIC_LIBS is OFF, we might have static
+          # libraries as a result.
+          add_library(Boost::${COMPONENT} UNKNOWN IMPORTED)
+        endif()
+        if(Boost_INCLUDE_DIRS)
+          set_target_properties(Boost::${COMPONENT} PROPERTIES
+            INTERFACE_INCLUDE_DIRECTORIES "${Boost_INCLUDE_DIRS}")
+        endif()
+        if(EXISTS "${Boost_${UPPERCOMPONENT}_LIBRARY}")
+          set_target_properties(Boost::${COMPONENT} PROPERTIES
+            IMPORTED_LINK_INTERFACE_LANGUAGES "CXX"
+            IMPORTED_LOCATION "${Boost_${UPPERCOMPONENT}_LIBRARY}")
+        endif()
+        if(EXISTS "${Boost_${UPPERCOMPONENT}_LIBRARY_RELEASE}")
+          set_property(TARGET Boost::${COMPONENT} APPEND PROPERTY
+            IMPORTED_CONFIGURATIONS RELEASE)
+          set_target_properties(Boost::${COMPONENT} PROPERTIES
+            IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "CXX"
+            IMPORTED_LOCATION_RELEASE "${Boost_${UPPERCOMPONENT}_LIBRARY_RELEASE}")
+        endif()
+        if(EXISTS "${Boost_${UPPERCOMPONENT}_LIBRARY_DEBUG}")
+          set_property(TARGET Boost::${COMPONENT} APPEND PROPERTY
+            IMPORTED_CONFIGURATIONS DEBUG)
+          set_target_properties(Boost::${COMPONENT} PROPERTIES
+            IMPORTED_LINK_INTERFACE_LANGUAGES_DEBUG "CXX"
+            IMPORTED_LOCATION_DEBUG "${Boost_${UPPERCOMPONENT}_LIBRARY_DEBUG}")
+        endif()
+        if(_Boost_${UPPERCOMPONENT}_DEPENDENCIES)
+          unset(_Boost_${UPPERCOMPONENT}_TARGET_DEPENDENCIES)
+          foreach(dep ${_Boost_${UPPERCOMPONENT}_DEPENDENCIES})
+            list(APPEND _Boost_${UPPERCOMPONENT}_TARGET_DEPENDENCIES Boost::${dep})
+          endforeach()
+          if(COMPONENT STREQUAL "thread")
+            list(APPEND _Boost_${UPPERCOMPONENT}_TARGET_DEPENDENCIES Threads::Threads)
+          endif()
+          set_target_properties(Boost::${COMPONENT} PROPERTIES
+            INTERFACE_LINK_LIBRARIES "${_Boost_${UPPERCOMPONENT}_TARGET_DEPENDENCIES}")
+        endif()
+        if(_Boost_${UPPERCOMPONENT}_COMPILER_FEATURES)
+          set_target_properties(Boost::${COMPONENT} PROPERTIES
+            INTERFACE_COMPILE_FEATURES "${_Boost_${UPPERCOMPONENT}_COMPILER_FEATURES}")
+        endif()
+      endif()
+    endif()
+  endforeach()
+endif()
+
+# ------------------------------------------------------------------------
+#  Finalize
+# ------------------------------------------------------------------------
+
+# Report Boost_LIBRARIES
+set(Boost_LIBRARIES "")
+foreach(_comp IN LISTS Boost_FIND_COMPONENTS)
+  string(TOUPPER ${_comp} _uppercomp)
+  if(Boost_${_uppercomp}_FOUND)
+    list(APPEND Boost_LIBRARIES ${Boost_${_uppercomp}_LIBRARY})
+    if(_comp STREQUAL "thread")
+      list(APPEND Boost_LIBRARIES ${CMAKE_THREAD_LIBS_INIT})
+    endif()
+  endif()
+endforeach()
+
+# Configure display of cache entries in GUI.
+foreach(v BOOSTROOT BOOST_ROOT ${_Boost_VARS_INC} ${_Boost_VARS_LIB})
+  get_property(_type CACHE ${v} PROPERTY TYPE)
+  if(_type)
+    set_property(CACHE ${v} PROPERTY ADVANCED 1)
+    if("x${_type}" STREQUAL "xUNINITIALIZED")
+      if("x${v}" STREQUAL "xBoost_ADDITIONAL_VERSIONS")
+        set_property(CACHE ${v} PROPERTY TYPE STRING)
+      else()
+        set_property(CACHE ${v} PROPERTY TYPE PATH)
+      endif()
+    endif()
+  endif()
+endforeach()
+
+# Record last used values of input variables so we can
+# detect on the next run if the user changed them.
+foreach(v
+    ${_Boost_VARS_INC} ${_Boost_VARS_LIB}
+    ${_Boost_VARS_DIR} ${_Boost_VARS_NAME}
+    )
+  if(DEFINED ${v})
+    set(_${v}_LAST "${${v}}" CACHE INTERNAL "Last used ${v} value.")
+  else()
+    unset(_${v}_LAST CACHE)
+  endif()
+endforeach()
+
+# Maintain a persistent list of components requested anywhere since
+# the last flush.
+set(_Boost_COMPONENTS_SEARCHED "${_Boost_COMPONENTS_SEARCHED}")
+list(APPEND _Boost_COMPONENTS_SEARCHED ${Boost_FIND_COMPONENTS})
+list(REMOVE_DUPLICATES _Boost_COMPONENTS_SEARCHED)
+list(SORT _Boost_COMPONENTS_SEARCHED)
+set(_Boost_COMPONENTS_SEARCHED "${_Boost_COMPONENTS_SEARCHED}"
+  CACHE INTERNAL "Components requested for this build tree.")
+
+# Restore project's policies
+cmake_policy(POP)
diff --git a/cmake/FindMKL.cmake b/cmake/FindMKL.cmake
index 32e183baa..15a15f3ed 100644
--- a/cmake/FindMKL.cmake
+++ b/cmake/FindMKL.cmake
@@ -115,7 +115,7 @@ IF(WIN32 AND MKL_ROOT_DIR)
         IF (MKL_INCLUDE_DIR MATCHES "10.3")
                 SET(MKL_LIBS ${MKL_LIBS} libiomp5md)
         ENDIF()
-        
+
         FOREACH (LIB ${MKL_LIBS})
                 FIND_LIBRARY(${LIB}_PATH ${LIB} PATHS ${MKL_LIB_SEARCHPATH} ENV LIBRARY_PATH)
                 IF(${LIB}_PATH)
@@ -147,7 +147,7 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
                 ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
                 ${MKL_ROOT_DIR}/lib/
         )
-        
+
         # MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above)
         IF(NOT APPLE)
             FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY
@@ -231,6 +231,7 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
                         FIND_LIBRARY(MKL_IOMP5_LIBRARY
                           iomp5
                           PATHS
+                                ${MKL_ROOT_DIR}/lib/intel64
                                 ${MKL_ROOT_DIR}/../lib/intel64
                         )
                 ELSE()
@@ -254,7 +255,7 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
         ELSE()
             SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES})
         ENDIF()
-        
+
         MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY
                 MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY)
 ENDIF()
@@ -266,4 +267,4 @@ find_package_handle_standard_args(MKL DEFAULT_MSG MKL_INCLUDE_DIR MKL_LIBRARIES)
 #        LINK_DIRECTORIES(${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}) # hack
 #endif()
 
-MARK_AS_ADVANCED(MKL_INCLUDE_DIR MKL_LIBRARIES)
\ No newline at end of file
+MARK_AS_ADVANCED(MKL_INCLUDE_DIR MKL_LIBRARIES)
diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake
index aa35fea05..088ba7ad2 100644
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@@ -1,3 +1,8 @@
+# Set cmake policy to recognize the AppleClang compiler
+# independently from the Clang compiler.
+if(POLICY CMP0025)
+  cmake_policy(SET CMP0025 NEW)
+endif()
 
 # function:  list_append_cache(var [new_values ...])
 # Like "list(APPEND ...)" but working for CACHE variables.
@@ -81,6 +86,11 @@ if(MSVC)
     WINDOWS_LEAN_AND_MEAN
     NOMINMAX
 	)
+  # Avoid literally hundreds to thousands of warnings:
+  list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC
+	/wd4267 # warning C4267: 'initializing': conversion from 'size_t' to 'int', possible loss of data
+  )
+
 endif()
 
 # Other (non-preprocessor macros) compiler flags:
@@ -94,7 +104,8 @@ if(MSVC)
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING          /MD /O2  CACHE STRING "(User editable) Private compiler flags for Timing configuration.")
 else()
   # Common to all configurations, next for each configuration:
-  set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON          -Wall CACHE STRING "(User editable) Private compiler flags for all configurations.")
+  # "-fPIC" is to ensure proper code generation for shared libraries
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON          -Wall -fPIC CACHE STRING "(User editable) Private compiler flags for all configurations.")
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG           -g -fno-inline  CACHE STRING "(User editable) Private compiler flags for Debug configuration.")
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO  -g -O3  CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.")
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE         -O3  CACHE STRING "(User editable) Private compiler flags for Release configuration.")
diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake
index c3cad9d83..c8f876895 100644
--- a/cmake/GtsamCythonWrap.cmake
+++ b/cmake/GtsamCythonWrap.cmake
@@ -5,13 +5,17 @@ unset(PYTHON_EXECUTABLE CACHE)
 unset(CYTHON_EXECUTABLE CACHE)
 unset(PYTHON_INCLUDE_DIR CACHE)
 unset(PYTHON_MAJOR_VERSION CACHE)
+unset(PYTHON_LIBRARY CACHE)
 
-if(GTSAM_PYTHON_VERSION STREQUAL "Default")
-  find_package(PythonInterp REQUIRED)
-  find_package(PythonLibs REQUIRED)
-else()
-  find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
-  find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
+# Allow override from command line
+if(NOT DEFINED GTSAM_USE_CUSTOM_PYTHON_LIBRARY)
+  if(GTSAM_PYTHON_VERSION STREQUAL "Default")
+    find_package(PythonInterp REQUIRED)
+    find_package(PythonLibs REQUIRED)
+  else()
+    find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
+    find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
+  endif()
 endif()
 find_package(Cython 0.25.2 REQUIRED)
 
@@ -37,9 +41,8 @@ execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c"
 function(wrap_and_install_library_cython interface_header extra_imports install_path libs dependencies)
   # Paths for generated files
   get_filename_component(module_name "${interface_header}" NAME_WE)
-  set(generated_files_path "${PROJECT_BINARY_DIR}/cython/${module_name}")
+  set(generated_files_path "${install_path}")
   wrap_library_cython("${interface_header}" "${generated_files_path}" "${extra_imports}" "${libs}" "${dependencies}")
-  install_cython_wrapped_library("${interface_header}" "${generated_files_path}" "${install_path}")
 endfunction()
 
 function(set_up_required_cython_packages)
@@ -83,6 +86,15 @@ endfunction()
 #    - output_dir:   The output directory
 function(build_cythonized_cpp target cpp_file output_lib_we output_dir)
   add_library(${target} MODULE ${cpp_file})
+  
+  if(WIN32)
+    # Use .pyd extension instead of .dll on Windows
+    set_target_properties(${target} PROPERTIES SUFFIX ".pyd")
+
+    # Add full path to the Python library
+    target_link_libraries(${target} ${PYTHON_LIBRARIES})
+  endif()
+  
   if(APPLE)
     set(link_flags "-undefined dynamic_lookup")
   endif()
@@ -125,6 +137,10 @@ function(cythonize target pyx_file output_lib_we output_dir include_dirs libs in
     target_link_libraries(${target} "${libs}")
   endif()
   add_dependencies(${target} ${target}_pyx2cpp)
+
+  if(TARGET ${python_install_target})
+    add_dependencies(${python_install_target} ${target})
+  endif()
 endfunction()
 
 # Internal function that wraps a library and compiles the wrapper
@@ -137,9 +153,12 @@ function(wrap_library_cython interface_header generated_files_path extra_imports
   get_filename_component(module_name "${interface_header}" NAME_WE)
 
   # Wrap module to Cython pyx
-  message(STATUS "Cython wrapper generating ${module_name}.pyx")
+  message(STATUS "Cython wrapper generating ${generated_files_path}/${module_name}.pyx")
   set(generated_pyx "${generated_files_path}/${module_name}.pyx")
-  file(MAKE_DIRECTORY "${generated_files_path}")
+  if(NOT EXISTS ${generated_files_path})
+    file(MAKE_DIRECTORY "${generated_files_path}")
+  endif()
+
   add_custom_command(
     OUTPUT ${generated_pyx}
     DEPENDS ${interface_header} wrap
@@ -162,46 +181,6 @@ function(wrap_library_cython interface_header generated_files_path extra_imports
       COMMAND cmake -E remove_directory ${generated_files_path})
 endfunction()
 
-# Internal function that installs a wrap toolbox
-function(install_cython_wrapped_library interface_header generated_files_path install_path)
-  get_filename_component(module_name "${interface_header}" NAME_WE)
-
-  # NOTE: only installs .pxd and .pyx and binary files (not .cpp) - the trailing slash on the directory name
-  # here prevents creating the top-level module name directory in the destination.
-  # Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH/subdirectory if there is one
-  get_filename_component(location "${install_path}" PATH)
-  get_filename_component(name "${install_path}" NAME)
-  message(STATUS "Installing Cython Toolbox to ${location}${GTSAM_BUILD_TAG}/${name}") #${GTSAM_CYTHON_INSTALL_PATH}"
-
-  if(GTSAM_BUILD_TYPE_POSTFIXES)
-    foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
-      string(TOUPPER "${build_type}" build_type_upper)
-      if(${build_type_upper} STREQUAL "RELEASE")
-        set(build_type_tag "") # Don't create release mode tag on installed directory
-      else()
-        set(build_type_tag "${build_type}")
-      endif()
-
-      install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}${build_type_tag}/${name}"
-          CONFIGURATIONS "${build_type}"
-          PATTERN "build" EXCLUDE
-          PATTERN "CMakeFiles" EXCLUDE
-          PATTERN "Makefile" EXCLUDE
-          PATTERN "*.cmake" EXCLUDE
-          PATTERN "*.cpp" EXCLUDE
-          PATTERN "*.py" EXCLUDE)
-    endforeach()
-  else()
-    install(DIRECTORY "${generated_files_path}/" DESTINATION ${install_path}
-        PATTERN "build" EXCLUDE
-        PATTERN "CMakeFiles" EXCLUDE
-        PATTERN "Makefile" EXCLUDE
-        PATTERN "*.cmake" EXCLUDE
-        PATTERN "*.cpp" EXCLUDE
-        PATTERN "*.py" EXCLUDE)
-  endif()
-endfunction()
-
 # Helper function to install Cython scripts and handle multiple build types where the scripts
 # should be installed to all build type toolboxes
 #
@@ -219,50 +198,7 @@ function(install_cython_scripts source_directory dest_directory patterns)
   foreach(pattern ${patterns})
     list(APPEND patterns_args PATTERN "${pattern}")
   endforeach()
-  if(GTSAM_BUILD_TYPE_POSTFIXES)
-    foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
-      string(TOUPPER "${build_type}" build_type_upper)
-      if(${build_type_upper} STREQUAL "RELEASE")
-        set(build_type_tag "") # Don't create release mode tag on installed directory
-      else()
-        set(build_type_tag "${build_type}")
-      endif()
-      # Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH if there is one
-      get_filename_component(location "${dest_directory}" PATH)
-      get_filename_component(name "${dest_directory}" NAME)
-      install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}"
+
+  file(COPY "${source_directory}" DESTINATION "${dest_directory}"
             FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
-    endforeach()
-  else()
-    install(DIRECTORY "${source_directory}" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
-  endif()
-
 endfunction()
-
-# Helper function to install specific files and handle multiple build types where the scripts
-# should be installed to all build type toolboxes
-#
-# Arguments:
-#  source_files: The source files to be installed.
-#  dest_directory: The destination directory to install to.
-function(install_cython_files source_files dest_directory)
-
-  if(GTSAM_BUILD_TYPE_POSTFIXES)
-    foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
-      string(TOUPPER "${build_type}" build_type_upper)
-      if(${build_type_upper} STREQUAL "RELEASE")
-        set(build_type_tag "") # Don't create release mode tag on installed directory
-      else()
-        set(build_type_tag "${build_type}")
-      endif()
-      # Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH if there is one
-      get_filename_component(location "${dest_directory}" PATH)
-      get_filename_component(name "${dest_directory}" NAME)
-      install(FILES "${source_files}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}")
-    endforeach()
-  else()
-    install(FILES "${source_files}" DESTINATION "${dest_directory}")
-  endif()
-
-endfunction()
-
diff --git a/cmake/GtsamMakeConfigFile.cmake b/cmake/GtsamMakeConfigFile.cmake
index 2fff888ef..0479a2524 100644
--- a/cmake/GtsamMakeConfigFile.cmake
+++ b/cmake/GtsamMakeConfigFile.cmake
@@ -15,7 +15,7 @@ function(GtsamMakeConfigFile PACKAGE_NAME)
 		get_filename_component(name "${ARGV1}" NAME_WE)
 		set(EXTRA_FILE "${name}.cmake")
 		configure_file(${ARGV1} "${PROJECT_BINARY_DIR}/${EXTRA_FILE}" @ONLY)
-		install(FILES "${PROJECT_BINARY_DIR}/${EXTRA_FILE}" DESTINATION "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}")
+		install(FILES "${PROJECT_BINARY_DIR}/${EXTRA_FILE}" DESTINATION "${DEF_INSTALL_CMAKE_DIR}")
 	else()
 		set(EXTRA_FILE "_does_not_exist_")
 	endif()
diff --git a/cmake/example_cmake_find_gtsam/main.cpp b/cmake/example_cmake_find_gtsam/main.cpp
index 4d93e1b19..2904e00dd 100644
--- a/cmake/example_cmake_find_gtsam/main.cpp
+++ b/cmake/example_cmake_find_gtsam/main.cpp
@@ -33,7 +33,6 @@
 // Here we will use Between factors for the relative motion described by odometry measurements.
 // We will also use a Between Factor to encode the loop closure constraint
 // Also, we will initialize the robot at the origin using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 
 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
@@ -69,7 +68,7 @@ int main(int argc, char** argv) {
   // 2a. Add a prior on the first pose, setting it to the origin
   // A prior factor consists of a mean and a noise model (covariance matrix)
   noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
+  graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
 
   // For simplicity, we will use the same noise model for odometry and loop closures
   noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index a351ec52b..221025575 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -3,11 +3,32 @@ include(GtsamCythonWrap)
 
 # Create the cython toolbox for the gtsam library
 if (GTSAM_INSTALL_CYTHON_TOOLBOX)
+  # Add the new make target command
+  set(python_install_target python-install)
+  add_custom_target(${python_install_target}
+    COMMAND ${PYTHON_EXECUTABLE} ${GTSAM_CYTHON_INSTALL_PATH}/setup.py install
+    WORKING_DIRECTORY ${GTSAM_CYTHON_INSTALL_PATH})
+
   # build and include the eigency version of eigency
   add_subdirectory(gtsam_eigency)
-  include_directories(${PROJECT_BINARY_DIR}/cython/gtsam_eigency)
+  include_directories(${GTSAM_EIGENCY_INSTALL_PATH})
 
-  # wrap gtsam
+  # Fix for error "C1128: number of sections exceeded object file format limit"
+  if(MSVC)
+    add_compile_options(/bigobj)
+  endif()
+
+  # First set up all the package related files.
+  # This also ensures the below wrap operations work correctly.
+  set(CYTHON_INSTALL_REQUIREMENTS_FILE "${PROJECT_SOURCE_DIR}/cython/requirements.txt")
+
+  # Install the custom-generated __init__.py
+  # This makes the cython (sub-)directories into python packages, so gtsam can be found while wrapping gtsam_unstable
+  configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py ${GTSAM_CYTHON_INSTALL_PATH}/gtsam/__init__.py COPYONLY)
+  configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable/__init__.py COPYONLY)
+  configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${GTSAM_CYTHON_INSTALL_PATH}/setup.py)
+
+  # Wrap gtsam
   add_custom_target(gtsam_header DEPENDS "../gtsam.h")
   wrap_and_install_library_cython("../gtsam.h" # interface_header
                                   ""                  # extra imports
@@ -15,8 +36,9 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
                                   gtsam  # library to link with
                                   "wrap;cythonize_eigency;gtsam;gtsam_header"  # dependencies which need to be built before wrapping
                                   )
+  add_dependencies(${python_install_target} gtsam gtsam_header)
 
-  # wrap gtsam_unstable
+  # Wrap gtsam_unstable
   if(GTSAM_BUILD_UNSTABLE)
     add_custom_target(gtsam_unstable_header DEPENDS "../gtsam_unstable/gtsam_unstable.h")
     wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header
@@ -25,17 +47,9 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
                                     gtsam_unstable  # library to link with
                                     "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam"  # dependencies to be built before wrapping
                                     )
+    add_dependencies(${python_install_target} gtsam_unstable gtsam_unstable_header)
   endif()
 
-  file(READ "${PROJECT_SOURCE_DIR}/cython/requirements.txt" CYTHON_INSTALL_REQUIREMENTS)
-  file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS)
-
-  # Install the custom-generated __init__.py
-  # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable
-  configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py COPYONLY)
-  configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py COPYONLY)
-  configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py)
-  install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}")
   # install scripts and tests
   install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
   install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
diff --git a/cython/README.md b/cython/README.md
index bc6e346d9..f69b7a5a6 100644
--- a/cython/README.md
+++ b/cython/README.md
@@ -1,43 +1,51 @@
 # Python Wrapper
 
-This is the Cython/Python wrapper around the GTSAM C++ library.
+This is the Python wrapper around the GTSAM C++ library. We use Cython to generate the bindings to the underlying C++ code.
+
+## Requirements
+
+- If you want to build the GTSAM python library for a specific python version (eg 3.6),
+  use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used.
+- If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment),
+  then the environment should be active while building GTSAM.
+- This wrapper needs `Cython(>=0.25.2)`, `backports_abc(>=0.5)`, and `numpy(>=1.11.0)`. These can be installed as follows:
+
+  ```bash
+  pip install -r <gtsam_folder>/cython/requirements.txt
+  ```
+
+- For compatibility with GTSAM's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git),
+  named `gtsam_eigency`, to interface between C++'s Eigen and Python's numpy.
 
 ## Install
 
-- if you want to build the gtsam python library for a specific python version (eg 2.7), use the `-DGTSAM_PYTHON_VERSION=2.7` option when running `cmake` otherwise the default interpreter will be used.
-    - If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment) then the environment should be active while building gtsam.
-- This wrapper needs Cython(>=0.25.2), backports_abc>=0.5, and numpy. These can be installed as follows:
+- Run cmake with the `GTSAM_INSTALL_CYTHON_TOOLBOX` cmake flag enabled to configure building the wrapper. The wrapped module will be built and copied to the directory defined by `GTSAM_CYTHON_INSTALL_PATH`, which is by default `<PROJECT_BINARY_DIR>/cython` in Release mode and `<PROJECT_BINARY_DIR>/cython<CMAKE_BUILD_TYPE>` for other modes.
 
-```bash
- pip install -r <gtsam_folder>/cython/requirements.txt
-```
+- Build GTSAM and the wrapper with `make`.
 
-- For compatibility with gtsam's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git),
-named **gtsam_eigency**, to interface between C++'s Eigen and Python's numpy.
+- To install, simply run `make python-install`.
+  - The same command can be used to install into a virtual environment if it is active.
+  - **NOTE**: if you don't want GTSAM to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install GTSAM to a subdirectory of the build directory.
 
-- Build and install gtsam using cmake with `GTSAM_INSTALL_CYTHON_TOOLBOX` enabled.
-The wrapped module will be installed to `GTSAM_CYTHON_INSTALL_PATH`, which is
-by default: `<your CMAKE_INSTALL_PREFIX>/cython`
-
-- To use the library without installing system-wide: modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`:
-```bash
-export PYTHONPATH=$PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH>
-```
-- To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install`
-    - (the same command can be used to install into a virtual environment if it is active)
-    - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install gtsam to a subdirectory of the build directory.
-    - if you run `setup.py` from the build directory rather than the installation directory, the script will warn you with the message: `setup.py is being run from an unexpected location`.
-      Before `make install` is run, not all the components of the package have been copied across, so running `setup.py` from the build directory would result in an incomplete package.
+- You can also directly run `make python-install` without running `make`, and it will compile all the dependencies accordingly.
 
 ## Unit Tests
 
 The Cython toolbox also has a small set of unit tests located in the
 test directory. To run them:
 
-```bash
- cd <your GTSAM_CYTHON_INSTALL_PATH>
- python -m unittest discover
-```
+  ```bash
+  cd <GTSAM_CYTHON_INSTALL_PATH>
+  python -m unittest discover
+  ```
+
+## Utils
+
+TODO
+
+## Examples
+
+TODO
 
 ## Writing Your Own Scripts
 
@@ -46,79 +54,63 @@ See the tests for examples.
 ### Some Important Notes:
 
 - Vector/Matrix:
-  + GTSAM expects double-precision floating point vectors and matrices.
-    Hence, you should pass numpy matrices with dtype=float, or 'float64'.
-  + Also, GTSAM expects *column-major* matrices, unlike the default storage
-    scheme in numpy. Hence, you should pass column-major matrices to gtsam using
+
+  - GTSAM expects double-precision floating point vectors and matrices.
+    Hence, you should pass numpy matrices with `dtype=float`, or `float64`.
+  - Also, GTSAM expects _column-major_ matrices, unlike the default storage
+    scheme in numpy. Hence, you should pass column-major matrices to GTSAM using
     the flag order='F'. And you always get column-major matrices back.
-    For more details, see: https://github.com/wouterboomsma/eigency#storage-layout---why-arrays-are-sometimes-transposed
-  + Passing row-major matrices of different dtype, e.g. 'int', will also work
+    For more details, see [this link](https://github.com/wouterboomsma/eigency#storage-layout---why-arrays-are-sometimes-transposed).
+  - Passing row-major matrices of different dtype, e.g. `int`, will also work
     as the wrapper converts them to column-major and dtype float for you,
     using numpy.array.astype(float, order='F', copy=False).
     However, this will result a copy if your matrix is not in the expected type
     and storage order.
 
-- Inner namespace: Classes in inner namespace will be prefixed by <innerNamespace>_ in Python.
-Examples: noiseModel_Gaussian, noiseModel_mEstimator_Tukey
+- Inner namespace: Classes in inner namespace will be prefixed by <innerNamespace>\_ in Python.
+
+  Examples: `noiseModel_Gaussian`, `noiseModel_mEstimator_Tukey`
 
 - Casting from a base class to a derive class must be done explicitly.
-Examples:
-```Python
-      noiseBase = factor.noiseModel()
-      noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase)
-```
 
-## Wrapping Your Own Project That Uses GTSAM
+  Examples:
 
-- Set PYTHONPATH to include ${GTSAM_CYTHON_INSTALL_PATH}
-  + so that it can find gtsam Cython header: gtsam/gtsam.pxd
+  ```python
+  noiseBase = factor.noiseModel()
+  noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase)
+  ```
 
-- In your CMakeList.txt
-```cmake
-find_package(GTSAM REQUIRED) # Make sure gtsam's install folder is in your PATH
-set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${GTSAM_DIR}/../GTSAMCMakeTools")
+## Wrapping Custom GTSAM-based Project
 
-# Wrap
-include(GtsamCythonWrap)
-include_directories(${GTSAM_EIGENCY_INSTALL_PATH})
-wrap_and_install_library_cython("your_project_interface.h"
-                                "from gtsam.gtsam cimport *" # extra import of gtsam/gtsam.pxd Cython header
-                                "your_install_path"
-                                "libraries_to_link_with_the_cython_module"
-                                "dependencies_which_need_to_be_built_before_the_wrapper"
-                                )
-#Optional: install_cython_scripts and install_cython_files. See GtsamCythonWrap.cmake.
-```
+Please refer to the template project and the corresponding tutorial available [here](https://github.com/borglab/GTSAM-project-python).
 
 ## KNOWN ISSUES
 
-  - Doesn't work with python3 installed from homebrew
-    - size-related issue: can only wrap up to a certain number of classes: up to mEstimator!
-    - Guess: 64 vs 32b? disutils Compiler flags?
-  - Bug with Cython 0.24: instantiated factor classes return FastVector<size_t> for keys(), which can't be casted to FastVector<Key>
-    - Upgrading to 0.25 solves the problem
-  - Need default constructor and default copy constructor for almost every classes... :(
-    - support these constructors by default and declare "delete" for special classes?
-
+- Doesn't work with python3 installed from homebrew
+  - size-related issue: can only wrap up to a certain number of classes: up to mEstimator!
+  - Guess: 64 vs 32b? disutils Compiler flags?
+- Bug with Cython 0.24: instantiated factor classes return FastVector<size_t> for keys(), which can't be casted to FastVector<Key>
+  - Upgrading to 0.25 solves the problem
+- Need default constructor and default copy constructor for almost every classes... :(
+  - support these constructors by default and declare "delete" for special classes?
 
 ### TODO
 
 - [ ] allow duplication of parent' functions in child classes. Not allowed for now due to conflicts in Cython.
-- [ ] a common header for boost shared_ptr? (Or wait until everything is switched to std::shared_ptr in gtsam?)
+- [ ] a common header for boost shared_ptr? (Or wait until everything is switched to std::shared_ptr in GTSAM?)
 - [ ] inner namespaces ==> inner packages?
 - [ ] Wrap fixed-size Matrices/Vectors?
 
-
 ### Completed/Cancelled:
 
-- [x] Fix Python tests: don't use " import <package> * ": Bad style!!! (18-03-17 19:50)
+- [x] Fix Python tests: don't use " import <package> \* ": Bad style!!! (18-03-17 19:50)
 - [x] Unit tests for cython wrappers @done (18-03-17 18:45) -- simply compare generated files
 - [x] Wrap unstable @done (18-03-17 15:30)
-- [x] Unify cython/gtsam.h and the original gtsam.h @done (18-03-17 15:30)
+- [x] Unify cython/GTSAM.h and the original GTSAM.h @done (18-03-17 15:30)
 - [x] 18-03-17: manage to unify the two versions by removing std container stubs from the matlab version,and keeping KeyList/KeyVector/KeySet as in the matlab version. Probably Cython 0.25 fixes the casting problem.
 - [x] 06-03-17: manage to remove the requirements for default and copy constructors
 - [ ] 25-11-16: Try to unify but failed. Main reasons are: Key/size_t, std containers, KeyVector/KeyList/KeySet. Matlab doesn't need to know about Key, but I can't make Cython to ignore Key as it couldn't cast KeyVector, i.e. FastVector<Key>, to FastVector<size_t>.
-- [ ] Marginal and JointMarginal: revert changes @failed (17-03-17 11:00) -- Cython does need a default constructor! It produces cpp code like this: ```gtsam::JointMarginal __pyx_t_1;```  Users don't have to wrap this constructor, however.
+- [ ] Marginal and JointMarginal: revert changes @failed (17-03-17 11:00) -- Cython does need a default constructor! It produces cpp code like this: `GTSAM::JointMarginal __pyx_t_1;` Users don't have to wrap this constructor, however.
 - [x] Convert input numpy Matrix/Vector to float dtype and storage order 'F' automatically, cannot crash! @done (15-03-17 13:00)
 - [x] Remove requirements.txt - Frank: don't bother with only 2 packages and a special case for eigency! @done (08-03-17 10:30)
 - [x] CMake install script @done (25-11-16 02:30)
@@ -132,7 +124,7 @@ wrap_and_install_library_cython("your_project_interface.h"
 - [x] Casting from parent and grandparents @done (16-11-16 17:00)
 - [x] Allow overloading constructors. The current solution is annoying!!! @done (16-11-16 17:00)
 - [x] Support "print obj" @done (16-11-16 17:00)
-- [x] methods for FastVector: at, [], ...  @done (16-11-16 17:00)
+- [x] methods for FastVector: at, [], ... @done (16-11-16 17:00)
 - [x] Cython: Key and size_t: traits<size_t> doesn't exist @done (16-09-12 18:34)
 - [x] KeyVector, KeyList, KeySet... @done (16-09-13 17:19)
 - [x] [Nice to have] parse typedef @done (16-09-13 17:19)
diff --git a/cython/gtsam/examples/GPSFactorExample.py b/cython/gtsam/examples/GPSFactorExample.py
new file mode 100644
index 000000000..493a07725
--- /dev/null
+++ b/cython/gtsam/examples/GPSFactorExample.py
@@ -0,0 +1,56 @@
+"""
+GTSAM Copyright 2010-2018, 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
+
+Simple robot motion example, with prior and one GPS measurements
+Author: Mandy Xie
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import numpy as np
+
+import gtsam
+
+import matplotlib.pyplot as plt
+import gtsam.utils.plot as gtsam_plot
+
+# ENU Origin is where the plane was in hold next to runway
+lat0 = 33.86998
+lon0 = -84.30626
+h0 = 274
+
+# Create noise models
+GPS_NOISE = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
+PRIOR_NOISE = gtsam.noiseModel_Isotropic.Sigma(6, 0.25)
+
+# Create an empty nonlinear factor graph
+graph = gtsam.NonlinearFactorGraph()
+
+# Add a prior on the first point, setting it to the origin
+# A prior factor consists of a mean and a noise model (covariance matrix)
+priorMean = gtsam.Pose3()  # prior at origin
+graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE))
+
+# Add GPS factors
+gps = gtsam.Point3(lat0, lon0, h0)
+graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE))
+print("\nFactor Graph:\n{}".format(graph))
+
+# Create the data structure to hold the initialEstimate estimate to the solution
+# For illustrative purposes, these have been deliberately set to incorrect values
+initial = gtsam.Values()
+initial.insert(1, gtsam.Pose3())
+print("\nInitial Estimate:\n{}".format(initial))
+
+# optimize using Levenberg-Marquardt optimization
+params = gtsam.LevenbergMarquardtParams()
+optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
+result = optimizer.optimize()
+print("\nFinal Result:\n{}".format(result))
+
diff --git a/cython/gtsam/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py
index 6cc8979f1..0e01766e7 100644
--- a/cython/gtsam/examples/ImuFactorExample.py
+++ b/cython/gtsam/examples/ImuFactorExample.py
@@ -5,32 +5,27 @@ All Rights Reserved
 
 See LICENSE for the license information
 
-A script validating the ImuFactor inference.
+A script validating and demonstrating the ImuFactor inference.
+
+Author: Frank Dellaert, Varun Agrawal
 """
 
 from __future__ import print_function
 
 import math
 
+import gtsam
 import matplotlib.pyplot as plt
 import numpy as np
+from gtsam import symbol_shorthand_B as B
+from gtsam import symbol_shorthand_V as V
+from gtsam import symbol_shorthand_X as X
+from gtsam.utils.plot import plot_pose3
 from mpl_toolkits.mplot3d import Axes3D
 
-import gtsam
-from gtsam.utils.plot import plot_pose3
 from PreintegrationExample import POSES_FIG, PreintegrationExample
 
-BIAS_KEY = int(gtsam.symbol(ord('b'), 0))
-
-
-def X(key):
-    """Create symbol for pose key."""
-    return gtsam.symbol(ord('x'), key)
-
-
-def V(key):
-    """Create symbol for velocity key."""
-    return gtsam.symbol(ord('v'), key)
+BIAS_KEY = B(0)
 
 
 np.set_printoptions(precision=3, suppress=True)
@@ -76,8 +71,14 @@ class ImuFactorExample(PreintegrationExample):
         initial.insert(BIAS_KEY, self.actualBias)
         for i in range(num_poses):
             state_i = self.scenario.navState(float(i))
-            initial.insert(X(i), state_i.pose())
-            initial.insert(V(i), state_i.velocity())
+
+            poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1)
+            pose = state_i.pose().compose(poseNoise)
+
+            velocity = state_i.velocity() + np.random.randn(3)*0.1
+
+            initial.insert(X(i), pose)
+            initial.insert(V(i), velocity)
 
         # simulate the loop
         i = 0  # state index
@@ -88,6 +89,12 @@ class ImuFactorExample(PreintegrationExample):
             measuredAcc = self.runner.measuredSpecificForce(t)
             pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
 
+            poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1)
+
+            actual_state_i = gtsam.NavState(
+                actual_state_i.pose().compose(poseNoise),
+                actual_state_i.velocity() + np.random.randn(3)*0.1)
+
             # Plot IMU many times
             if k % 10 == 0:
                 self.plotImu(t, measuredOmega, measuredAcc)
@@ -133,6 +140,9 @@ class ImuFactorExample(PreintegrationExample):
             pose_i = result.atPose3(X(i))
             plot_pose3(POSES_FIG, pose_i, 0.1)
             i += 1
+
+        gtsam.utils.plot.set_axes_equal(POSES_FIG)
+
         print(result.atimuBias_ConstantBias(BIAS_KEY))
 
         plt.ioff()
diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py
index 10bd3de11..0d98f08fe 100644
--- a/cython/gtsam/examples/ImuFactorExample2.py
+++ b/cython/gtsam/examples/ImuFactorExample2.py
@@ -8,22 +8,19 @@ from __future__ import print_function
 
 import math
 
-import matplotlib.pyplot as plt
-import numpy as np
-from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
-
 import gtsam
 import gtsam.utils.plot as gtsam_plot
-
-
-def X(key):
-    """Create symbol for pose key."""
-    return gtsam.symbol(ord('x'), key)
-
-
-def V(key):
-    """Create symbol for velocity key."""
-    return gtsam.symbol(ord('v'), key)
+import matplotlib.pyplot as plt
+import numpy as np
+from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
+                   ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
+                   PinholeCameraCal3_S2, Point3, Pose3,
+                   PriorFactorConstantBias, PriorFactorPose3,
+                   PriorFactorVector, Rot3, Values)
+from gtsam import symbol_shorthand_B as B
+from gtsam import symbol_shorthand_V as V
+from gtsam import symbol_shorthand_X as X
+from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
 
 
 def vector3(x, y, z):
@@ -69,8 +66,8 @@ PARAMS.setUse2ndOrderCoriolis(False)
 PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
 
 BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
-DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0),
-                    gtsam.Point3(0.05, -0.10, 0.20))
+DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
+              Point3(0.05, -0.10, 0.20))
 
 
 def IMU_example():
@@ -78,10 +75,10 @@ def IMU_example():
 
     # Start with a camera on x-axis looking at origin
     radius = 30
-    up = gtsam.Point3(0, 0, 1)
-    target = gtsam.Point3(0, 0, 0)
-    position = gtsam.Point3(radius, 0, 0)
-    camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2())
+    up = Point3(0, 0, 1)
+    target = Point3(0, 0, 0)
+    position = Point3(radius, 0, 0)
+    camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
     pose_0 = camera.pose()
 
     # Create the set of ground-truth landmarks and poses
@@ -90,37 +87,37 @@ def IMU_example():
 
     angular_velocity_vector = vector3(0, -angular_velocity, 0)
     linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
-    scenario = gtsam.ConstantTwistScenario(
+    scenario = ConstantTwistScenario(
         angular_velocity_vector, linear_velocity_vector, pose_0)
 
     # Create a factor graph
-    newgraph = gtsam.NonlinearFactorGraph()
+    newgraph = NonlinearFactorGraph()
 
     # Create (incremental) ISAM2 solver
-    isam = gtsam.ISAM2()
+    isam = ISAM2()
 
     # Create the initial estimate to the solution
     # Intentionally initialize the variables off from the ground truth
-    initialEstimate = gtsam.Values()
+    initialEstimate = Values()
 
     # Add a prior on pose x0. This indirectly specifies where the origin is.
     # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
     noise = gtsam.noiseModel_Diagonal.Sigmas(
-        np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
-    newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
+        np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
+    newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise))
 
     # Add imu priors
-    biasKey = gtsam.symbol(ord('b'), 0)
+    biasKey = B(0)
     biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
-    biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
-                                              biasnoise)
+    biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
+                                        biasnoise)
     newgraph.push_back(biasprior)
     initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
     velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
 
     # Calculate with correct initial velocity
     n_velocity = vector3(0, angular_velocity * radius, 0)
-    velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise)
+    velprior = PriorFactorVector(V(0), n_velocity, velnoise)
     newgraph.push_back(velprior)
     initialEstimate.insert(V(0), n_velocity)
 
@@ -141,7 +138,7 @@ def IMU_example():
             # Add Bias variables periodically
             if i % 5 == 0:
                 biasKey += 1
-                factor = gtsam.BetweenFactorConstantBias(
+                factor = BetweenFactorConstantBias(
                     biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
                 newgraph.add(factor)
                 initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
@@ -154,8 +151,7 @@ def IMU_example():
             accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
 
             # Add Imu Factor
-            imufac = gtsam.ImuFactor(
-                X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
+            imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
             newgraph.add(imufac)
 
             # insert new velocity, which is wrong
@@ -168,7 +164,7 @@ def IMU_example():
         ISAM2_plot(result)
 
         # reset
-        newgraph = gtsam.NonlinearFactorGraph()
+        newgraph = NonlinearFactorGraph()
         initialEstimate.clear()
 
 
diff --git a/cython/gtsam/examples/PlanarSLAMExample.py b/cython/gtsam/examples/PlanarSLAMExample.py
index fb038d043..c84f0f834 100644
--- a/cython/gtsam/examples/PlanarSLAMExample.py
+++ b/cython/gtsam/examples/PlanarSLAMExample.py
@@ -13,9 +13,10 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
 
 from __future__ import print_function
 
-import numpy as np
-
 import gtsam
+import numpy as np
+from gtsam import symbol_shorthand_L as L
+from gtsam import symbol_shorthand_X as X
 
 # Create noise models
 PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
@@ -26,11 +27,11 @@ MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
 graph = gtsam.NonlinearFactorGraph()
 
 # Create the keys corresponding to unknown variables in the factor graph
-X1 = gtsam.symbol(ord('x'), 1)
-X2 = gtsam.symbol(ord('x'), 2)
-X3 = gtsam.symbol(ord('x'), 3)
-L1 = gtsam.symbol(ord('l'), 4)
-L2 = gtsam.symbol(ord('l'), 5)
+X1 = X(1)
+X2 = X(2)
+X3 = X(3)
+L1 = L(4)
+L2 = L(5)
 
 # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
 graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
diff --git a/cython/gtsam/examples/Pose2SLAMExample_g2o.py b/cython/gtsam/examples/Pose2SLAMExample_g2o.py
index 3aee7daff..09114370d 100644
--- a/cython/gtsam/examples/Pose2SLAMExample_g2o.py
+++ b/cython/gtsam/examples/Pose2SLAMExample_g2o.py
@@ -53,16 +53,15 @@ graph, initial = gtsam.readG2o(g2oFile, is3D)
 assert args.kernel == "none", "Supplied kernel type is not yet implemented"
 
 # Add prior on the pose having index (key) = 0
-graphWithPrior = graph
 priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
-graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
+graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
 
 params = gtsam.GaussNewtonParams()
 params.setVerbosity("Termination")
 params.setMaxIterations(maxIterations)
 # parameters.setRelativeErrorTol(1e-5)
 # Create the optimizer ...
-optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
+optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
 # ... and optimize
 result = optimizer.optimize()
 
@@ -83,7 +82,7 @@ else:
     print ("Done!")
 
 if args.plot:
-    resultPoses = gtsam.extractPose2(result)
+    resultPoses = gtsam.utilities_extractPose2(result)
     for i in range(resultPoses.shape[0]):
         plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
     plt.show()
diff --git a/cython/gtsam/examples/Pose3SLAMExample_g2o.py b/cython/gtsam/examples/Pose3SLAMExample_g2o.py
index 38c5db275..3c1a54f7b 100644
--- a/cython/gtsam/examples/Pose3SLAMExample_g2o.py
+++ b/cython/gtsam/examples/Pose3SLAMExample_g2o.py
@@ -43,18 +43,17 @@ priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
                                                          1e-4, 1e-4, 1e-4))
 
 print("Adding prior to g2o file ")
-graphWithPrior = graph
 firstKey = initial.keys().at(0)
-graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
+graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
 
 params = gtsam.GaussNewtonParams()
 params.setVerbosity("Termination")  # this will show info about stopping conds
-optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
+optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
 result = optimizer.optimize()
 print("Optimization complete")
 
-print("initial error = ", graphWithPrior.error(initial))
-print("final error = ", graphWithPrior.error(result))
+print("initial error = ", graph.error(initial))
+print("final error = ", graph.error(result))
 
 if args.output is None:
     print("Final Result:\n{}".format(result))
@@ -66,7 +65,7 @@ else:
     print ("Done!")
 
 if args.plot:
-    resultPoses = gtsam.allPose3s(result)
+    resultPoses = gtsam.utilities_allPose3s(result)
     for i in range(resultPoses.size()):
         plot.plot_pose3(1, resultPoses.atPose3(i))
     plt.show()
diff --git a/cython/gtsam/examples/README.md b/cython/gtsam/examples/README.md
index 35ec4f66d..99bce00e2 100644
--- a/cython/gtsam/examples/README.md
+++ b/cython/gtsam/examples/README.md
@@ -1,7 +1,7 @@
 These examples are almost identical to the old handwritten python wrapper
 examples. However, there are just some slight name changes, for example
 `noiseModel.Diagonal` becomes `noiseModel_Diagonal` etc...
-Also, annoyingly, instead of `gtsam.Symbol('b', 0)` we now need to say `gtsam.symbol(ord('b'), 0))`
+Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthand_B(0)` or `B(0)` if we use python aliasing.
 
 # Porting Progress
 
diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py
index bf46f09d5..e02def2f9 100644
--- a/cython/gtsam/examples/SFMExample.py
+++ b/cython/gtsam/examples/SFMExample.py
@@ -11,17 +11,17 @@ A structure-from-motion problem on a simulated dataset
 from __future__ import print_function
 
 import gtsam
+import matplotlib.pyplot as plt
 import numpy as np
+from gtsam import symbol_shorthand_L as L
+from gtsam import symbol_shorthand_X as X
 from gtsam.examples import SFMdata
 from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
-                         GenericProjectionFactorCal3_S2, NonlinearFactorGraph,
-                         Point3, Pose3, PriorFactorPoint3, PriorFactorPose3,
-                         Rot3, SimpleCamera, Values)
-
-
-def symbol(name: str, index: int) -> int:
-    """ helper for creating a symbol without explicitly casting 'name' from str to int """
-    return gtsam.symbol(ord(name), index)
+                         GenericProjectionFactorCal3_S2, Marginals,
+                         NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
+                         Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3,
+                         SimpleCamera, Values)
+from gtsam.utils import plot
 
 
 def main():
@@ -70,23 +70,23 @@ def main():
     # Add a prior on pose x1. This indirectly specifies where the origin is.
     # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
     pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
-    factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
+    factor = PriorFactorPose3(X(0), poses[0], pose_noise)
     graph.push_back(factor)
 
     # Simulated measurements from each camera pose, adding them to the factor graph
     for i, pose in enumerate(poses):
-        camera = SimpleCamera(pose, K)
+        camera = PinholeCameraCal3_S2(pose, K)
         for j, point in enumerate(points):
             measurement = camera.project(point)
             factor = GenericProjectionFactorCal3_S2(
-                measurement, measurement_noise, symbol('x', i), symbol('l', j), K)
+                measurement, measurement_noise, X(i), L(j), K)
             graph.push_back(factor)
 
     # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
     # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
     # between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
     point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
-    factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
+    factor = PriorFactorPoint3(L(0), points[0], point_noise)
     graph.push_back(factor)
     graph.print_('Factor Graph:\n')
 
@@ -94,13 +94,11 @@ def main():
     # Intentionally initialize the variables off from the ground truth
     initial_estimate = Values()
     for i, pose in enumerate(poses):
-        r = Rot3.Rodrigues(-0.1, 0.2, 0.25)
-        t = Point3(0.05, -0.10, 0.20)
-        transformed_pose = pose.compose(Pose3(r, t))
-        initial_estimate.insert(symbol('x', i), transformed_pose)
+        transformed_pose = pose.retract(0.1*np.random.randn(6,1))
+        initial_estimate.insert(X(i), transformed_pose)
     for j, point in enumerate(points):
-        transformed_point = Point3(point.vector() + np.array([-0.25, 0.20, 0.15]))
-        initial_estimate.insert(symbol('l', j), transformed_point)
+        transformed_point = Point3(point.vector() + 0.1*np.random.randn(3))
+        initial_estimate.insert(L(j), transformed_point)
     initial_estimate.print_('Initial Estimates:\n')
 
     # Optimize the graph and print results
@@ -113,6 +111,11 @@ def main():
     print('initial error = {}'.format(graph.error(initial_estimate)))
     print('final error = {}'.format(graph.error(result)))
 
+    marginals = Marginals(graph, result)
+    plot.plot_3d_points(1, result, marginals=marginals)
+    plot.plot_trajectory(1, result, marginals=marginals, scale=8)
+    plot.set_axes_equal(1)
+    plt.show()
 
 if __name__ == '__main__':
     main()
diff --git a/cython/gtsam/examples/SFMdata.py b/cython/gtsam/examples/SFMdata.py
index 1d5c499fa..c586f7e52 100644
--- a/cython/gtsam/examples/SFMdata.py
+++ b/cython/gtsam/examples/SFMdata.py
@@ -25,14 +25,15 @@ def createPoints():
 
 def createPoses(K):
     # Create the set of ground-truth poses
-    radius = 30.0
+    radius = 40.0
+    height = 10.0
     angles = np.linspace(0, 2*np.pi, 8, endpoint=False)
     up = gtsam.Point3(0, 0, 1)
     target = gtsam.Point3(0, 0, 0)
     poses = []
     for theta in angles:
         position = gtsam.Point3(radius*np.cos(theta),
-                                radius*np.sin(theta), 0.0)
+                                radius*np.sin(theta), height)
         camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
         poses.append(camera.pose())
     return poses
diff --git a/cython/gtsam/examples/SimpleRotation.py b/cython/gtsam/examples/SimpleRotation.py
index 6e9b1320b..4e82d3778 100644
--- a/cython/gtsam/examples/SimpleRotation.py
+++ b/cython/gtsam/examples/SimpleRotation.py
@@ -10,8 +10,9 @@ This example will perform a relatively trivial optimization on
 a single variable with a single factor.
 """
 
-import numpy as np
 import gtsam
+import numpy as np
+from gtsam import symbol_shorthand_X as X
 
 
 def main():
@@ -33,7 +34,7 @@ def main():
     prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
     prior.print_('goal angle')
     model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
-    key = gtsam.symbol(ord('x'), 1)
+    key = X(1)
     factor = gtsam.PriorFactorRot2(key, prior, model)
 
     """
diff --git a/cython/gtsam/examples/VisualISAM2Example.py b/cython/gtsam/examples/VisualISAM2Example.py
index e2caee6d4..49e6ca95c 100644
--- a/cython/gtsam/examples/VisualISAM2Example.py
+++ b/cython/gtsam/examples/VisualISAM2Example.py
@@ -13,23 +13,14 @@ Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
 
 from __future__ import print_function
 
-import matplotlib.pyplot as plt
-import numpy as np
-from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
-
 import gtsam
 import gtsam.utils.plot as gtsam_plot
+import matplotlib.pyplot as plt
+import numpy as np
+from gtsam import symbol_shorthand_L as L
+from gtsam import symbol_shorthand_X as X
 from gtsam.examples import SFMdata
-
-
-def X(i):
-    """Create key for pose i."""
-    return int(gtsam.symbol(ord('x'), i))
-
-
-def L(j):
-    """Create key for landmark j."""
-    return int(gtsam.symbol(ord('l'), j))
+from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
 
 
 def visual_ISAM2_plot(result):
@@ -120,7 +111,7 @@ def visual_ISAM2_example():
         if i == 0:
             # Add a prior on pose x0
             pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array(
-                [0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))  # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+                [0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))  # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
             graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise))
 
             # Add a prior on landmark l0
diff --git a/cython/gtsam/examples/VisualISAMExample.py b/cython/gtsam/examples/VisualISAMExample.py
index f4d81eaf7..5cc37867b 100644
--- a/cython/gtsam/examples/VisualISAMExample.py
+++ b/cython/gtsam/examples/VisualISAMExample.py
@@ -18,12 +18,9 @@ from gtsam.examples import SFMdata
 from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
                          NonlinearFactorGraph, NonlinearISAM, Point3, Pose3,
                          PriorFactorPoint3, PriorFactorPose3, Rot3,
-                         SimpleCamera, Values)
-
-
-def symbol(name: str, index: int) -> int:
-    """ helper for creating a symbol without explicitly casting 'name' from str to int """
-    return gtsam.symbol(ord(name), index)
+                         PinholeCameraCal3_S2, Values)
+from gtsam import symbol_shorthand_L as L
+from gtsam import symbol_shorthand_X as X
 
 
 def main():
@@ -54,11 +51,11 @@ def main():
 
     # Loop over the different poses, adding the observations to iSAM incrementally
     for i, pose in enumerate(poses):
-        camera = SimpleCamera(pose, K)
+        camera = PinholeCameraCal3_S2(pose, K)
         # Add factors for each landmark observation
         for j, point in enumerate(points):
             measurement = camera.project(point)
-            factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, symbol('x', i), symbol('l', j), K)
+            factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, X(i), L(j), K)
             graph.push_back(factor)
 
         # Intentionally initialize the variables off from the ground truth
@@ -66,7 +63,7 @@ def main():
         initial_xi = pose.compose(noise)
 
         # Add an initial guess for the current pose
-        initial_estimate.insert(symbol('x', i), initial_xi)
+        initial_estimate.insert(X(i), initial_xi)
 
         # If this is the first iteration, add a prior on the first pose to set the coordinate frame
         # and a prior on the first landmark to set the scale
@@ -75,12 +72,12 @@ def main():
         if i == 0:
             # Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
             pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
-            factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
+            factor = PriorFactorPose3(X(0), poses[0], pose_noise)
             graph.push_back(factor)
 
             # Add a prior on landmark l0
             point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
-            factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
+            factor = PriorFactorPoint3(L(0), points[0], point_noise)
             graph.push_back(factor)
 
             # Add initial guesses to all observed landmarks
@@ -88,7 +85,7 @@ def main():
             for j, point in enumerate(points):
                 # Intentionally initialize the variables off from the ground truth
                 initial_lj = points[j].vector() + noise
-                initial_estimate.insert(symbol('l', j), Point3(initial_lj))
+                initial_estimate.insert(L(j), Point3(initial_lj))
         else:
             # Update iSAM with the new factors
             isam.update(graph, initial_estimate)
diff --git a/cython/gtsam/tests/test_FrobeniusFactor.py b/cython/gtsam/tests/test_FrobeniusFactor.py
new file mode 100644
index 000000000..f3f5354bb
--- /dev/null
+++ b/cython/gtsam/tests/test_FrobeniusFactor.py
@@ -0,0 +1,56 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+FrobeniusFactor unit tests.
+Author: Frank Dellaert
+"""
+# pylint: disable=no-name-in-module, import-error, invalid-name
+import unittest
+
+import numpy as np
+from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
+                   FrobeniusWormholeFactor, SOn)
+
+id = SO4()
+v1 = np.array([0, 0, 0, 0.1, 0, 0])
+Q1 = SO4.Expmap(v1)
+v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
+Q2 = SO4.Expmap(v2)
+
+
+class TestFrobeniusFactorSO4(unittest.TestCase):
+    """Test FrobeniusFactor factors."""
+
+    def test_frobenius_factor(self):
+        """Test creation of a factor that calculates the Frobenius norm."""
+        factor = FrobeniusFactorSO4(1, 2)
+        actual = factor.evaluateError(Q1, Q2)
+        expected = (Q2.matrix()-Q1.matrix()).transpose().reshape((16,))
+        np.testing.assert_array_equal(actual, expected)
+
+    def test_frobenius_between_factor(self):
+        """Test creation of a Frobenius BetweenFactor."""
+        factor = FrobeniusBetweenFactorSO4(1, 2, Q1.between(Q2))
+        actual = factor.evaluateError(Q1, Q2)
+        expected = np.zeros((16,))
+        np.testing.assert_array_almost_equal(actual, expected)
+
+    def test_frobenius_wormhole_factor(self):
+        """Test creation of a factor that calculates Shonan error."""
+        R1 = SO3.Expmap(v1[3:])
+        R2 = SO3.Expmap(v2[3:])
+        factor = FrobeniusWormholeFactor(1, 2, Rot3(R1.between(R2).matrix()), p=4)
+        I4 = SOn(4)
+        Q1 = I4.retract(v1)
+        Q2 = I4.retract(v2)
+        actual = factor.evaluateError(Q1, Q2)
+        expected = np.zeros((12,))
+        np.testing.assert_array_almost_equal(actual, expected, decimal=4)
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/cython/gtsam/tests/test_GaussianFactorGraph.py b/cython/gtsam/tests/test_GaussianFactorGraph.py
new file mode 100644
index 000000000..983825d8b
--- /dev/null
+++ b/cython/gtsam/tests/test_GaussianFactorGraph.py
@@ -0,0 +1,92 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for Linear Factor Graphs.
+Author: Frank Dellaert & Gerry Chen
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+import numpy as np
+from gtsam import symbol_shorthand_X as X
+from gtsam.utils.test_case import GtsamTestCase
+
+
+def create_graph():
+    """Create a basic linear factor graph for testing"""
+    graph = gtsam.GaussianFactorGraph()
+    
+    x0 = X(0)
+    x1 = X(1)
+    x2 = X(2)
+    
+    BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
+    PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
+
+    graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE)
+    graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE)
+    graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE)
+
+    return graph, (x0, x1, x2)
+
+class TestGaussianFactorGraph(GtsamTestCase):
+    """Tests for Gaussian Factor Graphs."""
+
+    def test_fg(self):
+        """Test solving a linear factor graph"""
+        graph, X = create_graph()
+        result = graph.optimize()
+
+        EXPECTEDX = [0, 1, 3]
+
+        # check solutions
+        self.assertAlmostEqual(EXPECTEDX[0], result.at(X[0]), delta=1e-8)
+        self.assertAlmostEqual(EXPECTEDX[1], result.at(X[1]), delta=1e-8)
+        self.assertAlmostEqual(EXPECTEDX[2], result.at(X[2]), delta=1e-8)
+
+    def test_convertNonlinear(self):
+        """Test converting a linear factor graph to a nonlinear one"""
+        graph, X = create_graph()
+
+        EXPECTEDM = [1, 2, 3]
+
+        # create nonlinear factor graph for marginalization
+        nfg = gtsam.LinearContainerFactor.ConvertLinearGraph(graph)
+        optimizer = gtsam.LevenbergMarquardtOptimizer(nfg, gtsam.Values())
+        nlresult = optimizer.optimizeSafely()
+
+        # marginalize
+        marginals = gtsam.Marginals(nfg, nlresult)
+        m = [marginals.marginalCovariance(x) for x in X]
+
+        # check linear marginalizations
+        self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8)
+        self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
+        self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
+    
+    def test_linearMarginalization(self):
+        """Marginalize a linear factor graph"""
+        graph, X = create_graph()
+        result = graph.optimize()
+
+        EXPECTEDM = [1, 2, 3]
+
+        # linear factor graph marginalize
+        marginals = gtsam.Marginals(graph, result)
+        m = [marginals.marginalCovariance(x) for x in X]
+
+        # check linear marginalizations
+        self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8)
+        self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
+        self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/cython/gtsam/tests/test_KarcherMeanFactor.py b/cython/gtsam/tests/test_KarcherMeanFactor.py
new file mode 100644
index 000000000..6976decc1
--- /dev/null
+++ b/cython/gtsam/tests/test_KarcherMeanFactor.py
@@ -0,0 +1,80 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+KarcherMeanFactor unit tests.
+Author: Frank Dellaert
+"""
+
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+import unittest
+
+import gtsam
+import numpy as np
+from gtsam.utils.test_case import GtsamTestCase
+
+KEY = 0
+MODEL = gtsam.noiseModel_Unit.Create(3)
+
+
+def find_Karcher_mean_Rot3(rotations):
+    """Find the Karcher mean of given values."""
+    # Cost function C(R) = \sum PriorFactor(R_i)::error(R)
+    # No closed form solution.
+    graph = gtsam.NonlinearFactorGraph()
+    for R in rotations:
+        graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
+    initial = gtsam.Values()
+    initial.insert(KEY, gtsam.Rot3())
+    result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
+    return result.atRot3(KEY)
+
+
+# Rot3 version
+R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0]))
+
+
+class TestKarcherMean(GtsamTestCase):
+
+    def test_find(self):
+        # Check that optimizing for Karcher mean (which minimizes Between distance)
+        # gets correct result.
+        rotations = {R, R.inverse()}
+        expected = gtsam.Rot3()
+        actual = find_Karcher_mean_Rot3(rotations)
+        self.gtsamAssertEquals(expected, actual)
+
+    def test_factor(self):
+        """Check that the InnerConstraint factor leaves the mean unchanged."""
+        # Make a graph with two variables, one between, and one InnerConstraint
+        # The optimal result should satisfy the between, while moving the other
+        # variable to make the mean the same as before.
+        # Mean of R and R' is identity. Let's make a BetweenFactor making R21 =
+        # R*R*R, i.e. geodesic length is 3 rather than 2.
+        graph = gtsam.NonlinearFactorGraph()
+        R12 = R.compose(R.compose(R))
+        graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
+        keys = gtsam.KeyVector()
+        keys.push_back(1)
+        keys.push_back(2)
+        graph.add(gtsam.KarcherMeanFactorRot3(keys))
+
+        initial = gtsam.Values()
+        initial.insert(1, R.inverse())
+        initial.insert(2, R)
+        expected = find_Karcher_mean_Rot3([R, R.inverse()])
+
+        result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
+        actual = find_Karcher_mean_Rot3(
+            [result.atRot3(1), result.atRot3(2)])
+        self.gtsamAssertEquals(expected, actual)
+        self.gtsamAssertEquals(
+            R12, result.atRot3(1).between(result.atRot3(2)))
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/cython/gtsam/tests/test_NonlinearOptimizer.py b/cython/gtsam/tests/test_NonlinearOptimizer.py
index efefb218a..985dc30a2 100644
--- a/cython/gtsam/tests/test_NonlinearOptimizer.py
+++ b/cython/gtsam/tests/test_NonlinearOptimizer.py
@@ -17,7 +17,8 @@ import unittest
 import gtsam
 from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
                    GaussNewtonParams, LevenbergMarquardtOptimizer,
-                   LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
+                   LevenbergMarquardtParams, PCGSolverParameters,
+                   DummyPreconditionerParameters, NonlinearFactorGraph, Ordering,
                    Point2, PriorFactorPoint2, Values)
 from gtsam.utils.test_case import GtsamTestCase
 
@@ -61,6 +62,16 @@ class TestScenario(GtsamTestCase):
             fg, initial_values, lmParams).optimize()
         self.assertAlmostEqual(0, fg.error(actual2))
 
+        # Levenberg-Marquardt
+        lmParams = LevenbergMarquardtParams.CeresDefaults()
+        lmParams.setLinearSolverType("ITERATIVE")
+        cgParams = PCGSolverParameters()
+        cgParams.setPreconditionerParams(DummyPreconditionerParameters())
+        lmParams.setIterativeParams(cgParams)
+        actual2 = LevenbergMarquardtOptimizer(
+            fg, initial_values, lmParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual2))
+
         # Dogleg
         dlParams = DoglegParams()
         dlParams.setOrdering(ordering)
diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py
index 577a13304..138f1ff13 100644
--- a/cython/gtsam/tests/test_Pose3.py
+++ b/cython/gtsam/tests/test_Pose3.py
@@ -6,8 +6,9 @@ All Rights Reserved
 See LICENSE for the license information
 
 Pose3 unit tests.
-Author: Frank Dellaert & Duy Nguyen Ta (Python)
+Author: Frank Dellaert, Duy Nguyen Ta
 """
+# pylint: disable=no-name-in-module
 import math
 import unittest
 
diff --git a/cython/gtsam/tests/test_PriorFactor.py b/cython/gtsam/tests/test_PriorFactor.py
index 0acf50c2c..66207b800 100644
--- a/cython/gtsam/tests/test_PriorFactor.py
+++ b/cython/gtsam/tests/test_PriorFactor.py
@@ -35,5 +35,27 @@ class TestPriorFactor(GtsamTestCase):
         values.insert(key, priorVector)
         self.assertEqual(factor.error(values), 0)
 
+    def test_AddPrior(self):
+        """
+        Test adding prior factors directly to factor graph via the .addPrior method.
+        """
+        # define factor graph
+        graph = gtsam.NonlinearFactorGraph()
+
+        # define and add Pose3 prior
+        key = 5
+        priorPose3 = gtsam.Pose3()
+        model = gtsam.noiseModel_Unit.Create(6)
+        graph.addPriorPose3(key, priorPose3, model)
+        self.assertEqual(graph.size(), 1)
+
+        # define and add Vector prior
+        key = 3
+        priorVector = np.array([0., 0., 0.])
+        model = gtsam.noiseModel_Unit.Create(3)
+        graph.addPriorVector(key, priorVector, model)
+        self.assertEqual(graph.size(), 2)
+
+
 if __name__ == "__main__":
     unittest.main()
diff --git a/cython/gtsam/tests/test_SO4.py b/cython/gtsam/tests/test_SO4.py
new file mode 100644
index 000000000..648bd1710
--- /dev/null
+++ b/cython/gtsam/tests/test_SO4.py
@@ -0,0 +1,59 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+SO4 unit tests.
+Author: Frank Dellaert
+"""
+# pylint: disable=no-name-in-module, import-error
+import unittest
+
+import numpy as np
+from gtsam import SO4
+
+I4 = SO4()
+v1 = np.array([0, 0, 0, .1, 0, 0])
+v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
+Q1 = SO4.Expmap(v1)
+Q2 = SO4.Expmap(v2)
+
+
+class TestSO4(unittest.TestCase):
+    """Test selected SO4 methods."""
+
+    def test_constructor(self):
+        """Construct from matrix."""
+        matrix = np.eye(4)
+        so4 = SO4(matrix)
+        self.assertIsInstance(so4, SO4)
+
+    def test_retract(self):
+        """Test retraction to manifold."""
+        v = np.zeros((6,), np.float)
+        actual = I4.retract(v)
+        self.assertTrue(actual.equals(I4, 1e-9))
+
+    def test_local(self):
+        """Check localCoordinates for trivial case."""
+        v0 = np.zeros((6,), np.float)
+        actual = I4.localCoordinates(I4)
+        np.testing.assert_array_almost_equal(actual, v0)
+
+    def test_compose(self):
+        """Check compose works in subgroup."""
+        expected = SO4.Expmap(2*v1)
+        actual = Q1.compose(Q1)
+        self.assertTrue(actual.equals(expected, 1e-9))
+
+    def test_vec(self):
+        """Check wrapping of vec."""
+        expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1])
+        actual = I4.vec()
+        np.testing.assert_array_equal(actual, expected)
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/cython/gtsam/tests/test_SOn.py b/cython/gtsam/tests/test_SOn.py
new file mode 100644
index 000000000..7599363e2
--- /dev/null
+++ b/cython/gtsam/tests/test_SOn.py
@@ -0,0 +1,59 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Dynamic SO(n) unit tests.
+Author: Frank Dellaert
+"""
+# pylint: disable=no-name-in-module, import-error
+import unittest
+
+import numpy as np
+from gtsam import SOn
+
+I4 = SOn(4)
+v1 = np.array([0, 0, 0, .1, 0, 0])
+v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
+Q1 = I4.retract(v1)
+Q2 = I4.retract(v2)
+
+
+class TestSO4(unittest.TestCase):
+    """Test selected SOn methods."""
+
+    def test_constructor(self):
+        """Construct from matrix."""
+        matrix = np.eye(4)
+        so4 = SOn.FromMatrix(matrix)
+        self.assertIsInstance(so4, SOn)
+
+    def test_retract(self):
+        """Test retraction to manifold."""
+        v = np.zeros((6,), np.float)
+        actual = I4.retract(v)
+        self.assertTrue(actual.equals(I4, 1e-9))
+
+    def test_local(self):
+        """Check localCoordinates for trivial case."""
+        v0 = np.zeros((6,), np.float)
+        actual = I4.localCoordinates(I4)
+        np.testing.assert_array_almost_equal(actual, v0)
+
+    def test_compose(self):
+        """Check compose works in subgroup."""
+        expected = I4.retract(2*v1)
+        actual = Q1.compose(Q1)
+        self.assertTrue(actual.equals(expected, 1e-3)) # not exmap so only approximate
+
+    def test_vec(self):
+        """Check wrapping of vec."""
+        expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1])
+        actual = I4.vec()
+        np.testing.assert_array_equal(actual, expected)
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/cython/gtsam/tests/test_SimpleCamera.py
index efdfec561..a3654a5f1 100644
--- a/cython/gtsam/tests/test_SimpleCamera.py
+++ b/cython/gtsam/tests/test_SimpleCamera.py
@@ -5,7 +5,7 @@ All Rights Reserved
 
 See LICENSE for the license information
 
-SimpleCamera unit tests.
+PinholeCameraCal3_S2 unit tests.
 Author: Frank Dellaert & Duy Nguyen Ta (Python)
 """
 import math
@@ -14,7 +14,7 @@ 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
 from gtsam.utils.test_case import GtsamTestCase
 
 K = Cal3_S2(625, 625, 0, 0, 0)
@@ -23,14 +23,14 @@ class TestSimpleCamera(GtsamTestCase):
 
     def test_constructor(self):
         pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5))
-        camera = SimpleCamera(pose1, K)
+        camera = PinholeCameraCal3_S2(pose1, K)
         self.gtsamAssertEquals(camera.calibration(), K, 1e-9)
         self.gtsamAssertEquals(camera.pose(), pose1, 1e-9)
 
     def test_level2(self):
         # Create a level camera, looking in Y-direction
         pose2 = Pose2(0.4,0.3,math.pi/2.0)
-        camera = SimpleCamera.Level(K, pose2, 0.1)
+        camera = PinholeCameraCal3_S2.Level(K, pose2, 0.1)
 
         # expected
         x = Point3(1,0,0)
diff --git a/cython/gtsam/tests/test_dsf_map.py b/cython/gtsam/tests/test_dsf_map.py
index 6eb551034..73ddcb050 100644
--- a/cython/gtsam/tests/test_dsf_map.py
+++ b/cython/gtsam/tests/test_dsf_map.py
@@ -30,8 +30,10 @@ class TestDSFMap(GtsamTestCase):
         pair1 = gtsam.IndexPair(1, 18)
         self.assertEqual(key(dsf.find(pair1)), key(pair1))
         pair2 = gtsam.IndexPair(2, 2)
+        
+        # testing the merge feature of dsf
         dsf.merge(pair1, pair2)
-        self.assertTrue(dsf.find(pair1), dsf.find(pair1))
+        self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2)))
 
 
 if __name__ == '__main__':
diff --git a/cython/gtsam/tests/test_logging_optimizer.py b/cython/gtsam/tests/test_logging_optimizer.py
new file mode 100644
index 000000000..2560a72a2
--- /dev/null
+++ b/cython/gtsam/tests/test_logging_optimizer.py
@@ -0,0 +1,108 @@
+"""
+Unit tests for optimization that logs to comet.ml.
+Author: Jing Wu and Frank Dellaert
+"""
+# pylint: disable=invalid-name
+
+import sys
+if sys.version_info.major >= 3:
+    from io import StringIO
+else:
+    from cStringIO import StringIO
+
+import unittest
+from datetime import datetime
+
+import gtsam
+import numpy as np
+from gtsam import Rot3
+from gtsam.utils.test_case import GtsamTestCase
+
+from gtsam.utils.logging_optimizer import gtsam_optimize
+
+KEY = 0
+MODEL = gtsam.noiseModel_Unit.Create(3)
+
+
+class TestOptimizeComet(GtsamTestCase):
+    """Check correct logging to comet.ml."""
+
+    def setUp(self):
+        """Set up a small Karcher mean optimization example."""
+        # Grabbed from KarcherMeanFactor unit tests.
+        R = Rot3.Expmap(np.array([0.1, 0, 0]))
+        rotations = {R, R.inverse()}  # mean is the identity
+        self.expected = Rot3()
+
+        graph = gtsam.NonlinearFactorGraph()
+        for R in rotations:
+            graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
+        initial = gtsam.Values()
+        initial.insert(KEY, R)
+        self.params = gtsam.GaussNewtonParams()
+        self.optimizer = gtsam.GaussNewtonOptimizer(
+            graph, initial, self.params)
+
+        self.lmparams = gtsam.LevenbergMarquardtParams()
+        self.lmoptimizer = gtsam.LevenbergMarquardtOptimizer(
+            graph, initial, self.lmparams
+        )
+
+        # setup output capture
+        self.capturedOutput = StringIO()
+        sys.stdout = self.capturedOutput
+
+    def tearDown(self):
+        """Reset print capture."""
+        sys.stdout = sys.__stdout__
+
+    def test_simple_printing(self):
+        """Test with a simple hook."""
+
+        # Provide a hook that just prints
+        def hook(_, error):
+            print(error)
+
+        # Only thing we require from optimizer is an iterate method
+        gtsam_optimize(self.optimizer, self.params, hook)
+
+        # Check that optimizing yields the identity.
+        actual = self.optimizer.values()
+        self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
+
+    def test_lm_simple_printing(self):
+        """Make sure we are properly terminating LM"""
+        def hook(_, error):
+            print(error)
+
+        gtsam_optimize(self.lmoptimizer, self.lmparams, hook)
+
+        actual = self.lmoptimizer.values()
+        self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
+
+    @unittest.skip("Not a test we want run every time, as needs comet.ml account")
+    def test_comet(self):
+        """Test with a comet hook."""
+        from comet_ml import Experiment
+        comet = Experiment(project_name="Testing",
+                           auto_output_logging="native")
+        comet.log_dataset_info(name="Karcher", path="shonan")
+        comet.add_tag("GaussNewton")
+        comet.log_parameter("method", "GaussNewton")
+        time = datetime.now()
+        comet.set_name("GaussNewton-" + str(time.month) + "/" + str(time.day) + " "
+                       + str(time.hour)+":"+str(time.minute)+":"+str(time.second))
+
+        # I want to do some comet thing here
+        def hook(optimizer, error):
+            comet.log_metric("Karcher error",
+                             error, optimizer.iterations())
+
+        gtsam_optimize(self.optimizer, self.params, hook)
+        comet.end()
+
+        actual = self.optimizer.values()
+        self.gtsamAssertEquals(actual.atRot3(KEY), self.expected)
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/cython/gtsam/utils/logging_optimizer.py b/cython/gtsam/utils/logging_optimizer.py
new file mode 100644
index 000000000..3d9175951
--- /dev/null
+++ b/cython/gtsam/utils/logging_optimizer.py
@@ -0,0 +1,52 @@
+"""
+Optimization with logging via a hook.
+Author: Jing Wu and Frank Dellaert
+"""
+# pylint: disable=invalid-name
+
+from gtsam import NonlinearOptimizer, NonlinearOptimizerParams
+import gtsam
+
+
+def optimize(optimizer, check_convergence, hook):
+    """ Given an optimizer and a convergence check, iterate until convergence.
+        After each iteration, hook(optimizer, error) is called.
+        After the function, use values and errors to get the result.
+        Arguments:
+            optimizer (T): needs an iterate and an error function.
+            check_convergence: T * float * float -> bool
+            hook -- hook function to record the error
+    """
+    # the optimizer is created with default values which incur the error below
+    current_error = optimizer.error()
+    hook(optimizer, current_error)
+
+    # Iterative loop
+    while True:
+        # Do next iteration
+        optimizer.iterate()
+        new_error = optimizer.error()
+        hook(optimizer, new_error)
+        if check_convergence(optimizer, current_error, new_error):
+            return
+        current_error = new_error
+
+
+def gtsam_optimize(optimizer,
+                   params,
+                   hook):
+    """ Given an optimizer and params, iterate until convergence.
+        After each iteration, hook(optimizer) is called.
+        After the function, use values and errors to get the result.
+        Arguments:
+                optimizer {NonlinearOptimizer} -- Nonlinear optimizer
+                params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters
+                hook -- hook function to record the error
+    """
+    def check_convergence(optimizer, current_error, new_error):
+        return (optimizer.iterations() >= params.getMaxIterations()) or (
+            gtsam.checkConvergence(params.getRelativeErrorTol(), params.getAbsoluteErrorTol(), params.getErrorTol(),
+                                   current_error, new_error)) or (
+            isinstance(optimizer, gtsam.LevenbergMarquardtOptimizer) and optimizer.lambda_() > params.getlambdaUpperBound())
+    optimize(optimizer, check_convergence, hook)
+    return optimizer.values()
diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py
index 3871fa18c..b67444fc1 100644
--- a/cython/gtsam/utils/plot.py
+++ b/cython/gtsam/utils/plot.py
@@ -3,10 +3,109 @@
 import numpy as np
 import matplotlib.pyplot as plt
 from matplotlib import patches
+from mpl_toolkits.mplot3d import Axes3D
+
+import gtsam
+
+
+def set_axes_equal(fignum):
+    """
+    Make axes of 3D plot have equal scale so that spheres appear as spheres,
+    cubes as cubes, etc..  This is one possible solution to Matplotlib's
+    ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
+
+    Args:
+      fignum (int): An integer representing the figure number for Matplotlib.
+    """
+    fig = plt.figure(fignum)
+    ax = fig.gca(projection='3d')
+
+    limits = np.array([
+        ax.get_xlim3d(),
+        ax.get_ylim3d(),
+        ax.get_zlim3d(),
+    ])
+
+    origin = np.mean(limits, axis=1)
+    radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0]))
+
+    ax.set_xlim3d([origin[0] - radius, origin[0] + radius])
+    ax.set_ylim3d([origin[1] - radius, origin[1] + radius])
+    ax.set_zlim3d([origin[2] - radius, origin[2] + radius])
+
+
+def ellipsoid(xc, yc, zc, 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. 
+
+    Returns:
+        tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot.
+    """
+    u = np.linspace(0, 2*np.pi, n+1)
+    v = np.linspace(0, np.pi, n+1)
+    x = -rx * np.outer(np.cos(u), np.sin(v)).T
+    y = -ry * np.outer(np.sin(u), np.sin(v)).T
+    z = -rz * np.outer(np.ones_like(u), np.cos(v)).T
+
+    return x, y, z
+
+
+def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
+    """
+    Plots a Gaussian as an uncertainty ellipse
+
+    Based on Maybeck Vol 1, page 366
+    k=2.296 corresponds to 1 std, 68.26% of all probability
+    k=11.82 corresponds to 3 std, 99.74% of all probability
+
+    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.
+        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].
+    """
+    k = 11.82
+    U, S, _ = np.linalg.svd(P)
+
+    radii = k * np.sqrt(S)
+    radii = radii * scale
+    rx, ry, rz = radii
+
+    # generate data for "unrotated" ellipsoid
+    xc, yc, zc = ellipsoid(0, 0, 0, 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) + \
+        np.kron(U[:, 2:3], zc)
+    n = data.shape[1]
+    x = data[0:n, :] + origin[0]
+    y = data[n:2*n, :] + origin[1]
+    z = data[2*n:, :] + origin[2]
+
+    axes.plot_surface(x, y, z, alpha=alpha, cmap='hot')
 
 
 def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
-    """Plot a 2D pose on given axis 'axes' with given 'axis_length'."""
+    """
+    Plot a 2D pose on given axis `axes` with given `axis_length`.
+
+    Args:
+        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.
+    """
     # get rotation and translation (center)
     gRp = pose.rotation().matrix()  # rotation from pose to global
     t = pose.translation()
@@ -35,32 +134,66 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
                              np.rad2deg(angle), fill=False)
         axes.add_patch(e1)
 
+
 def plot_pose2(fignum, pose, axis_length=0.1, covariance=None):
-    """Plot a 2D pose on given figure with given 'axis_length'."""
+    """
+    Plot a 2D pose on given figure with given `axis_length`.
+
+    Args:
+        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.
+    """
     # get figure object
     fig = plt.figure(fignum)
     axes = fig.gca()
-    plot_pose2_on_axes(axes, pose, axis_length, covariance)
+    plot_pose2_on_axes(axes, pose, axis_length=axis_length,
+                       covariance=covariance)
 
 
-def plot_point3_on_axes(axes, point, linespec):
-    """Plot a 3D point on given axis 'axes' with given 'linespec'."""
+def plot_point3_on_axes(axes, point, linespec, P=None):
+    """
+    Plot a 3D point on given axis `axes` with given `linespec`.
+
+    Args:
+        axes (matplotlib.axes.Axes): Matplotlib axes.
+        point (gtsam.Point3): The point to be plotted.
+        linespec (string): String representing formatting options for Matplotlib.
+        P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+    """
     axes.plot([point.x()], [point.y()], [point.z()], linespec)
+    if P is not None:
+        plot_covariance_ellipse_3d(axes, point.vector(), P)
 
 
-def plot_point3(fignum, point, linespec):
-    """Plot a 3D point on given figure with given 'linespec'."""
+def plot_point3(fignum, point, linespec, P=None):
+    """
+    Plot a 3D point on given figure with given `linespec`.
+
+    Args:
+        fignum (int): Integer representing the figure number to use for plotting.
+        point (gtsam.Point3): The point to be plotted.
+        linespec (string): String representing formatting options for Matplotlib.
+        P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+    """
     fig = plt.figure(fignum)
     axes = fig.gca(projection='3d')
-    plot_point3_on_axes(axes, point, linespec)
+    plot_point3_on_axes(axes, point, linespec, P)
 
 
-def plot_3d_points(fignum, values, linespec, marginals=None):
+def plot_3d_points(fignum, values, linespec="g*", marginals=None):
     """
-    Plots the Point3s in 'values', with optional covariances.
+    Plots the Point3s in `values`, with optional covariances.
     Finds all the Point3 objects in the given Values object and plots them.
     If a Marginals object is given, this function will also plot marginal
     covariance ellipses for each point.
+
+    Args:
+        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.
+        covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
     """
 
     keys = values.keys()
@@ -68,23 +201,33 @@ def plot_3d_points(fignum, values, linespec, marginals=None):
     # Plot points and covariance matrices
     for i in range(keys.size()):
         try:
-            p = values.atPoint3(keys.at(i))
-            # if haveMarginals
-            #     P = marginals.marginalCovariance(key);
-            #     gtsam.plot_point3(p, linespec, P);
-            # else
-            plot_point3(fignum, p, linespec)
+            key = keys.at(i)
+            point = values.atPoint3(key)
+            if marginals is not None:
+                covariance = marginals.marginalCovariance(key)
+            else:
+                covariance = None
+
+            plot_point3(fignum, point, linespec, covariance)
+
         except RuntimeError:
             continue
             # I guess it's not a Point3
 
 
-def plot_pose3_on_axes(axes, pose, axis_length=0.1):
-    """Plot a 3D pose on given axis 'axes' with given 'axis_length'."""
+def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
+    """
+    Plot a 3D pose on given axis `axes` with given `axis_length`.
+
+    Args:
+        axes (matplotlib.axes.Axes): Matplotlib axes.
+        point (gtsam.Point3): The point to be plotted.
+        linespec (string): String representing formatting options for Matplotlib.
+        P (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
-    t = pose.translation()
-    origin = np.array([t.x(), t.y(), t.z()])
+    origin = pose.translation().vector()
 
     # draw the camera axes
     x_axis = origin + gRp[:, 0] * axis_length
@@ -100,17 +243,83 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1):
     axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-')
 
     # plot the covariance
-    # TODO (dellaert): make this work
-    # if (nargin>2) && (~isempty(P))
-    #     pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame
-    #     gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
-    #     gtsam.covarianceEllipse3D(origin,gPp);
-    # end
+    if P is not None:
+        # covariance matrix in pose coordinate frame
+        pPp = P[3:6, 3:6]
+        # convert the covariance matrix to global coordinate frame
+        gPp = gRp @ pPp @ gRp.T
+        plot_covariance_ellipse_3d(axes, origin, gPp)
 
 
-def plot_pose3(fignum, pose, axis_length=0.1):
-    """Plot a 3D pose on given figure with given 'axis_length'."""
+def plot_pose3(fignum, pose, axis_length=0.1, P=None):
+    """
+    Plot a 3D pose on given figure with given `axis_length`.
+
+    Args:
+        fignum (int): Integer representing the figure number to use for plotting.
+        pose (gtsam.Pose3): 3D pose to be plotted.
+        linespec (string): String representing formatting options for Matplotlib.
+        P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+    """
     # get figure object
     fig = plt.figure(fignum)
     axes = fig.gca(projection='3d')
-    plot_pose3_on_axes(axes, pose, axis_length)
+    plot_pose3_on_axes(axes, pose, P=P,
+                       axis_length=axis_length)
+
+
+def plot_trajectory(fignum, values, scale=1, marginals=None):
+    """
+    Plot a complete 3D trajectory using poses in `values`.
+
+    Args:
+        fignum (int): Integer representing the figure number to use for plotting.
+        values (gtsam.Values): Values dict containing the poses.
+        scale (float): Value to scale the poses by.
+        marginals (gtsam.Marginals): Marginalized probability values of the estimation.
+            Used to plot uncertainty bounds.
+    """
+    pose3Values = gtsam.utilities_allPose3s(values)
+    keys = gtsam.KeyVector(pose3Values.keys())
+    lastIndex = None
+
+    for i in range(keys.size()):
+        key = keys.at(i)
+        try:
+            pose = pose3Values.atPose3(key)
+        except:
+            print("Warning: no Pose3 at key: {0}".format(key))
+
+        if lastIndex is not None:
+            lastKey = keys.at(lastIndex)
+            try:
+                lastPose = pose3Values.atPose3(lastKey)
+            except:
+                print("Warning: no Pose3 at key: {0}".format(lastKey))
+                pass
+
+            if marginals:
+                covariance = marginals.marginalCovariance(lastKey)
+            else:
+                covariance = None
+
+            plot_pose3(fignum, lastPose,  P=covariance,
+                       axis_length=scale)
+
+        lastIndex = i
+
+    # Draw final pose
+    if lastIndex is not None:
+        lastKey = keys.at(lastIndex)
+        try:
+            lastPose = pose3Values.atPose3(lastKey)
+            if marginals:
+                covariance = marginals.marginalCovariance(lastKey)
+            else:
+                covariance = None
+
+            plot_pose3(fignum, lastPose, P=covariance,
+                       axis_length=scale)
+
+        except:
+            pass
diff --git a/cython/gtsam/utils/visual_data_generator.py b/cython/gtsam/utils/visual_data_generator.py
index 91194c565..f04588e70 100644
--- a/cython/gtsam/utils/visual_data_generator.py
+++ b/cython/gtsam/utils/visual_data_generator.py
@@ -1,8 +1,9 @@
 from __future__ import print_function
 
 import numpy as np
-from math import pi, cos, sin
+
 import gtsam
+from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3
 
 
 class Options:
@@ -10,7 +11,7 @@ class Options:
     Options to generate test scenario
     """
 
-    def __init__(self, triangle=False, nrCameras=3, K=gtsam.Cal3_S2()):
+    def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()):
         """
         Options to generate test scenario
         @param triangle: generate a triangle scene with 3 points if True, otherwise
@@ -27,10 +28,10 @@ class GroundTruth:
     Object holding generated ground-truth data
     """
 
-    def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
+    def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
         self.K = K
-        self.cameras = [gtsam.Pose3()] * nrCameras
-        self.points = [gtsam.Point3()] * nrPoints
+        self.cameras = [Pose3()] * nrCameras
+        self.points = [Point3()] * nrPoints
 
     def print_(self, s=""):
         print(s)
@@ -52,11 +53,11 @@ class Data:
     class NoiseModels:
         pass
 
-    def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
+    def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
         self.K = K
-        self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras]
+        self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras]
         self.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
-        self.odometry = [gtsam.Pose3()] * nrCameras
+        self.odometry = [Pose3()] * nrCameras
 
         # Set Noise parameters
         self.noiseModels = Data.NoiseModels()
@@ -73,7 +74,7 @@ class Data:
 def generate_data(options):
     """ Generate ground-truth and measurement data. """
 
-    K = gtsam.Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
+    K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
     nrPoints = 3 if options.triangle else 8
 
     truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints)
@@ -83,26 +84,26 @@ def generate_data(options):
     if options.triangle:  # Create a triangle target, just 3 points on a plane
         r = 10
         for j in range(len(truth.points)):
-            theta = j * 2 * pi / nrPoints
-            truth.points[j] = gtsam.Point3(r * cos(theta), r * sin(theta), 0)
+            theta = j * 2 * np.pi / nrPoints
+            truth.points[j] = Point3(r * np.cos(theta), r * np.sin(theta), 0)
     else:  # 3D landmarks as vertices of a cube
         truth.points = [
-            gtsam.Point3(10, 10, 10), gtsam.Point3(-10, 10, 10),
-            gtsam.Point3(-10, -10, 10), gtsam.Point3(10, -10, 10),
-            gtsam.Point3(10, 10, -10), gtsam.Point3(-10, 10, -10),
-            gtsam.Point3(-10, -10, -10), gtsam.Point3(10, -10, -10)
+            Point3(10, 10, 10), Point3(-10, 10, 10),
+            Point3(-10, -10, 10), Point3(10, -10, 10),
+            Point3(10, 10, -10), Point3(-10, 10, -10),
+            Point3(-10, -10, -10), Point3(10, -10, -10)
         ]
 
     # Create camera cameras on a circle around the triangle
     height = 10
     r = 40
     for i in range(options.nrCameras):
-        theta = i * 2 * pi / options.nrCameras
-        t = gtsam.Point3(r * cos(theta), r * sin(theta), height)
-        truth.cameras[i] = gtsam.SimpleCamera.Lookat(t,
-                                                     gtsam.Point3(),
-                                                     gtsam.Point3(0, 0, 1),
-                                                     truth.K)
+        theta = i * 2 * np.pi / options.nrCameras
+        t = Point3(r * np.cos(theta), r * np.sin(theta), height)
+        truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
+                                                       Point3(),
+                                                       Point3(0, 0, 1),
+                                                       truth.K)
         # Create measurements
         for j in range(nrPoints):
             # All landmarks seen in every frame
diff --git a/cython/gtsam_eigency/CMakeLists.txt b/cython/gtsam_eigency/CMakeLists.txt
index 77bead834..a0cf0fbde 100644
--- a/cython/gtsam_eigency/CMakeLists.txt
+++ b/cython/gtsam_eigency/CMakeLists.txt
@@ -4,11 +4,11 @@ include(GtsamCythonWrap)
 # so that the cython-generated header "conversions_api.h" can be found when cythonizing eigency's core
 # and eigency's cython pxd headers can be found when cythonizing gtsam
 file(COPY "." DESTINATION ".")
-set(OUTPUT_DIR "${PROJECT_BINARY_DIR}/cython/gtsam_eigency")
+set(OUTPUT_DIR "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency")
 set(EIGENCY_INCLUDE_DIR ${OUTPUT_DIR})
 
 # This is to make the build/cython/gtsam_eigency folder a python package
-configure_file(__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_eigency/__init__.py)
+configure_file(__init__.py.in ${OUTPUT_DIR}/__init__.py)
 
 # include eigency headers
 include_directories(${EIGENCY_INCLUDE_DIR})
@@ -16,8 +16,8 @@ include_directories(${EIGENCY_INCLUDE_DIR})
 # Cythonize and build eigency
 message(STATUS "Cythonize and build eigency")
 # Important trick: use "../gtsam_eigency/conversions.pyx" to let cython know that the conversions module is
-# a part of the gtsam_eigency package and generate the function call import_gtsam_igency__conversions()
-# in conversions_api.h correctly!!!
+# a part of the gtsam_eigency package and generate the function call import_gtsam_eigency__conversions()
+# in conversions_api.h correctly!
 cythonize(cythonize_eigency_conversions "../gtsam_eigency/conversions.pyx" "conversions"
   "${OUTPUT_DIR}" "${EIGENCY_INCLUDE_DIR}" "" "" "")
 cythonize(cythonize_eigency_core "../gtsam_eigency/core.pyx" "core"
@@ -37,13 +37,6 @@ add_dependencies(cythonize_eigency_core cythonize_eigency_conversions)
 add_custom_target(cythonize_eigency)
 add_dependencies(cythonize_eigency cythonize_eigency_conversions cythonize_eigency_core)
 
-# install
-install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
-        DESTINATION "${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}"
-        PATTERN "CMakeLists.txt" EXCLUDE
-        PATTERN "__init__.py.in" EXCLUDE)
-install(TARGETS cythonize_eigency_core cythonize_eigency_conversions
-        DESTINATION "${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}/gtsam_eigency")
-install(FILES ${OUTPUT_DIR}/conversions_api.h DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}/gtsam_eigency)
-configure_file(__init__.py.in ${OUTPUT_DIR}/__init__.py)
-install(FILES ${OUTPUT_DIR}/__init__.py DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}/gtsam_eigency)
+if(TARGET ${python_install_target})
+  add_dependencies(${python_install_target} cythonize_eigency)
+endif()
diff --git a/cython/gtsam_unstable/examples/TimeOfArrivalExample.py b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py
new file mode 100644
index 000000000..6ba06f0f2
--- /dev/null
+++ b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py
@@ -0,0 +1,128 @@
+"""
+GTSAM Copyright 2010-2020, 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
+
+Track a moving object "Time of Arrival" measurements at 4 microphones.
+Author: Frank Dellaert
+"""
+# pylint: disable=invalid-name, no-name-in-module
+
+from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
+                   NonlinearFactorGraph, Point3, Values, noiseModel_Isotropic)
+from gtsam_unstable import Event, TimeOfArrival, TOAFactor
+
+# units
+MS = 1e-3
+CM = 1e-2
+
+# Instantiate functor with speed of sound value
+TIME_OF_ARRIVAL = TimeOfArrival(330)
+
+
+def define_microphones():
+    """Create microphones."""
+    height = 0.5
+    microphones = []
+    microphones.append(Point3(0, 0, height))
+    microphones.append(Point3(403 * CM, 0, height))
+    microphones.append(Point3(403 * CM, 403 * CM, height))
+    microphones.append(Point3(0, 403 * CM, 2 * height))
+    return microphones
+
+
+def create_trajectory(n):
+    """Create ground truth trajectory."""
+    trajectory = []
+    timeOfEvent = 10
+    # simulate emitting a sound every second while moving on straight line
+    for key in range(n):
+        trajectory.append(
+            Event(timeOfEvent, 245 * CM + key * 1.0, 201.5 * CM, (212 - 45) * CM))
+        timeOfEvent += 1
+
+    return trajectory
+
+
+def simulate_one_toa(microphones, event):
+    """Simulate time-of-arrival measurements for a single event."""
+    return [TIME_OF_ARRIVAL.measure(event, microphones[i])
+            for i in range(len(microphones))]
+
+
+def simulate_toa(microphones, trajectory):
+    """Simulate time-of-arrival measurements for an entire trajectory."""
+    return [simulate_one_toa(microphones, event)
+            for event in trajectory]
+
+
+def create_graph(microphones, simulatedTOA):
+    """Create factor graph."""
+    graph = NonlinearFactorGraph()
+
+    # Create a noise model for the TOA error
+    model = noiseModel_Isotropic.Sigma(1, 0.5 * MS)
+
+    K = len(microphones)
+    key = 0
+    for toa in simulatedTOA:
+        for i in range(K):
+            factor = TOAFactor(key, microphones[i], toa[i], model)
+            graph.push_back(factor)
+        key += 1
+
+    return graph
+
+
+def create_initial_estimate(n):
+    """Create initial estimate for n events."""
+    initial = Values()
+    zero = Event()
+    for key in range(n):
+        TOAFactor.InsertEvent(key, zero, initial)
+    return initial
+
+
+def toa_example():
+    """Run example with 4 microphones and 5 events in a straight line."""
+    # Create microphones
+    microphones = define_microphones()
+    K = len(microphones)
+    for i in range(K):
+        print("mic {} = {}".format(i, microphones[i]))
+
+    # Create a ground truth trajectory
+    n = 5
+    groundTruth = create_trajectory(n)
+    for event in groundTruth:
+        print(event)
+
+    # Simulate time-of-arrival measurements
+    simulatedTOA = simulate_toa(microphones, groundTruth)
+    for key in range(n):
+        for i in range(K):
+            print("z_{}{} = {} ms".format(key, i, simulatedTOA[key][i] / MS))
+
+    # create factor graph
+    graph = create_graph(microphones, simulatedTOA)
+    print(graph.at(0))
+
+    # Create initial estimate
+    initial_estimate = create_initial_estimate(n)
+    print(initial_estimate)
+
+    # Optimize using Levenberg-Marquardt optimization.
+    params = LevenbergMarquardtParams()
+    params.setAbsoluteErrorTol(1e-10)
+    params.setVerbosityLM("SUMMARY")
+    optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params)
+    result = optimizer.optimize()
+    print("Final Result:\n", result)
+
+
+if __name__ == '__main__':
+    toa_example()
+    print("Example complete")
diff --git a/cython/requirements.txt b/cython/requirements.txt
index cd77b097d..8d3c7aeb4 100644
--- a/cython/requirements.txt
+++ b/cython/requirements.txt
@@ -1,3 +1,3 @@
 Cython>=0.25.2
 backports_abc>=0.5
-numpy>=1.12.0
+numpy>=1.11.0
diff --git a/cython/setup.py.in b/cython/setup.py.in
index c35e54079..98a05c9f6 100644
--- a/cython/setup.py.in
+++ b/cython/setup.py.in
@@ -5,17 +5,24 @@ try:
 except ImportError:
     from distutils.core import setup, find_packages
 
-if 'SETUP_PY_NO_CHECK' not in os.environ:
-    script_path = os.path.abspath(os.path.realpath(__file__))
-    install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}', 'setup.py')))
-    if script_path != install_path:
-        print('setup.py is being run from an unexpected location: "{}"'.format(script_path))
-        print('please run `make install` and run the script from there')
-        sys.exit(1)
-
-
 packages = find_packages()
 
+package_data = {
+    package:
+        [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.pyd')]
+        for package in packages
+}
+
+cython_install_requirements = open("${CYTHON_INSTALL_REQUIREMENTS_FILE}").readlines()
+
+install_requires = [line.strip() \
+    for line in cython_install_requirements \
+    if len(line.strip()) > 0 and not line.strip().startswith('#')
+]
+
+# Cleaner to read in the contents rather than copy them over.
+readme_contents = open("${PROJECT_SOURCE_DIR}/README.md").read()
+
 setup(
     name='gtsam',
     description='Georgia Tech Smoothing And Mapping library',
@@ -25,7 +32,9 @@ setup(
     author_email='frank.dellaert@gtsam.org',
     license='Simplified BSD license',
     keywords='slam sam robotics localization mapping optimization',
-    long_description='''${README_CONTENTS}''',
+    long_description=readme_contents,
+    long_description_content_type='text/markdown',
+    python_requires='>=2.7',
     # https://pypi.org/pypi?%3Aaction=list_classifiers
     classifiers=[
         'Development Status :: 5 - Production/Stable',
@@ -41,11 +50,6 @@ setup(
     ],
 
     packages=packages,
-    package_data={package:
-        [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.dll')]
-        for package in packages
-    },
-    install_requires=[line.strip() for line in '''
-${CYTHON_INSTALL_REQUIREMENTS}
-'''.splitlines() if len(line.strip()) > 0 and not line.strip().startswith('#')]
+    package_data=package_data,
+    install_requires=install_requires
 )
diff --git a/debian/README.md b/debian/README.md
deleted file mode 100644
index 74eb351cd..000000000
--- a/debian/README.md
+++ /dev/null
@@ -1,12 +0,0 @@
-# How to build a GTSAM debian package
-
-To use the ``debuild`` command, install the ``devscripts`` package
-
-    sudo apt install devscripts
-
-Change into the gtsam directory, then run:
-
-    debuild -us -uc -j4
-
-Adjust the ``-j4`` depending on how many CPUs you want to build on in
-parallel. 
diff --git a/debian/changelog b/debian/changelog
deleted file mode 100644
index ef5d5ab97..000000000
--- a/debian/changelog
+++ /dev/null
@@ -1,5 +0,0 @@
-gtsam (4.0.0-1berndpfrommer) bionic; urgency=medium
-
-  * initial release
-
- -- Bernd Pfrommer <bernd.pfrommer@gmail.com>  Wed, 18 Jul 2018 20:36:44 -0400
diff --git a/debian/compat b/debian/compat
deleted file mode 100644
index ec635144f..000000000
--- a/debian/compat
+++ /dev/null
@@ -1 +0,0 @@
-9
diff --git a/debian/control b/debian/control
deleted file mode 100644
index 9a37e4377..000000000
--- a/debian/control
+++ /dev/null
@@ -1,15 +0,0 @@
-Source: gtsam
-Section: libs
-Priority: optional
-Maintainer: Frank Dellaert <frank@cc.gatech.edu>
-Uploaders: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>, Bernd Pfrommer <bernd.pfrommer@gmail.com>
-Build-Depends: cmake, libboost-all-dev (>= 1.58), libeigen3-dev, libtbb-dev, debhelper (>=9)
-Standards-Version: 3.9.7
-Homepage: https://github.com/borglab/gtsam
-Vcs-Browser: https://github.com/borglab/gtsam
-
-Package: libgtsam-dev
-Architecture: any
-Depends: ${shlibs:Depends}, ${misc:Depends}
-Description: Georgia Tech Smoothing and Mapping Library
- gtsam: Georgia Tech Smoothing and Mapping library for SLAM type applications
diff --git a/debian/copyright b/debian/copyright
deleted file mode 100644
index c2f41d83d..000000000
--- a/debian/copyright
+++ /dev/null
@@ -1,15 +0,0 @@
-Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/
-Upstream-Name: gtsam
-Source: https://bitbucket.org/gtborg/gtsam.git
-
-Files: *
-Copyright: 2017, Frank Dellaert
-License: BSD
-
-Files: gtsam/3rdparty/CCOLAMD/*
-Copyright: 2005-2011, Univ. of Florida.  Authors: Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore.  Closely based on COLAMD by Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert. http://www.cise.ufl.edu/research/sparse
-License: GNU LESSER GENERAL PUBLIC LICENSE
-
-Files: gtsam/3rdparty/Eigen/*
-Copyright: 2017, Multiple Authors
-License: MPL2
diff --git a/debian/gtsam-docs.docs b/debian/gtsam-docs.docs
deleted file mode 100644
index e69de29bb..000000000
diff --git a/debian/rules b/debian/rules
deleted file mode 100755
index 6a8d21c00..000000000
--- a/debian/rules
+++ /dev/null
@@ -1,25 +0,0 @@
-#!/usr/bin/make -f
-# See debhelper(7) (uncomment to enable)
-# output every command that modifies files on the build system.
-export DH_VERBOSE = 1
-
-
-# see FEATURE AREAS in dpkg-buildflags(1)
-#export DEB_BUILD_MAINT_OPTIONS = hardening=+all
-
-# see ENVIRONMENT in dpkg-buildflags(1)
-# package maintainers to append CFLAGS
-#export DEB_CFLAGS_MAINT_APPEND  = -Wall -pedantic
-# package maintainers to append LDFLAGS
-#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed
-
-
-%:
-	dh $@ --parallel
-
-
-# dh_make generated override targets
-# This is example for Cmake (See https://bugs.debian.org/641051 )
-override_dh_auto_configure:
-	dh_auto_configure -- -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=/usr -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_WRAP=OFF -DGTSAM_BUILD_DOCS=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DGTSAM_INSTALL_GEOGRAPHICLIB=OFF -DGTSAM_BUILD_TYPE_POSTFIXES=OFF
-
diff --git a/debian/source/format b/debian/source/format
deleted file mode 100644
index 163aaf8d8..000000000
--- a/debian/source/format
+++ /dev/null
@@ -1 +0,0 @@
-3.0 (quilt)
diff --git a/doc/Code/OdometryExample.cpp b/doc/Code/OdometryExample.cpp
index 6e736e2d7..2befa9dc2 100644
--- a/doc/Code/OdometryExample.cpp
+++ b/doc/Code/OdometryExample.cpp
@@ -5,7 +5,7 @@ NonlinearFactorGraph graph;
 Pose2 priorMean(0.0, 0.0, 0.0);
 noiseModel::Diagonal::shared_ptr priorNoise =
   noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
+graph.addPrior(1, priorMean, priorNoise);
 
 // Add two odometry factors
 Pose2 odometry(2.0, 0.0, 0.0);
diff --git a/doc/Code/Pose2SLAMExample.cpp b/doc/Code/Pose2SLAMExample.cpp
index 1738aabc5..192ec11a4 100644
--- a/doc/Code/Pose2SLAMExample.cpp
+++ b/doc/Code/Pose2SLAMExample.cpp
@@ -1,7 +1,7 @@
 NonlinearFactorGraph graph;
 noiseModel::Diagonal::shared_ptr priorNoise =
   noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
+graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
 
 // Add odometry factors
 noiseModel::Diagonal::shared_ptr model =
diff --git a/doc/gtsam-coordinate-frames.lyx b/doc/gtsam-coordinate-frames.lyx
index 33d0dd977..cfb44696b 100644
--- a/doc/gtsam-coordinate-frames.lyx
+++ b/doc/gtsam-coordinate-frames.lyx
@@ -2291,15 +2291,11 @@ uncalibration
  used in the residual).
 \end_layout
 
-\begin_layout Standard
-\begin_inset Note Note
-status collapsed
-
 \begin_layout Section
 Noise models of prior factors
 \end_layout
 
-\begin_layout Plain Layout
+\begin_layout Standard
 The simplest way to describe noise models is by an example.
  Let's take a prior factor on a 3D pose 
 \begin_inset Formula $x\in\SE 3$
@@ -2353,7 +2349,7 @@ e\left(x\right)=\norm{h\left(x\right)}_{\Sigma}^{2}=h\left(x\right)^{\t}\Sigma^{
  useful answer out quickly ]
 \end_layout
 
-\begin_layout Plain Layout
+\begin_layout Standard
 The density induced by a noise model on the prior factor is Gaussian in
  the tangent space about the linearization point.
  Suppose that the pose is linearized at 
@@ -2431,7 +2427,7 @@ Here we see that the update
 .
 \end_layout
 
-\begin_layout Plain Layout
+\begin_layout Standard
 This means that to draw random pose samples, we actually draw random samples
  of 
 \begin_inset Formula $\delta x$
@@ -2456,7 +2452,7 @@ This means that to draw random pose samples, we actually draw random samples
 Noise models of between factors
 \end_layout
 
-\begin_layout Plain Layout
+\begin_layout Standard
 The noise model of a BetweenFactor is a bit more complicated.
  The unwhitened error is
 \begin_inset Formula 
@@ -2516,11 +2512,6 @@ e\left(\delta x_{1}\right) & \approx\norm{\log\left(z^{-1}\left(x_{1}\exp\delta
 \end_inset
 
 
-\end_layout
-
-\end_inset
-
-
 \end_layout
 
 \end_body
diff --git a/doc/gtsam-coordinate-frames.pdf b/doc/gtsam-coordinate-frames.pdf
index 3613ef0ac..77910b4cf 100644
Binary files a/doc/gtsam-coordinate-frames.pdf and b/doc/gtsam-coordinate-frames.pdf differ
diff --git a/doc/math.lyx b/doc/math.lyx
index b579d3ea4..2533822a7 100644
--- a/doc/math.lyx
+++ b/doc/math.lyx
@@ -1,7 +1,9 @@
-#LyX 2.1 created this file. For more info see http://www.lyx.org/
-\lyxformat 474
+#LyX 2.3 created this file. For more info see http://www.lyx.org/
+\lyxformat 544
 \begin_document
 \begin_header
+\save_transient_properties true
+\origin unavailable
 \textclass article
 \use_default_options false
 \begin_modules
@@ -14,16 +16,18 @@ theorems-ams-bytype
 \language_package default
 \inputencoding auto
 \fontencoding global
-\font_roman times
-\font_sans default
-\font_typewriter default
-\font_math auto
+\font_roman "times" "default"
+\font_sans "default" "default"
+\font_typewriter "default" "default"
+\font_math "auto" "auto"
 \font_default_family rmdefault
 \use_non_tex_fonts false
 \font_sc false
 \font_osf false
-\font_sf_scale 100
-\font_tt_scale 100
+\font_sf_scale 100 100
+\font_tt_scale 100 100
+\use_microtype false
+\use_dash_ligatures true
 \graphics default
 \default_output_format default
 \output_sync 0
@@ -53,6 +57,7 @@ theorems-ams-bytype
 \suppress_date false
 \justification true
 \use_refstyle 0
+\use_minted 0
 \index Index
 \shortcut idx
 \color #008000
@@ -65,7 +70,10 @@ theorems-ams-bytype
 \tocdepth 3
 \paragraph_separation indent
 \paragraph_indentation default
-\quotes_language english
+\is_math_indent 0
+\math_numbering_side default
+\quotes_style english
+\dynamic_quotes 0
 \papercolumns 1
 \papersides 1
 \paperpagestyle default
@@ -98,6 +106,11 @@ width "100col%"
 special "none"
 height "1in"
 height_special "totalheight"
+thickness "0.4pt"
+separation "3pt"
+shadowsize "4pt"
+framecolor "black"
+backgroundcolor "none"
 status collapsed
 
 \begin_layout Plain Layout
@@ -654,6 +667,7 @@ reference "eq:LocalBehavior"
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Spivak65book"
+literal "true"
 
 \end_inset
 
@@ -934,6 +948,7 @@ See
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Spivak65book"
+literal "true"
 
 \end_inset
 
@@ -1025,6 +1040,7 @@ See
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Spivak65book"
+literal "true"
 
 \end_inset
 
@@ -2209,6 +2225,7 @@ instantaneous velocity
 LatexCommand cite
 after "page 51 for rotations, page 419 for SE(3)"
 key "Murray94book"
+literal "true"
 
 \end_inset
 
@@ -2965,7 +2982,7 @@ B^{T} & I_{3}\end{array}\right]
 \begin_layout Subsection
 \begin_inset CommandInset label
 LatexCommand label
-name "sub:Pushforward-of-Between"
+name "subsec:Pushforward-of-Between"
 
 \end_inset
 
@@ -3419,6 +3436,7 @@ A retraction
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Absil07book"
+literal "true"
 
 \end_inset
 
@@ -3873,7 +3891,7 @@ BetweenFactor
 , derived in Section 
 \begin_inset CommandInset ref
 LatexCommand ref
-reference "sub:Pushforward-of-Between"
+reference "subsec:Pushforward-of-Between"
 
 \end_inset
 
@@ -4430,7 +4448,7 @@ In the case of
 \begin_inset Formula $\SOthree$
 \end_inset
 
- the vector space is  
+ the vector space is 
 \begin_inset Formula $\Rthree$
 \end_inset
 
@@ -4502,7 +4520,7 @@ reference "Th:InverseAction"
 \begin_layout Subsection
 \begin_inset CommandInset label
 LatexCommand label
-name "sub:3DAngularVelocities"
+name "subsec:3DAngularVelocities"
 
 \end_inset
 
@@ -4695,6 +4713,7 @@ Absil
 LatexCommand cite
 after "page 58"
 key "Absil07book"
+literal "true"
 
 \end_inset
 
@@ -5395,6 +5414,7 @@ While not a Lie group, we can define an exponential map, which is given
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Ma01ijcv"
+literal "true"
 
 \end_inset
 
@@ -5402,6 +5422,7 @@ key "Ma01ijcv"
 \begin_inset CommandInset href
 LatexCommand href
 name "http://stat.fsu.edu/~anuj/CVPR_Tutorial/Part2.pdf"
+literal "false"
 
 \end_inset
 
@@ -5605,6 +5626,7 @@ The exponential map uses
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Absil07book"
+literal "true"
 
 \end_inset
 
@@ -6293,7 +6315,7 @@ d^{c}=R_{w}^{c}\left(d^{w}+(t^{w}v^{w})v^{w}-t^{w}\right)
 \end_layout
 
 \begin_layout Section
-Line3 (Ocaml)
+Line3
 \end_layout
 
 \begin_layout Standard
@@ -6345,6 +6367,14 @@ R'=R(I+\Omega)
 
 \end_inset
 
+
+\end_layout
+
+\begin_layout Subsection
+Projecting Line3
+\end_layout
+
+\begin_layout Standard
 Projecting a line to 2D can be done easily, as both 
 \begin_inset Formula $v$
 \end_inset
@@ -6430,13 +6460,21 @@ or the
 
 \end_layout
 
+\begin_layout Subsection
+Action of 
+\begin_inset Formula $\SEthree$
+\end_inset
+
+ on the line
+\end_layout
+
 \begin_layout Standard
 Transforming a 3D line 
 \begin_inset Formula $(R,(a,b))$
 \end_inset
 
  from a world coordinate frame to a camera frame 
-\begin_inset Formula $(R_{w}^{c},t^{w})$
+\begin_inset Formula $T_{c}^{w}=(R_{c}^{w},t^{w})$
 \end_inset
 
  is done by
@@ -6466,17 +6504,115 @@ b'=b-R_{2}^{T}t^{w}
 
 \end_inset
 
-Again, we need to redo the derivatives, as R is incremented from the right.
- The first argument is incremented from the left, but the result is incremented
- on the right:
+where 
+\begin_inset Formula $R_{1}$
+\end_inset
+
+ and 
+\begin_inset Formula $R_{2}$
+\end_inset
+
+ are the columns of 
+\begin_inset Formula $R$
+\end_inset
+
+ , as before.
+ 
+\end_layout
+
+\begin_layout Standard
+To find the derivatives, the transformation of a line 
+\begin_inset Formula $l^{w}=(R,a,b)$
+\end_inset
+
+ from world coordinates to a camera coordinate frame 
+\begin_inset Formula $T_{c}^{w}$
+\end_inset
+
+, specified in world coordinates, can be written as a function 
+\begin_inset Formula $f:\SEthree\times L\rightarrow L$
+\end_inset
+
+, as given above, i.e., 
 \begin_inset Formula 
-\begin{eqnarray*}
-R'(I+\Omega')=(AB)(I+\Omega') & = & (I+\Skew{S\omega})AB\\
-I+\Omega' & = & (AB)^{T}(I+\Skew{S\omega})(AB)\\
-\Omega' & = & R'^{T}\Skew{S\omega}R'\\
-\Omega' & = & \Skew{R'^{T}S\omega}\\
-\omega' & = & R'^{T}S\omega
-\end{eqnarray*}
+\[
+f(T_{c}^{w},l^{w})=\left(\left(R_{c}^{w}\right)^{T}R,a-R_{1}^{T}t^{w},b-R_{2}^{T}t^{w}\right).
+\]
+
+\end_inset
+
+Let us find the Jacobian 
+\begin_inset Formula $J_{1}$
+\end_inset
+
+ of 
+\begin_inset Formula $f$
+\end_inset
+
+ with respect to the first argument 
+\begin_inset Formula $T_{c}^{w}$
+\end_inset
+
+, which should obey
+\begin_inset Formula 
+\begin{align*}
+f(T_{c}^{w}e^{\xihat},l^{w}) & \approx f(T_{c}^{w},l^{w})+J_{1}\xi
+\end{align*}
+
+\end_inset
+
+Note that
+\begin_inset Formula 
+\[
+T_{c}^{w}e^{\xihat}\approx\left[\begin{array}{cc}
+R_{c}^{w}\left(I_{3}+\Skew{\omega}\right) & t^{w}+R_{c}^{w}v\\
+0 & 1
+\end{array}\right]
+\]
+
+\end_inset
+
+Let's write this out separately for each of 
+\begin_inset Formula $R,a,b$
+\end_inset
+
+:
+\begin_inset Formula 
+\begin{align*}
+\left(R_{c}^{w}\left(I_{3}+\Skew{\omega}\right)\right)^{T}R & \approx\left(R_{c}^{w}\right)^{T}R(I+\left[J_{R\omega}\omega\right]_{\times})\\
+a-R_{1}^{T}\left(t^{w}+R_{c}^{w}v\right) & \approx a-R_{1}^{T}t^{w}+J_{av}v\\
+b-R_{2}^{T}\left(t^{w}+R_{c}^{w}v\right) & \approx b-R_{2}^{T}t^{w}+J_{bv}v
+\end{align*}
+
+\end_inset
+
+Simplifying, we get:
+\begin_inset Formula 
+\begin{align*}
+-\Skew{\omega}R' & \approx R'\left[J_{R\omega}\omega\right]_{\times}\\
+-R_{1}^{T}R_{c}^{w} & \approx J_{av}\\
+-R_{2}^{T}R_{c}^{w} & \approx J_{bv}
+\end{align*}
+
+\end_inset
+
+which gives the expressions for 
+\begin_inset Formula $J_{av}$
+\end_inset
+
+ and 
+\begin_inset Formula $J_{bv}$
+\end_inset
+
+.
+ The top line can be further simplified:
+\begin_inset Formula 
+\begin{align*}
+-\Skew{\omega}R' & \approx R'\left[J_{R\omega}\omega\right]_{\times}\\
+-R'^{T}\Skew{\omega}R' & \approx\left[J_{R\omega}\omega\right]_{\times}\\
+-\Skew{R'^{T}\omega} & \approx\left[J_{R\omega}\omega\right]_{\times}\\
+-R'^{T} & \approx J_{R\omega}
+\end{align*}
 
 \end_inset
 
@@ -6687,6 +6823,7 @@ Spivak
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Spivak65book"
+literal "true"
 
 \end_inset
 
@@ -6795,6 +6932,7 @@ The following is adapted from Appendix A in
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Murray94book"
+literal "true"
 
 \end_inset
 
@@ -6924,6 +7062,7 @@ might be
 LatexCommand cite
 after "page 45"
 key "Hall00book"
+literal "true"
 
 \end_inset
 
diff --git a/doc/math.pdf b/doc/math.pdf
index ab8f1f69a..8dc7270f1 100644
Binary files a/doc/math.pdf and b/doc/math.pdf differ
diff --git a/doc/robust.pdf b/doc/robust.pdf
new file mode 100644
index 000000000..67b853f44
Binary files /dev/null and b/doc/robust.pdf differ
diff --git a/docker/README.md b/docker/README.md
new file mode 100644
index 000000000..0c136f94c
--- /dev/null
+++ b/docker/README.md
@@ -0,0 +1,21 @@
+# Instructions
+
+Build all docker images, in order:
+
+```bash
+(cd ubuntu-boost-tbb && ./build.sh)
+(cd ubuntu-gtsam && ./build.sh)
+(cd ubuntu-gtsam-python && ./build.sh)
+(cd ubuntu-gtsam-python-vnc && ./build.sh)
+```
+
+Then launch with: 
+
+    docker run -p 5900:5900 dellaert/ubuntu-gtsam-python-vnc:bionic
+
+Then open a remote VNC X client, for example:
+
+    sudo apt-get install tigervnc-viewer
+    xtigervncviewer :5900
+
+
diff --git a/docker/ubuntu-boost-tbb-eigen3/Dockerfile b/docker/ubuntu-boost-tbb-eigen3/Dockerfile
deleted file mode 100644
index 33aa1ab96..000000000
--- a/docker/ubuntu-boost-tbb-eigen3/Dockerfile
+++ /dev/null
@@ -1,18 +0,0 @@
-# Get the base Ubuntu image from Docker Hub
-FROM ubuntu:bionic
-
-# Update apps on the base image
-RUN apt-get -y update && apt-get install -y
-
-# Install C++
-RUN apt-get -y install build-essential 
-
-# Install boost and cmake
-RUN apt-get -y install libboost-all-dev cmake
-
-# Install TBB
-RUN apt-get -y install libtbb-dev
-
-# Install latest Eigen
-RUN apt-get install -y libeigen3-dev
-
diff --git a/docker/ubuntu-boost-tbb/Dockerfile b/docker/ubuntu-boost-tbb/Dockerfile
new file mode 100644
index 000000000..9f6eea3b8
--- /dev/null
+++ b/docker/ubuntu-boost-tbb/Dockerfile
@@ -0,0 +1,19 @@
+# Basic Ubuntu 18.04 image with Boost and TBB installed. To be used for building further downstream packages.
+
+# Get the base Ubuntu image from Docker Hub
+FROM ubuntu:bionic
+
+# Disable GUI prompts
+ENV DEBIAN_FRONTEND noninteractive
+
+# Update apps on the base image
+RUN apt-get -y update && apt-get -y install
+
+# Install C++
+RUN apt-get -y install build-essential  apt-utils
+
+# Install boost and cmake
+RUN apt-get -y install libboost-all-dev cmake
+
+# Install TBB
+RUN apt-get -y install libtbb-dev
diff --git a/docker/ubuntu-boost-tbb/build.sh b/docker/ubuntu-boost-tbb/build.sh
new file mode 100755
index 000000000..2dac4c3db
--- /dev/null
+++ b/docker/ubuntu-boost-tbb/build.sh
@@ -0,0 +1,3 @@
+# Build command for Docker image
+# TODO(dellaert): use docker compose and/or cmake
+docker build --no-cache -t dellaert/ubuntu-boost-tbb:bionic .
diff --git a/docker/ubuntu-gtsam-python-vnc/Dockerfile b/docker/ubuntu-gtsam-python-vnc/Dockerfile
new file mode 100644
index 000000000..61ecd9b9a
--- /dev/null
+++ b/docker/ubuntu-gtsam-python-vnc/Dockerfile
@@ -0,0 +1,20 @@
+# This GTSAM image connects to the host X-server via VNC to provide a Graphical User Interface for interaction.
+
+# Get the base Ubuntu/GTSAM image from Docker Hub
+FROM dellaert/ubuntu-gtsam-python:bionic
+
+# Things needed to get a python GUI
+ENV DEBIAN_FRONTEND noninteractive
+RUN apt install -y python-tk
+RUN python3 -m pip install matplotlib
+
+# Install a VNC X-server, Frame buffer, and windows manager
+RUN apt install -y x11vnc xvfb fluxbox
+
+# Finally, install wmctrl needed for bootstrap script
+RUN apt install -y wmctrl
+
+# Copy bootstrap script and make sure it runs
+COPY bootstrap.sh /
+
+CMD '/bootstrap.sh'
diff --git a/docker/ubuntu-gtsam-python-vnc/bootstrap.sh b/docker/ubuntu-gtsam-python-vnc/bootstrap.sh
new file mode 100755
index 000000000..21356138f
--- /dev/null
+++ b/docker/ubuntu-gtsam-python-vnc/bootstrap.sh
@@ -0,0 +1,111 @@
+#!/bin/bash
+
+# Based on: http://www.richud.com/wiki/Ubuntu_Fluxbox_GUI_with_x11vnc_and_Xvfb
+
+main() {
+    log_i "Starting xvfb virtual display..."
+    launch_xvfb
+    log_i "Starting window manager..."
+    launch_window_manager
+    log_i "Starting VNC server..."
+    run_vnc_server
+}
+
+launch_xvfb() {
+    local xvfbLockFilePath="/tmp/.X1-lock"
+    if [ -f "${xvfbLockFilePath}" ]
+    then
+        log_i "Removing xvfb lock file '${xvfbLockFilePath}'..."
+        if ! rm -v "${xvfbLockFilePath}"
+        then
+            log_e "Failed to remove xvfb lock file"
+            exit 1
+        fi
+    fi
+
+    # Set defaults if the user did not specify envs.
+    export DISPLAY=${XVFB_DISPLAY:-:1}
+    local screen=${XVFB_SCREEN:-0}
+    local resolution=${XVFB_RESOLUTION:-1280x960x24}
+    local timeout=${XVFB_TIMEOUT:-5}
+
+    # Start and wait for either Xvfb to be fully up or we hit the timeout.
+    Xvfb ${DISPLAY} -screen ${screen} ${resolution} &
+    local loopCount=0
+    until xdpyinfo -display ${DISPLAY} > /dev/null 2>&1
+    do
+        loopCount=$((loopCount+1))
+        sleep 1
+        if [ ${loopCount} -gt ${timeout} ]
+        then
+            log_e "xvfb failed to start"
+            exit 1
+        fi
+    done
+}
+
+launch_window_manager() {
+    local timeout=${XVFB_TIMEOUT:-5}
+
+    # Start and wait for either fluxbox to be fully up or we hit the timeout.
+    fluxbox &
+    local loopCount=0
+    until wmctrl -m > /dev/null 2>&1
+    do
+        loopCount=$((loopCount+1))
+        sleep 1
+        if [ ${loopCount} -gt ${timeout} ]
+        then
+            log_e "fluxbox failed to start"
+            exit 1
+        fi
+    done
+}
+
+run_vnc_server() {
+    local passwordArgument='-nopw'
+
+    if [ -n "${VNC_SERVER_PASSWORD}" ]
+    then
+        local passwordFilePath="${HOME}/.x11vnc.pass"
+        if ! x11vnc -storepasswd "${VNC_SERVER_PASSWORD}" "${passwordFilePath}"
+        then
+            log_e "Failed to store x11vnc password"
+            exit 1
+        fi
+        passwordArgument=-"-rfbauth ${passwordFilePath}"
+        log_i "The VNC server will ask for a password"
+    else
+        log_w "The VNC server will NOT ask for a password"
+    fi
+
+    x11vnc -ncache 10 -ncache_cr -display ${DISPLAY} -forever ${passwordArgument} &
+    wait $!
+}
+
+log_i() {
+    log "[INFO] ${@}"
+}
+
+log_w() {
+    log "[WARN] ${@}"
+}
+
+log_e() {
+    log "[ERROR] ${@}"
+}
+
+log() {
+    echo "[$(date '+%Y-%m-%d %H:%M:%S')] ${@}"
+}
+
+control_c() {
+    echo ""
+    exit
+}
+
+trap control_c SIGINT SIGTERM SIGHUP
+
+main
+
+exit
diff --git a/docker/ubuntu-gtsam-python-vnc/build.sh b/docker/ubuntu-gtsam-python-vnc/build.sh
new file mode 100755
index 000000000..8d280252f
--- /dev/null
+++ b/docker/ubuntu-gtsam-python-vnc/build.sh
@@ -0,0 +1,4 @@
+# Build command for Docker image
+# TODO(dellaert): use docker compose and/or cmake
+# Needs to be run in docker/ubuntu-gtsam-python-vnc directory
+docker build -t dellaert/ubuntu-gtsam-python-vnc:bionic .
diff --git a/docker/ubuntu-gtsam-python-vnc/vnc.sh b/docker/ubuntu-gtsam-python-vnc/vnc.sh
new file mode 100755
index 000000000..c0ab692c6
--- /dev/null
+++ b/docker/ubuntu-gtsam-python-vnc/vnc.sh
@@ -0,0 +1,5 @@
+# After running this script, connect VNC client to 0.0.0.0:5900
+docker run -it \
+    --workdir="/usr/src/gtsam" \
+    -p 5900:5900 \
+    dellaert/ubuntu-gtsam-python-vnc:bionic
\ No newline at end of file
diff --git a/docker/ubuntu-gtsam-python/Dockerfile b/docker/ubuntu-gtsam-python/Dockerfile
new file mode 100644
index 000000000..c733ceb19
--- /dev/null
+++ b/docker/ubuntu-gtsam-python/Dockerfile
@@ -0,0 +1,31 @@
+#  GTSAM Ubuntu image with Python wrapper support.
+
+# Get the base Ubuntu/GTSAM image from Docker Hub
+FROM dellaert/ubuntu-gtsam:bionic
+
+# Install pip
+RUN apt-get install -y python3-pip python3-dev
+
+# Install python wrapper requirements
+RUN python3 -m pip install -U -r /usr/src/gtsam/cython/requirements.txt
+
+# Run cmake again, now with cython toolbox on
+WORKDIR /usr/src/gtsam/build
+RUN cmake \
+    -DCMAKE_BUILD_TYPE=Release \
+    -DGTSAM_WITH_EIGEN_MKL=OFF \
+    -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
+    -DGTSAM_BUILD_TIMING_ALWAYS=OFF \
+    -DGTSAM_BUILD_TESTS=OFF \
+    -DGTSAM_INSTALL_CYTHON_TOOLBOX=ON \
+    -DGTSAM_PYTHON_VERSION=3\
+    ..
+
+# Build again, as ubuntu-gtsam image cleaned
+RUN make -j4 install && make clean
+
+# Needed to run python wrapper:
+RUN echo 'export PYTHONPATH=/usr/local/cython/:$PYTHONPATH' >> /root/.bashrc
+
+# Run bash
+CMD ["bash"]
diff --git a/docker/ubuntu-gtsam-python/build.sh b/docker/ubuntu-gtsam-python/build.sh
new file mode 100755
index 000000000..1696f6c61
--- /dev/null
+++ b/docker/ubuntu-gtsam-python/build.sh
@@ -0,0 +1,3 @@
+# Build command for Docker image
+# TODO(dellaert): use docker compose and/or cmake
+docker build --no-cache -t dellaert/ubuntu-gtsam-python:bionic .
diff --git a/docker/ubuntu-gtsam/Dockerfile b/docker/ubuntu-gtsam/Dockerfile
new file mode 100644
index 000000000..187c76314
--- /dev/null
+++ b/docker/ubuntu-gtsam/Dockerfile
@@ -0,0 +1,36 @@
+# Ubuntu image with GTSAM installed. Configured with  Boost and TBB support.
+
+# Get the base Ubuntu image from Docker Hub
+FROM dellaert/ubuntu-boost-tbb:bionic
+
+# Install git
+RUN apt-get update && \
+    apt-get install -y git
+
+# Install compiler
+RUN apt-get install -y build-essential
+
+# Clone GTSAM (develop branch)
+WORKDIR /usr/src/
+RUN git clone --single-branch --branch develop https://github.com/borglab/gtsam.git
+
+# Change to build directory. Will be created automatically.
+WORKDIR /usr/src/gtsam/build
+# Run cmake
+RUN cmake \
+    -DCMAKE_BUILD_TYPE=Release \
+    -DGTSAM_WITH_EIGEN_MKL=OFF \
+    -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
+    -DGTSAM_BUILD_TIMING_ALWAYS=OFF \
+    -DGTSAM_BUILD_TESTS=OFF \
+    -DGTSAM_INSTALL_CYTHON_TOOLBOX=OFF \
+    ..
+
+# Build
+RUN make -j4 install && make clean
+
+# Needed to link with GTSAM
+RUN echo 'export LD_LIBRARY_PATH=/usr/local/lib:LD_LIBRARY_PATH' >> /root/.bashrc
+
+# Run bash
+CMD ["bash"]
diff --git a/docker/ubuntu-gtsam/build.sh b/docker/ubuntu-gtsam/build.sh
new file mode 100755
index 000000000..bf545e9c2
--- /dev/null
+++ b/docker/ubuntu-gtsam/build.sh
@@ -0,0 +1,3 @@
+# Build command for Docker image
+# TODO(dellaert): use docker compose and/or cmake
+docker build --no-cache -t dellaert/ubuntu-gtsam:bionic .
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index 7251c2b6f..476f4ae21 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -1,7 +1,4 @@
 set (excluded_examples
-    DiscreteBayesNet_FG.cpp
-    UGM_chain.cpp
-    UGM_small.cpp
     elaboratePoint2KalmanFilter.cpp
 )
 
diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp
index e3408b67b..b12418098 100644
--- a/examples/CameraResectioning.cpp
+++ b/examples/CameraResectioning.cpp
@@ -18,7 +18,8 @@
 
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <boost/make_shared.hpp>
 
 using namespace gtsam;
@@ -47,7 +48,7 @@ public:
   /// evaluate the error
   virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
       boost::none) const {
-    SimpleCamera camera(pose, *K_);
+    PinholeCamera<Cal3_S2> camera(pose, *K_);
     return camera.project(P_, H, boost::none, boost::none) - p_;
   }
 };
diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp
index 082b4c0f9..aa0bde8f6 100644
--- a/examples/CreateSFMExampleData.cpp
+++ b/examples/CreateSFMExampleData.cpp
@@ -31,19 +31,19 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
         Cal3Bundler()) {
 
   // Class that will gather all data
-  SfM_data data;
+  SfmData data;
 
   // Create two cameras
   Rot3 aRb = Rot3::Yaw(M_PI_2);
   Point3 aTb(0.1, 0, 0);
   Pose3 identity, aPb(aRb, aTb);
-  data.cameras.push_back(SfM_Camera(pose1, K));
-  data.cameras.push_back(SfM_Camera(pose2, K));
+  data.cameras.push_back(SfmCamera(pose1, K));
+  data.cameras.push_back(SfmCamera(pose2, K));
 
   for(const Point3& p: P) {
 
     // Create the track
-    SfM_Track track;
+    SfmTrack track;
     track.p = p;
     track.r = 1;
     track.g = 1;
diff --git a/examples/Data/Klaus3.g2o b/examples/Data/Klaus3.g2o
new file mode 100644
index 000000000..4c7233fa7
--- /dev/null
+++ b/examples/Data/Klaus3.g2o
@@ -0,0 +1,6 @@
+VERTEX_SE3:QUAT 0 -3.865747774038187 0.06639337702667497 -0.16064874691945374 0.024595211709139555 0.49179523413089893 -0.06279232989379242 0.8680954132776109
+VERTEX_SE3:QUAT 1 -3.614793159814815 0.04774490041587656 -0.2837650367985949 0.00991721787943912 0.4854918961891193 -0.042343290945895576 0.8731588132957809
+VERTEX_SE3:QUAT 2 -3.255096913553434 0.013296754286114112 -0.5339792269680574 -0.027851108010665374 0.585478168397957 -0.05088341463532465 0.8086102325762403
+EDGE_SE3:QUAT 0 1 0.2509546142233723 -0.01864847661079841 -0.12311628987914114 -0.022048798853273946 -0.01796327847857683 0.010210006313668573 0.9995433591728293 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
+EDGE_SE3:QUAT 0 2 0.6106508604847534 -0.05309662274056086 -0.3733304800486037 -0.054972994022992064 0.10432547598981769 -0.02221474884651081 0.9927742290779572 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
+EDGE_SE3:QUAT 1 2 0.3596962462613811 -0.03444814612976245 -0.25021419016946256 -0.03174661848656213 0.11646825423134777 -0.02951742735854383 0.9922479626852876 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
diff --git a/examples/Data/pose3Localizationexample.txt b/examples/Data/pose3Localizationexample.txt
new file mode 100644
index 000000000..a35005aa2
--- /dev/null
+++ b/examples/Data/pose3Localizationexample.txt
@@ -0,0 +1,9 @@
+VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
+VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
+VERTEX_SE3:QUAT 2 1.993500 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446
+VERTEX_SE3:QUAT 3 2.004291 1.024305 0.018047 0.331798 -0.200659 0.919323 0.067024
+VERTEX_SE3:QUAT 4 0.999908 1.055073 0.020212 -0.035697 -0.462490 0.445933 0.765488
+EDGE_SE3:QUAT 0 1   1.001367 0.015390 0.004948   0.190253 0.283162 -0.392318 0.854230   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
+EDGE_SE3:QUAT 1 2   0.523923 0.776654 0.326659   0.311512 0.656877 -0.678505 0.105373   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
+EDGE_SE3:QUAT 2 3   0.910927 0.055169 -0.411761   0.595795 -0.561677 0.079353 0.568551   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
+EDGE_SE3:QUAT 3 4   0.775288 0.228798 -0.596923   -0.592077 0.303380 -0.513226 0.542221   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
diff --git a/examples/Data/toyExample.g2o b/examples/Data/toyExample.g2o
new file mode 100755
index 000000000..5ff1ba74a
--- /dev/null
+++ b/examples/Data/toyExample.g2o
@@ -0,0 +1,11 @@
+VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
+VERTEX_SE3:QUAT 1 0 0 0 0 0 0 1
+VERTEX_SE3:QUAT 2 0 0 0 0.00499994 0.00499994 0.00499994 0.999963
+VERTEX_SE3:QUAT 3 0 0 0 -0.00499994 -0.00499994 -0.00499994 0.999963
+VERTEX_SE3:QUAT 4 0 0 0 0.00499994 0.00499994 0.00499994 0.999963
+EDGE_SE3:QUAT 1 2 1 2 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
+EDGE_SE3:QUAT 2 3 -3.26795e-07 1 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
+EDGE_SE3:QUAT 3 4 1 1 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
+EDGE_SE3:QUAT 3 1 6.9282e-07 2 0 0 0 1 1.73205e-07 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
+EDGE_SE3:QUAT 1 4 -1 1 0 0 0 -0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
+EDGE_SE3:QUAT 0 1 0 0 0 0 0 0 1 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
diff --git a/examples/DiscreteBayesNetExample.cpp b/examples/DiscreteBayesNetExample.cpp
new file mode 100644
index 000000000..5dca116c3
--- /dev/null
+++ b/examples/DiscreteBayesNetExample.cpp
@@ -0,0 +1,83 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  DiscreteBayesNetExample.cpp
+ * @brief   Discrete Bayes Net example with famous Asia Bayes Network
+ * @author  Frank Dellaert
+ * @date  JULY 10, 2020
+ */
+
+#include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/discrete/DiscreteMarginals.h>
+#include <gtsam/inference/BayesNet.h>
+
+#include <iomanip>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char **argv) {
+  DiscreteBayesNet asia;
+  DiscreteKey Asia(0, 2), Smoking(4, 2), Tuberculosis(3, 2), LungCancer(6, 2),
+      Bronchitis(7, 2), Either(5, 2), XRay(2, 2), Dyspnea(1, 2);
+  asia.add(Asia % "99/1");
+  asia.add(Smoking % "50/50");
+
+  asia.add(Tuberculosis | Asia = "99/1 95/5");
+  asia.add(LungCancer | Smoking = "99/1 90/10");
+  asia.add(Bronchitis | Smoking = "70/30 40/60");
+
+  asia.add((Either | Tuberculosis, LungCancer) = "F T T T");
+
+  asia.add(XRay | Either = "95/5 2/98");
+  asia.add((Dyspnea | Either, Bronchitis) = "9/1 2/8 3/7 1/9");
+
+  // print
+  vector<string> pretty = {"Asia",    "Dyspnea", "XRay",       "Tuberculosis",
+                           "Smoking", "Either",  "LungCancer", "Bronchitis"};
+  auto formatter = [pretty](Key key) { return pretty[key]; };
+  asia.print("Asia", formatter);
+
+  // Convert to factor graph
+  DiscreteFactorGraph fg(asia);
+
+  // Create solver and eliminate
+  Ordering ordering;
+  ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7);
+  DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering);
+
+  // solve
+  DiscreteFactor::sharedValues mpe = chordal->optimize();
+  GTSAM_PRINT(*mpe);
+
+  // We can also build a Bayes tree (directed junction tree).
+  // The elimination order above will do fine:
+  auto bayesTree = fg.eliminateMultifrontal(ordering);
+  bayesTree->print("bayesTree", formatter);
+
+  // add evidence, we were in Asia and we have dyspnea
+  fg.add(Asia, "0 1");
+  fg.add(Dyspnea, "0 1");
+
+  // solve again, now with evidence
+  DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering);
+  DiscreteFactor::sharedValues mpe2 = chordal2->optimize();
+  GTSAM_PRINT(*mpe2);
+
+  // We can also sample from it
+  cout << "\n10 samples:" << endl;
+  for (size_t i = 0; i < 10; i++) {
+    DiscreteFactor::sharedValues sample = chordal2->sample();
+    GTSAM_PRINT(*sample);
+  }
+  return 0;
+}
diff --git a/examples/DiscreteBayesNet_FG.cpp b/examples/DiscreteBayesNet_FG.cpp
index 6eb08c12e..121df4bef 100644
--- a/examples/DiscreteBayesNet_FG.cpp
+++ b/examples/DiscreteBayesNet_FG.cpp
@@ -15,105 +15,106 @@
  * @author  Abhijit
  * @date  Jun 4, 2012
  *
- * We use the famous Rain/Cloudy/Sprinkler Example of [Russell & Norvig, 2009, p529]
- * You may be familiar with other graphical model packages like BNT (available
- * at http://bnt.googlecode.com/svn/trunk/docs/usage.html) where this is used as an
- * example. The following demo is same as that in the above link, except that
- * everything is using GTSAM.
+ * We use the famous Rain/Cloudy/Sprinkler Example of [Russell & Norvig, 2009,
+ * p529] You may be familiar with other graphical model packages like BNT
+ * (available at http://bnt.googlecode.com/svn/trunk/docs/usage.html) where this
+ * is used as an example. The following demo is same as that in the above link,
+ * except that everything is using GTSAM.
  */
 
 #include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/discrete/DiscreteSequentialSolver.h>
+#include <gtsam/discrete/DiscreteMarginals.h>
+
 #include <iomanip>
 
 using namespace std;
 using namespace gtsam;
 
 int main(int argc, char **argv) {
+  // Define keys and a print function
+  Key C(1), S(2), R(3), W(4);
+  auto print = [=](DiscreteFactor::sharedValues values) {
+    cout << boolalpha << "Cloudy = " << static_cast<bool>((*values)[C])
+         << "  Sprinkler = " << static_cast<bool>((*values)[S])
+         << "  Rain = " << boolalpha << static_cast<bool>((*values)[R])
+         << "  WetGrass = " << static_cast<bool>((*values)[W]) << endl;
+  };
 
   // We assume binary state variables
   // we have 0 == "False" and 1 == "True"
   const size_t nrStates = 2;
 
   // define variables
-  DiscreteKey Cloudy(1, nrStates), Sprinkler(2, nrStates), Rain(3, nrStates),
-      WetGrass(4, nrStates);
+  DiscreteKey Cloudy(C, nrStates), Sprinkler(S, nrStates), Rain(R, nrStates),
+      WetGrass(W, nrStates);
 
   // create Factor Graph of the bayes net
   DiscreteFactorGraph graph;
 
   // add factors
-  graph.add(Cloudy, "0.5 0.5"); //P(Cloudy)
-  graph.add(Cloudy & Sprinkler, "0.5 0.5 0.9 0.1"); //P(Sprinkler | Cloudy)
-  graph.add(Cloudy & Rain, "0.8 0.2 0.2 0.8"); //P(Rain | Cloudy)
+  graph.add(Cloudy, "0.5 0.5");                      // P(Cloudy)
+  graph.add(Cloudy & Sprinkler, "0.5 0.5 0.9 0.1");  // P(Sprinkler | Cloudy)
+  graph.add(Cloudy & Rain, "0.8 0.2 0.2 0.8");       // P(Rain | Cloudy)
   graph.add(Sprinkler & Rain & WetGrass,
-      "1 0 0.1 0.9 0.1 0.9 0.001 0.99"); //P(WetGrass | Sprinkler, Rain)
+            "1 0 0.1 0.9 0.1 0.9 0.001 0.99");  // P(WetGrass | Sprinkler, Rain)
 
-  // Alternatively we can also create a DiscreteBayesNet, add DiscreteConditional
-  // factors and create a FactorGraph from it. (See testDiscreteBayesNet.cpp)
+  // Alternatively we can also create a DiscreteBayesNet, add
+  // DiscreteConditional factors and create a FactorGraph from it. (See
+  // testDiscreteBayesNet.cpp)
 
   // Since this is a relatively small distribution, we can as well print
   // the whole distribution..
   cout << "Distribution of Example: " << endl;
   cout << setw(11) << "Cloudy(C)" << setw(14) << "Sprinkler(S)" << setw(10)
-      << "Rain(R)" << setw(14) << "WetGrass(W)" << setw(15) << "P(C,S,R,W)"
-      << endl;
+       << "Rain(R)" << setw(14) << "WetGrass(W)" << setw(15) << "P(C,S,R,W)"
+       << endl;
   for (size_t a = 0; a < nrStates; a++)
     for (size_t m = 0; m < nrStates; m++)
       for (size_t h = 0; h < nrStates; h++)
         for (size_t c = 0; c < nrStates; c++) {
           DiscreteFactor::Values values;
-          values[Cloudy.first] = c;
-          values[Sprinkler.first] = h;
-          values[Rain.first] = m;
-          values[WetGrass.first] = a;
+          values[C] = c;
+          values[S] = h;
+          values[R] = m;
+          values[W] = a;
           double prodPot = graph(values);
-          cout << boolalpha << setw(8) << (bool) c << setw(14)
-              << (bool) h << setw(12) << (bool) m << setw(13)
-              << (bool) a << setw(16) << prodPot << endl;
+          cout << setw(8) << static_cast<bool>(c) << setw(14)
+               << static_cast<bool>(h) << setw(12) << static_cast<bool>(m)
+               << setw(13) << static_cast<bool>(a) << setw(16) << prodPot
+               << endl;
         }
 
-
   // "Most Probable Explanation", i.e., configuration with largest value
-  DiscreteSequentialSolver solver(graph);
-  DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
-  cout <<"\nMost Probable Explanation (MPE):" << endl;
-  cout << boolalpha << "Cloudy = " << (bool)(*optimalDecoding)[Cloudy.first]
-                  << "  Sprinkler = " << (bool)(*optimalDecoding)[Sprinkler.first]
-                  << "  Rain = " << boolalpha << (bool)(*optimalDecoding)[Rain.first]
-                  << "  WetGrass = " << (bool)(*optimalDecoding)[WetGrass.first]<< endl;
+  DiscreteFactor::sharedValues mpe = graph.eliminateSequential()->optimize();
+  cout << "\nMost Probable Explanation (MPE):" << endl;
+  print(mpe);
 
+  // "Inference" We show an inference query like: probability that the Sprinkler
+  // was on; given that the grass is wet i.e. P( S | C=0) = ?
 
-  // "Inference" We show an inference query like: probability that the Sprinkler was on;
-  // given that the grass is wet i.e. P( S | W=1) =?
-  cout << "\nInference Query: Probability of Sprinkler being on given Grass is Wet" << endl;
+  // add evidence that it is not Cloudy
+  graph.add(Cloudy, "1 0");
 
-  // Method 1: we can compute the joint marginal P(S,W) and from that we can compute
-  // P(S | W=1) = P(S,W=1)/P(W=1) We do this in following three steps..
+  // solve again, now with evidence
+  DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
+  DiscreteFactor::sharedValues mpe_with_evidence = chordal->optimize();
 
-  //Step1: Compute P(S,W)
-  DiscreteFactorGraph jointFG;
-  jointFG = *solver.jointFactorGraph(DiscreteKeys(Sprinkler & WetGrass).indices());
-  DecisionTreeFactor probSW = jointFG.product();
+  cout << "\nMPE given C=0:" << endl;
+  print(mpe_with_evidence);
 
-  //Step2: Compute P(W)
-  DiscreteFactor::shared_ptr probW = solver.marginalFactor(WetGrass.first);
-
-  //Step3: Computer P(S | W=1) = P(S,W=1)/P(W=1)
-  DiscreteFactor::Values values;
-  values[WetGrass.first] = 1;
-
-  //print P(S=0|W=1)
-  values[Sprinkler.first] = 0;
-  cout << "P(S=0|W=1) = " << probSW(values)/(*probW)(values) << endl;
-
-  //print P(S=1|W=1)
-  values[Sprinkler.first] = 1;
-  cout << "P(S=1|W=1) = " << probSW(values)/(*probW)(values) << endl;
-
-  // TODO: Method 2 : One way is to modify the factor graph to
-  // incorporate the evidence node and compute the marginal
-  // TODO: graph.addEvidence(Cloudy,0);
+  // we can also calculate arbitrary marginals:
+  DiscreteMarginals marginals(graph);
+  cout << "\nP(S=1|C=0):" << marginals.marginalProbabilities(Sprinkler)[1]
+       << endl;
+  cout << "\nP(R=0|C=0):" << marginals.marginalProbabilities(Rain)[0] << endl;
+  cout << "\nP(W=1|C=0):" << marginals.marginalProbabilities(WetGrass)[1]
+       << endl;
 
+  // We can also sample from it
+  cout << "\n10 samples:" << endl;
+  for (size_t i = 0; i < 10; i++) {
+    DiscreteFactor::sharedValues sample = chordal->sample();
+    print(sample);
+  }
   return 0;
 }
diff --git a/examples/FisheyeExample.cpp b/examples/FisheyeExample.cpp
new file mode 100644
index 000000000..223149299
--- /dev/null
+++ b/examples/FisheyeExample.cpp
@@ -0,0 +1,130 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    FisheyeExample.cpp
+ * @brief   A visualSLAM example for the structure-from-motion problem on a
+ * simulated dataset. This version uses a fisheye camera model and a GaussNewton
+ * solver to solve the graph in one batch
+ * @author  ghaggin
+ * @Date    Apr 9,2020
+ */
+
+/**
+ * A structure-from-motion example with landmarks
+ *  - The landmarks form a 10 meter cube
+ *  - The robot rotates around the landmarks, always facing towards the cube
+ */
+
+// For loading the data
+#include "SFMdata.h"
+
+// Camera observations of landmarks will be stored as Point2 (x, y).
+#include <gtsam/geometry/Point2.h>
+
+// Each variable in the system (poses and landmarks) must be identified with a
+// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols
+// (X1, X2, L1). Here we will use Symbols
+#include <gtsam/inference/Symbol.h>
+
+// Use GaussNewtonOptimizer to solve graph
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+
+// In GTSAM, measurement functions are represented as 'factors'. Several common
+// factors have been provided with the library for solving robotics/SLAM/Bundle
+// Adjustment problems. Here we will use Projection factors to model the
+// camera's landmark observations. Also, we will initialize the robot at some
+// location using a Prior factor.
+#include <gtsam/geometry/Cal3Fisheye.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/ProjectionFactor.h>
+
+#include <fstream>
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+using symbol_shorthand::L;  // for landmarks
+using symbol_shorthand::X;  // for poses
+
+/* ************************************************************************* */
+int main(int argc, char *argv[]) {
+  // Define the camera calibration parameters
+  auto K = boost::make_shared<Cal3Fisheye>(
+      278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035,
+      0.020727425669427896, -0.012786476702685545, 0.0025242267320687625);
+
+  // Define the camera observation noise model, 1 pixel stddev
+  auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
+
+  // Create the set of ground-truth landmarks
+  const vector<Point3> points = createPoints();
+
+  // Create the set of ground-truth poses
+  const vector<Pose3> poses = createPoses();
+
+  // Create a Factor Graph and Values to hold the new data
+  NonlinearFactorGraph graph;
+  Values initialEstimate;
+
+  // Add a prior on pose x0, 0.1 rad on roll,pitch,yaw, and 30cm std on x,y,z
+  auto posePrior = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
+  graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], posePrior);
+
+  // Add a prior on landmark l0
+  auto pointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
+  graph.emplace_shared<PriorFactor<Point3>>(L(0), points[0], pointPrior);
+
+  // Add initial guesses to all observed landmarks
+  // Intentionally initialize the variables off from the ground truth
+  static const Point3 kDeltaPoint(-0.25, 0.20, 0.15);
+  for (size_t j = 0; j < points.size(); ++j)
+    initialEstimate.insert<Point3>(L(j), points[j] + kDeltaPoint);
+
+  // Loop over the poses, adding the observations to the graph
+  for (size_t i = 0; i < poses.size(); ++i) {
+    // Add factors for each landmark observation
+    for (size_t j = 0; j < points.size(); ++j) {
+      PinholeCamera<Cal3Fisheye> camera(poses[i], *K);
+      Point2 measurement = camera.project(points[j]);
+      graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3Fisheye>>(
+          measurement, measurementNoise, X(i), L(j), K);
+    }
+
+    // Add an initial guess for the current pose
+    // Intentionally initialize the variables off from the ground truth
+    static const Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
+                                  Point3(0.05, -0.10, 0.20));
+    initialEstimate.insert(X(i), poses[i] * kDeltaPose);
+  }
+
+  GaussNewtonParams params;
+  params.setVerbosity("TERMINATION");
+  params.maxIterations = 10000;
+
+  std::cout << "Optimizing the factor graph" << std::endl;
+  GaussNewtonOptimizer optimizer(graph, initialEstimate, params);
+  Values result = optimizer.optimize();
+  std::cout << "Optimization complete" << std::endl;
+
+  std::cout << "initial error=" << graph.error(initialEstimate) << std::endl;
+  std::cout << "final error=" << graph.error(result) << std::endl;
+
+  std::ofstream os("examples/vio_batch.dot");
+  graph.saveGraph(os, result);
+
+  return 0;
+}
+/* ************************************************************************* */
diff --git a/examples/HMMExample.cpp b/examples/HMMExample.cpp
new file mode 100644
index 000000000..ee861e381
--- /dev/null
+++ b/examples/HMMExample.cpp
@@ -0,0 +1,94 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, 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  DiscreteBayesNetExample.cpp
+ * @brief   Hidden Markov Model example, discrete.
+ * @author  Frank Dellaert
+ * @date  July 12, 2020
+ */
+
+#include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/discrete/DiscreteMarginals.h>
+#include <gtsam/inference/BayesNet.h>
+
+#include <iomanip>
+#include <sstream>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char **argv) {
+  const int nrNodes = 4;
+  const size_t nrStates = 3;
+
+  // Define variables as well as ordering
+  Ordering ordering;
+  vector<DiscreteKey> keys;
+  for (int k = 0; k < nrNodes; k++) {
+    DiscreteKey key_i(k, nrStates);
+    keys.push_back(key_i);
+    ordering.emplace_back(k);
+  }
+
+  // Create HMM as a DiscreteBayesNet
+  DiscreteBayesNet hmm;
+
+  // Define backbone
+  const string transition = "8/1/1 1/8/1 1/1/8";
+  for (int k = 1; k < nrNodes; k++) {
+    hmm.add(keys[k] | keys[k - 1] = transition);
+  }
+
+  // Add some measurements, not needed for all time steps!
+  hmm.add(keys[0] % "7/2/1");
+  hmm.add(keys[1] % "1/9/0");
+  hmm.add(keys.back() % "5/4/1");
+
+  // print
+  hmm.print("HMM");
+
+  // Convert to factor graph
+  DiscreteFactorGraph factorGraph(hmm);
+
+  // Create solver and eliminate
+  // This will create a DAG ordered with arrow of time reversed
+  DiscreteBayesNet::shared_ptr chordal =
+      factorGraph.eliminateSequential(ordering);
+  chordal->print("Eliminated");
+
+  // solve
+  DiscreteFactor::sharedValues mpe = chordal->optimize();
+  GTSAM_PRINT(*mpe);
+
+  // We can also sample from it
+  cout << "\n10 samples:" << endl;
+  for (size_t k = 0; k < 10; k++) {
+    DiscreteFactor::sharedValues sample = chordal->sample();
+    GTSAM_PRINT(*sample);
+  }
+
+  // Or compute the marginals. This re-eliminates the FG into a Bayes tree
+  cout << "\nComputing Node Marginals .." << endl;
+  DiscreteMarginals marginals(factorGraph);
+  for (int k = 0; k < nrNodes; k++) {
+    Vector margProbs = marginals.marginalProbabilities(keys[k]);
+    stringstream ss;
+    ss << "marginal " << k;
+    print(margProbs, ss.str());
+  }
+
+  // TODO(frank): put in the glue to have DiscreteMarginals produce *arbitrary*
+  // joints efficiently, by the Bayes tree shortcut magic. All the code is there
+  // but it's not yet connected.
+
+  return 0;
+}
diff --git a/examples/IMUKittiExampleGPS.cpp b/examples/IMUKittiExampleGPS.cpp
new file mode 100644
index 000000000..f1d89b47a
--- /dev/null
+++ b/examples/IMUKittiExampleGPS.cpp
@@ -0,0 +1,359 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 IMUKittiExampleGPS
+ * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE
+ * @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ Electronics
+ */
+
+// GTSAM related includes.
+#include <gtsam/navigation/CombinedImuFactor.h>
+#include <gtsam/navigation/GPSFactor.h>
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/ISAM2Params.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/inference/Symbol.h>
+
+#include <cstring>
+#include <fstream>
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)
+using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
+using symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
+
+struct KittiCalibration {
+    double body_ptx;
+    double body_pty;
+    double body_ptz;
+    double body_prx;
+    double body_pry;
+    double body_prz;
+    double accelerometer_sigma;
+    double gyroscope_sigma;
+    double integration_sigma;
+    double accelerometer_bias_sigma;
+    double gyroscope_bias_sigma;
+    double average_delta_t;
+};
+
+struct ImuMeasurement {
+    double time;
+    double dt;
+    Vector3 accelerometer;
+    Vector3 gyroscope;  // omega
+};
+
+struct GpsMeasurement {
+    double time;
+    Vector3 position;  // x,y,z
+};
+
+const string output_filename = "IMUKittiExampleGPSResults.csv";
+
+void loadKittiData(KittiCalibration& kitti_calibration,
+                   vector<ImuMeasurement>& imu_measurements,
+                   vector<GpsMeasurement>& gps_measurements) {
+    string line;
+
+    // Read IMU metadata and compute relative sensor pose transforms
+    // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma
+    // AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT
+    string imu_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt");
+    ifstream imu_metadata(imu_metadata_file.c_str());
+
+    printf("-- Reading sensor metadata\n");
+
+    getline(imu_metadata, line, '\n');  // ignore the first line
+
+    // Load Kitti calibration
+    getline(imu_metadata, line, '\n');
+    sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
+           &kitti_calibration.body_ptx,
+           &kitti_calibration.body_pty,
+           &kitti_calibration.body_ptz,
+           &kitti_calibration.body_prx,
+           &kitti_calibration.body_pry,
+           &kitti_calibration.body_prz,
+           &kitti_calibration.accelerometer_sigma,
+           &kitti_calibration.gyroscope_sigma,
+           &kitti_calibration.integration_sigma,
+           &kitti_calibration.accelerometer_bias_sigma,
+           &kitti_calibration.gyroscope_bias_sigma,
+           &kitti_calibration.average_delta_t);
+    printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
+           kitti_calibration.body_ptx,
+           kitti_calibration.body_pty,
+           kitti_calibration.body_ptz,
+           kitti_calibration.body_prx,
+           kitti_calibration.body_pry,
+           kitti_calibration.body_prz,
+           kitti_calibration.accelerometer_sigma,
+           kitti_calibration.gyroscope_sigma,
+           kitti_calibration.integration_sigma,
+           kitti_calibration.accelerometer_bias_sigma,
+           kitti_calibration.gyroscope_bias_sigma,
+           kitti_calibration.average_delta_t);
+
+    // Read IMU data
+    // Time dt accelX accelY accelZ omegaX omegaY omegaZ
+    string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt");
+    printf("-- Reading IMU measurements from file\n");
+    {
+        ifstream imu_data(imu_data_file.c_str());
+        getline(imu_data, line, '\n');  // ignore the first line
+
+        double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, gyro_y = 0, gyro_z = 0;
+        while (!imu_data.eof()) {
+            getline(imu_data, line, '\n');
+            sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf",
+                   &time, &dt,
+                   &acc_x, &acc_y, &acc_z,
+                   &gyro_x, &gyro_y, &gyro_z);
+
+            ImuMeasurement measurement;
+            measurement.time = time;
+            measurement.dt = dt;
+            measurement.accelerometer = Vector3(acc_x, acc_y, acc_z);
+            measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z);
+            imu_measurements.push_back(measurement);
+        }
+    }
+
+    // Read GPS data
+    // Time,X,Y,Z
+    string gps_data_file = findExampleDataFile("KittiGps_converted.txt");
+    printf("-- Reading GPS measurements from file\n");
+    {
+        ifstream gps_data(gps_data_file.c_str());
+        getline(gps_data, line, '\n');  // ignore the first line
+
+        double time = 0, gps_x = 0, gps_y = 0, gps_z = 0;
+        while (!gps_data.eof()) {
+            getline(gps_data, line, '\n');
+            sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z);
+
+            GpsMeasurement measurement;
+            measurement.time = time;
+            measurement.position = Vector3(gps_x, gps_y, gps_z);
+            gps_measurements.push_back(measurement);
+        }
+    }
+}
+
+int main(int argc, char* argv[]) {
+    KittiCalibration kitti_calibration;
+    vector<ImuMeasurement> imu_measurements;
+    vector<GpsMeasurement> gps_measurements;
+    loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
+
+    Vector6 BodyP = (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, kitti_calibration.body_ptz,
+                                  kitti_calibration.body_prx, kitti_calibration.body_pry, kitti_calibration.body_prz)
+                    .finished();
+    auto body_T_imu = Pose3::Expmap(BodyP);
+    if (!body_T_imu.equals(Pose3(), 1e-5)) {
+        printf("Currently only support IMUinBody is identity, i.e. IMU and body frame are the same");
+        exit(-1);
+    }
+
+    // Configure different variables
+    // double t_offset = gps_measurements[0].time;
+    size_t first_gps_pose = 1;
+    size_t gps_skip = 10;  // Skip this many GPS measurements each time
+    double g = 9.8;
+    auto w_coriolis = Vector3::Zero();  // zero vector
+
+    // Configure noise models
+    auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0),
+                                                                          Vector3::Constant(1.0/0.07))
+                                                            .finished());
+
+    // Set initial conditions for the estimated trajectory
+    // initial pose is the reference frame (navigation frame)
+    auto current_pose_global = Pose3(Rot3(), gps_measurements[first_gps_pose].position);
+    // the vehicle is stationary at the beginning at position 0,0,0
+    Vector3 current_velocity_global = Vector3::Zero();
+    auto current_bias = imuBias::ConstantBias();  // init with zero bias
+
+    auto sigma_init_x = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0),
+                                                                       Vector3::Constant(1.0))
+                                                         .finished());
+    auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0));
+    auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100),
+                                                                   Vector3::Constant(5.00e-05))
+                                                     .finished());
+
+    // Set IMU preintegration parameters
+    Matrix33 measured_acc_cov = I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
+    Matrix33 measured_omega_cov = I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);
+    // error committed in integrating position from velocities
+    Matrix33 integration_error_cov = I_3x3 * pow(kitti_calibration.integration_sigma, 2);
+
+    auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g);
+    imu_params->accelerometerCovariance = measured_acc_cov;     // acc white noise in continuous
+    imu_params->integrationCovariance = integration_error_cov;  // integration uncertainty continuous
+    imu_params->gyroscopeCovariance = measured_omega_cov;       // gyro white noise in continuous
+    imu_params->omegaCoriolis = w_coriolis;
+
+    std::shared_ptr<PreintegratedImuMeasurements> current_summarized_measurement = nullptr;
+
+    // Set ISAM2 parameters and create ISAM2 solver object
+    ISAM2Params isam_params;
+    isam_params.factorization = ISAM2Params::CHOLESKY;
+    isam_params.relinearizeSkip = 10;
+
+    ISAM2 isam(isam_params);
+
+    // Create the factor graph and values object that will store new factors and values to add to the incremental graph
+    NonlinearFactorGraph new_factors;
+    Values new_values;  // values storing the initial estimates of new nodes in the factor graph
+
+    /// Main loop:
+    /// (1) we read the measurements
+    /// (2) we create the corresponding factors in the graph
+    /// (3) we solve the graph to obtain and optimal estimate of robot trajectory
+    printf("-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n");
+    size_t j = 0;
+    for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
+        // At each non=IMU measurement we initialize a new node in the graph
+        auto current_pose_key = X(i);
+        auto current_vel_key = V(i);
+        auto current_bias_key = B(i);
+        double t = gps_measurements[i].time;
+
+        if (i == first_gps_pose) {
+            // Create initial estimate and prior on initial pose, velocity, and biases
+            new_values.insert(current_pose_key, current_pose_global);
+            new_values.insert(current_vel_key, current_velocity_global);
+            new_values.insert(current_bias_key, current_bias);
+            new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, current_pose_global, sigma_init_x);
+            new_factors.emplace_shared<PriorFactor<Vector3>>(current_vel_key, current_velocity_global, sigma_init_v);
+            new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(current_bias_key, current_bias, sigma_init_b);
+        } else {
+            double t_previous = gps_measurements[i-1].time;
+
+            // Summarize IMU data between the previous GPS measurement and now
+            current_summarized_measurement = std::make_shared<PreintegratedImuMeasurements>(imu_params, current_bias);
+            static size_t included_imu_measurement_count = 0;
+            while (j < imu_measurements.size() && imu_measurements[j].time <= t) {
+                if (imu_measurements[j].time >= t_previous) {
+                    current_summarized_measurement->integrateMeasurement(imu_measurements[j].accelerometer,
+                                                                         imu_measurements[j].gyroscope,
+                                                                         imu_measurements[j].dt);
+                    included_imu_measurement_count++;
+                }
+                j++;
+            }
+
+            // Create IMU factor
+            auto previous_pose_key = X(i-1);
+            auto previous_vel_key = V(i-1);
+            auto previous_bias_key = B(i-1);
+
+            new_factors.emplace_shared<ImuFactor>(previous_pose_key, previous_vel_key,
+                                                  current_pose_key, current_vel_key,
+                                                  previous_bias_key, *current_summarized_measurement);
+
+            // Bias evolution as given in the IMU metadata
+            auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector6() <<
+                   Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma),
+                   Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.gyroscope_bias_sigma))
+                 .finished());
+            new_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(previous_bias_key,
+                                                                             current_bias_key,
+                                                                             imuBias::ConstantBias(),
+                                                                             sigma_between_b);
+
+            // Create GPS factor
+            auto gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position);
+            if ((i % gps_skip) == 0) {
+                new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, gps_pose, noise_model_gps);
+                new_values.insert(current_pose_key, gps_pose);
+
+                printf("################ POSE INCLUDED AT TIME %lf ################\n", t);
+                gps_pose.translation().print();
+                printf("\n\n");
+            } else {
+                new_values.insert(current_pose_key, current_pose_global);
+            }
+
+            // Add initial values for velocity and bias based on the previous estimates
+            new_values.insert(current_vel_key, current_velocity_global);
+            new_values.insert(current_bias_key, current_bias);
+
+            // Update solver
+            // =======================================================================
+            // We accumulate 2*GPSskip GPS measurements before updating the solver at
+            // first so that the heading becomes observable.
+            if (i > (first_gps_pose + 2*gps_skip)) {
+                printf("################ NEW FACTORS AT TIME %lf ################\n", t);
+                new_factors.print();
+
+                isam.update(new_factors, new_values);
+
+                // Reset the newFactors and newValues list
+                new_factors.resize(0);
+                new_values.clear();
+
+                // Extract the result/current estimates
+                Values result = isam.calculateEstimate();
+
+                current_pose_global = result.at<Pose3>(current_pose_key);
+                current_velocity_global = result.at<Vector3>(current_vel_key);
+                current_bias = result.at<imuBias::ConstantBias>(current_bias_key);
+
+                printf("\n################ POSE AT TIME %lf ################\n", t);
+                current_pose_global.print();
+                printf("\n\n");
+            }
+        }
+    }
+
+    // Save results to file
+    printf("\nWriting results to file...\n");
+    FILE* fp_out = fopen(output_filename.c_str(), "w+");
+    fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n");
+
+    Values result = isam.calculateEstimate();
+    for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
+        auto pose_key = X(i);
+        auto vel_key = V(i);
+        auto bias_key = B(i);
+
+        auto pose = result.at<Pose3>(pose_key);
+        auto velocity = result.at<Vector3>(vel_key);
+        auto bias = result.at<imuBias::ConstantBias>(bias_key);
+
+        auto pose_quat = pose.rotation().toQuaternion();
+        auto gps = gps_measurements[i].position;
+
+        cout << "State at #" << i << endl;
+        cout << "Pose:" << endl << pose << endl;
+        cout << "Velocity:" << endl << velocity << endl;
+        cout << "Bias:" << endl << bias << endl;
+
+        fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
+                gps_measurements[i].time,
+                pose.x(), pose.y(), pose.z(),
+                pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(),
+                gps(0), gps(1), gps(2));
+    }
+
+    fclose(fp_out);
+}
diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp
index 8331ade04..7dbd8323b 100644
--- a/examples/ISAM2Example_SmartFactor.cpp
+++ b/examples/ISAM2Example_SmartFactor.cpp
@@ -4,7 +4,8 @@
  * @author Alexander (pumaking on BitBucket)
  */
 
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/SmartProjectionPoseFactor.h>
@@ -27,7 +28,7 @@ int main(int argc, char* argv[]) {
       noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
 
   Vector6 sigmas;
-  sigmas << Vector3::Constant(0.3), Vector3::Constant(0.1);
+  sigmas << Vector3::Constant(0.1), Vector3::Constant(0.3);
   auto noise = noiseModel::Diagonal::Sigmas(sigmas);
 
   ISAM2Params parameters;
@@ -56,7 +57,7 @@ int main(int argc, char* argv[]) {
   vector<Pose3> poses = {pose1, pose2, pose3, pose4, pose5};
 
   // Add first pose
-  graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], noise);
+  graph.addPrior(X(0), poses[0], noise);
   initialEstimate.insert(X(0), poses[0].compose(delta));
 
   // Create smart factor with measurement from first pose only
@@ -70,7 +71,7 @@ int main(int argc, char* argv[]) {
     cout << "i = " << i << endl;
 
     // Add prior on new pose
-    graph.emplace_shared<PriorFactor<Pose3>>(X(i), poses[i], noise);
+    graph.addPrior(X(i), poses[i], noise);
     initialEstimate.insert(X(i), poses[i].compose(delta));
 
     // "Simulate" measurement from this pose
diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp
index f39e9f4eb..c0a102d11 100644
--- a/examples/ISAM2_SmartFactorStereo_IMU.cpp
+++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp
@@ -129,18 +129,16 @@ int main(int argc, char* argv[]) {
   // Pose prior - at identity
   auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
-  graph.emplace_shared<PriorFactor<Pose3>>(X(1), Pose3::identity(),
-                                           priorPoseNoise);
+  graph.addPrior(X(1), Pose3::identity(), priorPoseNoise);
   initialEstimate.insert(X(0), Pose3::identity());
 
   // Bias prior
-  graph.add(PriorFactor<imuBias::ConstantBias>(B(1), imu.priorImuBias,
-                                               imu.biasNoiseModel));
+  graph.addPrior(B(1), imu.priorImuBias,
+                                               imu.biasNoiseModel);
   initialEstimate.insert(B(0), imu.priorImuBias);
 
   // Velocity prior - assume stationary
-  graph.add(
-      PriorFactor<Vector3>(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel));
+  graph.addPrior(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel);
   initialEstimate.insert(V(0), Vector3(0, 0, 0));
 
   int lastFrame = 1;
diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp
index 25a6adf51..cb3650421 100644
--- a/examples/ImuFactorExample2.cpp
+++ b/examples/ImuFactorExample2.cpp
@@ -1,12 +1,12 @@
 
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/navigation/Scenario.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 
 #include <vector>
 
@@ -34,7 +34,7 @@ int main(int argc, char* argv[]) {
   double radius = 30;
   const Point3 up(0, 0, 1), target(0, 0, 0);
   const Point3 position(radius, 0, 0);
-  const SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
+  const auto camera = PinholeCamera<Cal3_S2>::Lookat(position, target, up);
   const auto pose_0 = camera.pose();
 
   // Now, create a constant-twist scenario that makes the camera orbit the
@@ -60,21 +60,18 @@ int main(int argc, char* argv[]) {
   // 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z.
   auto noise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
-  newgraph.push_back(PriorFactor<Pose3>(X(0), pose_0, noise));
+  newgraph.addPrior(X(0), pose_0, noise);
 
   // Add imu priors
   Key biasKey = Symbol('b', 0);
   auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1));
-  PriorFactor<imuBias::ConstantBias> biasprior(biasKey, imuBias::ConstantBias(),
-                                               biasnoise);
-  newgraph.push_back(biasprior);
+  newgraph.addPrior(biasKey, imuBias::ConstantBias(), biasnoise);
   initialEstimate.insert(biasKey, imuBias::ConstantBias());
   auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
 
   Vector n_velocity(3);
   n_velocity << 0, angular_velocity * radius, 0;
-  PriorFactor<Vector> velprior(V(0), n_velocity, velnoise);
-  newgraph.push_back(velprior);
+  newgraph.addPrior(V(0), n_velocity, velnoise);
 
   initialEstimate.insert(V(0), n_velocity);
 
diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp
index f04b73f6b..63355631b 100644
--- a/examples/ImuFactorsExample.cpp
+++ b/examples/ImuFactorsExample.cpp
@@ -11,14 +11,16 @@
 
 /**
  * @file imuFactorsExample
- * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor navigation code.
+ * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor
+ * navigation code.
  * @author Garrett (ghemann@gmail.com), Luca Carlone
  */
 
 /**
- * Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS
- *  - you can test imuFactor (resp. combinedImuFactor) by commenting (resp. uncommenting)
- *  the line #define USE_COMBINED (few lines below)
+ * Example of use of the imuFactors (imuFactor and combinedImuFactor) in
+ * conjunction with GPS
+ *  - imuFactor is used by default. You can test combinedImuFactor by
+ *  appending a `-c` flag at the end (see below for example command).
  *  - we read IMU and GPS data from a CSV file, with the following format:
  *  A row starting with "i" is the first initial position formatted with
  *  N, E, D, qx, qY, qZ, qW, velN, velE, velD
@@ -26,52 +28,84 @@
  *  linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
  *  A row starting with "1" is a gps correction formatted with
  *  N, E, D, qX, qY, qZ, qW
- *  Note that for GPS correction, we're only using the position not the rotation. The
- *  rotation is provided in the file for ground truth comparison.
+ * Note that for GPS correction, we're only using the position not the
+ * rotation. The rotation is provided in the file for ground truth comparison.
+ *
+ *  Usage: ./ImuFactorsExample [data_csv_path] [-c]
+ *  optional arguments:
+ *    data_csv_path           path to the CSV file with the IMU data.
+ *    -c                      use CombinedImuFactor
+ *  Note: Define USE_LM to use Levenberg Marquardt Optimizer
+ *        By default ISAM2 is used
  */
 
 // GTSAM related includes.
 #include <gtsam/navigation/CombinedImuFactor.h>
 #include <gtsam/navigation/GPSFactor.h>
 #include <gtsam/navigation/ImuFactor.h>
-#include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/dataset.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/inference/Symbol.h>
+
+#include <cstring>
 #include <fstream>
 #include <iostream>
 
-// Uncomment line below to use the CombinedIMUFactor as opposed to the standard ImuFactor.
-// #define USE_COMBINED
+// Uncomment the following to use Levenberg Marquardt Optimizer
+// #define USE_LM
 
 using namespace gtsam;
 using namespace std;
 
-using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
-using symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
-using symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
+using symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
+using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
+using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)
 
-const string output_filename = "imuFactorExampleResults.csv";
+static const char output_filename[] = "imuFactorExampleResults.csv";
+static const char use_combined_imu_flag[3] = "-c";
 
-// This will either be PreintegratedImuMeasurements (for ImuFactor) or
-// PreintegratedCombinedMeasurements (for CombinedImuFactor).
-PreintegrationType *imu_preintegrated_;
-
-int main(int argc, char* argv[])
-{
+int main(int argc, char* argv[]) {
   string data_filename;
+  bool use_combined_imu = false;
+
+#ifndef USE_LM
+  printf("Using ISAM2\n");
+  ISAM2Params parameters;
+  parameters.relinearizeThreshold = 0.01;
+  parameters.relinearizeSkip = 1;
+  ISAM2 isam2(parameters);
+#else
+  printf("Using Levenberg Marquardt Optimizer\n");
+#endif
+
   if (argc < 2) {
     printf("using default CSV file\n");
     data_filename = findExampleDataFile("imuAndGPSdata.csv");
+  } else if (argc < 3) {
+    if (strcmp(argv[1], use_combined_imu_flag) == 0) {
+      printf("using CombinedImuFactor\n");
+      use_combined_imu = true;
+      printf("using default CSV file\n");
+      data_filename = findExampleDataFile("imuAndGPSdata.csv");
+    } else {
+      data_filename = argv[1];
+    }
   } else {
     data_filename = argv[1];
+    if (strcmp(argv[2], use_combined_imu_flag) == 0) {
+      printf("using CombinedImuFactor\n");
+      use_combined_imu = true;
+    }
   }
 
   // Set up output file for plotting errors
-  FILE* fp_out = fopen(output_filename.c_str(), "w+");
-  fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,gt_qy,gt_qz,gt_qw\n");
+  FILE* fp_out = fopen(output_filename, "w+");
+  fprintf(fp_out,
+          "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,"
+          "gt_qy,gt_qz,gt_qw\n");
 
   // Begin parsing the CSV file.  Input the first line for initialization.
   // From there, we'll iterate through the file and we'll preintegrate the IMU
@@ -80,9 +114,9 @@ int main(int argc, char* argv[])
   string value;
 
   // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
-  Eigen::Matrix<double,10,1> initial_state = Eigen::Matrix<double,10,1>::Zero();
-  getline(file, value, ','); // i
-  for (int i=0; i<9; i++) {
+  Vector10 initial_state;
+  getline(file, value, ',');  // i
+  for (int i = 0; i < 9; i++) {
     getline(file, value, ',');
     initial_state(i) = atof(value.c_str());
   }
@@ -90,13 +124,14 @@ int main(int argc, char* argv[])
   initial_state(9) = atof(value.c_str());
   cout << "initial state:\n" << initial_state.transpose() << "\n\n";
 
-  // Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
+  // Assemble initial quaternion through GTSAM constructor
+  // ::quaternion(w,x,y,z);
   Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
                                          initial_state(4), initial_state(5));
   Point3 prior_point(initial_state.head<3>());
   Pose3 prior_pose(prior_rotation, prior_point);
   Vector3 prior_velocity(initial_state.tail<3>());
-  imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
+  imuBias::ConstantBias prior_imu_bias;  // assume zero initial bias
 
   Values initial_values;
   int correction_count = 0;
@@ -104,53 +139,64 @@ int main(int argc, char* argv[])
   initial_values.insert(V(correction_count), prior_velocity);
   initial_values.insert(B(correction_count), prior_imu_bias);
 
-  // Assemble prior noise model and add it the graph.
-  noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()); // rad,rad,rad,m, m, m
-  noiseModel::Diagonal::shared_ptr velocity_noise_model = noiseModel::Isotropic::Sigma(3,0.1); // m/s
-  noiseModel::Diagonal::shared_ptr bias_noise_model = noiseModel::Isotropic::Sigma(6,1e-3);
+  // Assemble prior noise model and add it the graph.`
+  auto pose_noise_model = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5)
+          .finished());  // rad,rad,rad,m, m, m
+  auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1);  // m/s
+  auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 1e-3);
 
   // Add all prior factors (pose, velocity, bias) to the graph.
-  NonlinearFactorGraph *graph = new NonlinearFactorGraph();
-  graph->add(PriorFactor<Pose3>(X(correction_count), prior_pose, pose_noise_model));
-  graph->add(PriorFactor<Vector3>(V(correction_count), prior_velocity,velocity_noise_model));
-  graph->add(PriorFactor<imuBias::ConstantBias>(B(correction_count), prior_imu_bias,bias_noise_model));
+  NonlinearFactorGraph* graph = new NonlinearFactorGraph();
+  graph->addPrior(X(correction_count), prior_pose, pose_noise_model);
+  graph->addPrior(V(correction_count), prior_velocity, velocity_noise_model);
+  graph->addPrior(B(correction_count), prior_imu_bias, bias_noise_model);
 
   // We use the sensor specs to build the noise model for the IMU factor.
   double accel_noise_sigma = 0.0003924;
   double gyro_noise_sigma = 0.000205689024915;
   double accel_bias_rw_sigma = 0.004905;
   double gyro_bias_rw_sigma = 0.000001454441043;
-  Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2);
-  Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2);
-  Matrix33 integration_error_cov = Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities
-  Matrix33 bias_acc_cov = Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2);
-  Matrix33 bias_omega_cov = Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2);
-  Matrix66 bias_acc_omega_int = Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration
+  Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2);
+  Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2);
+  Matrix33 integration_error_cov =
+      I_3x3 * 1e-8;  // error committed in integrating position from velocities
+  Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
+  Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
+  Matrix66 bias_acc_omega_int =
+      I_6x6 * 1e-5;  // error in the bias used for preintegration
 
-  boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
+  auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
   // PreintegrationBase params:
-  p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
-  p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
+  p->accelerometerCovariance =
+      measured_acc_cov;  // acc white noise in continuous
+  p->integrationCovariance =
+      integration_error_cov;  // integration uncertainty continuous
   // should be using 2nd order integration
   // PreintegratedRotation params:
-  p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
+  p->gyroscopeCovariance =
+      measured_omega_cov;  // gyro white noise in continuous
   // PreintegrationCombinedMeasurements params:
-  p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
-  p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
+  p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
+  p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
   p->biasAccOmegaInt = bias_acc_omega_int;
 
-#ifdef USE_COMBINED
-  imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias);
-#else
-  imu_preintegrated_ = new PreintegratedImuMeasurements(p, prior_imu_bias);
-#endif
+  std::shared_ptr<PreintegrationType> preintegrated = nullptr;
+  if (use_combined_imu) {
+    preintegrated =
+        std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);
+  } else {
+    preintegrated =
+        std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);
+  }
+  assert(preintegrated);
 
-  // Store previous state for the imu integration and the latest predicted outcome.
+  // Store previous state for imu integration and latest predicted outcome.
   NavState prev_state(prior_pose, prior_velocity);
   NavState prop_state = prev_state;
   imuBias::ConstantBias prev_bias = prior_imu_bias;
 
-  // Keep track of the total error over the entire run for a simple performance metric.
+  // Keep track of total error over the entire run as simple performance metric.
   double current_position_error = 0.0, current_orientation_error = 0.0;
 
   double output_time = 0.0;
@@ -159,14 +205,13 @@ int main(int argc, char* argv[])
 
   // All priors have been set up, now iterate through the data file.
   while (file.good()) {
-
     // Parse out first value
     getline(file, value, ',');
     int type = atoi(value.c_str());
 
-    if (type == 0) { // IMU measurement
-      Eigen::Matrix<double,6,1> imu = Eigen::Matrix<double,6,1>::Zero();
-      for (int i=0; i<5; ++i) {
+    if (type == 0) {  // IMU measurement
+      Vector6 imu;
+      for (int i = 0; i < 5; ++i) {
         getline(file, value, ',');
         imu(i) = atof(value.c_str());
       }
@@ -174,11 +219,11 @@ int main(int argc, char* argv[])
       imu(5) = atof(value.c_str());
 
       // Adding the IMU preintegration.
-      imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
+      preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
 
-    } else if (type == 1) { // GPS measurement
-      Eigen::Matrix<double,7,1> gps = Eigen::Matrix<double,7,1>::Zero();
-      for (int i=0; i<6; ++i) {
+    } else if (type == 1) {  // GPS measurement
+      Vector7 gps;
+      for (int i = 0; i < 6; ++i) {
         getline(file, value, ',');
         gps(i) = atof(value.c_str());
       }
@@ -188,50 +233,62 @@ int main(int argc, char* argv[])
       correction_count++;
 
       // Adding IMU factor and GPS factor and optimizing.
-#ifdef USE_COMBINED
-      PreintegratedCombinedMeasurements *preint_imu_combined = dynamic_cast<PreintegratedCombinedMeasurements*>(imu_preintegrated_);
-      CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
-                                   X(correction_count  ), V(correction_count  ),
-                                   B(correction_count-1), B(correction_count  ),
-                                   *preint_imu_combined);
-      graph->add(imu_factor);
-#else
-      PreintegratedImuMeasurements *preint_imu = dynamic_cast<PreintegratedImuMeasurements*>(imu_preintegrated_);
-      ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
-                           X(correction_count  ), V(correction_count  ),
-                           B(correction_count-1),
-                           *preint_imu);
-      graph->add(imu_factor);
-      imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
-      graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
-                                                      B(correction_count  ),
-                                                      zero_bias, bias_noise_model));
-#endif
+      if (use_combined_imu) {
+        auto preint_imu_combined =
+            dynamic_cast<const PreintegratedCombinedMeasurements&>(
+                *preintegrated);
+        CombinedImuFactor imu_factor(
+            X(correction_count - 1), V(correction_count - 1),
+            X(correction_count), V(correction_count), B(correction_count - 1),
+            B(correction_count), preint_imu_combined);
+        graph->add(imu_factor);
+      } else {
+        auto preint_imu =
+            dynamic_cast<const PreintegratedImuMeasurements&>(*preintegrated);
+        ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1),
+                             X(correction_count), V(correction_count),
+                             B(correction_count - 1), preint_imu);
+        graph->add(imu_factor);
+        imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
+        graph->add(BetweenFactor<imuBias::ConstantBias>(
+            B(correction_count - 1), B(correction_count), zero_bias,
+            bias_noise_model));
+      }
 
-      noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3,1.0);
+      auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0);
       GPSFactor gps_factor(X(correction_count),
-                           Point3(gps(0),  // N,
-                                  gps(1),  // E,
-                                  gps(2)), // D,
+                           Point3(gps(0),   // N,
+                                  gps(1),   // E,
+                                  gps(2)),  // D,
                            correction_noise);
       graph->add(gps_factor);
 
       // Now optimize and compare results.
-      prop_state = imu_preintegrated_->predict(prev_state, prev_bias);
+      prop_state = preintegrated->predict(prev_state, prev_bias);
       initial_values.insert(X(correction_count), prop_state.pose());
       initial_values.insert(V(correction_count), prop_state.v());
       initial_values.insert(B(correction_count), prev_bias);
 
+      Values result;
+#ifdef USE_LM
       LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
-      Values result = optimizer.optimize();
+      result = optimizer.optimize();
+#else
+      isam2.update(*graph, initial_values);
+      isam2.update();
+      result = isam2.calculateEstimate();
 
+      // reset the graph
+      graph->resize(0);
+      initial_values.clear();
+#endif
       // Overwrite the beginning of the preintegration for the next step.
       prev_state = NavState(result.at<Pose3>(X(correction_count)),
                             result.at<Vector3>(V(correction_count)));
       prev_bias = result.at<imuBias::ConstantBias>(B(correction_count));
 
       // Reset the preintegration object.
-      imu_preintegrated_->resetIntegrationAndSetBias(prev_bias);
+      preintegrated->resetIntegrationAndSetBias(prev_bias);
 
       // Print out the position and orientation error for comparison.
       Vector3 gtsam_position = prev_state.pose().translation();
@@ -242,19 +299,19 @@ int main(int argc, char* argv[])
       Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
       Quaternion quat_error = gtsam_quat * gps_quat.inverse();
       quat_error.normalize();
-      Vector3 euler_angle_error(quat_error.x()*2,
-                                 quat_error.y()*2,
-                                 quat_error.z()*2);
+      Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2,
+                                quat_error.z() * 2);
       current_orientation_error = euler_angle_error.norm();
 
       // display statistics
-      cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n";
+      cout << "Position error:" << current_position_error << "\t "
+           << "Angular error:" << current_orientation_error << "\n";
 
       fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
-              output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2),
-              gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(),
-              gps(0), gps(1), gps(2),
-              gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());
+              output_time, gtsam_position(0), gtsam_position(1),
+              gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(),
+              gtsam_quat.w(), gps(0), gps(1), gps(2), gps_quat.x(),
+              gps_quat.y(), gps_quat.z(), gps_quat.w());
 
       output_time += 1.0;
 
@@ -264,6 +321,7 @@ int main(int argc, char* argv[])
     }
   }
   fclose(fp_out);
-  cout << "Complete, results written to " << output_filename << "\n\n";;
+  cout << "Complete, results written to " << output_filename << "\n\n";
+
   return 0;
 }
diff --git a/examples/InverseKinematicsExampleExpressions.cpp b/examples/InverseKinematicsExampleExpressions.cpp
index 9e86886e7..1df83d9a1 100644
--- a/examples/InverseKinematicsExampleExpressions.cpp
+++ b/examples/InverseKinematicsExampleExpressions.cpp
@@ -22,7 +22,6 @@
 #include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/nonlinear/expressions.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/expressions.h>
 
 #include <cmath>
diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp
index 3c07fd4e3..a28ead5fe 100644
--- a/examples/LocalizationExample.cpp
+++ b/examples/LocalizationExample.cpp
@@ -66,12 +66,11 @@ using namespace gtsam;
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 class UnaryFactor: public NoiseModelFactor1<Pose2> {
-
   // The factor will hold a measurement consisting of an (X,Y) location
   // We could this with a Point2 but here we just use two doubles
   double mx_, my_;
 
-public:
+ public:
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<UnaryFactor> shared_ptr;
 
@@ -85,15 +84,15 @@ public:
   // The first is the 'evaluateError' function. This function implements the desired measurement
   // function, returning a vector of errors when evaluated at the provided variable value. It
   // must also calculate the Jacobians for this measurement function, if requested.
-  Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const
-  {
+  Vector evaluateError(const Pose2& q,
+                       boost::optional<Matrix&> H = boost::none) const {
     // The measurement function for a GPS-like measurement is simple:
     // error_x = pose.x - measurement.x
     // error_y = pose.y - measurement.y
     // Consequently, the Jacobians are:
     // [ derror_x/dx  derror_x/dy  derror_x/dtheta ] = [1 0 0]
     // [ derror_y/dx  derror_y/dy  derror_y/dtheta ] = [0 1 0]
-    if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
+    if (H) (*H) = (Matrix(2, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0).finished();
     return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
   }
 
@@ -107,29 +106,28 @@ public:
   // Additionally, we encourage you the use of unit testing your custom factors,
   // (as all GTSAM factors are), in which you would need an equals and print, to satisfy the
   // GTSAM_CONCEPT_TESTABLE_INST(T) defined in Testable.h, but these are not needed below.
-
-}; // UnaryFactor
+};  // UnaryFactor
 
 
 int main(int argc, char** argv) {
-
   // 1. Create a factor graph container and add factors to it
   NonlinearFactorGraph graph;
 
   // 2a. Add odometry factors
   // For simplicity, we will use the same noise model for each odometry factor
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
   graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise);
 
   // 2b. Add "GPS-like" measurements
   // We will use our custom UnaryFactor for this.
-  noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
+  auto unaryNoise =
+      noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));  // 10cm std on x,y
   graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
   graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
   graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
-  graph.print("\nFactor Graph:\n"); // print
+  graph.print("\nFactor Graph:\n");  // print
 
   // 3. Create the data structure to hold the initialEstimate estimate to the solution
   // For illustrative purposes, these have been deliberately set to incorrect values
@@ -137,7 +135,7 @@ int main(int argc, char** argv) {
   initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
   initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
   initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
-  initialEstimate.print("\nInitial Estimate:\n"); // print
+  initialEstimate.print("\nInitial Estimate:\n");  // print
 
   // 4. Optimize using Levenberg-Marquardt optimization. The optimizer
   // accepts an optional set of configuration parameters, controlling
diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp
index 76b5098f6..b91e113a4 100644
--- a/examples/METISOrderingExample.cpp
+++ b/examples/METISOrderingExample.cpp
@@ -11,7 +11,8 @@
 
 /**
  * @file METISOrdering.cpp
- * @brief Simple robot motion example, with prior and two odometry measurements, using a METIS ordering
+ * @brief Simple robot motion example, with prior and two odometry measurements,
+ * using a METIS ordering
  * @author Frank Dellaert
  * @author Andrew Melim
  */
@@ -22,12 +23,11 @@
  */
 
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/BetweenFactor.h>
 
 using namespace std;
 using namespace gtsam;
@@ -35,26 +35,26 @@ using namespace gtsam;
 int main(int argc, char** argv) {
   NonlinearFactorGraph graph;
 
-  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.emplace_shared<PriorFactor<Pose2> >(1, priorMean, priorNoise);
+  Pose2 priorMean(0.0, 0.0, 0.0);  // prior at origin
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  graph.addPrior(1, priorMean, priorNoise);
 
   Pose2 odometry(2.0, 0.0, 0.0);
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
-  graph.print("\nFactor Graph:\n"); // print
+  graph.print("\nFactor Graph:\n");  // print
 
   Values initial;
   initial.insert(1, Pose2(0.5, 0.0, 0.2));
   initial.insert(2, Pose2(2.3, 0.1, -0.2));
   initial.insert(3, Pose2(4.1, 0.1, 0.1));
-  initial.print("\nInitial Estimate:\n"); // print
+  initial.print("\nInitial Estimate:\n");  // print
 
   // optimize using Levenberg-Marquardt optimization
   LevenbergMarquardtParams params;
-  // In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType"
-  // By default this parameter is set to OrderingType::COLAMD
+  // In order to specify the ordering type, we need to se the "orderingType". By
+  // default this parameter is set to OrderingType::COLAMD
   params.orderingType = Ordering::METIS;
   LevenbergMarquardtOptimizer optimizer(graph, initial, params);
   Values result = optimizer.optimize();
diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp
index 6dc0d9a93..1944d9e46 100644
--- a/examples/OdometryExample.cpp
+++ b/examples/OdometryExample.cpp
@@ -29,7 +29,6 @@
 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
 // Here we will use Between factors for the relative motion described by odometry measurements.
 // Also, we will initialize the robot at the origin using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 
 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
@@ -57,24 +56,23 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // Create an empty nonlinear factor graph
   NonlinearFactorGraph graph;
 
   // Add a prior on the first pose, setting it to the origin
   // A prior factor consists of a mean and a noise model (covariance matrix)
-  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.emplace_shared<PriorFactor<Pose2> >(1, priorMean, priorNoise);
+  Pose2 priorMean(0.0, 0.0, 0.0);  // prior at origin
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  graph.addPrior(1, priorMean, priorNoise);
 
   // Add odometry factors
   Pose2 odometry(2.0, 0.0, 0.0);
   // For simplicity, we will use the same noise model for each odometry factor
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
   graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
-  graph.print("\nFactor Graph:\n"); // print
+  graph.print("\nFactor Graph:\n");  // print
 
   // Create the data structure to hold the initialEstimate estimate to the solution
   // For illustrative purposes, these have been deliberately set to incorrect values
@@ -82,7 +80,7 @@ int main(int argc, char** argv) {
   initial.insert(1, Pose2(0.5, 0.0, 0.2));
   initial.insert(2, Pose2(2.3, 0.1, -0.2));
   initial.insert(3, Pose2(4.1, 0.1, 0.1));
-  initial.print("\nInitial Estimate:\n"); // print
+  initial.print("\nInitial Estimate:\n");  // print
 
   // optimize using Levenberg-Marquardt optimization
   Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp
index 52638fdca..ede27131d 100644
--- a/examples/PlanarSLAMExample.cpp
+++ b/examples/PlanarSLAMExample.cpp
@@ -40,7 +40,6 @@
 // Here we will use a RangeBearing factor for the range-bearing measurements to identified
 // landmarks, and Between factors for the relative motion described by odometry measurements.
 // Also, we will initialize the robot at the origin using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
 
@@ -70,35 +69,36 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // Create a factor graph
   NonlinearFactorGraph graph;
 
   // Create the keys we need for this simple example
-  static Symbol x1('x',1), x2('x',2), x3('x',3);
-  static Symbol l1('l',1), l2('l',2);
+  static Symbol x1('x', 1), x2('x', 2), x3('x', 3);
+  static Symbol l1('l', 1), l2('l', 2);
 
-  // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
-  Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
-  graph.emplace_shared<PriorFactor<Pose2> >(x1, prior, priorNoise); // add directly to graph
+  // Add a prior on pose x1 at the origin. A prior factor consists of a mean and
+  // a noise model (covariance matrix)
+  Pose2 prior(0.0, 0.0, 0.0);  // prior mean is at origin
+  auto priorNoise = noiseModel::Diagonal::Sigmas(
+      Vector3(0.3, 0.3, 0.1));            // 30cm std on x,y, 0.1 rad on theta
+  graph.addPrior(x1, prior, priorNoise);  // add directly to graph
 
   // Add two odometry factors
-  Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
+  Pose2 odometry(2.0, 0.0, 0.0);
+  // create a measurement for both factors (the same in this case)
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(
+      Vector3(0.2, 0.2, 0.1));  // 20cm std on x,y, 0.1 rad on theta
   graph.emplace_shared<BetweenFactor<Pose2> >(x1, x2, odometry, odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(x2, x3, odometry, odometryNoise);
 
   // Add Range-Bearing measurements to two different landmarks
   // create a noise model for the landmark measurements
-  noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
+  auto measurementNoise = noiseModel::Diagonal::Sigmas(
+      Vector2(0.1, 0.2));  // 0.1 rad std on bearing, 20cm on range
   // create the measurement values - indices are (pose id, landmark id)
-  Rot2 bearing11 = Rot2::fromDegrees(45),
-       bearing21 = Rot2::fromDegrees(90),
+  Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90),
        bearing32 = Rot2::fromDegrees(90);
-  double range11 = std::sqrt(4.0+4.0),
-         range21 = 2.0,
-         range32 = 2.0;
+  double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0;
 
   // Add Bearing-Range factors
   graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
@@ -111,7 +111,7 @@ int main(int argc, char** argv) {
   // Create (deliberately inaccurate) initial estimate
   Values initialEstimate;
   initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
-  initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
+  initialEstimate.insert(x2, Pose2(2.3, 0.1, -0.2));
   initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
   initialEstimate.insert(l1, Point2(1.8, 2.1));
   initialEstimate.insert(l2, Point2(4.1, 1.8));
@@ -139,4 +139,3 @@ int main(int argc, char** argv) {
 
   return 0;
 }
-
diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp
index f977a08af..f3d555930 100644
--- a/examples/Pose2SLAMExample.cpp
+++ b/examples/Pose2SLAMExample.cpp
@@ -36,7 +36,6 @@
 // Here we will use Between factors for the relative motion described by odometry measurements.
 // We will also use a Between Factor to encode the loop closure constraint
 // Also, we will initialize the robot at the origin using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 
 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
@@ -65,21 +64,20 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // 1. Create a factor graph container and add factors to it
   NonlinearFactorGraph graph;
 
   // 2a. Add a prior on the first pose, setting it to the origin
   // A prior factor consists of a mean and a noise model (covariance matrix)
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
 
   // For simplicity, we will use the same noise model for odometry and loop closures
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
   // Create odometry (Between) factors between consecutive poses
-  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0     ), model);
+  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
@@ -89,17 +87,17 @@ int main(int argc, char** argv) {
   // these constraints may be identified in many ways, such as appearance-based techniques
   // with camera images. We will use another Between Factor to enforce this constraint:
   graph.emplace_shared<BetweenFactor<Pose2> >(5, 2, Pose2(2, 0, M_PI_2), model);
-  graph.print("\nFactor Graph:\n"); // print
+  graph.print("\nFactor Graph:\n");  // print
 
   // 3. Create the data structure to hold the initialEstimate estimate to the solution
   // For illustrative purposes, these have been deliberately set to incorrect values
   Values initialEstimate;
-  initialEstimate.insert(1, Pose2(0.5, 0.0,  0.2   ));
-  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2   ));
-  initialEstimate.insert(3, Pose2(4.1, 0.1,  M_PI_2));
-  initialEstimate.insert(4, Pose2(4.0, 2.0,  M_PI  ));
+  initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
+  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
+  initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
+  initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI));
   initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
-  initialEstimate.print("\nInitial Estimate:\n"); // print
+  initialEstimate.print("\nInitial Estimate:\n");  // print
 
   // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
   // The optimizer accepts an optional set of configuration parameters,
diff --git a/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp
index 1f6de6cb1..41037d012 100644
--- a/examples/Pose2SLAMExampleExpressions.cpp
+++ b/examples/Pose2SLAMExampleExpressions.cpp
@@ -21,7 +21,6 @@
 #include <gtsam/nonlinear/ExpressionFactorGraph.h>
 
 // For an explanation of headers below, please see Pose2SLAMExample.cpp
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@@ -31,7 +30,6 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // 1. Create a factor graph container and add factors to it
   ExpressionFactorGraph graph;
 
@@ -39,31 +37,32 @@ int main(int argc, char** argv) {
   Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5);
 
   // 2a. Add a prior on the first pose, setting it to the origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise);
 
-  // For simplicity, we will use the same noise model for odometry and loop closures
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  // For simplicity, we use the same noise model for odometry and loop closures
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
-  graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0     ), model);
-  graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model);
-  graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model);
-  graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model);
+  graph.addExpressionFactor(between(x1, x2), Pose2(2, 0, 0), model);
+  graph.addExpressionFactor(between(x2, x3), Pose2(2, 0, M_PI_2), model);
+  graph.addExpressionFactor(between(x3, x4), Pose2(2, 0, M_PI_2), model);
+  graph.addExpressionFactor(between(x4, x5), Pose2(2, 0, M_PI_2), model);
 
   // 2c. Add the loop closure constraint
-  graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model);
-  graph.print("\nFactor Graph:\n"); // print
+  graph.addExpressionFactor(between(x5, x2), Pose2(2, 0, M_PI_2), model);
+  graph.print("\nFactor Graph:\n");  // print
 
-  // 3. Create the data structure to hold the initialEstimate estimate to the solution
-  // For illustrative purposes, these have been deliberately set to incorrect values
+  // 3. Create the data structure to hold the initialEstimate estimate to the
+  // solution For illustrative purposes, these have been deliberately set to
+  // incorrect values
   Values initialEstimate;
-  initialEstimate.insert(1, Pose2(0.5, 0.0,  0.2   ));
-  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2   ));
-  initialEstimate.insert(3, Pose2(4.1, 0.1,  M_PI_2));
-  initialEstimate.insert(4, Pose2(4.0, 2.0,  M_PI  ));
+  initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
+  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
+  initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
+  initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI));
   initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
-  initialEstimate.print("\nInitial Estimate:\n"); // print
+  initialEstimate.print("\nInitial Estimate:\n");  // print
 
   // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
   GaussNewtonParams parameters;
diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp
index 35f9884e1..fb3869a50 100644
--- a/examples/Pose2SLAMExample_g2o.cpp
+++ b/examples/Pose2SLAMExample_g2o.cpp
@@ -19,7 +19,6 @@
  */
 
 #include <gtsam/slam/dataset.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <fstream>
@@ -29,60 +28,60 @@ using namespace gtsam;
 
 // HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
 int main(const int argc, const char *argv[]) {
-
   string kernelType = "none";
-  int maxIterations = 100; // default
-  string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
+  int maxIterations = 100;                                    // default
+  string g2oFile = findExampleDataFile("noisyToyGraph.txt");  // default
 
   // Parse user's inputs
-  if (argc > 1){
-    g2oFile = argv[1]; // input dataset filename
-    // outputFile = g2oFile = argv[2]; // done later
+  if (argc > 1) {
+    g2oFile = argv[1];  // input dataset filename
   }
-  if (argc > 3){
-    maxIterations = atoi(argv[3]); // user can specify either tukey or huber
+  if (argc > 3) {
+    maxIterations = atoi(argv[3]);  // user can specify either tukey or huber
   }
-  if (argc > 4){
-    kernelType = argv[4]; // user can specify either tukey or huber
+  if (argc > 4) {
+    kernelType = argv[4];  // user can specify either tukey or huber
   }
 
   // reading file and creating factor graph
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
   bool is3D = false;
-  if(kernelType.compare("none") == 0){
-    boost::tie(graph, initial) = readG2o(g2oFile,is3D);
+  if (kernelType.compare("none") == 0) {
+    boost::tie(graph, initial) = readG2o(g2oFile, is3D);
   }
-  if(kernelType.compare("huber") == 0){
+  if (kernelType.compare("huber") == 0) {
     std::cout << "Using robust kernel: huber " << std::endl;
-    boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER);
+    boost::tie(graph, initial) =
+        readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
   }
-  if(kernelType.compare("tukey") == 0){
+  if (kernelType.compare("tukey") == 0) {
     std::cout << "Using robust kernel: tukey " << std::endl;
-    boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY);
+    boost::tie(graph, initial) =
+        readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
   }
 
   // Add prior on the pose having index (key) = 0
-  NonlinearFactorGraph graphWithPrior = *graph;
-  noiseModel::Diagonal::shared_ptr priorModel = //
+  auto priorModel =  //
       noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
-  graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+  graph->addPrior(0, Pose2(), priorModel);
   std::cout << "Adding prior on pose 0 " << std::endl;
 
   GaussNewtonParams params;
   params.setVerbosity("TERMINATION");
   if (argc > 3) {
     params.maxIterations = maxIterations;
-    std::cout << "User required to perform maximum  " << params.maxIterations << " iterations "<< std::endl;
+    std::cout << "User required to perform maximum  " << params.maxIterations
+              << " iterations " << std::endl;
   }
 
   std::cout << "Optimizing the factor graph" << std::endl;
-  GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
+  GaussNewtonOptimizer optimizer(*graph, *initial, params);
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;
 
-  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
-  std::cout << "final error=" <<graph->error(result)<< std::endl;
+  std::cout << "initial error=" << graph->error(*initial) << std::endl;
+  std::cout << "final error=" << graph->error(result) << std::endl;
 
   if (argc < 3) {
     result.print("result");
diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp
index c3d901507..c4301d28d 100644
--- a/examples/Pose2SLAMExample_graph.cpp
+++ b/examples/Pose2SLAMExample_graph.cpp
@@ -17,7 +17,6 @@
  */
 
 // For an explanation of headers below, please see Pose2SLAMExample.cpp
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -43,7 +42,7 @@ int main (int argc, char** argv) {
   // Add a Gaussian prior on first poses
   Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
   SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
-  graph->push_back(PriorFactor<Pose2>(0, priorMean, priorNoise));
+  graph -> addPrior(0, priorMean, priorNoise);
 
   // Single Step Optimization using Levenberg-Marquardt
   Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize();
diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp
index 99711da2d..27d556725 100644
--- a/examples/Pose2SLAMExample_graphviz.cpp
+++ b/examples/Pose2SLAMExample_graphviz.cpp
@@ -17,7 +17,6 @@
  */
 
 // For an explanation of headers below, please see Pose2SLAMExample.cpp
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -26,20 +25,20 @@
 using namespace std;
 using namespace gtsam;
 
-int main (int argc, char** argv) {
-
+int main(int argc, char** argv) {
   // 1. Create a factor graph container and add factors to it
   NonlinearFactorGraph graph;
 
   // 2a. Add a prior on the first pose, setting it to the origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
 
-  // For simplicity, we will use the same noise model for odometry and loop closures
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  // For simplicity, we will use the same noise model for odometry and loop
+  // closures
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
-  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0     ), model);
+  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
@@ -50,10 +49,10 @@ int main (int argc, char** argv) {
   // 3. Create the data structure to hold the initial estimate to the solution
   // For illustrative purposes, these have been deliberately set to incorrect values
   Values initial;
-  initial.insert(1, Pose2(0.5, 0.0,  0.2   ));
-  initial.insert(2, Pose2(2.3, 0.1, -0.2   ));
-  initial.insert(3, Pose2(4.1, 0.1,  M_PI_2));
-  initial.insert(4, Pose2(4.0, 2.0,  M_PI  ));
+  initial.insert(1, Pose2(0.5, 0.0, 0.2));
+  initial.insert(2, Pose2(2.3, 0.1, -0.2));
+  initial.insert(3, Pose2(4.1, 0.1, M_PI_2));
+  initial.insert(4, Pose2(4.0, 2.0, M_PI));
   initial.insert(5, Pose2(2.1, 2.1, -M_PI_2));
 
   // Single Step Optimization using Levenberg-Marquardt
diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp
index b83dfa1db..1d02c64fa 100644
--- a/examples/Pose2SLAMExample_lago.cpp
+++ b/examples/Pose2SLAMExample_lago.cpp
@@ -21,7 +21,6 @@
 
 #include <gtsam/slam/lago.h>
 #include <gtsam/slam/dataset.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <fstream>
 
@@ -29,7 +28,6 @@ using namespace std;
 using namespace gtsam;
 
 int main(const int argc, const char *argv[]) {
-
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -42,14 +40,12 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile);
 
   // Add prior on the pose having index (key) = 0
-  NonlinearFactorGraph graphWithPrior = *graph;
-  noiseModel::Diagonal::shared_ptr priorModel = //
-      noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
-  graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
-  graphWithPrior.print();
+  auto priorModel = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
+  graph->addPrior(0, Pose2(), priorModel);
+  graph->print();
 
   std::cout << "Computing LAGO estimate" << std::endl;
-  Values estimateLago = lago::initialize(graphWithPrior);
+  Values estimateLago = lago::initialize(*graph);
   std::cout << "done!" << std::endl;
 
   if (argc < 3) {
@@ -57,7 +53,10 @@ int main(const int argc, const char *argv[]) {
   } else {
     const string outputFile = argv[2];
     std::cout << "Writing results to file: " << outputFile << std::endl;
-    writeG2o(*graph, estimateLago, outputFile);
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, estimateLago, outputFile);
     std::cout << "done! " << std::endl;
   }
 
diff --git a/examples/Pose2SLAMStressTest.cpp b/examples/Pose2SLAMStressTest.cpp
index 0f306b7f4..d7ab55aaa 100644
--- a/examples/Pose2SLAMStressTest.cpp
+++ b/examples/Pose2SLAMStressTest.cpp
@@ -19,7 +19,6 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/StereoFactor.h>
 
 #include <random>
@@ -48,7 +47,7 @@ void testGtsam(int numberNodes) {
   Pose3 first = Pose3(first_M);
 
   NonlinearFactorGraph graph;
-  graph.add(PriorFactor<Pose3>(0, first, priorModel));
+  graph.addPrior(0, first, priorModel);
 
   // vo noise model
   auto VOCovarianceModel = noiseModel::Isotropic::Variance(6, 1e-3);
diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp
index 9b459d7b8..cd74b2c07 100644
--- a/examples/Pose2SLAMwSPCG.cpp
+++ b/examples/Pose2SLAMwSPCG.cpp
@@ -17,7 +17,6 @@
  */
 
 // For an explanation of headers below, please see Pose2SLAMExample.cpp
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -29,45 +28,45 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // 1. Create a factor graph container and add factors to it
   NonlinearFactorGraph graph;
 
   // 2a. Add a prior on the first pose, setting it to the origin
-  Pose2 prior(0.0, 0.0, 0.0); // prior at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.emplace_shared<PriorFactor<Pose2> >(1, prior, priorNoise);
+  Pose2 prior(0.0, 0.0, 0.0);  // prior at origin
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  graph.addPrior(1, prior, priorNoise);
 
   // 2b. Add odometry factors
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
 
   // 2c. Add the loop closure constraint
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
-  graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0), model);
-  graph.print("\nFactor Graph:\n"); // print
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0),
+                                              model);
+  graph.print("\nFactor Graph:\n");  // print
 
-
-  // 3. Create the data structure to hold the initialEstimate estimate to the solution
+  // 3. Create the data structure to hold the initialEstimate estimate to the
+  // solution
   Values initialEstimate;
   initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
   initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1));
   initialEstimate.insert(3, Pose2(2.1, 1.9, 2.8));
   initialEstimate.insert(4, Pose2(-.3, 2.5, 4.2));
-  initialEstimate.insert(5, Pose2(0.1,-0.7, 5.8));
-  initialEstimate.print("\nInitial Estimate:\n"); // print
+  initialEstimate.insert(5, Pose2(0.1, -0.7, 5.8));
+  initialEstimate.print("\nInitial Estimate:\n");  // print
 
   // 4. Single Step Optimization using Levenberg-Marquardt
   LevenbergMarquardtParams parameters;
   parameters.verbosity = NonlinearOptimizerParams::ERROR;
   parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
 
-  // LM is still the outer optimization loop, but by specifying "Iterative" below
-  // We indicate that an iterative linear solver should be used.
-  // In addition, the *type* of the iterativeParams decides on the type of
+  // LM is still the outer optimization loop, but by specifying "Iterative"
+  // below We indicate that an iterative linear solver should be used. In
+  // addition, the *type* of the iterativeParams decides on the type of
   // iterative solver, in this case the SPCG (subgraph PCG)
   parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
   parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
diff --git a/examples/Pose3Localization.cpp b/examples/Pose3Localization.cpp
new file mode 100644
index 000000000..1667b43d9
--- /dev/null
+++ b/examples/Pose3Localization.cpp
@@ -0,0 +1,83 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Pose3Localization.cpp
+ * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
+ * Syntax for the script is ./Pose3Localization input.g2o output.g2o
+ * @date Aug 25, 2014
+ * @author Luca Carlone
+ */
+
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <fstream>
+
+using namespace std;
+using namespace gtsam;
+
+int main(const int argc, const char* argv[]) {
+  // Read graph from file
+  string g2oFile;
+  if (argc < 2)
+    g2oFile = findExampleDataFile("pose3Localizationexample.txt");
+  else
+    g2oFile = argv[1];
+
+  NonlinearFactorGraph::shared_ptr graph;
+  Values::shared_ptr initial;
+  bool is3D = true;
+  boost::tie(graph, initial) = readG2o(g2oFile, is3D);
+
+  // Add prior on the first key
+  auto priorModel = noiseModel::Diagonal::Variances(
+      (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+  Key firstKey = 0;
+  for (const Values::ConstKeyValuePair& key_value : *initial) {
+    std::cout << "Adding prior to g2o file " << std::endl;
+    firstKey = key_value.key;
+    graph->addPrior(firstKey, Pose3(), priorModel);
+    break;
+  }
+
+  std::cout << "Optimizing the factor graph" << std::endl;
+  GaussNewtonParams params;
+  params.setVerbosity("TERMINATION");  // show info about stopping conditions
+  GaussNewtonOptimizer optimizer(*graph, *initial, params);
+  Values result = optimizer.optimize();
+  std::cout << "Optimization complete" << std::endl;
+
+  std::cout << "initial error=" << graph->error(*initial) << std::endl;
+  std::cout << "final error=" << graph->error(result) << std::endl;
+
+  if (argc < 3) {
+    result.print("result");
+  } else {
+    const string outputFile = argv[2];
+    std::cout << "Writing results to file: " << outputFile << std::endl;
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, result, outputFile);
+    std::cout << "done! " << std::endl;
+  }
+
+  // Calculate and print marginal covariances for all variables
+  Marginals marginals(*graph, result);
+  for (const auto& key_value : result) {
+    auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
+    if (!p) continue;
+    std::cout << marginals.marginalCovariance(key_value.key) << endl;
+  }
+  return 0;
+}
diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp
index cceaac4ee..abadce975 100644
--- a/examples/Pose3SLAMExample_changeKeys.cpp
+++ b/examples/Pose3SLAMExample_changeKeys.cpp
@@ -19,7 +19,6 @@
 
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <fstream>
 
 using namespace std;
diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp
index 5066222e5..1cca92f59 100644
--- a/examples/Pose3SLAMExample_g2o.cpp
+++ b/examples/Pose3SLAMExample_g2o.cpp
@@ -10,24 +10,22 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Pose3SLAMExample_initializePose3.cpp
+ * @file Pose3SLAMExample_g2o.cpp
  * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
- * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * Syntax for the script is ./Pose3SLAMExample_g2o input.g2o output.g2o
  * @date Aug 25, 2014
  * @author Luca Carlone
  */
 
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <fstream>
 
 using namespace std;
 using namespace gtsam;
 
-int main(const int argc, const char *argv[]) {
-
+int main(const int argc, const char* argv[]) {
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -41,33 +39,35 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  NonlinearFactorGraph graphWithPrior = *graph;
-  noiseModel::Diagonal::shared_ptr priorModel = //
-      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+  auto priorModel = noiseModel::Diagonal::Variances(
+      (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for(const Values::ConstKeyValuePair& key_value: *initial) {
+  for (const Values::ConstKeyValuePair& key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
-    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }
 
   std::cout << "Optimizing the factor graph" << std::endl;
   GaussNewtonParams params;
-  params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
-  GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
+  params.setVerbosity("TERMINATION");  //  show info about stopping conditions
+  GaussNewtonOptimizer optimizer(*graph, *initial, params);
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;
 
-  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
-  std::cout << "final error=" <<graph->error(result)<< std::endl;
+  std::cout << "initial error=" << graph->error(*initial) << std::endl;
+  std::cout << "final error=" << graph->error(result) << std::endl;
 
   if (argc < 3) {
     result.print("result");
   } else {
     const string outputFile = argv[2];
     std::cout << "Writing results to file: " << outputFile << std::endl;
-    writeG2o(*graph, result, outputFile);
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, result, outputFile);
     std::cout << "done! " << std::endl;
   }
   return 0;
diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp
index e90d014c0..00a546bb2 100644
--- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp
+++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp
@@ -10,9 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Pose3SLAMExample_initializePose3.cpp
+ * @file Pose3SLAMExample_initializePose3Chordal.cpp
  * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
- * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * Syntax for the script is ./Pose3SLAMExample_initializePose3Chordal input.g2o output.g2o
  * @date Aug 25, 2014
  * @author Luca Carlone
  */
@@ -20,14 +20,12 @@
 #include <gtsam/slam/InitializePose3.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <fstream>
 
 using namespace std;
 using namespace gtsam;
 
-int main(const int argc, const char *argv[]) {
-
+int main(const int argc, const char* argv[]) {
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -41,27 +39,29 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  NonlinearFactorGraph graphWithPrior = *graph;
-  noiseModel::Diagonal::shared_ptr priorModel = //
-      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+  auto priorModel = noiseModel::Diagonal::Variances(
+      (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for(const Values::ConstKeyValuePair& key_value: *initial) {
+  for (const Values::ConstKeyValuePair& key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
-    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }
 
   std::cout << "Initializing Pose3 - chordal relaxation" << std::endl;
-  Values initialization = InitializePose3::initialize(graphWithPrior);
+  Values initialization = InitializePose3::initialize(*graph);
   std::cout << "done!" << std::endl;
 
   if (argc < 3) {
     initialization.print("initialization");
   } else {
     const string outputFile = argv[2];
-    std::cout << "Writing results to file: " << outputFile  << std::endl;
-    writeG2o(*graph, initialization, outputFile);
+    std::cout << "Writing results to file: " << outputFile << std::endl;
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, initialization, outputFile);
     std::cout << "done! " << std::endl;
   }
   return 0;
diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp
index 10960cf31..2f92afb9e 100644
--- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp
+++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp
@@ -10,9 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Pose3SLAMExample_initializePose3.cpp
+ * @file Pose3SLAMExample_initializePose3Gradient.cpp
  * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
- * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * Syntax for the script is ./Pose3SLAMExample_initializePose3Gradient input.g2o output.g2o
  * @date Aug 25, 2014
  * @author Luca Carlone
  */
@@ -20,14 +20,12 @@
 #include <gtsam/slam/InitializePose3.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <fstream>
 
 using namespace std;
 using namespace gtsam;
 
-int main(const int argc, const char *argv[]) {
-
+int main(const int argc, const char* argv[]) {
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -41,31 +39,35 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  NonlinearFactorGraph graphWithPrior = *graph;
-  noiseModel::Diagonal::shared_ptr priorModel = //
-      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+  auto priorModel = noiseModel::Diagonal::Variances(
+      (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for(const Values::ConstKeyValuePair& key_value: *initial) {
+  for (const Values::ConstKeyValuePair& key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
-    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }
 
   std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
   bool useGradient = true;
-  Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
+  Values initialization =
+      InitializePose3::initialize(*graph, *initial, useGradient);
   std::cout << "done!" << std::endl;
 
-  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
-  std::cout << "initialization error=" <<graph->error(initialization)<< std::endl;
+  std::cout << "initial error=" << graph->error(*initial) << std::endl;
+  std::cout << "initialization error=" << graph->error(initialization)
+            << std::endl;
 
   if (argc < 3) {
     initialization.print("initialization");
   } else {
     const string outputFile = argv[2];
-    std::cout << "Writing results to file: " << outputFile  << std::endl;
-    writeG2o(*graph, initialization, outputFile);
+    std::cout << "Writing results to file: " << outputFile << std::endl;
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, initialization, outputFile);
     std::cout << "done! " << std::endl;
   }
   return 0;
diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp
index 7b1667e12..c8e31e1c5 100644
--- a/examples/RangeISAMExample_plaza2.cpp
+++ b/examples/RangeISAMExample_plaza2.cpp
@@ -37,7 +37,6 @@
 
 // In GTSAM, measurement functions are represented as 'factors'. Several common factors
 // have been provided with the library for solving robotics SLAM problems.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/RangeFactor.h>
 #include <gtsam/slam/dataset.h>
@@ -124,7 +123,7 @@ int main (int argc, char** argv) {
   Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
       M_PI - 2.02108900000000);
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
+  newFactors.addPrior(0, pose0, priorNoise);
   Values initial;
   initial.insert(0, pose0);
 
diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp
index 0e48bb892..fddca8169 100644
--- a/examples/SFMExample.cpp
+++ b/examples/SFMExample.cpp
@@ -30,7 +30,6 @@
 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
 // Here we will use Projection factors to model the camera's landmark observations.
 // Also, we will initialize the robot at some location using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 
 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
@@ -57,12 +56,12 @@ using namespace gtsam;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Define the camera calibration parameters
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+  auto measurementNoise =
+      noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
 
   // Create the set of ground-truth landmarks
   vector<Point3> points = createPoints();
@@ -74,32 +73,45 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // Add a prior on pose x1. This indirectly specifies where the origin is.
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
+  auto poseNoise = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
+          .finished());  // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  graph.addPrior(Symbol('x', 0), poses[0], poseNoise);  // add directly to graph
 
-  // Simulated measurements from each camera pose, adding them to the factor graph
+  // Simulated measurements from each camera pose, adding them to the factor
+  // graph
   for (size_t i = 0; i < poses.size(); ++i) {
-    SimpleCamera camera(poses[i], *K);
+    PinholeCamera<Cal3_S2> camera(poses[i], *K);
     for (size_t j = 0; j < points.size(); ++j) {
       Point2 measurement = camera.project(points[j]);
-      graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
+      graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(
+          measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
     }
   }
 
-  // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
-  // Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
-  // between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
-  noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
-  graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
+  // Because the structure-from-motion problem has a scale ambiguity, the
+  // problem is still under-constrained Here we add a prior on the position of
+  // the first landmark. This fixes the scale by indicating the distance between
+  // the first camera and the first landmark. All other landmark positions are
+  // interpreted using this scale.
+  auto pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
+  graph.addPrior(Symbol('l', 0), points[0],
+                 pointNoise);  // add directly to graph
   graph.print("Factor Graph:\n");
 
   // Create the data structure to hold the initial estimate to the solution
   // Intentionally initialize the variables off from the ground truth
   Values initialEstimate;
-  for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
-  for (size_t j = 0; j < points.size(); ++j)
-    initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
+  for (size_t i = 0; i < poses.size(); ++i) {
+    auto corrupted_pose = poses[i].compose(
+        Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)));
+    initialEstimate.insert(
+        Symbol('x', i), corrupted_pose);
+  }
+  for (size_t j = 0; j < points.size(); ++j) {
+    Point3 corrupted_point = points[j] + Point3(-0.25, 0.20, 0.15);
+    initialEstimate.insert<Point3>(Symbol('l', j), corrupted_point);
+  }
   initialEstimate.print("Initial Estimates:\n");
 
   /* Optimize the graph and print results */
diff --git a/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp
index 191664ef6..73b6f27f7 100644
--- a/examples/SFMExampleExpressions.cpp
+++ b/examples/SFMExampleExpressions.cpp
@@ -27,7 +27,7 @@
 #include <gtsam/nonlinear/ExpressionFactorGraph.h>
 
 // Header order is close to far
-#include <examples/SFMdata.h>
+#include "SFMdata.h"
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/nonlinear/DoglegOptimizer.h>
 #include <gtsam/nonlinear/Values.h>
@@ -67,7 +67,7 @@ int main(int argc, char* argv[]) {
   // Simulated measurements from each camera pose, adding them to the factor graph
   for (size_t i = 0; i < poses.size(); ++i) {
     Pose3_ x('x', i);
-    SimpleCamera camera(poses[i], K);
+    PinholeCamera<Cal3_S2> camera(poses[i], K);
     for (size_t j = 0; j < points.size(); ++j) {
       Point2 measurement = camera.project(points[j]);
       // Below an expression for the prediction of the measurement:
diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp
index ea50f7163..3768ee2a3 100644
--- a/examples/SFMExampleExpressions_bal.cpp
+++ b/examples/SFMExampleExpressions_bal.cpp
@@ -28,7 +28,7 @@
 // Header order is close to far
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/slam/dataset.h> // for loading BAL datasets !
+#include <gtsam/slam/dataset.h>  // for loading BAL datasets !
 #include <vector>
 
 using namespace std;
@@ -37,57 +37,53 @@ using namespace noiseModel;
 using symbol_shorthand::C;
 using symbol_shorthand::P;
 
-// An SfM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
+// An SfmCamera is defined in datase.h as a camera with unknown Cal3Bundler calibration
 // and has a total of 9 free parameters
 
-/* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Find default file, but if an argument is given, try loading a file
   string filename = findExampleDataFile("dubrovnik-3-7-pre");
-  if (argc > 1)
-    filename = string(argv[1]);
+  if (argc > 1) filename = string(argv[1]);
 
   // Load the SfM data from file
-  SfM_data mydata;
+  SfmData mydata;
   readBAL(filename, mydata);
-  cout
-      << boost::format("read %1% tracks on %2% cameras\n")
-          % mydata.number_tracks() % mydata.number_cameras();
+  cout << boost::format("read %1% tracks on %2% cameras\n") %
+              mydata.number_tracks() % mydata.number_cameras();
 
   // Create a factor graph
   ExpressionFactorGraph graph;
 
   // Here we don't use a PriorFactor but directly the ExpressionFactor class
   // First, we create an expression to the pose from the first camera
-  Expression<SfM_Camera> camera0_(C(0));
+  Expression<SfmCamera> camera0_(C(0));
   // Then, to get its pose:
-  Pose3_ pose0_(&SfM_Camera::getPose, camera0_);
+  Pose3_ pose0_(&SfmCamera::getPose, camera0_);
   // Finally, we say it should be equal to first guess
   graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(),
-      noiseModel::Isotropic::Sigma(6, 0.1));
+                            noiseModel::Isotropic::Sigma(6, 0.1));
 
   // similarly, we create a prior on the first point
   Point3_ point0_(P(0));
   graph.addExpressionFactor(point0_, mydata.tracks[0].p,
-      noiseModel::Isotropic::Sigma(3, 0.1));
+                            noiseModel::Isotropic::Sigma(3, 0.1));
 
   // We share *one* noiseModel between all projection factors
-  noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2,
-      1.0); // one pixel in u and v
+  auto noise = noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
 
-  // Simulated measurements from each camera pose, adding them to the factor graph
+  // Simulated measurements from each camera pose, adding them to the factor
+  // graph
   size_t j = 0;
-  for(const SfM_Track& track: mydata.tracks) {
+  for (const SfmTrack& track : mydata.tracks) {
     // Leaf expression for j^th point
     Point3_ point_('p', j);
-    for(const SfM_Measurement& m: track.measurements) {
+    for (const SfmMeasurement& m : track.measurements) {
       size_t i = m.first;
       Point2 uv = m.second;
       // Leaf expression for i^th camera
-      Expression<SfM_Camera> camera_(C(i));
+      Expression<SfmCamera> camera_(C(i));
       // Below an expression for the prediction of the measurement:
-      Point2_ predict_ = project2<SfM_Camera>(camera_, point_);
+      Point2_ predict_ = project2<SfmCamera>(camera_, point_);
       // Again, here we use an ExpressionFactor
       graph.addExpressionFactor(predict_, uv, noise);
     }
@@ -98,10 +94,8 @@ int main(int argc, char* argv[]) {
   Values initial;
   size_t i = 0;
   j = 0;
-  for(const SfM_Camera& camera: mydata.cameras)
-    initial.insert(C(i++), camera);
-  for(const SfM_Track& track: mydata.tracks)
-    initial.insert(P(j++), track.p);
+  for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera);
+  for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p);
 
   /* Optimize the graph and print results */
   Values result;
@@ -117,5 +111,3 @@ int main(int argc, char* argv[]) {
 
   return 0;
 }
-/* ************************************************************************* */
-
diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp
index e9d02b15c..86e6b8ae9 100644
--- a/examples/SFMExample_SmartFactor.cpp
+++ b/examples/SFMExample_SmartFactor.cpp
@@ -44,7 +44,7 @@ int main(int argc, char* argv[]) {
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr measurementNoise =
+  auto measurementNoise =
       noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
   // Create the set of ground-truth landmarks and poses
@@ -80,15 +80,15 @@ int main(int argc, char* argv[]) {
 
   // Add a prior on pose x0. This indirectly specifies where the origin is.
   // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
-      (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
-  graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
+  auto noise = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
+  graph.addPrior(0, poses[0], noise);
 
   // Because the structure-from-motion problem has a scale ambiguity, the problem is
   // still under-constrained. Here we add a prior on the second pose x1, so this will
   // fix the scale by indicating the distance between x0 and x1.
   // Because these two are fixed, the rest of the poses will be also be fixed.
-  graph.emplace_shared<PriorFactor<Pose3> >(1, poses[1], noise); // add directly to graph
+  graph.addPrior(1, poses[1], noise); // add directly to graph
 
   graph.print("Factor Graph:\n");
 
@@ -117,7 +117,7 @@ int main(int argc, char* argv[]) {
       // The output of point() is in boost::optional<Point3>, as sometimes
       // the triangulation operation inside smart factor will encounter degeneracy.
       boost::optional<Point3> point = smart->point(result);
-      if (point) // ignore if boost::optional return NULL
+      if (point) // ignore if boost::optional return nullptr
         landmark_result.insert(j, *point);
     }
   }
diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp
index da7c5ba9b..3f553efc6 100644
--- a/examples/SFMExample_SmartFactorPCG.cpp
+++ b/examples/SFMExample_SmartFactorPCG.cpp
@@ -35,13 +35,12 @@ typedef PinholePose<Cal3_S2> Camera;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Define the camera calibration parameters
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr measurementNoise =
-      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+  auto measurementNoise =
+      noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
 
   // Create the set of ground-truth landmarks and poses
   vector<Point3> points = createPoints();
@@ -52,17 +51,16 @@ int main(int argc, char* argv[]) {
 
   // Simulated measurements from each camera pose, adding them to the factor graph
   for (size_t j = 0; j < points.size(); ++j) {
-
-    // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
+    // every landmark represent a single landmark, we use shared pointer to init
+    // the factor, and then insert measurements.
     SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
 
     for (size_t i = 0; i < poses.size(); ++i) {
-
       // generate the 2D measurement
       Camera camera(poses[i], K);
       Point2 measurement = camera.project(points[j]);
 
-      // call add() function to add measurement into a single factor, here we need to add:
+      // call add() function to add measurement into a single factor
       smartfactor->add(measurement, i);
     }
 
@@ -72,12 +70,12 @@ int main(int argc, char* argv[]) {
 
   // Add a prior on pose x0. This indirectly specifies where the origin is.
   // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
-      (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
-  graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
+  auto noise = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
+  graph.addPrior(0, poses[0], noise);
 
   // Fix the scale ambiguity by adding a prior
-  graph.emplace_shared<PriorFactor<Pose3> >(1, poses[0], noise);
+  graph.addPrior(1, poses[0], noise);
 
   // Create the initial estimate to the solution
   Values initialEstimate;
@@ -85,9 +83,9 @@ int main(int argc, char* argv[]) {
   for (size_t i = 0; i < poses.size(); ++i)
     initialEstimate.insert(i, poses[i].compose(delta));
 
-  // We will use LM in the outer optimization loop, but by specifying "Iterative" below
-  // We indicate that an iterative linear solver should be used.
-  // In addition, the *type* of the iterativeParams decides on the type of
+  // We will use LM in the outer optimization loop, but by specifying
+  // "Iterative" below We indicate that an iterative linear solver should be
+  // used. In addition, the *type* of the iterativeParams decides on the type of
   // iterative solver, in this case the SPCG (subgraph PCG)
   LevenbergMarquardtParams parameters;
   parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
@@ -110,11 +108,10 @@ int main(int argc, char* argv[]) {
   result.print("Final results:\n");
   Values landmark_result;
   for (size_t j = 0; j < points.size(); ++j) {
-    SmartFactor::shared_ptr smart = //
-        boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
+    auto smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
     if (smart) {
       boost::optional<Point3> point = smart->point(result);
-      if (point) // ignore if boost::optional return NULL
+      if (point)  // ignore if boost::optional return nullptr
         landmark_result.insert(j, *point);
     }
   }
diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp
index a3a416eb3..ffb5b195b 100644
--- a/examples/SFMExample_bal.cpp
+++ b/examples/SFMExample_bal.cpp
@@ -19,7 +19,6 @@
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/slam/dataset.h> // for loading BAL datasets !
 #include <vector>
@@ -32,7 +31,7 @@ using symbol_shorthand::P;
 // We will be using a projection factor that ties a SFM_Camera to a 3D point.
 // An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
 // and has a total of 9 free parameters
-typedef GeneralSFMFactor<SfM_Camera,Point3> MyFactor;
+typedef GeneralSFMFactor<SfmCamera,Point3> MyFactor;
 
 /* ************************************************************************* */
 int main (int argc, char* argv[]) {
@@ -42,7 +41,7 @@ int main (int argc, char* argv[]) {
   if (argc>1) filename = string(argv[1]);
 
   // Load the SfM data from file
-  SfM_data mydata;
+  SfmData mydata;
   readBAL(filename, mydata);
   cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
 
@@ -50,13 +49,13 @@ int main (int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // We share *one* noiseModel between all projection factors
-  noiseModel::Isotropic::shared_ptr noise =
+  auto noise =
       noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
   // Add measurements to the factor graph
   size_t j = 0;
-  for(const SfM_Track& track: mydata.tracks) {
-    for(const SfM_Measurement& m: track.measurements) {
+  for(const SfmTrack& track: mydata.tracks) {
+    for(const SfmMeasurement& m: track.measurements) {
       size_t i = m.first;
       Point2 uv = m.second;
       graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
@@ -66,14 +65,14 @@ int main (int argc, char* argv[]) {
 
   // Add a prior on pose x1. This indirectly specifies where the origin is.
   // and a prior on the position of the first landmark to fix the scale
-  graph.emplace_shared<PriorFactor<SfM_Camera> >(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1));
-  graph.emplace_shared<PriorFactor<Point3> >    (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
+  graph.addPrior(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1));
+  graph.addPrior(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
 
   // Create initial estimate
   Values initial;
   size_t i = 0; j = 0;
-  for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera);
-  for(const SfM_Track& track: mydata.tracks)    initial.insert(P(j++), track.p);
+  for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera);
+  for(const SfmTrack& track: mydata.tracks)    initial.insert(P(j++), track.p);
 
   /* Optimize the graph and print results */
   Values result;
diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp
index 13a07dda5..b79a9fa28 100644
--- a/examples/SFMExample_bal_COLAMD_METIS.cpp
+++ b/examples/SFMExample_bal_COLAMD_METIS.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    SFMExample.cpp
+ * @file    SFMExample_bal_COLAMD_METIS.cpp
  * @brief   This file is to compare the ordering performance for COLAMD vs METIS.
  * Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file.
  * @author  Frank Dellaert, Zhaoyang Lv
@@ -21,9 +21,8 @@
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
-#include <gtsam/slam/dataset.h> // for loading BAL datasets !
+#include <gtsam/slam/dataset.h>  // for loading BAL datasets !
 
 #include <gtsam/base/timing.h>
 
@@ -35,50 +34,52 @@ using symbol_shorthand::C;
 using symbol_shorthand::P;
 
 // We will be using a projection factor that ties a SFM_Camera to a 3D point.
-// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
-// and has a total of 9 free parameters
-typedef GeneralSFMFactor<SfM_Camera,Point3> MyFactor;
+// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler
+// calibration and has a total of 9 free parameters
+typedef GeneralSFMFactor<SfmCamera, Point3> MyFactor;
 
-/* ************************************************************************* */
-int main (int argc, char* argv[]) {
 
+int main(int argc, char* argv[]) {
   // Find default file, but if an argument is given, try loading a file
   string filename = findExampleDataFile("dubrovnik-3-7-pre");
-  if (argc>1) filename = string(argv[1]);
+  if (argc > 1) filename = string(argv[1]);
 
   // Load the SfM data from file
-  SfM_data mydata;
+  SfmData mydata;
   readBAL(filename, mydata);
-  cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
+  cout << boost::format("read %1% tracks on %2% cameras\n") %
+              mydata.number_tracks() % mydata.number_cameras();
 
   // Create a factor graph
   NonlinearFactorGraph graph;
 
   // We share *one* noiseModel between all projection factors
-  noiseModel::Isotropic::shared_ptr noise =
-      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+  auto noise = noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
 
   // Add measurements to the factor graph
   size_t j = 0;
-  for(const SfM_Track& track: mydata.tracks) {
-    for(const SfM_Measurement& m: track.measurements) {
+  for (const SfmTrack& track : mydata.tracks) {
+    for (const SfmMeasurement& m : track.measurements) {
       size_t i = m.first;
       Point2 uv = m.second;
-      graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
+      graph.emplace_shared<MyFactor>(
+          uv, noise, C(i), P(j));  // note use of shorthand symbols C and P
     }
     j += 1;
   }
 
   // Add a prior on pose x1. This indirectly specifies where the origin is.
   // and a prior on the position of the first landmark to fix the scale
-  graph.emplace_shared<PriorFactor<SfM_Camera> >(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1));
-  graph.emplace_shared<PriorFactor<Point3> >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
+  graph.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
+  graph.addPrior(P(0), mydata.tracks[0].p,
+                 noiseModel::Isotropic::Sigma(3, 0.1));
 
   // Create initial estimate
   Values initial;
-  size_t i = 0; j = 0;
-  for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera);
-  for(const SfM_Track& track: mydata.tracks)    initial.insert(P(j++), track.p);
+  size_t i = 0;
+  j = 0;
+  for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera);
+  for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p);
 
   /** ---------------  COMPARISON  -----------------------**/
   /** ----------------------------------------------------**/
@@ -99,7 +100,7 @@ int main (int argc, char* argv[]) {
   }
 
   // expect they have different ordering results
-  if(params_using_COLAMD.ordering == params_using_METIS.ordering) {
+  if (params_using_COLAMD.ordering == params_using_METIS.ordering) {
     cout << "COLAMD and METIS produce the same ordering. "
          << "Problem here!!!" << endl;
   }
@@ -121,8 +122,7 @@ int main (int argc, char* argv[]) {
     cout << e.what();
   }
 
-
-  { // printing the result
+  {  // printing the result
 
     cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl;
     cout << "METIS final error: " << graph.error(result_METIS) << endl;
@@ -130,15 +130,13 @@ int main (int argc, char* argv[]) {
     cout << endl << endl;
 
     cout << "Time comparison by solving " << filename << " results:" << endl;
-    cout << boost::format("%1% point tracks and %2% cameras\n") \
-            % mydata.number_tracks() % mydata.number_cameras() \
+    cout << boost::format("%1% point tracks and %2% cameras\n") %
+                mydata.number_tracks() % mydata.number_cameras()
          << endl;
 
     tictoc_print_();
   }
 
-
   return 0;
 }
-/* ************************************************************************* */
 
diff --git a/examples/SFMdata.h b/examples/SFMdata.h
index 5462868c9..04d3c9e47 100644
--- a/examples/SFMdata.h
+++ b/examples/SFMdata.h
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    SFMMdata.h
+ * @file    SFMdata.h
  * @brief   Simple example for the structure-from-motion problems
  * @author  Duy-Nguyen Ta
  */
@@ -30,7 +30,8 @@
 #include <gtsam/geometry/Point3.h>
 
 // We will also need a camera object to hold calibration information and perform projections.
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 
 /* ************************************************************************* */
 std::vector<gtsam::Point3> createPoints() {
diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp
index 93e010543..201860012 100644
--- a/examples/SelfCalibrationExample.cpp
+++ b/examples/SelfCalibrationExample.cpp
@@ -33,8 +33,7 @@
 #include <gtsam/nonlinear/Values.h>
 
 // SFM-specific factors
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/GeneralSFMFactor.h> // does calibration !
+#include <gtsam/slam/GeneralSFMFactor.h>  // does calibration !
 
 // Standard headers
 #include <vector>
@@ -42,9 +41,7 @@
 using namespace std;
 using namespace gtsam;
 
-/* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Create the set of ground-truth
   vector<Point3> points = createPoints();
   vector<Pose3> poses = createPoses();
@@ -53,38 +50,51 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // Add a prior on pose x1.
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
+  auto poseNoise = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
+          .finished());  // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
 
-  // Simulated measurements from each camera pose, adding them to the factor graph
+  // Simulated measurements from each camera pose, adding them to the factor
+  // graph
   Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
-  noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
+  auto measurementNoise =
+      noiseModel::Isotropic::Sigma(2, 1.0);
   for (size_t i = 0; i < poses.size(); ++i) {
     for (size_t j = 0; j < points.size(); ++j) {
-      SimpleCamera camera(poses[i], K);
+      PinholeCamera<Cal3_S2> camera(poses[i], K);
       Point2 measurement = camera.project(points[j]);
-      // The only real difference with the Visual SLAM example is that here we use a
-      // different factor type, that also calculates the Jacobian with respect to calibration
-      graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0));
+      // The only real difference with the Visual SLAM example is that here we
+      // use a different factor type, that also calculates the Jacobian with
+      // respect to calibration
+      graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(
+          measurement, measurementNoise, Symbol('x', i), Symbol('l', j),
+          Symbol('K', 0));
     }
   }
 
   // Add a prior on the position of the first landmark.
-  noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
-  graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
+  auto pointNoise =
+      noiseModel::Isotropic::Sigma(3, 0.1);
+  graph.addPrior(Symbol('l', 0), points[0],
+                 pointNoise);  // add directly to graph
 
   // Add a prior on the calibration.
-  noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished());
-  graph.emplace_shared<PriorFactor<Cal3_S2> >(Symbol('K', 0), K, calNoise);
+  auto calNoise = noiseModel::Diagonal::Sigmas(
+      (Vector(5) << 500, 500, 0.1, 100, 100).finished());
+  graph.addPrior(Symbol('K', 0), K, calNoise);
 
   // Create the initial estimate to the solution
   // now including an estimate on the camera calibration parameters
   Values initialEstimate;
   initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
   for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+    initialEstimate.insert(
+        Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25),
+                                               Point3(0.05, -0.10, 0.20))));
   for (size_t j = 0; j < points.size(); ++j)
-    initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
+    initialEstimate.insert<Point3>(Symbol('l', j),
+                                   points[j] + Point3(-0.25, 0.20, 0.15));
 
   /* Optimize the graph and print results */
   Values result = DoglegOptimizer(graph, initialEstimate).optimize();
@@ -92,5 +102,4 @@ int main(int argc, char* argv[]) {
 
   return 0;
 }
-/* ************************************************************************* */
 
diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp
index f70a44eec..19909e353 100644
--- a/examples/SimpleRotation.cpp
+++ b/examples/SimpleRotation.cpp
@@ -32,8 +32,8 @@
 
 // In GTSAM, measurement functions are represented as 'factors'. Several common factors
 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
-// We will apply a simple prior on the rotation
-#include <gtsam/slam/PriorFactor.h>
+// We will apply a simple prior on the rotation. We do so via the `addPrior` convenience
+// method in NonlinearFactorGraph.
 
 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
 // are nonlinear factors, we will need a Nonlinear Factor Graph.
@@ -59,7 +59,6 @@ using namespace gtsam;
 const double degree = M_PI / 180;
 
 int main() {
-
   /**
    *    Step 1: Create a factor to express a unary constraint
    * The "prior" in this case is the measurement from a sensor,
@@ -76,9 +75,8 @@ int main() {
    */
   Rot2 prior = Rot2::fromAngle(30 * degree);
   prior.print("goal angle");
-  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
-  Symbol key('x',1);
-  PriorFactor<Rot2> factor(key, prior, model);
+  auto model = noiseModel::Isotropic::Sigma(1, 1 * degree);
+  Symbol key('x', 1);
 
   /**
    *    Step 2: Create a graph container and add the factor to it
@@ -90,7 +88,7 @@ int main() {
    * many more factors would be added.
    */
   NonlinearFactorGraph graph;
-  graph.push_back(factor);
+  graph.addPrior(key, prior, model);
   graph.print("full graph");
 
   /**
diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp
index 80ac08e03..d1155fe4c 100644
--- a/examples/SolverComparer.cpp
+++ b/examples/SolverComparer.cpp
@@ -34,7 +34,6 @@
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/slam/dataset.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@@ -58,9 +57,8 @@
 #include <iostream>
 
 #ifdef GTSAM_USE_TBB
-#include <tbb/tbb.h>
-#undef max // TBB seems to include windows.h and we don't want these macros
-#undef min
+#include <tbb/task_arena.h> // tbb::task_arena
+#include <tbb/task_group.h> // tbb::task_group
 #endif
 
 using namespace std;
@@ -206,10 +204,11 @@ int main(int argc, char *argv[]) {
   }
 
 #ifdef GTSAM_USE_TBB
-  std::unique_ptr<tbb::task_scheduler_init> init;
+  tbb::task_arena arena;
+  tbb::task_group tg;
   if(nThreads > 0) {
     cout << "Using " << nThreads << " threads" << endl;
-    init.reset(new tbb::task_scheduler_init(nThreads));
+    arena.initialize(nThreads);
   } else
     cout << "Using threads for all processors" << endl;
 #else
@@ -219,6 +218,10 @@ int main(int argc, char *argv[]) {
   }
 #endif
 
+#ifdef GTSAM_USE_TBB
+  arena.execute([&]{
+  tg.run_and_wait([&]{
+#endif
   // Run mode
   if(incremental)
     runIncremental();
@@ -230,6 +233,10 @@ int main(int argc, char *argv[]) {
     runPerturb();
   else if(stats)
     runStats();
+#ifdef GTSAM_USE_TBB
+  });
+  });
+#endif
 
   return 0;
 }
@@ -248,7 +255,7 @@ void runIncremental()
   cout << "Looking for first measurement from step " << firstStep << endl;
   size_t nextMeasurement = 0;
   bool havePreviousPose = false;
-  Key firstPose;
+  Key firstPose = 0;
   while(nextMeasurement < datasetMeasurements.size())
   {
     if(BetweenFactor<Pose>::shared_ptr factor =
@@ -281,7 +288,7 @@ void runIncremental()
     NonlinearFactorGraph newFactors;
     Values newVariables;
 
-    newFactors.push_back(boost::make_shared<PriorFactor<Pose> >(firstPose, Pose(), noiseModel::Unit::Create(3)));
+    newFactors.addPrior(firstPose, Pose(), noiseModel::Unit::Create(3));
     newVariables.insert(firstPose, Pose());
 
     isam2.update(newFactors, newVariables);
@@ -464,7 +471,7 @@ void runBatch()
   cout << "Creating batch optimizer..." << endl;
 
   NonlinearFactorGraph measurements = datasetMeasurements;
-  measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(3)));
+  measurements.addPrior(0, Pose(), noiseModel::Unit::Create(3));
 
   gttic_(Create_optimizer);
   GaussNewtonParams params;
diff --git a/examples/StereoVOExample.cpp b/examples/StereoVOExample.cpp
index 65ab5422d..4e9415547 100644
--- a/examples/StereoVOExample.cpp
+++ b/examples/StereoVOExample.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file SteroVOExample.cpp
+ * @file StereoVOExample.cpp
  * @brief A stereo visual odometry example
  * @date May 25, 2014
  * @author Stephen Camp
@@ -34,17 +34,17 @@
 using namespace std;
 using namespace gtsam;
 
-int main(int argc, char** argv){
-
-  //create graph object, add first pose at origin with key '1'
+int main(int argc, char** argv) {
+  // create graph object, add first pose at origin with key '1'
   NonlinearFactorGraph graph;
   Pose3 first_pose;
   graph.emplace_shared<NonlinearEquality<Pose3> >(1, Pose3());
 
-  //create factor noise model with 3 sigmas of value 1
-  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
-  //create stereo camera calibration object with .2m between cameras
-  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
+  // create factor noise model with 3 sigmas of value 1
+  const auto model = noiseModel::Isotropic::Sigma(3, 1);
+  // create stereo camera calibration object with .2m between cameras
+  const Cal3_S2Stereo::shared_ptr K(
+      new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
 
   //create and add stereo factors between first pose (key value 1) and the three landmarks
   graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(520, 480, 440), model, 1, 3, K);
@@ -56,17 +56,18 @@ int main(int argc, char** argv){
   graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(70, 20, 490), model, 2, 4, K);
   graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(320, 270, 115), model, 2, 5, K);
 
-  //create Values object to contain initial estimates of camera poses and landmark locations
+  // create Values object to contain initial estimates of camera poses and
+  // landmark locations
   Values initial_estimate;
 
-  //create and add iniital estimates
+  // create and add iniital estimates
   initial_estimate.insert(1, first_pose);
   initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
   initial_estimate.insert(3, Point3(1, 1, 5));
   initial_estimate.insert(4, Point3(-1, 1, 5));
   initial_estimate.insert(5, Point3(0, -0.5, 5));
 
-  //create Levenberg-Marquardt optimizer for resulting factor graph, optimize
+  // create Levenberg-Marquardt optimizer for resulting factor graph, optimize
   LevenbergMarquardtOptimizer optimizer(graph, initial_estimate);
   Values result = optimizer.optimize();
 
diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp
index 5eeb6ac3c..af81db703 100644
--- a/examples/StereoVOExample_large.cpp
+++ b/examples/StereoVOExample_large.cpp
@@ -10,7 +10,7 @@
 * -------------------------------------------------------------------------- */
 
 /**
-* @file SteroVOExample.cpp
+* @file StereoVOExample_large.cpp
 * @brief A stereo visual odometry example
 * @date May 25, 2014
 * @author Stephen Camp
@@ -41,31 +41,31 @@
 using namespace std;
 using namespace gtsam;
 
-int main(int argc, char** argv){
-
+int main(int argc, char** argv) {
   Values initial_estimate;
   NonlinearFactorGraph graph;
-  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
+  const auto model = noiseModel::Isotropic::Sigma(3, 1);
 
   string calibration_loc = findExampleDataFile("VO_calibration.txt");
   string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
   string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
 
-  //read camera calibration info from file
+  // read camera calibration info from file
   // focal lengths fx, fy, skew s, principal point u0, v0, baseline b
   double fx, fy, s, u0, v0, b;
   ifstream calibration_file(calibration_loc.c_str());
   cout << "Reading calibration info" << endl;
   calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
 
-  //create stereo camera calibration object
-  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b));
+  // create stereo camera calibration object
+  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, s, u0, v0, b));
 
   ifstream pose_file(pose_loc.c_str());
   cout << "Reading camera poses" << endl;
   int pose_id;
-  MatrixRowMajor m(4,4);
-  //read camera pose parameters and use to make initial estimates of camera poses
+  MatrixRowMajor m(4, 4);
+  // read camera pose parameters and use to make initial estimates of camera
+  // poses
   while (pose_file >> pose_id) {
     for (int i = 0; i < 16; i++) {
       pose_file >> m.data()[i];
@@ -76,33 +76,37 @@ int main(int argc, char** argv){
   // camera and landmark keys
   size_t x, l;
 
-  // pixel coordinates uL, uR, v (same for left/right images due to rectification)
-  // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
+  // pixel coordinates uL, uR, v (same for left/right images due to
+  // rectification) landmark coordinates X, Y, Z in camera frame, resulting from
+  // triangulation
   double uL, uR, v, X, Y, Z;
   ifstream factor_file(factor_loc.c_str());
   cout << "Reading stereo factors" << endl;
-  //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
+  // read stereo measurement details from file and use to create and add
+  // GenericStereoFactor objects to the graph representation
   while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
-    graph.emplace_shared<
-        GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model,
-            Symbol('x', x), Symbol('l', l), K);
-    //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
+    graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(
+        StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K);
+    // if the landmark variable included in this factor has not yet been added
+    // to the initial variable value estimate, add it
     if (!initial_estimate.exists(Symbol('l', l))) {
       Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
-      //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space
+      // transformFrom() transforms the input Point3 from the camera pose space,
+      // camPose, to the global space
       Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z));
       initial_estimate.insert(Symbol('l', l), worldPoint);
     }
   }
 
-  Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
-  //constrain the first pose such that it cannot change from its original value during optimization
+  Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x', 1));
+  // constrain the first pose such that it cannot change from its original value
+  // during optimization
   // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
   // QR is much slower than Cholesky, but numerically more stable
-  graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
+  graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x', 1), first_pose);
 
   cout << "Optimizing" << endl;
-  //create Levenberg-Marquardt optimizer to optimize the factor graph
+  // create Levenberg-Marquardt optimizer to optimize the factor graph
   LevenbergMarquardtParams params;
   params.orderingType = Ordering::METIS;
   LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params);
diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp
index 178fa513f..4bc5144f4 100644
--- a/examples/TimeTBB.cpp
+++ b/examples/TimeTBB.cpp
@@ -28,9 +28,12 @@ using boost::assign::list_of;
 
 #ifdef GTSAM_USE_TBB
 
-#include <tbb/tbb.h>
-#undef max // TBB seems to include windows.h and we don't want these macros
-#undef min
+#include <tbb/blocked_range.h>           // tbb::blocked_range
+#include <tbb/tick_count.h>              // tbb::tick_count
+#include <tbb/parallel_for.h>            // tbb::parallel_for
+#include <tbb/cache_aligned_allocator.h> // tbb::cache_aligned_allocator
+#include <tbb/task_arena.h>              // tbb::task_arena
+#include <tbb/task_group.h>              // tbb::task_group
 
 static const DenseIndex numberOfProblems = 1000000;
 static const DenseIndex problemSize = 4;
@@ -67,10 +70,14 @@ struct WorkerWithoutAllocation
 };
 
 /* ************************************************************************* */
-map<int, double> testWithoutMemoryAllocation()
+map<int, double> testWithoutMemoryAllocation(int num_threads)
 {
   // A function to do some matrix operations without allocating any memory
 
+  // Create task_arena and task_group
+  tbb::task_arena arena(num_threads);
+  tbb::task_group tg;
+
   // Now call it
   vector<double> results(numberOfProblems);
 
@@ -79,7 +86,14 @@ map<int, double> testWithoutMemoryAllocation()
   for(size_t grainSize: grainSizes)
   {
     tbb::tick_count t0 = tbb::tick_count::now();
-    tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithoutAllocation(results));
+
+    // Run parallel code (as a task group) inside of task arena
+    arena.execute([&]{
+      tg.run_and_wait([&]{
+        tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithoutAllocation(results));
+      });
+    });
+
     tbb::tick_count t1 = tbb::tick_count::now();
     cout << "Without memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
     timingResults[(int)grainSize] = (t1 - t0).seconds();
@@ -120,10 +134,14 @@ struct WorkerWithAllocation
 };
 
 /* ************************************************************************* */
-map<int, double> testWithMemoryAllocation()
+map<int, double> testWithMemoryAllocation(int num_threads)
 {
   // A function to do some matrix operations with allocating memory
 
+  // Create task_arena and task_group
+  tbb::task_arena arena(num_threads);
+  tbb::task_group tg;
+
   // Now call it
   vector<double> results(numberOfProblems);
 
@@ -132,7 +150,14 @@ map<int, double> testWithMemoryAllocation()
   for(size_t grainSize: grainSizes)
   {
     tbb::tick_count t0 = tbb::tick_count::now();
-    tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
+
+    // Run parallel code (as a task group) inside of task arena
+    arena.execute([&]{
+      tg.run_and_wait([&]{
+        tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
+      });
+    });
+
     tbb::tick_count t1 = tbb::tick_count::now();
     cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
     timingResults[(int)grainSize] = (t1 - t0).seconds();
@@ -153,9 +178,8 @@ int main(int argc, char* argv[])
   for(size_t n: numThreads)
   {
     cout << "With " << n << " threads:" << endl;
-    tbb::task_scheduler_init init((int)n);
-    results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
-    results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation();
+    results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation((int)n);
+    results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation((int)n);
     cout << endl;
   }
 
diff --git a/examples/UGM_chain.cpp b/examples/UGM_chain.cpp
index 4ce4e7af4..3a885a844 100644
--- a/examples/UGM_chain.cpp
+++ b/examples/UGM_chain.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file small.cpp
+ * @file UGM_chain.cpp
  * @brief UGM (undirected graphical model) examples: chain
  * @author Frank Dellaert
  * @author Abhijit Kundu
@@ -19,10 +19,9 @@
  * for more explanation. This code demos the same example using GTSAM.
  */
 
-#include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/discrete/DiscreteSequentialSolver.h>
-#include <gtsam/discrete/DiscreteMarginals.h>
 #include <gtsam/base/timing.h>
+#include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/discrete/DiscreteMarginals.h>
 
 #include <iomanip>
 
@@ -30,9 +29,8 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
-    // Set Number of Nodes in the Graph
-    const int nrNodes = 60;
+  // Set Number of Nodes in the Graph
+  const int nrNodes = 60;
 
   // Each node takes 1 of 7 possible states denoted by 0-6 in following order:
   // ["VideoGames"  "Industry"  "GradSchool"  "VideoGames(with PhD)"
@@ -40,70 +38,55 @@ int main(int argc, char** argv) {
   const size_t nrStates = 7;
 
   // define variables
-    vector<DiscreteKey> nodes;
-    for (int i = 0; i < nrNodes; i++){
-        DiscreteKey dk(i, nrStates);
-        nodes.push_back(dk);
-    }
+  vector<DiscreteKey> nodes;
+  for (int i = 0; i < nrNodes; i++) {
+    DiscreteKey dk(i, nrStates);
+    nodes.push_back(dk);
+  }
 
   // create graph
   DiscreteFactorGraph graph;
 
   // add node potentials
   graph.add(nodes[0], ".3 .6 .1 0 0 0 0");
-    for (int i = 1; i < nrNodes; i++)
-        graph.add(nodes[i], "1 1 1 1 1 1 1");
+  for (int i = 1; i < nrNodes; i++) graph.add(nodes[i], "1 1 1 1 1 1 1");
 
-    const std::string edgePotential =   ".08 .9 .01 0 0 0 .01 "
-                                        ".03 .95 .01 0 0 0 .01 "
-                                        ".06 .06 .75 .05 .05 .02 .01 "
-                                        "0 0 0 .3 .6 .09 .01 "
-                                        "0 0 0 .02 .95 .02 .01 "
-                                        "0 0 0 .01 .01 .97 .01 "
-                                        "0 0 0 0 0 0 1";
+  const std::string edgePotential =
+      ".08 .9 .01 0 0 0 .01 "
+      ".03 .95 .01 0 0 0 .01 "
+      ".06 .06 .75 .05 .05 .02 .01 "
+      "0 0 0 .3 .6 .09 .01 "
+      "0 0 0 .02 .95 .02 .01 "
+      "0 0 0 .01 .01 .97 .01 "
+      "0 0 0 0 0 0 1";
 
   // add edge potentials
   for (int i = 0; i < nrNodes - 1; i++)
     graph.add(nodes[i] & nodes[i + 1], edgePotential);
 
   cout << "Created Factor Graph with " << nrNodes << " variable nodes and "
-      << graph.size() << " factors (Unary+Edge).";
+       << graph.size() << " factors (Unary+Edge).";
 
   // "Decoding", i.e., configuration with largest value
   // We use sequential variable elimination
-  DiscreteSequentialSolver solver(graph);
-  DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
+  DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
+  DiscreteFactor::sharedValues optimalDecoding = chordal->optimize();
   optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n");
 
   // "Inference" Computing marginals for each node
-
-
-  cout << "\nComputing Node Marginals ..(Sequential Elimination)" << endl;
-  gttic_(Sequential);
-  for (vector<DiscreteKey>::iterator itr = nodes.begin(); itr != nodes.end();
-      ++itr) {
-    //Compute the marginal
-    Vector margProbs = solver.marginalProbabilities(*itr);
-
-    //Print the marginals
-    cout << "Node#" << setw(4) << itr->first << " :  ";
-    print(margProbs);
-  }
-  gttoc_(Sequential);
-
   // Here we'll make use of DiscreteMarginals class, which makes use of
   // bayes-tree based shortcut evaluation of marginals
   DiscreteMarginals marginals(graph);
 
   cout << "\nComputing Node Marginals ..(BayesTree based)" << endl;
   gttic_(Multifrontal);
-  for (vector<DiscreteKey>::iterator itr = nodes.begin(); itr != nodes.end();
-      ++itr) {
-    //Compute the marginal
-    Vector margProbs = marginals.marginalProbabilities(*itr);
+  for (vector<DiscreteKey>::iterator it = nodes.begin(); it != nodes.end();
+       ++it) {
+    // Compute the marginal
+    Vector margProbs = marginals.marginalProbabilities(*it);
 
-    //Print the marginals
-    cout << "Node#" << setw(4) << itr->first << " :  ";
+    // Print the marginals
+    cout << "Node#" << setw(4) << it->first << " :  ";
     print(margProbs);
   }
   gttoc_(Multifrontal);
@@ -111,4 +94,3 @@ int main(int argc, char** argv) {
   tictoc_print_();
   return 0;
 }
-
diff --git a/examples/UGM_small.cpp b/examples/UGM_small.cpp
index f5338bc67..27a6205a3 100644
--- a/examples/UGM_small.cpp
+++ b/examples/UGM_small.cpp
@@ -10,15 +10,16 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file small.cpp
+ * @file UGM_small.cpp
  * @brief UGM (undirected graphical model) examples: small
  * @author Frank Dellaert
  *
  * See http://www.di.ens.fr/~mschmidt/Software/UGM/small.html
  */
 
+#include <gtsam/base/Vector.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/discrete/DiscreteSequentialSolver.h>
+#include <gtsam/discrete/DiscreteMarginals.h>
 
 using namespace std;
 using namespace gtsam;
@@ -61,24 +62,24 @@ int main(int argc, char** argv) {
 
   // "Decoding", i.e., configuration with largest value (MPE)
   // We use sequential variable elimination
-  DiscreteSequentialSolver solver(graph);
-  DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
+  DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
+  DiscreteFactor::sharedValues optimalDecoding = chordal->optimize();
   optimalDecoding->print("\noptimalDecoding");
 
   // "Inference" Computing marginals
   cout << "\nComputing Node Marginals .." << endl;
-  Vector margProbs;
+  DiscreteMarginals marginals(graph);
 
-  margProbs = solver.marginalProbabilities(Cathy);
+  Vector margProbs = marginals.marginalProbabilities(Cathy);
   print(margProbs, "Cathy's Node Marginal:");
 
-  margProbs = solver.marginalProbabilities(Heather);
+  margProbs = marginals.marginalProbabilities(Heather);
   print(margProbs, "Heather's Node Marginal");
 
-  margProbs = solver.marginalProbabilities(Mark);
+  margProbs = marginals.marginalProbabilities(Mark);
   print(margProbs, "Mark's Node Marginal");
 
-  margProbs = solver.marginalProbabilities(Allison);
+  margProbs = marginals.marginalProbabilities(Allison);
   print(margProbs, "Allison's Node Marginal");
 
   return 0;
diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp
index 751b776f6..b05fd9e6f 100644
--- a/examples/VisualISAM2Example.cpp
+++ b/examples/VisualISAM2Example.cpp
@@ -47,7 +47,6 @@
 // Adjustment problems. Here we will use Projection factors to model the
 // camera's landmark observations. Also, we will initialize the robot at some
 // location using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 
 #include <vector>
@@ -89,7 +88,7 @@ int main(int argc, char* argv[]) {
   for (size_t i = 0; i < poses.size(); ++i) {
     // Add factors for each landmark observation
     for (size_t j = 0; j < points.size(); ++j) {
-      SimpleCamera camera(poses[i], *K);
+      PinholeCamera<Cal3_S2> camera(poses[i], *K);
       Point2 measurement = camera.project(points[j]);
       graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(
           measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
@@ -108,15 +107,13 @@ int main(int argc, char* argv[]) {
     if (i == 0) {
       // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
       static auto kPosePrior = noiseModel::Diagonal::Sigmas(
-          (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))
+          (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
               .finished());
-      graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0],
-                                                kPosePrior);
+      graph.addPrior(Symbol('x', 0), poses[0], kPosePrior);
 
       // Add a prior on landmark l0
       static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
-      graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0],
-                                                 kPointPrior);
+      graph.addPrior(Symbol('l', 0), points[0], kPointPrior);
 
       // Add initial guesses to all observed landmarks
       // Intentionally initialize the variables off from the ground truth
diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp
index 8ca1be596..c18781169 100644
--- a/examples/VisualISAMExample.cpp
+++ b/examples/VisualISAMExample.cpp
@@ -38,7 +38,6 @@
 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
 // Here we will use Projection factors to model the camera's landmark observations.
 // Also, we will initialize the robot at some location using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 
 // We want to use iSAM to solve the structure-from-motion problem incrementally, so
@@ -57,13 +56,11 @@ using namespace gtsam;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Define the camera calibration parameters
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr noise = //
-      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+  auto noise = noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
 
   // Create the set of ground-truth landmarks
   vector<Point3> points = createPoints();
@@ -82,11 +79,10 @@ int main(int argc, char* argv[]) {
 
   // Loop over the different poses, adding the observations to iSAM incrementally
   for (size_t i = 0; i < poses.size(); ++i) {
-
     // Add factors for each landmark observation
     for (size_t j = 0; j < points.size(); ++j) {
       // Create ground truth measurement
-      SimpleCamera camera(poses[i], *K);
+      PinholeCamera<Cal3_S2> camera(poses[i], *K);
       Point2 measurement = camera.project(points[j]);
       // Add measurement
       graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, noise,
@@ -106,14 +102,14 @@ int main(int argc, char* argv[]) {
     // adding it to iSAM.
     if (i == 0) {
       // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-      noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
-          (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
-      graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
+      auto poseNoise = noiseModel::Diagonal::Sigmas(
+          (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
+      graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
 
       // Add a prior on landmark l0
-      noiseModel::Isotropic::shared_ptr pointNoise =
+      auto pointNoise =
           noiseModel::Isotropic::Sigma(3, 0.1);
-      graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise);
+      graph.addPrior(Symbol('l', 0), points[0], pointNoise);
 
       // Add initial guesses to all observed landmarks
       Point3 noise(-0.25, 0.20, 0.15);
diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp
index 49b99a5b6..7693fa4e4 100644
--- a/examples/easyPoint2KalmanFilter.cpp
+++ b/examples/easyPoint2KalmanFilter.cpp
@@ -23,7 +23,7 @@
 
 #include <gtsam/nonlinear/ExtendedKalmanFilter.h>
 #include <gtsam/inference/Symbol.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Point2.h>
 
diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp
index 1dde7efb5..a2ad8cf0b 100644
--- a/examples/elaboratePoint2KalmanFilter.cpp
+++ b/examples/elaboratePoint2KalmanFilter.cpp
@@ -20,7 +20,7 @@
  * @author Stephen Williams
  */
 
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 //#include <gtsam/nonlinear/Ordering.h>
 #include <gtsam/inference/Symbol.h>
diff --git a/gtsam.h b/gtsam.h
index bf3575580..2cd30be42 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -281,7 +281,7 @@ virtual class Value {
 };
 
 #include <gtsam/base/GenericValue.h>
-template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2,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::SimpleCamera, gtsam::imuBias::ConstantBias}>
 virtual class GenericValue : gtsam::Value {
   void serializable() const;
 };
@@ -537,6 +537,89 @@ class Rot2 {
   void serialize() const;
 };
 
+#include <gtsam/geometry/SO3.h>
+class SO3 {
+  // Standard Constructors
+  SO3();
+  SO3(Matrix R);
+  static gtsam::SO3 FromMatrix(Matrix R);
+  static gtsam::SO3 AxisAngle(const Vector axis, double theta);
+  static gtsam::SO3 ClosestTo(const Matrix M);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::SO3& other, double tol) const;
+
+  // Group
+  static gtsam::SO3 identity();
+  gtsam::SO3 inverse() const;
+  gtsam::SO3 between(const gtsam::SO3& R) const;
+  gtsam::SO3 compose(const gtsam::SO3& R) const;
+
+  // Manifold
+  gtsam::SO3 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::SO3& R) const;
+  static gtsam::SO3 Expmap(Vector v);
+
+  // Other methods
+  Vector vec() const;
+  Matrix matrix() const;
+};
+
+#include <gtsam/geometry/SO4.h>
+class SO4 {
+  // Standard Constructors
+  SO4();
+  SO4(Matrix R);
+  static gtsam::SO4 FromMatrix(Matrix R);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::SO4& other, double tol) const;
+
+  // Group
+  static gtsam::SO4 identity();
+  gtsam::SO4 inverse() const;
+  gtsam::SO4 between(const gtsam::SO4& Q) const;
+  gtsam::SO4 compose(const gtsam::SO4& Q) const;
+
+  // Manifold
+  gtsam::SO4 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::SO4& Q) const;
+  static gtsam::SO4 Expmap(Vector v);
+
+  // Other methods
+  Vector vec() const;
+  Matrix matrix() const;
+};
+
+#include <gtsam/geometry/SOn.h>
+class SOn {
+  // Standard Constructors
+  SOn(size_t n);
+  static gtsam::SOn FromMatrix(Matrix R);
+  static gtsam::SOn Lift(size_t n, Matrix R);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::SOn& other, double tol) const;
+
+  // Group
+  static gtsam::SOn identity();
+  gtsam::SOn inverse() const;
+  gtsam::SOn between(const gtsam::SOn& Q) const;
+  gtsam::SOn compose(const gtsam::SOn& Q) const;
+
+  // Manifold
+  gtsam::SOn retract(Vector v) const;
+  Vector localCoordinates(const gtsam::SOn& Q) const;
+  static gtsam::SOn Expmap(Vector v);
+
+  // Other methods
+  Vector vec() const;
+  Matrix matrix() const;
+};
+
 #include <gtsam/geometry/Rot3.h>
 class Rot3 {
   // Standard Constructors and Named Constructors
@@ -557,8 +640,10 @@ class Rot3 {
   static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft)
   static gtsam::Rot3 Ypr(double y, double p, double r);
   static gtsam::Rot3 Quaternion(double w, double x, double y, double z);
+  static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle);
   static gtsam::Rot3 Rodrigues(Vector v);
   static gtsam::Rot3 Rodrigues(double wx, double wy, double wz);
+  static gtsam::Rot3 ClosestTo(const Matrix M);
 
   // Testable
   void print(string s) const;
@@ -591,8 +676,10 @@ class Rot3 {
   double roll() const;
   double pitch() const;
   double yaw() const;
+  pair<gtsam::Unit3, double> axisAngle() const;
 //  Vector toQuaternion() const;  // FIXME: Can't cast to Vector properly
   Vector quaternion() const;
+  gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -731,6 +818,7 @@ class Unit3 {
   // Other functionality
   Matrix basis() const;
   Matrix skew() const;
+  gtsam::Point3 point3() const;
 
   // Manifold
   static size_t Dim();
@@ -791,6 +879,7 @@ class Cal3_S2 {
   double py() const;
   gtsam::Point2 principalPoint() const;
   Vector vector() const;
+  Matrix K() const;
   Matrix matrix() const;
   Matrix matrix_inverse() const;
 
@@ -1037,6 +1126,8 @@ virtual class SimpleCamera {
 
 };
 
+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;
@@ -1080,6 +1171,9 @@ class StereoCamera {
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
     gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements,
     double rank_tol, bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+    gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements,
+    double rank_tol, bool optimize);
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
     gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements,
     double rank_tol, bool optimize);
@@ -1219,7 +1313,7 @@ class SymbolicBayesTree {
 // class SymbolicBayesTreeClique {
 //   BayesTreeClique();
 //   BayesTreeClique(CONDITIONAL* conditional);
-// //  BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
+// //  BayesTreeClique(const pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
 //
 //   bool equals(const This& other, double tol) const;
 //   void print(string s) const;
@@ -1281,6 +1375,7 @@ virtual class Base {
 };
 
 virtual class Gaussian : gtsam::noiseModel::Base {
+  static gtsam::noiseModel::Gaussian* Information(Matrix R);
   static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
   static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
 
@@ -1362,6 +1457,9 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base {
 
   // enabling serialization functionality
   void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
 };
 
 virtual class Fair: gtsam::noiseModel::mEstimator::Base {
@@ -1370,6 +1468,9 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base {
 
   // enabling serialization functionality
   void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
 };
 
 virtual class Huber: gtsam::noiseModel::mEstimator::Base {
@@ -1378,6 +1479,20 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base {
 
   // enabling serialization functionality
   void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
+
+virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
+  Cauchy(double k);
+  static gtsam::noiseModel::mEstimator::Cauchy* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
 };
 
 virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
@@ -1386,6 +1501,9 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
 
   // enabling serialization functionality
   void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
 };
 
 virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
@@ -1394,8 +1512,43 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
 
   // enabling serialization functionality
   void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
 };
 
+virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
+  GemanMcClure(double c);
+  static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
+
+virtual class DCS: gtsam::noiseModel::mEstimator::Base {
+  DCS(double c);
+  static gtsam::noiseModel::mEstimator::DCS* Create(double c);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
+
+virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
+  L2WithDeadZone(double k);
+  static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double loss(double error) const;
+};
 
 }///\namespace mEstimator
 
@@ -1411,17 +1564,15 @@ virtual class Robust : gtsam::noiseModel::Base {
 
 #include <gtsam/linear/Sampler.h>
 class Sampler {
-    //Constructors
+  // Constructors
   Sampler(gtsam::noiseModel::Diagonal* model, int seed);
   Sampler(Vector sigmas, int seed);
-  Sampler(int seed);
 
-    //Standard Interface
+  // Standard Interface
   size_t dim() const;
   Vector sigmas() const;
   gtsam::noiseModel::Diagonal* model() const;
   Vector sample();
-  Vector sampleNewModel(gtsam::noiseModel::Diagonal* model);
 };
 
 #include <gtsam/linear/VectorValues.h>
@@ -1657,6 +1808,9 @@ virtual class GaussianConditional : gtsam::GaussianFactor {
   gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const;
   void solveTransposeInPlace(gtsam::VectorValues& gy) const;
   void scaleFrontalsBySigma(gtsam::VectorValues& gy) const;
+  Matrix R() const;
+  Matrix S() const;
+  Vector d() const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -1784,6 +1938,22 @@ virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParamete
   void print() const;
 };
 
+#include <gtsam/linear/Preconditioner.h>
+virtual class PreconditionerParameters {
+  PreconditionerParameters();
+};
+
+virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
+  DummyPreconditionerParameters();
+};
+
+#include <gtsam/linear/PCGSolver.h>
+virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
+  PCGSolverParameters();
+  void print(string s);
+  void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner);
+};
+
 #include <gtsam/linear/SubgraphSolver.h>
 virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
   SubgraphSolverParameters();
@@ -1824,6 +1994,35 @@ size_t symbol(char chr, size_t index);
 char symbolChr(size_t key);
 size_t symbolIndex(size_t key);
 
+namespace symbol_shorthand {
+  size_t A(size_t j);
+  size_t B(size_t j);
+  size_t C(size_t j);
+  size_t D(size_t j);
+  size_t E(size_t j);
+  size_t F(size_t j);
+  size_t G(size_t j);
+  size_t H(size_t j);
+  size_t I(size_t j);
+  size_t J(size_t j);
+  size_t K(size_t j);
+  size_t L(size_t j);
+  size_t M(size_t j);
+  size_t N(size_t j);
+  size_t O(size_t j);
+  size_t P(size_t j);
+  size_t Q(size_t j);
+  size_t R(size_t j);
+  size_t S(size_t j);
+  size_t T(size_t j);
+  size_t U(size_t j);
+  size_t V(size_t j);
+  size_t W(size_t j);
+  size_t X(size_t j);
+  size_t Y(size_t j);
+  size_t Z(size_t j);
+}///\namespace symbol
+
 // Default keyformatter
 void PrintKeyList  (const gtsam::KeyList& keys);
 void PrintKeyList  (const gtsam::KeyList& keys, string s);
@@ -1897,6 +2096,9 @@ 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}>
+  void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
+
   // NonlinearFactorGraph
   void printErrors(const gtsam::Values& values) const;
   double error(const gtsam::Values& values) const;
@@ -1974,14 +2176,18 @@ class Values {
   void insert(size_t j, const gtsam::Point3& point3);
   void insert(size_t j, const gtsam::Rot2& rot2);
   void insert(size_t j, const gtsam::Pose2& pose2);
+  void insert(size_t j, const gtsam::SO3& R);
+  void insert(size_t j, const gtsam::SO4& Q);
+  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::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::SimpleCamera& simpel_camera);
+  void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
+  void insert(size_t j, const gtsam::NavState& nav_state);
   void insert(size_t j, Vector vector);
   void insert(size_t j, Matrix matrix);
 
@@ -1989,6 +2195,9 @@ class Values {
   void update(size_t j, const gtsam::Point3& point3);
   void update(size_t j, const gtsam::Rot2& rot2);
   void update(size_t j, const gtsam::Pose2& pose2);
+  void update(size_t j, const gtsam::SO3& R);
+  void update(size_t j, const gtsam::SO4& Q);
+  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::Cal3_S2& cal3_s2);
@@ -1996,10 +2205,11 @@ class Values {
   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::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::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, 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::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
   T at(size_t j);
 
   /// version for double
@@ -2011,6 +2221,10 @@ class Values {
 class Marginals {
   Marginals(const gtsam::NonlinearFactorGraph& graph,
       const gtsam::Values& solution);
+  Marginals(const gtsam::GaussianFactorGraph& gfgraph,
+      const gtsam::Values& solution);
+  Marginals(const gtsam::GaussianFactorGraph& gfgraph,
+      const gtsam::VectorValues& solutionvec);
 
   void print(string s) const;
   Matrix marginalCovariance(size_t variable) const;
@@ -2095,8 +2309,10 @@ virtual class NonlinearOptimizerParams {
 };
 
 bool checkConvergence(double relativeErrorTreshold,
-    double absoluteErrorTreshold, double errorThreshold,
-    double currentError, double newError);
+                      double absoluteErrorTreshold, double errorThreshold,
+                      double currentError, double newError);
+bool checkConvergence(const gtsam::NonlinearOptimizerParams& params,
+                      double currentError, double newError);
 
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams {
@@ -2294,7 +2510,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, Vector, Matrix}>
+                     gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, Vector, Matrix}>
   VALUE calculateEstimate(size_t key) const;
   gtsam::Values calculateBestEstimate() const;
   Matrix marginalCovariance(size_t key) const;
@@ -2331,8 +2547,8 @@ class NonlinearISAM {
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/StereoPoint2.h>
 
-#include <gtsam/slam/PriorFactor.h>
-template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+#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}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
   T prior() const;
@@ -2343,7 +2559,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
 
 
 #include <gtsam/slam/BetweenFactor.h>
-template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
+template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
 virtual class BetweenFactor : gtsam::NoiseModelFactor {
   BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
   T measured() const;
@@ -2355,7 +2571,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
 
 
 #include <gtsam/nonlinear/NonlinearEquality.h>
-template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+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::imuBias::ConstantBias}>
 virtual class NonlinearEquality : gtsam::NoiseModelFactor {
   // Constructor - forces exact evaluation
   NonlinearEquality(size_t j, const T& feasible);
@@ -2381,9 +2597,9 @@ typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
 typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
-typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
+typedef gtsam::RangeFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> RangeFactorSimpleCameraPoint;
 typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
-typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
+typedef gtsam::RangeFactor<gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3_S2> RangeFactorSimpleCamera;
 
 
 #include <gtsam/sam/RangeFactor.h>
@@ -2474,7 +2690,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
   GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey);
   gtsam::Point2 measured() const;
 };
-typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
 // due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
 //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 
@@ -2563,6 +2779,20 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
 };
 
 #include <gtsam/slam/dataset.h>
+class SfmTrack {
+  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;
+};
+
+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::SfmTrack track(size_t idx) const;
+};
+
 string findExampleDataFile(string name);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
     gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
@@ -2582,8 +2812,10 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
 // std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
 class BetweenFactorPose3s
 {
+  BetweenFactorPose3s();
   size_t size() const;
   gtsam::BetweenFactorPose3* at(size_t i) const;
+  void push_back(const gtsam::BetweenFactorPose3* factor);
 };
 
 #include <gtsam/slam/InitializePose3.h>
@@ -2614,6 +2846,40 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool
 void writeG2o(const gtsam::NonlinearFactorGraph& graph,
     const gtsam::Values& estimate, string filename);
 
+#include <gtsam/slam/KarcherMeanFactor-inl.h>
+template<T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
+virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
+  KarcherMeanFactor(const gtsam::KeyVector& keys);
+};
+
+#include <gtsam/slam/FrobeniusFactor.h>
+gtsam::noiseModel::Isotropic* ConvertPose3NoiseModel(
+    gtsam::noiseModel::Base* model, size_t d);
+
+template<T = {gtsam::SO3, gtsam::SO4}>
+virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
+  FrobeniusFactor(size_t key1, size_t key2);
+  FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model);
+
+  Vector evaluateError(const T& R1, const T& R2);
+};
+
+template<T = {gtsam::SO3, gtsam::SO4}>
+virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
+  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12);
+  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model);
+
+  Vector evaluateError(const T& R1, const T& R2);
+};
+
+virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor {
+  FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
+                          size_t p);
+  FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
+                          size_t p, gtsam::noiseModel::Base* model);
+  Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2);
+};
+
 //*************************************************************************
 // Navigation
 //*************************************************************************
@@ -2727,11 +2993,14 @@ class PreintegratedImuMeasurements {
   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
       double deltaT);
   void resetIntegration();
+  void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
+
   Matrix preintMeasCov() const;
   double deltaTij() const;
   gtsam::Rot3 deltaRij() const;
   Vector deltaPij() const;
   Vector deltaVij() const;
+  gtsam::imuBias::ConstantBias biasHat() const;
   Vector biasHatVector() const;
   gtsam::NavState predict(const gtsam::NavState& state_i,
       const gtsam::imuBias::ConstantBias& bias) const;
@@ -2750,7 +3019,33 @@ virtual class ImuFactor: gtsam::NonlinearFactor {
 };
 
 #include <gtsam/navigation/CombinedImuFactor.h>
+virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
+  PreintegrationCombinedParams(Vector n_gravity);
+
+  static gtsam::PreintegrationCombinedParams* MakeSharedD(double g);
+  static gtsam::PreintegrationCombinedParams* MakeSharedU(double g);
+  static gtsam::PreintegrationCombinedParams* MakeSharedD();  // default g = 9.81
+  static gtsam::PreintegrationCombinedParams* MakeSharedU();  // default g = 9.81
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol);
+
+  void setBiasAccCovariance(Matrix cov);
+  void setBiasOmegaCovariance(Matrix cov);
+  void setBiasAccOmegaInt(Matrix cov);
+  
+  Matrix getBiasAccCovariance() const ;
+  Matrix getBiasOmegaCovariance() const ;
+  Matrix getBiasAccOmegaInt() const;
+ 
+};
+
 class PreintegratedCombinedMeasurements {
+// Constructors
+  PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params);
+  PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params,
+				    const gtsam::imuBias::ConstantBias& bias);
   // Testable
   void print(string s) const;
   bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
@@ -2760,11 +3055,14 @@ class PreintegratedCombinedMeasurements {
   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
       double deltaT);
   void resetIntegration();
+  void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
+
   Matrix preintMeasCov() const;
   double deltaTij() const;
   gtsam::Rot3 deltaRij() const;
   Vector deltaPij() const;
   Vector deltaVij() const;
+  gtsam::imuBias::ConstantBias biasHat() const;
   Vector biasHatVector() const;
   gtsam::NavState predict(const gtsam::NavState& state_i,
       const gtsam::imuBias::ConstantBias& bias) const;
@@ -2848,6 +3146,31 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor {
   gtsam::Unit3 bRef() const;
 };
 
+#include <gtsam/navigation/GPSFactor.h>
+virtual class GPSFactor : gtsam::NonlinearFactor{
+  GPSFactor(size_t key, const gtsam::Point3& gpsIn,
+            const gtsam::noiseModel::Base* model);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::GPSFactor& expected, double tol);
+
+  // Standard Interface
+  gtsam::Point3 measurementIn() const;
+};
+
+virtual class GPSFactor2 : gtsam::NonlinearFactor {
+  GPSFactor2(size_t key, const gtsam::Point3& gpsIn,
+            const gtsam::noiseModel::Base* model);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::GPSFactor2& expected, double tol);
+
+  // Standard Interface
+  gtsam::Point3 measurementIn() const;
+};
+
 #include <gtsam/navigation/Scenario.h>
 virtual class Scenario {
   gtsam::Pose3 pose(double t) const;
@@ -2917,7 +3240,7 @@ namespace utilities {
   void perturbPoint2(gtsam::Values& values, double sigma, int seed);
   void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed);
   void perturbPoint3(gtsam::Values& values, double sigma, int seed);
-  void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
+  void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCameraCal3_S2& c, Vector J, Matrix Z, double depth);
   void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
   void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
   Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c
index d5e8da2f0..9a08e3ea8 100644
--- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c
+++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c
@@ -1560,9 +1560,8 @@ PUBLIC Int CCOLAMD_2	    /* returns TRUE if successful, FALSE otherwise */
     Int *dead_cols ;
     Int set1 ;
     Int set2 ;
-#ifndef NDEBUG
     Int cs ;
-#endif
+
     int ok ;
 
 #ifndef NDEBUG
@@ -1910,10 +1909,8 @@ PUBLIC Int CCOLAMD_2	    /* returns TRUE if successful, FALSE otherwise */
             p [k] = col ;
             ASSERT (A [col] == EMPTY) ;
 
-#ifndef NDEBUG
-	    	cs = CMEMBER (col) ;
+	    cs = CMEMBER (col) ;
             ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ;
-#endif
 
             A [col] = k ;
             k++ ;
@@ -1929,11 +1926,11 @@ PUBLIC Int CCOLAMD_2	    /* returns TRUE if successful, FALSE otherwise */
             if (A [col] == EMPTY)
             {
                 k = Col [col].shared2.order ;
+		cs = CMEMBER (col) ;
 #ifndef NDEBUG
-				cs = CMEMBER (col) ;
                 dead_cols [cs]-- ;
-                ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ;
 #endif
+                ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ;
                 DEBUG1 (("ccolamd output ordering: k "ID" col "ID
                     " (dense or null col)\n", k, col)) ;
                 p [k] = col ;
diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt
index ed18b7aad..c8fecc339 100644
--- a/gtsam/3rdparty/CMakeLists.txt
+++ b/gtsam/3rdparty/CMakeLists.txt
@@ -1,6 +1,6 @@
 # install CCOLAMD headers
-install(FILES CCOLAMD/Include/ccolamd.h DESTINATION include/gtsam/3rdparty/CCOLAMD)
-install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION include/gtsam/3rdparty/SuiteSparse_config)
+install(FILES CCOLAMD/Include/ccolamd.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/CCOLAMD)
+install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/SuiteSparse_config)
 
 if(NOT GTSAM_USE_SYSTEM_EIGEN)
 	# Find plain .h files
@@ -12,12 +12,12 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 		get_filename_component(filename ${eigen_dir} NAME)
 		if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn")))
 			list(APPEND eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/Eigen/${filename}")
-			install(FILES Eigen/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/Eigen)
+			install(FILES Eigen/Eigen/${filename} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen/Eigen)
 		endif()
 	endforeach(eigen_dir)
 
 	if(GTSAM_WITH_EIGEN_UNSUPPORTED)
-		message("-- Installing Eigen Unsupported modules")
+		message(STATUS "Installing Eigen Unsupported modules")
 		# do the same for the unsupported eigen folder
 		file(GLOB_RECURSE  unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h")
 
@@ -26,7 +26,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 			get_filename_component(filename ${unsupported_eigen_dir} NAME)
 			if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES "CXX11") OR (${filename} MATCHES ".svn")))
 				list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}")
-				install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/unsupported/Eigen)
+				install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen/unsupported/Eigen)
 			endif()
 		endforeach(unsupported_eigen_dir)
 	endif()
@@ -37,12 +37,12 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 
 	# install Eigen - only the headers in our 3rdparty directory
 	install(DIRECTORY Eigen/Eigen
-		DESTINATION include/gtsam/3rdparty/Eigen
+		DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen
 		FILES_MATCHING PATTERN "*.h")
 
 	if(GTSAM_WITH_EIGEN_UNSUPPORTED)
 		install(DIRECTORY Eigen/unsupported/Eigen
-			DESTINATION include/gtsam/3rdparty/Eigen/unsupported/
+			DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen/unsupported/
 			FILES_MATCHING PATTERN "*.h")
 	endif()
 
diff --git a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt
index 9887d5804..e2f1dd6b4 100644
--- a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt
@@ -39,9 +39,9 @@ endif()
 add_dependencies(blas eigen_blas eigen_blas_static)
 
 install(TARGETS eigen_blas eigen_blas_static
-        RUNTIME DESTINATION bin
-        LIBRARY DESTINATION lib
-        ARCHIVE DESTINATION lib)
+        RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+        LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+        ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR})
 
 if(EIGEN_Fortran_COMPILER_WORKS)
 
diff --git a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt
index 6df1fa958..9aa209faa 100644
--- a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt
@@ -103,9 +103,9 @@ endif()
 add_dependencies(lapack eigen_lapack eigen_lapack_static)
 
 install(TARGETS eigen_lapack eigen_lapack_static
-        RUNTIME DESTINATION bin
-        LIBRARY DESTINATION lib
-        ARCHIVE DESTINATION lib)
+        RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+        LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+        ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR})
 
         
         
diff --git a/gtsam/3rdparty/SuiteSparse_config/Makefile b/gtsam/3rdparty/SuiteSparse_config/Makefile
index aa858aeab..695a327a4 100644
--- a/gtsam/3rdparty/SuiteSparse_config/Makefile
+++ b/gtsam/3rdparty/SuiteSparse_config/Makefile
@@ -7,8 +7,8 @@ export SUITESPARSE
 
 # version of SuiteSparse_config is also version of SuiteSparse meta-package
 LIBRARY = libsuitesparseconfig
-VERSION = 4.5.6
-SO_VERSION = 4
+VERSION = 5.4.0
+SO_VERSION = 5
 
 default: library
 
diff --git a/gtsam/3rdparty/SuiteSparse_config/README.txt b/gtsam/3rdparty/SuiteSparse_config/README.txt
index 8129f5a04..8555cc459 100644
--- a/gtsam/3rdparty/SuiteSparse_config/README.txt
+++ b/gtsam/3rdparty/SuiteSparse_config/README.txt
@@ -1,4 +1,4 @@
-SuiteSparse_config, 2017, Timothy A. Davis, http://www.suitesparse.com
+SuiteSparse_config, 2018, Timothy A. Davis, http://www.suitesparse.com
 (formerly the UFconfig package)
 
 This directory contains a default SuiteSparse_config.mk file.  It tries to
@@ -37,6 +37,7 @@ SuiteSparse_config is not required by these packages:
 
   CSparse       a Concise Sparse matrix package
   MATLAB_Tools  toolboxes for use in MATLAB
+  GraphBLAS     graph algorithms in the language of linear algebra
 
 In addition, the xerbla/ directory contains Fortan and C versions of the
 BLAS/LAPACK xerbla routine, which is called when an invalid input is passed to
diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c
index b491539fe..595e46781 100644
--- a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c
+++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c
@@ -4,7 +4,7 @@
 
 /* SuiteSparse configuration : memory manager and printf functions. */
 
-/* Copyright (c) 2013, Timothy A. Davis.  No licensing restrictions
+/* Copyright (c) 2013-2018, Timothy A. Davis.  No licensing restrictions
  * apply to this file or to the SuiteSparse_config directory.
  * Author: Timothy A. Davis.
  */
diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h
index 7d4de65d3..9e28c0530 100644
--- a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h
+++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h
@@ -177,38 +177,7 @@ int SuiteSparse_divcomplex
 /* SuiteSparse is not a package itself, but a collection of packages, some of
  * which must be used together (UMFPACK requires AMD, CHOLMOD requires AMD,
  * COLAMD, CAMD, and CCOLAMD, etc).  A version number is provided here for the
- * collection itself.  The versions of packages within each version of
- * SuiteSparse are meant to work together.  Combining one package from one
- * version of SuiteSparse, with another package from another version of
- * SuiteSparse, may or may not work.
- *
- * SuiteSparse contains the following packages:
- *
- *  SuiteSparse_config version 4.5.6 (version always the same as SuiteSparse)
- *  AMD             version 2.4.6
- *  BTF             version 1.2.6
- *  CAMD            version 2.4.6
- *  CCOLAMD         version 2.9.6
- *  CHOLMOD         version 3.0.11
- *  COLAMD          version 2.9.6
- *  CSparse         version 3.1.9
- *  CXSparse        version 3.1.9
- *  GPUQREngine     version 1.0.5
- *  KLU             version 1.3.8
- *  LDL             version 2.2.6
- *  RBio            version 2.2.6
- *  SPQR            version 2.0.8
- *  SuiteSparse_GPURuntime  version 1.0.5
- *  UMFPACK         version 5.7.6
- *  MATLAB_Tools    various packages & M-files
- *  xerbla          version 1.0.3
- *
- * Other package dependencies:
- *  BLAS            required by CHOLMOD and UMFPACK
- *  LAPACK          required by CHOLMOD
- *  METIS 5.1.0     required by CHOLMOD (optional) and KLU (optional)
- *  CUBLAS, CUDART  NVIDIA libraries required by CHOLMOD and SPQR when
- *                  they are compiled with GPU acceleration.
+ * collection itself, which is also the version number of SuiteSparse_config.
  */
 
 int SuiteSparse_version     /* returns SUITESPARSE_VERSION */
@@ -233,11 +202,11 @@ int SuiteSparse_version     /* returns SUITESPARSE_VERSION */
 */
 #define SUITESPARSE_HAS_VERSION_FUNCTION
 
-#define SUITESPARSE_DATE "Oct 3, 2017"
+#define SUITESPARSE_DATE "Dec 28, 2018"
 #define SUITESPARSE_VER_CODE(main,sub) ((main) * 1000 + (sub))
-#define SUITESPARSE_MAIN_VERSION 4
-#define SUITESPARSE_SUB_VERSION 5
-#define SUITESPARSE_SUBSUB_VERSION 6
+#define SUITESPARSE_MAIN_VERSION 5
+#define SUITESPARSE_SUB_VERSION 4
+#define SUITESPARSE_SUBSUB_VERSION 0
 #define SUITESPARSE_VERSION \
     SUITESPARSE_VER_CODE(SUITESPARSE_MAIN_VERSION,SUITESPARSE_SUB_VERSION)
 
diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk
index 2c13a6010..19a39032a 100644
--- a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk
+++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk
@@ -3,9 +3,11 @@
 #===============================================================================
 
 # This file contains all configuration settings for all packages in SuiteSparse,
-# except for CSparse (which is stand-alone) and the packages in MATLAB_Tools.
+# except for CSparse (which is stand-alone), the packages in MATLAB_Tools,
+# and GraphBLAS.  The configuration settings for GraphBLAS are determined by
+# GraphBLAS/CMakeLists.txt
 
-SUITESPARSE_VERSION = 4.5.6
+SUITESPARSE_VERSION = 5.4.0
 
 #===============================================================================
 # Options you can change without editing this file:
@@ -57,6 +59,15 @@ SUITESPARSE_VERSION = 4.5.6
     INSTALL_INCLUDE ?= $(INSTALL)/include
     INSTALL_DOC ?= $(INSTALL)/share/doc/suitesparse-$(SUITESPARSE_VERSION)
 
+    CMAKE_OPTIONS ?= -DCMAKE_INSTALL_PREFIX=$(INSTALL)
+
+    #---------------------------------------------------------------------------
+    # parallel make
+    #---------------------------------------------------------------------------
+
+    # sequential make's by default
+    JOBS ?= 1
+
     #---------------------------------------------------------------------------
     # optimization level
     #---------------------------------------------------------------------------
@@ -78,19 +89,11 @@ SUITESPARSE_VERSION = 4.5.6
         CXX = g++
         BLAS = -lrefblas -lgfortran -lstdc++
         LAPACK = -llapack
-	CFLAGS += --coverage
-	OPTIMIZATION = -g
-	LDFLAGS += --coverage
+        CFLAGS += --coverage
+        OPTIMIZATION = -g
+        LDFLAGS += --coverage
     endif
 
-    #---------------------------------------------------------------------------
-    # CFLAGS for the C/C++ compiler
-    #---------------------------------------------------------------------------
-
-    # The CF macro is used by SuiteSparse Makefiles as a combination of
-    # CFLAGS, CPPFLAGS, TARGET_ARCH, and system-dependent settings.
-    CF ?= $(CFLAGS) $(CPPFLAGS) $(TARGET_ARCH) $(OPTIMIZATION) -fexceptions -fPIC
-
     #---------------------------------------------------------------------------
     # OpenMP is used in CHOLMOD
     #---------------------------------------------------------------------------
@@ -112,10 +115,12 @@ SUITESPARSE_VERSION = 4.5.6
     ifneq ($(AUTOCC),no)
         ifneq ($(shell which icc 2>/dev/null),)
             # use the Intel icc compiler for C codes, and -qopenmp for OpenMP
-            CC = icc -D_GNU_SOURCE
-            CXX = $(CC)
+            CC = icc
+            CFLAGS += -D_GNU_SOURCE
+            CXX = icpc
             CFOPENMP = -qopenmp -I$(MKLROOT)/include
-	    LDFLAGS += -openmp
+            LDFLAGS += -qopenmp
+            LDLIBS += -lm -lirc
         endif
         ifneq ($(shell which ifort 2>/dev/null),)
             # use the Intel ifort compiler for Fortran codes
@@ -123,6 +128,16 @@ SUITESPARSE_VERSION = 4.5.6
         endif
     endif
 
+    CMAKE_OPTIONS += -DCMAKE_CXX_COMPILER=$(CXX) -DCMAKE_C_COMPILER=$(CC)
+
+    #---------------------------------------------------------------------------
+    # CFLAGS for the C/C++ compiler
+    #---------------------------------------------------------------------------
+
+    # The CF macro is used by SuiteSparse Makefiles as a combination of
+    # CFLAGS, CPPFLAGS, TARGET_ARCH, and system-dependent settings.
+    CF ?= $(CFLAGS) $(CPPFLAGS) $(TARGET_ARCH) $(OPTIMIZATION) -fexceptions -fPIC
+
     #---------------------------------------------------------------------------
     # code formatting (for Tcov on Linux only)
     #---------------------------------------------------------------------------
@@ -157,7 +172,7 @@ SUITESPARSE_VERSION = 4.5.6
             #   $(MKLROOT)/lib/intel64/libmkl_intel_thread.a \
             #   -Wl,--end-group -lpthread -lm
             # using dynamic linking:
-            BLAS = -lmkl_intel_lp64 -lmkl_core -lmkl_intel_thread -lpthread -lm
+            BLAS = -lmkl_intel_lp64 -lmkl_core -lmkl_intel_thread -liomp5 -lpthread -lm
             LAPACK =
         else
             # use the OpenBLAS at http://www.openblas.net
@@ -223,12 +238,16 @@ SUITESPARSE_VERSION = 4.5.6
         CUBLAS_LIB    = $(CUDA_PATH)/lib64/libcublas.so
         CUDA_INC_PATH = $(CUDA_PATH)/include/
         CUDA_INC      = -I$(CUDA_INC_PATH)
+                MAGMA_INC     = -I/opt/magma-2.4.0/include/
+                MAGMA_LIB     = -L/opt/magma-2.4.0/lib/ -lmagma
         NVCC          = $(CUDA_PATH)/bin/nvcc
         NVCCFLAGS     = -Xcompiler -fPIC -O3 \
                             -gencode=arch=compute_30,code=sm_30 \
                             -gencode=arch=compute_35,code=sm_35 \
                             -gencode=arch=compute_50,code=sm_50 \
-                            -gencode=arch=compute_50,code=compute_50
+                            -gencode=arch=compute_53,code=sm_53 \
+                            -gencode=arch=compute_53,code=sm_53 \
+                            -gencode=arch=compute_60,code=compute_60
     endif
 
     #---------------------------------------------------------------------------
@@ -555,6 +574,7 @@ config:
 	@echo 'Install include files in: INSTALL_INCLUDE=' '$(INSTALL_INCLUDE)'
 	@echo 'Install documentation in: INSTALL_DOC=    ' '$(INSTALL_DOC)'
 	@echo 'Optimization level:       OPTIMIZATION=   ' '$(OPTIMIZATION)'
+	@echo 'parallel make jobs:       JOBS=           ' '$(JOBS)'
 	@echo 'BLAS library:             BLAS=           ' '$(BLAS)'
 	@echo 'LAPACK library:           LAPACK=         ' '$(LAPACK)'
 	@echo 'Intel TBB library:        TBB=            ' '$(TBB)'
diff --git a/gtsam/3rdparty/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt
index 8fca88e26..de46165ff 100644
--- a/gtsam/3rdparty/metis/CMakeLists.txt
+++ b/gtsam/3rdparty/metis/CMakeLists.txt
@@ -34,6 +34,11 @@ else()
   set(METIS_LIBRARY_TYPE STATIC)
 endif()
 
+# Allow a static METIS while building GTSAM as dynamic
+if(BUILD_STATIC_METIS)
+  set(METIS_LIBRARY_TYPE STATIC)
+endif()
+
 include(${GKLIB_PATH}/GKlibSystem.cmake)
 # Add include directories.
 include_directories(${GKLIB_PATH})
@@ -47,3 +52,17 @@ if(GTSAM_BUILD_METIS_EXECUTABLES)
 endif()
 
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
+# Export macros assumed in metis public headers to clients of the library.
+# This was added to solve MSVC build errors.
+if (TARGET metis AND GKlib_COPTIONS)
+  # Remove (possibly) duplicated symbols:
+  string(REPLACE -DLINUX "" GKlib_COPTIONS ${GKlib_COPTIONS})
+  string(REPLACE -DWIN32 "" GKlib_COPTIONS ${GKlib_COPTIONS})
+  string(REPLACE -DNDEBUG2 "" GKlib_COPTIONS ${GKlib_COPTIONS})
+  string(REPLACE -DNDEBUG "" GKlib_COPTIONS ${GKlib_COPTIONS})
+  string(REPLACE -pedantic "" GKlib_COPTIONS ${GKlib_COPTIONS})
+  string(REPLACE -std=c99 "" GKlib_COPTIONS ${GKlib_COPTIONS})
+  separate_arguments(GKlib_COPTIONS)
+  # Declare those flags as to-be-imported in "client libraries", i.e. "gtsam"
+  target_compile_options(metis INTERFACE ${GKlib_COPTIONS})
+endif()
diff --git a/gtsam/3rdparty/metis/GKlib/CMakeLists.txt b/gtsam/3rdparty/metis/GKlib/CMakeLists.txt
index 67b600aa6..ce9fa1124 100644
--- a/gtsam/3rdparty/metis/GKlib/CMakeLists.txt
+++ b/gtsam/3rdparty/metis/GKlib/CMakeLists.txt
@@ -16,6 +16,6 @@ include_directories("test")
 add_subdirectory("test")
 
 install(TARGETS GKlib
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib)
-install(FILES ${GKlib_includes} DESTINATION include)
+  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR})
+install(FILES ${GKlib_includes} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
diff --git a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
index fdf5e7511..330e989fa 100644
--- a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
+++ b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt
@@ -4,23 +4,26 @@ include_directories(.)
 file(GLOB metis_sources *.c)
 # Build libmetis.
 add_definitions(-fPIC)
-add_library(metis ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources})
+add_library(metis-gtsam ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources})
 if(UNIX)
-  target_link_libraries(metis m)
+  target_link_libraries(metis-gtsam m)
 endif()
 
 if(WIN32)
-		set_target_properties(metis PROPERTIES
+		set_target_properties(metis-gtsam PROPERTIES
 			PREFIX ""
 			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin")
 endif()
 
 if (APPLE)
-		set_target_properties(metis PROPERTIES
+		set_target_properties(metis-gtsam PROPERTIES
 			INSTALL_NAME_DIR
 			"${CMAKE_INSTALL_PREFIX}/lib")
  endif()
 
-install(TARGETS metis EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
-list(APPEND GTSAM_EXPORTED_TARGETS metis)
+install(TARGETS metis-gtsam EXPORT GTSAM-exports
+  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+  RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})
+list(APPEND GTSAM_EXPORTED_TARGETS metis-gtsam)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
diff --git a/gtsam/3rdparty/metis/programs/CMakeLists.txt b/gtsam/3rdparty/metis/programs/CMakeLists.txt
index 3aaf3577d..67af1d117 100644
--- a/gtsam/3rdparty/metis/programs/CMakeLists.txt
+++ b/gtsam/3rdparty/metis/programs/CMakeLists.txt
@@ -15,7 +15,7 @@ endforeach(prog)
 
 if(METIS_INSTALL)
   install(TARGETS gpmetis ndmetis mpmetis m2gmetis graphchk cmpfillin
-    RUNTIME DESTINATION bin)
+    RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})
 endif()
 
 # Try to find subversion revision.
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index b4a33943e..16dca6736 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -15,7 +15,7 @@ set (gtsam_subdirs
     sfm
     slam
     smart
-	navigation
+	  navigation
 )
 
 set(gtsam_srcs)
@@ -49,7 +49,7 @@ endif()
 
 # Common headers
 file(GLOB gtsam_core_headers "*.h")
-install(FILES ${gtsam_core_headers} DESTINATION include/gtsam)
+install(FILES ${gtsam_core_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam)
 
 # assemble core libaries
 foreach(subdir ${gtsam_subdirs})
@@ -86,10 +86,10 @@ configure_file(config.h.in config.h)
 set(library_name GTSAM) # For substitution in dllexport.h.in
 configure_file("${GTSAM_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h")
 list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h")
-install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION include/gtsam)
+install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam)
 
 if(GTSAM_SUPPORT_NESTED_DISSECTION)
-    list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis)
+    list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam)
 endif()
 
 # Versions
@@ -103,11 +103,7 @@ message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}")
 
 # BUILD_SHARED_LIBS automatically defines static/shared libs:
 add_library(gtsam ${gtsam_srcs})
-
-# Boost:
 target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES})
-target_include_directories(gtsam PUBLIC ${Boost_INCLUDE_DIR})
-# Other deps:
 target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES})
 
 # Apply build flags:
@@ -140,13 +136,13 @@ endif()
 target_include_directories(gtsam BEFORE PUBLIC
   # SuiteSparse_config
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/SuiteSparse_config>
-  $<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/SuiteSparse_config>
+  $<INSTALL_INTERFACE:include/gtsam/3rdparty/SuiteSparse_config>
   # CCOLAMD
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Include>
-  $<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/CCOLAMD>
+  $<INSTALL_INTERFACE:include/gtsam/3rdparty/CCOLAMD>
   # main gtsam includes:
   $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
-  $<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/include/>
+  $<INSTALL_INTERFACE:include/>
   # config.h
   $<BUILD_INTERFACE:${CMAKE_BINARY_DIR}>
   # unit tests:
@@ -157,7 +153,7 @@ if(GTSAM_SUPPORT_NESTED_DISSECTION)
     $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/include>
     $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
     $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
-    $<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/metis/>
+    $<INSTALL_INTERFACE:include/gtsam/3rdparty/metis/>
   )
 endif()
 
@@ -172,31 +168,35 @@ if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with
 		set_target_properties(gtsam PROPERTIES
 			PREFIX ""
 			DEFINE_SYMBOL GTSAM_EXPORTS
-    RUNTIME_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/bin")
+			RUNTIME_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/bin")
 	endif()
 endif()
 
-if (APPLE AND BUILD_SHARED_LIBS)
-	set_target_properties(gtsam PROPERTIES
-		INSTALL_NAME_DIR
-		"${CMAKE_INSTALL_PREFIX}/lib")
+if(WIN32) # library to help with demangling variable names on Windows
+  target_link_libraries(gtsam PRIVATE Dbghelp)
 endif()
 
 install(
 	TARGETS gtsam
 	EXPORT GTSAM-exports
-	LIBRARY DESTINATION lib
-	ARCHIVE DESTINATION lib
-	RUNTIME DESTINATION bin)
+	LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+	ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+	RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})
 
 list(APPEND GTSAM_EXPORTED_TARGETS gtsam)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
 
-# make sure that ccolamd compiles even in face of warnings
+# Make sure that ccolamd compiles even in face of warnings
+# and suppress all warnings from 3rd party code if Release build
 if(WIN32)
-    set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w")
+  set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "/w")
 else()
+  if("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
+    # Suppress all warnings from 3rd party sources.
+    set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w")
+  else()
     set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-Wno-error")
+  endif()
 endif()
 
 # Create the matlab toolbox for the gtsam library
diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h
index b8388057d..2d7cbd6db 100644
--- a/gtsam/base/ConcurrentMap.h
+++ b/gtsam/base/ConcurrentMap.h
@@ -29,14 +29,22 @@
 #  undef max
 #  undef ERROR
 
+#include <functional> // std::hash()
+
 // Use TBB concurrent_unordered_map for ConcurrentMap
-#  define CONCURRENT_MAP_BASE tbb::concurrent_unordered_map<KEY, VALUE>
+template <typename KEY, typename VALUE>
+using ConcurrentMapBase = tbb::concurrent_unordered_map<
+  KEY,
+  VALUE,
+  std::hash<KEY>
+  >;
 
 #else
 
 // If we're not using TBB, use a FastMap for ConcurrentMap
-#  include <gtsam/base/FastMap.h>
-#  define CONCURRENT_MAP_BASE gtsam::FastMap<KEY, VALUE>
+#include <gtsam/base/FastMap.h>
+template <typename KEY, typename VALUE>
+using ConcurrentMapBase = gtsam::FastMap<KEY, VALUE>;
 
 #endif
 
@@ -57,11 +65,11 @@ namespace gtsam {
  * @addtogroup base
  */
 template<typename KEY, typename VALUE>
-class ConcurrentMap : public CONCURRENT_MAP_BASE {
+class ConcurrentMap : public ConcurrentMapBase<KEY,VALUE> {
 
 public:
 
-  typedef CONCURRENT_MAP_BASE Base;
+  typedef ConcurrentMapBase<KEY,VALUE> Base;
 
   /** Default constructor */
   ConcurrentMap() {}
diff --git a/gtsam/base/DSFMap.h b/gtsam/base/DSFMap.h
index dfce185dc..1d6bfdefa 100644
--- a/gtsam/base/DSFMap.h
+++ b/gtsam/base/DSFMap.h
@@ -115,8 +115,8 @@ class DSFMap {
 /// Small utility class for representing a wrappable pairs of ints.
 class IndexPair : public std::pair<size_t,size_t> {
  public:
-  IndexPair(): std::pair<size_t,size_t>(0,0) {}
-  IndexPair(size_t i, size_t j) : std::pair<size_t,size_t>(i,j) {}
+  inline IndexPair(): std::pair<size_t,size_t>(0,0) {}
+  inline IndexPair(size_t i, size_t j) : std::pair<size_t,size_t>(i,j) {}
   inline size_t i() const { return first; };
   inline size_t j() const { return second; };
 };
diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index 52899fe45..2ac3eb80c 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -20,6 +20,7 @@
 #pragma once
 
 #include <gtsam/base/Manifold.h>
+#include <gtsam/base/types.h>
 #include <gtsam/base/Value.h>
 
 #include <boost/make_shared.hpp>
@@ -83,18 +84,15 @@ public:
 
   /// Virtual print function, uses traits
   virtual void print(const std::string& str) const {
-    std::cout << "(" << typeid(T).name() << ") ";
+    std::cout << "(" << demangle(typeid(T).name()) << ") ";
     traits<T>::Print(value_, str);
   }
 
     /**
      * Create a duplicate object returned as a pointer to the generic Value interface.
-     * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator.
-     * The result must be deleted with Value::deallocate_, not with the 'delete' operator.
      */
     virtual Value* clone_() const {
-      void *place = boost::singleton_pool<PoolTag, sizeof(GenericValue)>::malloc();
-      GenericValue* ptr = new (place) GenericValue(*this); // calls copy constructor to fill in
+      GenericValue* ptr = new GenericValue(*this); // calls copy constructor to fill in
       return ptr;
     }
 
@@ -102,8 +100,7 @@ public:
      * Destroy and deallocate this object, only if it was originally allocated using clone_().
      */
     virtual void deallocate_() const {
-      this->~GenericValue(); // Virtual destructor cleans up the derived object
-      boost::singleton_pool<PoolTag, sizeof(GenericValue)>::free((void*) this); // Release memory from pool
+      delete this;
     }
 
     /**
@@ -118,10 +115,7 @@ public:
       // Call retract on the derived class using the retract trait function
       const T retractResult = traits<T>::Retract(GenericValue<T>::value(), delta);
 
-      // Create a Value pointer copy of the result
-      void* resultAsValuePlace =
-          boost::singleton_pool<PoolTag, sizeof(GenericValue)>::malloc();
-      Value* resultAsValue = new (resultAsValuePlace) GenericValue(retractResult);
+      Value* resultAsValue = new GenericValue(retractResult);
 
       // Return the pointer to the Value base class
       return resultAsValue;
@@ -172,12 +166,6 @@ public:
       return *this;
     }
 
-  private:
-
-    /// Fake Tag struct for singleton pool allocator. In fact, it is never used!
-    struct PoolTag {
-    };
-
   private:
 
     /** Serialization function */
@@ -193,7 +181,7 @@ public:
   // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
   enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 /// use this macro instead of BOOST_CLASS_EXPORT for GenericValues
diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h
index bb49a84d6..fe730c934 100644
--- a/gtsam/base/Lie.h
+++ b/gtsam/base/Lie.h
@@ -35,9 +35,6 @@ namespace gtsam {
 template <class Class, int N>
 struct LieGroup {
 
-  BOOST_STATIC_ASSERT_MSG(N != Eigen::Dynamic,
-      "LieGroup not yet specialized for dynamically sized types.");
-
   enum { dimension = N };
   typedef OptionalJacobian<N, N> ChartJacobian;
   typedef Eigen::Matrix<double, N, N> Jacobian;
@@ -160,7 +157,6 @@ struct LieGroup {
     if (H2) *H2 = D_v_h;
     return v;
   }
-
 };
 
 /// tag to assert a type is a Lie group
@@ -174,7 +170,7 @@ namespace internal {
 /// Assumes existence of: identity, dimension, localCoordinates, retract,
 /// and additionally Logmap, Expmap, compose, between, and inverse
 template<class Class>
-struct LieGroupTraits {
+struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
   typedef lie_group_tag structure_category;
 
   /// @name Group
@@ -190,11 +186,6 @@ struct LieGroupTraits {
   typedef Eigen::Matrix<double, dimension, 1> TangentVector;
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;
 
-  BOOST_STATIC_ASSERT_MSG(dimension != Eigen::Dynamic,
-      "LieGroupTraits not yet specialized for dynamically sized types.");
-
-  static int GetDimension(const Class&) {return dimension;}
-
   static TangentVector Local(const Class& origin, const Class& other,
       ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
     return origin.localCoordinates(other, Horigin, Hother);
@@ -336,6 +327,21 @@ T interpolate(const T& X, const T& Y, double t) {
   return traits<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
 }
 
+/**
+ * Functor for transforming covariance of T.
+ * T needs to satisfy the Lie group concept.
+ */
+template<class T>
+class TransformCovariance
+{
+private:
+  typename T::Jacobian adjointMap_;
+public:
+  explicit TransformCovariance(const T &X) : adjointMap_{X.AdjointMap()} {}
+  typename T::Jacobian operator()(const typename T::Jacobian &covariance)
+  { return adjointMap_ * covariance * adjointMap_.transpose(); }
+};
+
 } // namespace gtsam
 
 /**
diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h
index 21df90513..9feb2b451 100644
--- a/gtsam/base/Manifold.h
+++ b/gtsam/base/Manifold.h
@@ -72,7 +72,7 @@ struct HasManifoldPrereqs {
 
 /// Extra manifold traits for fixed-dimension types
 template<class Class, int N>
-struct ManifoldImpl {
+struct GetDimensionImpl {
   // Compile-time dimensionality
   static int GetDimension(const Class&) {
     return N;
@@ -81,7 +81,7 @@ struct ManifoldImpl {
 
 /// Extra manifold traits for variable-dimension types
 template<class Class>
-struct ManifoldImpl<Class, Eigen::Dynamic> {
+struct GetDimensionImpl<Class, Eigen::Dynamic> {
   // Run-time dimensionality
   static int GetDimension(const Class& m) {
     return m.dim();
@@ -92,7 +92,7 @@ struct ManifoldImpl<Class, Eigen::Dynamic> {
 /// To use this for your class type, define:
 /// template<> struct traits<Class> : public internal::ManifoldTraits<Class> { };
 template<class Class>
-struct ManifoldTraits: ManifoldImpl<Class, Class::dimension> {
+struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
 
   // Check that Class has the necessary machinery
   BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
@@ -214,7 +214,7 @@ public:
   enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0
   };
 public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+	GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 // Define any direct product group to be a model of the multiplicative Group concept
diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp
index 705d84d25..e2d8f71d1 100644
--- a/gtsam/base/Matrix.cpp
+++ b/gtsam/base/Matrix.cpp
@@ -142,12 +142,12 @@ void print(const Matrix& A, const string &s, ostream& stream) {
   static const Eigen::IOFormat matlab(
       Eigen::StreamPrecision, // precision
       0, // flags
-      " ", // coeffSeparator
+      ", ", // coeffSeparator
       ";\n", // rowSeparator
-      " \t",  // rowPrefix
+      "\t",  // rowPrefix
       "", // rowSuffix
       "[\n", // matPrefix
-      "\n  ]" // matSuffix
+      "\n]" // matSuffix
       );
   cout << s << A.format(matlab) << endl;
 }
diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index 2910ce74c..1c1138438 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -17,6 +17,7 @@
  * @author  Frank Dellaert
  * @author  Alex Cunningham
  * @author  Alex Hagiopol
+ * @author  Varun Agrawal
  */
 
 // \callgraph
@@ -88,10 +89,9 @@ bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBas
 
   for(size_t i=0; i<m1; i++)
     for(size_t j=0; j<n1; j++) {
-      if(boost::math::isnan(A(i,j)) ^ boost::math::isnan(B(i,j)))
-        return false;
-      else if(std::abs(A(i,j) - B(i,j)) > tol)
+      if(!fpEqual(A(i,j), B(i,j), tol)) {
         return false;
+      }
     }
   return true;
 }
@@ -548,17 +548,47 @@ GTSAM_EXPORT Vector columnNormSquare(const Matrix &A);
 namespace boost {
   namespace serialization {
 
+    /**
+     * Ref. https://stackoverflow.com/questions/18382457/eigen-and-boostserialize/22903063#22903063
+     * 
+     * Eigen supports calling resize() on both static and dynamic matrices.
+     * This allows for a uniform API, with resize having no effect if the static matrix
+     * is already the correct size.
+     * https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html#TutorialMatrixSizesResizing
+     * 
+     * We use all the Matrix template parameters to ensure wide compatibility.
+     * 
+     * eigen_typekit in ROS uses the same code
+     * http://docs.ros.org/lunar/api/eigen_typekit/html/eigen__mqueue_8cpp_source.html
+     */
+
     // split version - sends sizes ahead
-    template<class Archive>
-    void save(Archive & ar, const gtsam::Matrix & m, unsigned int /*version*/) {
+    template<class Archive,
+             typename Scalar_,
+             int Rows_,
+             int Cols_,
+             int Ops_,
+             int MaxRows_,
+             int MaxCols_>
+    void save(Archive & ar,
+              const Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & m,
+              const unsigned int /*version*/) {
       const size_t rows = m.rows(), cols = m.cols();
       ar << BOOST_SERIALIZATION_NVP(rows);
       ar << BOOST_SERIALIZATION_NVP(cols);
       ar << make_nvp("data", make_array(m.data(), m.size()));
     }
 
-    template<class Archive>
-    void load(Archive & ar, gtsam::Matrix & m, unsigned int /*version*/) {
+    template<class Archive,
+             typename Scalar_,
+             int Rows_,
+             int Cols_,
+             int Ops_,
+             int MaxRows_,
+             int MaxCols_>
+    void load(Archive & ar,
+              Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & m,
+              const unsigned int /*version*/) {
       size_t rows, cols;
       ar >> BOOST_SERIALIZATION_NVP(rows);
       ar >> BOOST_SERIALIZATION_NVP(cols);
@@ -566,8 +596,25 @@ namespace boost {
       ar >> make_nvp("data", make_array(m.data(), m.size()));
     }
 
+    // templated version of BOOST_SERIALIZATION_SPLIT_FREE(Eigen::Matrix);
+    template<class Archive,
+             typename Scalar_,
+             int Rows_,
+             int Cols_,
+             int Ops_,
+             int MaxRows_,
+             int MaxCols_>
+    void serialize(Archive & ar,
+              Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & m,
+              const unsigned int version) {
+      split_free(ar, m, version);
+    }
+
+    // specialized to Matrix for MATLAB wrapper
+    template <class Archive>
+    void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) {
+      split_free(ar, m, version);
+    }
+
   } // namespace serialization
 } // namespace boost
-
-BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix);
-
diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h
index 166297d5f..602c5915e 100644
--- a/gtsam/base/OptionalJacobian.h
+++ b/gtsam/base/OptionalJacobian.h
@@ -55,7 +55,7 @@ private:
   }
 
   // Private and very dangerous constructor straight from memory
-  OptionalJacobian(double* data) : map_(NULL) {
+  OptionalJacobian(double* data) : map_(nullptr) {
     if (data) usurp(data);
   }
 
@@ -66,25 +66,25 @@ public:
 
   /// Default constructor acts like boost::none
   OptionalJacobian() :
-      map_(NULL) {
+      map_(nullptr) {
   }
 
   /// Constructor that will usurp data of a fixed-size matrix
   OptionalJacobian(Jacobian& fixed) :
-      map_(NULL) {
+      map_(nullptr) {
     usurp(fixed.data());
   }
 
   /// Constructor that will usurp data of a fixed-size matrix, pointer version
   OptionalJacobian(Jacobian* fixedPtr) :
-      map_(NULL) {
+      map_(nullptr) {
     if (fixedPtr)
       usurp(fixedPtr->data());
   }
 
   /// Constructor that will resize a dynamic matrix (unless already correct)
   OptionalJacobian(Eigen::MatrixXd& dynamic) :
-      map_(NULL) {
+      map_(nullptr) {
     dynamic.resize(Rows, Cols); // no malloc if correct size
     usurp(dynamic.data());
   }
@@ -93,12 +93,12 @@ public:
 
   /// Constructor with boost::none just makes empty
   OptionalJacobian(boost::none_t /*none*/) :
-      map_(NULL) {
+      map_(nullptr) {
   }
 
   /// Constructor compatible with old-style derivatives
   OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
-      map_(NULL) {
+      map_(nullptr) {
     if (optional) {
       optional->resize(Rows, Cols);
       usurp(optional->data());
@@ -110,11 +110,11 @@ public:
   /// Constructor that will usurp data of a block expression
   /// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible
   //  template <typename Derived, bool InnerPanel>
-  //  OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(NULL) { ?? }
+  //  OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(nullptr) { ?? }
 
   /// Return true is allocated, false if default constructor was used
   operator bool() const {
-    return map_.data() != NULL;
+    return map_.data() != nullptr;
   }
 
   /// De-reference, like boost optional
@@ -173,24 +173,25 @@ public:
 
   /// Default constructor acts like boost::none
   OptionalJacobian() :
-    pointer_(NULL) {
+    pointer_(nullptr) {
   }
 
-  /// Constructor that will resize a dynamic matrix (unless already correct)
-  OptionalJacobian(Eigen::MatrixXd& dynamic) :
-      pointer_(&dynamic) {
-  }
+  /// Construct from pointer to dynamic matrix
+  OptionalJacobian(Jacobian* pointer) : pointer_(pointer) {}
+
+  /// Construct from refrence to dynamic matrix
+  OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {}
 
 #ifndef OPTIONALJACOBIAN_NOBOOST
 
   /// Constructor with boost::none just makes empty
   OptionalJacobian(boost::none_t /*none*/) :
-    pointer_(NULL) {
+    pointer_(nullptr) {
   }
 
   /// Constructor compatible with old-style derivatives
   OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
-      pointer_(NULL) {
+      pointer_(nullptr) {
     if (optional) pointer_ = &(*optional);
   }
 
@@ -198,7 +199,7 @@ public:
 
   /// Return true is allocated, false if default constructor was used
   operator bool() const {
-    return pointer_!=NULL;
+    return pointer_!=nullptr;
   }
 
   /// De-reference, like boost optional
diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h
index 53a8912f6..4e030606d 100644
--- a/gtsam/base/SymmetricBlockMatrix.h
+++ b/gtsam/base/SymmetricBlockMatrix.h
@@ -416,4 +416,3 @@ namespace gtsam {
   class CholeskyFailed;
 
 }
-
diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h
index 73fb320e1..92c464940 100644
--- a/gtsam/base/Testable.h
+++ b/gtsam/base/Testable.h
@@ -107,7 +107,7 @@ namespace gtsam {
    * Template to create a binary predicate
    */
   template<class V>
-  struct equals : public std::binary_function<const V&, const V&, bool> {
+  struct equals : public std::function<bool(const V&, const V&)> {
     double tol_;
     equals(double tol = 1e-9) : tol_(tol) {}
     bool operator()(const V& expected, const V& actual) {
@@ -119,7 +119,7 @@ namespace gtsam {
    * Binary predicate on shared pointers
    */
   template<class V>
-  struct equals_star : public std::binary_function<const boost::shared_ptr<V>&, const boost::shared_ptr<V>&, bool> {
+  struct equals_star : public std::function<bool(const boost::shared_ptr<V>&, const boost::shared_ptr<V>&)> {
     double tol_;
     equals_star(double tol = 1e-9) : tol_(tol) {}
     bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
diff --git a/gtsam/base/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h
index ff82ff27c..0f7c6131d 100644
--- a/gtsam/base/ThreadsafeException.h
+++ b/gtsam/base/ThreadsafeException.h
@@ -11,7 +11,7 @@
 
 /**
  * @file     ThreadSafeException.h
- * @brief    Base exception type that uses tbb_exception if GTSAM is compiled with TBB
+ * @brief    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB
  * @author   Richard Roberts
  * @date     Aug 21, 2010
  * @addtogroup base
@@ -22,36 +22,31 @@
 #include <gtsam/config.h> // for GTSAM_USE_TBB
 
 #include <boost/optional/optional.hpp>
+#include <gtsam/dllexport.h>
 #include <string>
 #include <typeinfo>
+#include <exception>
 
 #ifdef GTSAM_USE_TBB
 #include <tbb/tbb_allocator.h>
-#include <tbb/tbb_exception.h>
 #include <tbb/scalable_allocator.h>
 #include <iostream>
 #endif
 
 namespace gtsam {
 
-/// Base exception type that uses tbb_exception if GTSAM is compiled with TBB.
+/// Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
 template<class DERIVED>
 class ThreadsafeException:
-#ifdef GTSAM_USE_TBB
-    public tbb::tbb_exception
-#else
 public std::exception
-#endif
 {
-#ifdef GTSAM_USE_TBB
 private:
-  typedef tbb::tbb_exception Base;
+  typedef std::exception Base;
+#ifdef GTSAM_USE_TBB
 protected:
   typedef std::basic_string<char, std::char_traits<char>,
       tbb::tbb_allocator<char> > String;
 #else
-private:
-  typedef std::exception Base;
 protected:
   typedef std::string String;
 #endif
@@ -81,43 +76,13 @@ protected:
   }
 
 public:
-  // Implement functions for tbb_exception
-#ifdef GTSAM_USE_TBB
-  virtual tbb::tbb_exception* move() throw () {
-    void* cloneMemory = scalable_malloc(sizeof(DERIVED));
-    if (!cloneMemory) {
-      std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl;
-      exit(-1);
-    }
-    DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast<DERIVED&>(*this));
-    clone->dynamic_ = true;
-    return clone;
-  }
-
-  virtual void destroy() throw () {
-    if (dynamic_) {
-      DERIVED* derivedPtr = static_cast<DERIVED*>(this);
-      derivedPtr->~DERIVED();
-      scalable_free(derivedPtr);
-    }
-  }
-
-  virtual void throw_self() {
-    throw *static_cast<DERIVED*>(this);
-  }
-
-  virtual const char* name() const throw () {
-    return typeid(DERIVED).name();
-  }
-#endif
-
   virtual const char* what() const throw () {
     return description_ ? description_->c_str() : "";
   }
 };
 
 /// Thread-safe runtime error exception
-class RuntimeErrorThreadsafe: public ThreadsafeException<RuntimeErrorThreadsafe> {
+class GTSAM_EXPORT RuntimeErrorThreadsafe: public ThreadsafeException<RuntimeErrorThreadsafe> {
 public:
   /// Construct with a string describing the exception
   RuntimeErrorThreadsafe(const std::string& description) :
diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index ac03c0f53..beec030ba 100644
--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@@ -14,6 +14,7 @@
  * @brief   typedef and functions to augment Eigen's Vectors
  * @author  Kai Ni
  * @author  Frank Dellaert
+ * @author  Varun Agrawal
  */
 
 #include <gtsam/base/Vector.h>
@@ -33,6 +34,45 @@ using namespace std;
 
 namespace gtsam {
 
+/* *************************************************************************
+ * References:
+ * 1. https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
+ * 2. https://floating-point-gui.de/errors/comparison/
+ * ************************************************************************* */
+bool fpEqual(double a, double b, double tol) {
+  using std::abs;
+  using std::isnan;
+  using std::isinf;
+
+  double DOUBLE_MIN_NORMAL = numeric_limits<double>::min() + 1.0;
+  double larger = (abs(b) > abs(a)) ? abs(b) : abs(a);
+
+  // handle NaNs
+  if(std::isnan(a) || isnan(b)) {
+    return isnan(a) && isnan(b);
+  }
+  // handle inf
+  else if(isinf(a) || isinf(b)) {
+    return isinf(a) && isinf(b);
+  }
+  // If the two values are zero or both are extremely close to it
+  // relative error is less meaningful here
+  else if(a == 0 || b == 0 || (abs(a) + abs(b)) < DOUBLE_MIN_NORMAL) {
+    return abs(a-b) <= tol * DOUBLE_MIN_NORMAL;
+  }
+  // Check if the numbers are really close
+  // Needed when comparing numbers near zero or tol is in vicinity
+  else if(abs(a-b) <= tol) {
+    return true;
+  }
+  // Use relative error
+  else if(abs(a-b) <= tol * min(larger, std::numeric_limits<double>::max())) {
+    return true;
+  }
+
+  return false;
+}
+
 /* ************************************************************************* */
 //3 argument call
 void print(const Vector& v, const string& s, ostream& stream) {
@@ -82,9 +122,7 @@ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) {
   if (vec1.size()!=vec2.size()) return false;
   size_t m = vec1.size();
   for(size_t i=0; i<m; ++i) {
-    if(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
-      return false;
-    if(std::abs(vec1[i] - vec2[i]) > tol)
+    if (!fpEqual(vec1[i], vec2[i], tol))
       return false;
   }
   return true;
@@ -95,9 +133,7 @@ bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol
   if (vec1.size()!=vec2.size()) return false;
   size_t m = vec1.size();
   for(size_t i=0; i<m; ++i) {
-    if(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
-      return false;
-    if(std::abs(vec1[i] - vec2[i]) > tol)
+    if (!fpEqual(vec1[i], vec2[i], tol))
       return false;
   }
   return true;
diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h
index 99c5a4d42..81be36c0a 100644
--- a/gtsam/base/Vector.h
+++ b/gtsam/base/Vector.h
@@ -15,6 +15,7 @@
  * @author  Kai Ni
  * @author  Frank Dellaert
  * @author  Alex Hagiopol
+ * @author  Varun Agrawal
  */
 
 // \callgraph
@@ -75,6 +76,19 @@ static_assert(
   "Error: GTSAM was built against a different version of Eigen");
 #endif
 
+/**
+ * Numerically stable function for comparing if floating point values are equal
+ * within epsilon tolerance.
+ * Used for vector and matrix comparison with C++11 compatible functions.
+ *
+ * If either value is NaN or Inf, we check for both values to be NaN or Inf
+ * respectively for the comparison to be true.
+ * If one is NaN/Inf and the other is not, returns false.
+ *
+ * Return true if two numbers are close wrt tol.
+ */
+GTSAM_EXPORT bool fpEqual(double a, double b, double tol);
+
 /**
  * print without optional string, must specify cout yourself
  */
diff --git a/gtsam/base/WeightedSampler.h b/gtsam/base/WeightedSampler.h
new file mode 100644
index 000000000..7c343b098
--- /dev/null
+++ b/gtsam/base/WeightedSampler.h
@@ -0,0 +1,137 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    WeightedSampler.h
+ * @brief   Fast sampling without replacement.
+ * @author  Frank Dellaert
+ * @date    May 2019
+ **/
+
+#pragma once
+
+#include <cmath>
+#include <queue>
+#include <random>
+#include <stdexcept>
+#include <utility>
+#include <vector>
+
+namespace gtsam {
+/*
+ * Fast sampling without replacement.
+ * Example usage:
+ *   std::mt19937 rng(42);
+ *   WeightedSampler<std::mt19937> sampler(&rng);
+ *   auto samples = sampler.sampleWithoutReplacement(5, weights);
+ */
+template <class Engine = std::mt19937>
+class WeightedSampler {
+ private:
+  Engine* engine_;  // random number generation engine
+
+ public:
+  /**
+   * Construct from random number generation engine
+   * We only store a pointer to it.
+   */
+  explicit WeightedSampler(Engine* engine) : engine_(engine) {}
+
+  std::vector<size_t> sampleWithoutReplacement(
+      size_t numSamples, const std::vector<double>& weights) {
+    // Implementation adapted from code accompanying paper at
+    // https://www.ethz.ch/content/dam/ethz/special-interest/baug/ivt/ivt-dam/vpl/reports/1101-1200/ab1141.pdf
+    const size_t n = weights.size();
+    if (n < numSamples) {
+      throw std::runtime_error(
+          "numSamples must be smaller than weights.size()");
+    }
+
+    // Return empty array if numSamples==0
+    std::vector<size_t> result(numSamples);
+    if (numSamples == 0) return result;
+
+    // Step 1: The first m items of V are inserted into reservoir
+    // Step 2: For each item v_i ∈ reservoir: Calculate a key k_i = u_i^(1/w),
+    // where u_i = random(0, 1)
+    // (Modification: Calculate and store -log k_i = e_i / w where e_i = exp(1),
+    //  reservoir is a priority queue that pops the *maximum* elements)
+    std::priority_queue<std::pair<double, size_t> > reservoir;
+
+    static const double kexp1 = std::exp(1.0);
+    for (auto it = weights.begin(); it != weights.begin() + numSamples; ++it) {
+      const double k_i = kexp1 / *it;
+      reservoir.push(std::make_pair(k_i, it - weights.begin() + 1));
+    }
+
+    // Step 4: Repeat Steps 5–10 until the population is exhausted
+    {
+      // Step 3: The threshold T_w is the minimum key of reservoir
+      // (Modification: This is now the logarithm)
+      // Step 10: The new threshold T w is the new minimum key of reservoir
+      const std::pair<double, size_t>& T_w = reservoir.top();
+
+      // Incrementing it is part of Step 7
+      for (auto it = weights.begin() + numSamples; it != weights.end(); ++it) {
+        // Step 5: Let r = random(0, 1) and X_w = log(r) / log(T_w)
+        // (Modification: Use e = -exp(1) instead of log(r))
+        const double X_w = kexp1 / T_w.first;
+
+        // Step 6: From the current item v_c skip items until item v_i, such
+        // that:
+        double w = 0.0;
+
+        // Step 7: w_c + w_{c+1} + ··· + w_{i−1} < X_w <= w_c + w_{c+1} + ··· +
+        // w_{i−1} + w_i
+        for (; it != weights.end(); ++it) {
+          w += *it;
+          if (X_w <= w) break;
+        }
+
+        // Step 7: No such item, terminate
+        if (it == weights.end()) break;
+
+        // Step 9: Let t_w = T_w^{w_i}, r_2 = random(t_w, 1) and v_i’s key: k_i
+        // = (r_2)^{1/w_i} (Mod: Let t_w = log(T_w) * {w_i}, e_2 =
+        // log(random(e^{t_w}, 1)) and v_i’s key: k_i = -e_2 / w_i)
+        const double t_w = -T_w.first * *it;
+        std::uniform_real_distribution<double> randomAngle(std::exp(t_w), 1.0);
+        const double e_2 = std::log(randomAngle(*engine_));
+        const double k_i = -e_2 / *it;
+
+        // Step 8: The item in reservoir with the minimum key is replaced by
+        // item v_i
+        reservoir.pop();
+        reservoir.push(std::make_pair(k_i, it - weights.begin() + 1));
+      }
+    }
+
+    for (auto iret = result.end(); iret != result.begin();) {
+      --iret;
+
+      if (reservoir.empty()) {
+        throw std::runtime_error(
+            "Reservoir empty before all elements have been filled");
+      }
+
+      *iret = reservoir.top().second - 1;
+      reservoir.pop();
+    }
+
+    if (!reservoir.empty()) {
+      throw std::runtime_error(
+          "Reservoir not empty after all elements have been filled");
+    }
+
+    return result;
+  }
+};  // namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam/base/debug.cpp b/gtsam/base/debug.cpp
index d6d2a4fe0..592b42d15 100644
--- a/gtsam/base/debug.cpp
+++ b/gtsam/base/debug.cpp
@@ -20,7 +20,7 @@
 #include <gtsam/config.h> // for GTSAM_USE_TBB
 
 #ifdef GTSAM_USE_TBB
-#include <tbb/mutex.h>
+#include <mutex> // std::mutex, std::unique_lock
 #endif
 
 namespace gtsam {
@@ -28,13 +28,13 @@ namespace gtsam {
 GTSAM_EXPORT FastMap<std::string, ValueWithDefault<bool, false> > debugFlags;
 
 #ifdef GTSAM_USE_TBB
-tbb::mutex debugFlagsMutex;
+std::mutex debugFlagsMutex;
 #endif
 
 /* ************************************************************************* */
 bool guardedIsDebug(const std::string& s) {
 #ifdef GTSAM_USE_TBB
-  tbb::mutex::scoped_lock lock(debugFlagsMutex);
+  std::unique_lock<std::mutex> lock(debugFlagsMutex);
 #endif
   return gtsam::debugFlags[s];
 }
@@ -42,7 +42,7 @@ bool guardedIsDebug(const std::string& s) {
 /* ************************************************************************* */
 void guardedSetDebug(const std::string& s, const bool v) {
 #ifdef GTSAM_USE_TBB
-  tbb::mutex::scoped_lock lock(debugFlagsMutex);
+  std::unique_lock<std::mutex> lock(debugFlagsMutex);
 #endif
   gtsam::debugFlags[s] = v;
 }
diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h
new file mode 100644
index 000000000..5281eec6d
--- /dev/null
+++ b/gtsam/base/make_shared.h
@@ -0,0 +1,67 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2020, 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 make_shared.h
+ * @brief make_shared trampoline function to ensure proper alignment
+ * @author Fan Jiang
+ */
+
+#pragma once
+
+#include <gtsam/base/types.h>
+
+#include <Eigen/Core>
+
+#include <boost/make_shared.hpp>
+
+#include <type_traits>
+
+namespace gtsam {
+  /// An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly
+  template<bool B, class T = void>
+  using enable_if_t = typename std::enable_if<B, T>::type;
+}
+
+namespace gtsam {
+
+  /**
+   * Add our own `make_shared` as a layer of wrapping on `boost::make_shared`
+   * This solves the problem with the stock `make_shared` that custom alignment is not respected, causing SEGFAULTs
+   * at runtime, which is notoriously hard to debug.
+   *
+   * Explanation
+   * ===============
+   * The template `needs_eigen_aligned_allocator<T>::value` will evaluate to `std::true_type` if the type alias
+   * `_eigen_aligned_allocator_trait = void` is present in a class, which is automatically added by the
+   * `GTSAM_MAKE_ALIGNED_OPERATOR_NEW` macro.
+   *
+   * This function declaration will only be taken when the above condition is true, so if some object does not need to
+   * be aligned, `gtsam::make_shared` will fall back to the next definition, which is a simple wrapper for
+   * `boost::make_shared`.
+   *
+   * @tparam T The type of object being constructed
+   * @tparam Args Type of the arguments of the constructor
+   * @param args Arguments of the constructor
+   * @return The object created as a boost::shared_ptr<T>
+   */
+  template<typename T, typename ... Args>
+  gtsam::enable_if_t<needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
+    return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), std::forward<Args>(args)...);
+  }
+
+  /// Fall back to the boost version if no need for alignment
+  template<typename T, typename ... Args>
+  gtsam::enable_if_t<!needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
+    return boost::make_shared<T>(std::forward<Args>(args)...);
+  }
+
+}
diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index a9a088108..831bb6bcc 100644
--- a/gtsam/base/numericalDerivative.h
+++ b/gtsam/base/numericalDerivative.h
@@ -453,7 +453,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
-inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
+inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   return numericalDerivative52<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
 }
@@ -509,7 +509,7 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
-inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
+inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   return numericalDerivative54<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
 }
@@ -537,11 +537,185 @@ typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
-inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
+inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   return numericalDerivative55<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
 }
 
+/**
+ * Compute numerical derivative in argument 1 of 6-argument function
+ * @param h quintic function yielding m-vector
+ * @param x1 n-dimensional first argument value
+ * @param x2 second argument value
+ * @param x3 third argument value
+ * @param x4 fourth argument value
+ * @param x5 fifth argument value
+ * @param x6 sixth argument value
+ * @param delta increment for numerical derivative
+ * @return m*n Jacobian computed via central differencing
+ */
+template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
+typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
+    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
+      "Template argument Y must be a manifold type.");
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
+      "Template argument X1 must be a manifold type.");
+  return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta);
+}
+
+template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
+inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
+    const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
+  return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
+}
+
+/**
+ * Compute numerical derivative in argument 2 of 6-argument function
+ * @param h quintic function yielding m-vector
+ * @param x1 n-dimensional first argument value
+ * @param x2 second argument value
+ * @param x3 third argument value
+ * @param x4 fourth argument value
+ * @param x5 fifth argument value
+ * @param x6 sixth argument value
+ * @param delta increment for numerical derivative
+ * @return m*n Jacobian computed via central differencing
+ */
+template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
+typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
+    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
+      "Template argument Y must be a manifold type.");
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
+      "Template argument X1 must be a manifold type.");
+  return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta);
+}
+
+template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
+inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
+    const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
+  return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
+}
+
+/**
+ * Compute numerical derivative in argument 3 of 6-argument function
+ * @param h quintic function yielding m-vector
+ * @param x1 n-dimensional first argument value
+ * @param x2 second argument value
+ * @param x3 third argument value
+ * @param x4 fourth argument value
+ * @param x5 fifth argument value
+ *  @param x6 sixth argument value
+ * @param delta increment for numerical derivative
+ * @return m*n Jacobian computed via central differencing
+ */
+template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
+typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
+    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
+      "Template argument Y must be a manifold type.");
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
+      "Template argument X1 must be a manifold type.");
+  return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta);
+}
+
+template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
+inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
+    const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
+  return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
+}
+
+/**
+ * Compute numerical derivative in argument 4 of 6-argument function
+ * @param h quintic function yielding m-vector
+ * @param x1 n-dimensional first argument value
+ * @param x2 second argument value
+ * @param x3 third argument value
+ * @param x4 fourth argument value
+ * @param x5 fifth argument value
+ * @param x6 sixth argument value
+ * @param delta increment for numerical derivative
+ * @return m*n Jacobian computed via central differencing
+ */
+template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
+typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
+    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
+      "Template argument Y must be a manifold type.");
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
+      "Template argument X1 must be a manifold type.");
+  return numericalDerivative11<Y, X4>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta);
+}
+
+template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
+inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
+    const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
+  return numericalDerivative64<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
+}
+
+/**
+ * Compute numerical derivative in argument 5 of 6-argument function
+ * @param h quintic function yielding m-vector
+ * @param x1 n-dimensional first argument value
+ * @param x2 second argument value
+ * @param x3 third argument value
+ * @param x4 fourth argument value
+ * @param x5 fifth argument value
+ * @param x6 sixth argument value
+ * @param delta increment for numerical derivative
+ * @return m*n Jacobian computed via central differencing
+ */
+template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
+typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
+    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
+      "Template argument Y must be a manifold type.");
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
+      "Template argument X1 must be a manifold type.");
+  return numericalDerivative11<Y, X5>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta);
+}
+
+template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
+inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
+    const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
+  return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
+}
+
+/**
+ * Compute numerical derivative in argument 6 of 6-argument function
+ * @param h quintic function yielding m-vector
+ * @param x1 n-dimensional first argument value
+ * @param x2 second argument value
+ * @param x3 third argument value
+ * @param x4 fourth argument value
+ * @param x5 fifth argument value
+ * @param x6 sixth argument value
+ * @param delta increment for numerical derivative
+ * @return m*n Jacobian computed via central differencing
+ */
+template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
+typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative66(
+    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
+      "Template argument Y must be a manifold type.");
+  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
+      "Template argument X1 must be a manifold type.");
+  return numericalDerivative11<Y, X6>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta);
+}
+
+template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
+inline typename internal::FixedSizeMatrix<Y,X6>::type numericalDerivative66(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
+    const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
+  return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
+}
+
 /**
  * Compute numerical Hessian matrix.  Requires a single-argument Lie->scalar
  * function.  This is implemented simply as the derivative of the gradient.
diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h
index e475465de..f589ecc5e 100644
--- a/gtsam/base/serialization.h
+++ b/gtsam/base/serialization.h
@@ -42,123 +42,218 @@
 
 namespace gtsam {
 
-// Serialization directly to strings in compressed format
-template<class T>
-std::string serialize(const T& input) {
-  std::ostringstream out_archive_stream;
+/** @name Standard serialization
+ *  Serialization in default compressed format
+ */
+///@{
+/// serializes to a stream
+template <class T>
+void serializeToStream(const T& input, std::ostream& out_archive_stream) {
   boost::archive::text_oarchive out_archive(out_archive_stream);
   out_archive << input;
-  return out_archive_stream.str();
 }
 
-template<class T>
-void deserialize(const std::string& serialized, T& output) {
-  std::istringstream in_archive_stream(serialized);
+/// deserializes from a stream
+template <class T>
+void deserializeFromStream(std::istream& in_archive_stream, T& output) {
   boost::archive::text_iarchive in_archive(in_archive_stream);
   in_archive >> output;
 }
 
-template<class T>
+/// serializes to a string
+template <class T>
+std::string serializeToString(const T& input) {
+  std::ostringstream out_archive_stream;
+  serializeToStream(input, out_archive_stream);
+  return out_archive_stream.str();
+}
+
+/// deserializes from a string
+template <class T>
+void deserializeFromString(const std::string& serialized, T& output) {
+  std::istringstream in_archive_stream(serialized);
+  deserializeFromStream(in_archive_stream, output);
+}
+
+/// serializes to a file
+template <class T>
 bool serializeToFile(const T& input, const std::string& filename) {
   std::ofstream out_archive_stream(filename.c_str());
-  if (!out_archive_stream.is_open())
-    return false;
-  boost::archive::text_oarchive out_archive(out_archive_stream);
-  out_archive << input;
+  if (!out_archive_stream.is_open()) return false;
+  serializeToStream(input, out_archive_stream);
   out_archive_stream.close();
   return true;
 }
 
-template<class T>
+/// deserializes from a file
+template <class T>
 bool deserializeFromFile(const std::string& filename, T& output) {
   std::ifstream in_archive_stream(filename.c_str());
-  if (!in_archive_stream.is_open())
-    return false;
-  boost::archive::text_iarchive in_archive(in_archive_stream);
-  in_archive >> output;
+  if (!in_archive_stream.is_open()) return false;
+  deserializeFromStream(in_archive_stream, output);
   in_archive_stream.close();
   return true;
 }
 
-// Serialization to XML format with named structures
-template<class T>
-std::string serializeXML(const T& input, const std::string& name="data") {
-  std::ostringstream out_archive_stream;
-  // braces to flush out_archive as it goes out of scope before taking str()
-  // fixes crash with boost 1.66-1.68
-  // see https://github.com/boostorg/serialization/issues/82
-  {
-    boost::archive::xml_oarchive out_archive(out_archive_stream);
-    out_archive << boost::serialization::make_nvp(name.c_str(), input);
-  }
-  return out_archive_stream.str();
+/// serializes to a string
+template <class T>
+std::string serialize(const T& input) {
+  return serializeToString(input);
 }
 
-template<class T>
-void deserializeXML(const std::string& serialized, T& output, const std::string& name="data") {
-  std::istringstream in_archive_stream(serialized);
-  boost::archive::xml_iarchive in_archive(in_archive_stream);
-  in_archive >> boost::serialization::make_nvp(name.c_str(), output);
+/// deserializes from a string
+template <class T>
+void deserialize(const std::string& serialized, T& output) {
+  deserializeFromString(serialized, output);
 }
+///@}
 
-template<class T>
-bool serializeToXMLFile(const T& input, const std::string& filename, const std::string& name="data") {
-  std::ofstream out_archive_stream(filename.c_str());
-  if (!out_archive_stream.is_open())
-    return false;
+/** @name XML Serialization
+ *  Serialization to XML format with named structures
+ */
+///@{
+/// serializes to a stream in XML
+template <class T>
+void serializeToXMLStream(const T& input, std::ostream& out_archive_stream,
+                  const std::string& name = "data") {
   boost::archive::xml_oarchive out_archive(out_archive_stream);
-  out_archive << boost::serialization::make_nvp(name.c_str(), input);;
-  out_archive_stream.close();
-  return true;
+  out_archive << boost::serialization::make_nvp(name.c_str(), input);
 }
 
-template<class T>
-bool deserializeFromXMLFile(const std::string& filename, T& output, const std::string& name="data") {
-  std::ifstream in_archive_stream(filename.c_str());
-  if (!in_archive_stream.is_open())
-    return false;
+/// deserializes from a stream in XML
+template <class T>
+void deserializeFromXMLStream(std::istream& in_archive_stream, T& output,
+                    const std::string& name = "data") {
   boost::archive::xml_iarchive in_archive(in_archive_stream);
   in_archive >> boost::serialization::make_nvp(name.c_str(), output);
-  in_archive_stream.close();
-  return true;
 }
 
-// Serialization to binary format with named structures
-template<class T>
-std::string serializeBinary(const T& input, const std::string& name="data") {
+/// serializes to a string in XML
+template <class T>
+std::string serializeToXMLString(const T& input,
+                         const std::string& name = "data") {
   std::ostringstream out_archive_stream;
-  boost::archive::binary_oarchive out_archive(out_archive_stream);
-  out_archive << boost::serialization::make_nvp(name.c_str(), input);
+  serializeToXMLStream(input, out_archive_stream, name);
   return out_archive_stream.str();
 }
 
-template<class T>
-void deserializeBinary(const std::string& serialized, T& output, const std::string& name="data") {
+/// deserializes from a string in XML
+template <class T>
+void deserializeFromXMLString(const std::string& serialized, T& output,
+                    const std::string& name = "data") {
   std::istringstream in_archive_stream(serialized);
-  boost::archive::binary_iarchive in_archive(in_archive_stream);
-  in_archive >> boost::serialization::make_nvp(name.c_str(), output);
+  deserializeFromXMLStream(in_archive_stream, output, name);
 }
 
-template<class T>
-bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") {
+/// serializes to an XML file
+template <class T>
+bool serializeToXMLFile(const T& input, const std::string& filename,
+                        const std::string& name = "data") {
   std::ofstream out_archive_stream(filename.c_str());
-  if (!out_archive_stream.is_open())
-    return false;
-  boost::archive::binary_oarchive out_archive(out_archive_stream);
-  out_archive << boost::serialization::make_nvp(name.c_str(), input);
+  if (!out_archive_stream.is_open()) return false;
+  serializeToXMLStream(input, out_archive_stream, name);
   out_archive_stream.close();
   return true;
 }
 
-template<class T>
-bool deserializeFromBinaryFile(const std::string& filename, T& output, const std::string& name="data") {
+/// deserializes from an XML file
+template <class T>
+bool deserializeFromXMLFile(const std::string& filename, T& output,
+                            const std::string& name = "data") {
   std::ifstream in_archive_stream(filename.c_str());
-  if (!in_archive_stream.is_open())
-    return false;
-  boost::archive::binary_iarchive in_archive(in_archive_stream);
-  in_archive >> boost::serialization::make_nvp(name.c_str(), output);
+  if (!in_archive_stream.is_open()) return false;
+  deserializeFromXMLStream(in_archive_stream, output, name);
   in_archive_stream.close();
   return true;
 }
 
-} // \namespace gtsam
+/// serializes to a string in XML
+template <class T>
+std::string serializeXML(const T& input,
+                         const std::string& name = "data") {
+  return serializeToXMLString(input, name);
+}
+
+/// deserializes from a string in XML
+template <class T>
+void deserializeXML(const std::string& serialized, T& output,
+                    const std::string& name = "data") {
+  deserializeFromXMLString(serialized, output, name);
+}
+///@}
+
+/** @name Binary Serialization
+ *  Serialization to binary format with named structures
+ */
+///@{
+/// serializes to a stream in binary
+template <class T>
+void serializeToBinaryStream(const T& input, std::ostream& out_archive_stream,
+                     const std::string& name = "data") {
+  boost::archive::binary_oarchive out_archive(out_archive_stream);
+  out_archive << boost::serialization::make_nvp(name.c_str(), input);
+}
+
+/// deserializes from a stream in binary
+template <class T>
+void deserializeFromBinaryStream(std::istream& in_archive_stream, T& output,
+                       const std::string& name = "data") {
+  boost::archive::binary_iarchive in_archive(in_archive_stream);
+  in_archive >> boost::serialization::make_nvp(name.c_str(), output);
+}
+
+/// serializes to a string in binary
+template <class T>
+std::string serializeToBinaryString(const T& input,
+                            const std::string& name = "data") {
+  std::ostringstream out_archive_stream;
+  serializeToBinaryStream(input, out_archive_stream, name);
+  return out_archive_stream.str();
+}
+
+/// deserializes from a string in binary
+template <class T>
+void deserializeFromBinaryString(const std::string& serialized, T& output,
+                       const std::string& name = "data") {
+  std::istringstream in_archive_stream(serialized);
+  deserializeFromBinaryStream(in_archive_stream, output, name);
+}
+
+/// serializes to a binary file
+template <class T>
+bool serializeToBinaryFile(const T& input, const std::string& filename,
+                           const std::string& name = "data") {
+  std::ofstream out_archive_stream(filename.c_str());
+  if (!out_archive_stream.is_open()) return false;
+  serializeToBinaryStream(input, out_archive_stream, name);
+  out_archive_stream.close();
+  return true;
+}
+
+/// deserializes from a binary file
+template <class T>
+bool deserializeFromBinaryFile(const std::string& filename, T& output,
+                               const std::string& name = "data") {
+  std::ifstream in_archive_stream(filename.c_str());
+  if (!in_archive_stream.is_open()) return false;
+  deserializeFromBinaryStream(in_archive_stream, output, name);
+  in_archive_stream.close();
+  return true;
+}
+
+/// serializes to a string in binary
+template <class T>
+std::string serializeBinary(const T& input,
+                            const std::string& name = "data") {
+  return serializeToBinaryString(input, name);
+}
+
+/// deserializes from a string in binary
+template <class T>
+void deserializeBinary(const std::string& serialized, T& output,
+                       const std::string& name = "data") {
+  deserializeFromBinaryString(serialized, output, name);
+}
+///@}
+
+}  // namespace gtsam
diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h
index 06b3462e9..5994a5e51 100644
--- a/gtsam/base/serializationTestHelpers.h
+++ b/gtsam/base/serializationTestHelpers.h
@@ -26,6 +26,7 @@
 #include <gtsam/base/serialization.h>
 
 #include <boost/serialization/serialization.hpp>
+#include <boost/filesystem.hpp>
 
 
 // whether to print the serialized text to stdout
@@ -40,22 +41,37 @@ T create() {
   return T();
 }
 
+// Creates or empties a folder in the build folder and returns the relative path
+boost::filesystem::path resetFilesystem(
+    boost::filesystem::path folder = "actual") {
+  boost::filesystem::remove_all(folder);
+  boost::filesystem::create_directory(folder);
+  return folder;
+}
+
 // Templated round-trip serialization
 template<class T>
 void roundtrip(const T& input, T& output) {
-  // Serialize
   std::string serialized = serialize(input);
   if (verbose) std::cout << serialized << std::endl << std::endl;
-
   deserialize(serialized, output);
 }
 
-// This version requires equality operator
+// Templated round-trip serialization using a file
+template<class T>
+void roundtripFile(const T& input, T& output) {
+  boost::filesystem::path path = resetFilesystem()/"graph.dat";
+  serializeToFile(input, path.string());
+  deserializeFromFile(path.string(), output);
+}
+
+// This version requires equality operator and uses string and file round-trips
 template<class T>
 bool equality(const T& input = T()) {
-  T output = create<T>();
+  T output = create<T>(), outputf = create<T>();
   roundtrip<T>(input,output);
-  return input==output;
+  roundtripFile<T>(input,outputf);
+  return (input==output) && (input==outputf);
 }
 
 // This version requires Testable
@@ -77,20 +93,26 @@ bool equalsDereferenced(const T& input) {
 // Templated round-trip serialization using XML
 template<class T>
 void roundtripXML(const T& input, T& output) {
-  // Serialize
   std::string serialized = serializeXML<T>(input);
   if (verbose) std::cout << serialized << std::endl << std::endl;
-
-  // De-serialize
   deserializeXML(serialized, output);
 }
 
+// Templated round-trip serialization using XML File
+template<class T>
+void roundtripXMLFile(const T& input, T& output) {
+  boost::filesystem::path path = resetFilesystem()/"graph.xml";
+  serializeToXMLFile(input, path.string());
+  deserializeFromXMLFile(path.string(), output);
+}
+
 // This version requires equality operator
 template<class T>
 bool equalityXML(const T& input = T()) {
-  T output = create<T>();
+  T output = create<T>(), outputf = create<T>();
   roundtripXML<T>(input,output);
-  return input==output;
+  roundtripXMLFile<T>(input,outputf);
+  return (input==output) && (input==outputf);
 }
 
 // This version requires Testable
@@ -112,20 +134,26 @@ bool equalsDereferencedXML(const T& input = T()) {
 // Templated round-trip serialization using XML
 template<class T>
 void roundtripBinary(const T& input, T& output) {
-  // Serialize
   std::string serialized = serializeBinary<T>(input);
   if (verbose) std::cout << serialized << std::endl << std::endl;
-
-  // De-serialize
   deserializeBinary(serialized, output);
 }
 
+// Templated round-trip serialization using Binary file
+template<class T>
+void roundtripBinaryFile(const T& input, T& output) {
+  boost::filesystem::path path = resetFilesystem()/"graph.bin";
+  serializeToBinaryFile(input, path.string());
+  deserializeFromBinaryFile(path.string(), output);
+}
+
 // This version requires equality operator
 template<class T>
 bool equalityBinary(const T& input = T()) {
-  T output = create<T>();
+  T output = create<T>(), outputf = create<T>();
   roundtripBinary<T>(input,output);
-  return input==output;
+  roundtripBinaryFile<T>(input,outputf);
+  return (input==output) && (input==outputf);
 }
 
 // This version requires Testable
diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp
index 1cbf71e1f..d27e43040 100644
--- a/gtsam/base/tests/testNumericalDerivative.cpp
+++ b/gtsam/base/tests/testNumericalDerivative.cpp
@@ -135,6 +135,86 @@ TEST(testNumericalDerivative, numericalHessian311) {
   EXPECT(assert_equal(expected33, actual33, 1e-5));
 }
 
+/* ************************************************************************* */
+Vector6 f6(const double x1, const double x2, const double x3, const double x4,
+           const double x5, const double x6) {
+  Vector6 result;
+  result << sin(x1), cos(x2), x3 * x3, x4 * x4 * x4, sqrt(x5), sin(x6) - cos(x6); 
+  return result;
+}
+
+/* ************************************************************************* */
+//
+TEST(testNumericalDerivative, numeriDerivative61) {
+  double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
+  
+  Matrix expected61 = (Matrix(6, 1) << cos(x1), 0, 0, 0, 0, 0).finished();
+  Matrix61 actual61 = numericalDerivative61<Vector6, double, double,
+      double, double, double, double>(f6, x1, x2, x3, x4, x5, x6);
+  
+  EXPECT(assert_equal(expected61, actual61, 1e-5));
+}
+
+/* ************************************************************************* */
+//
+TEST(testNumericalDerivative, numeriDerivative62) {
+  double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
+  
+  Matrix expected62 = (Matrix(6, 1) << 0, -sin(x2), 0, 0, 0, 0).finished();
+  Matrix61 actual62 = numericalDerivative62<Vector6, double, double, double,
+     double, double, double>(f6, x1, x2, x3, x4, x5, x6);
+  
+  EXPECT(assert_equal(expected62, actual62, 1e-5));
+}
+
+/* ************************************************************************* */
+//
+TEST(testNumericalDerivative, numeriDerivative63) {
+  double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
+  
+  Matrix expected63 = (Matrix(6, 1) << 0, 0, 2 * x3, 0, 0, 0).finished();
+  Matrix61 actual63 = numericalDerivative63<Vector6, double, double, double,
+     double, double, double>(f6, x1, x2, x3, x4, x5, x6);
+  
+  EXPECT(assert_equal(expected63, actual63, 1e-5));
+}
+
+/* ************************************************************************* */
+//
+TEST(testNumericalDerivative, numeriDerivative64) {
+  double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
+  
+  Matrix expected64 = (Matrix(6, 1) << 0, 0, 0, 3 * x4 * x4, 0, 0).finished();
+  Matrix61 actual64 = numericalDerivative64<Vector6, double, double, double,
+     double, double, double>(f6, x1, x2, x3, x4, x5, x6);
+  
+  EXPECT(assert_equal(expected64, actual64, 1e-5));
+}
+
+/* ************************************************************************* */
+//
+TEST(testNumericalDerivative, numeriDerivative65) {
+  double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
+  
+  Matrix expected65 = (Matrix(6, 1) << 0, 0, 0, 0, 0.5 / sqrt(x5), 0).finished();
+  Matrix61 actual65 = numericalDerivative65<Vector6, double, double, double,
+     double, double, double>(f6, x1, x2, x3, x4, x5, x6);
+  
+  EXPECT(assert_equal(expected65, actual65, 1e-5));
+}
+
+/* ************************************************************************* */
+//
+TEST(testNumericalDerivative, numeriDerivative66) {
+  double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
+  
+  Matrix expected66 = (Matrix(6, 1) << 0, 0, 0, 0, 0, cos(x6) + sin(x6)).finished();
+  Matrix61 actual66 = numericalDerivative66<Vector6, double, double, double, 
+      double, double, double>(f6, x1, x2, x3, x4, x5, x6);
+  
+  EXPECT(assert_equal(expected66, actual66, 1e-5));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;
diff --git a/gtsam/base/tests/testWeightedSampler.cpp b/gtsam/base/tests/testWeightedSampler.cpp
new file mode 100644
index 000000000..8ebcdfd2e
--- /dev/null
+++ b/gtsam/base/tests/testWeightedSampler.cpp
@@ -0,0 +1,40 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testWeightedSampler.cpp
+ * @brief   Unit test for WeightedSampler
+ * @author  Frank Dellaert
+ * @date    MAy 2019
+ **/
+
+#include <gtsam/base/WeightedSampler.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <random>
+
+using namespace std;
+using namespace gtsam;
+
+TEST(WeightedSampler, sampleWithoutReplacement) {
+  vector<double> weights{1, 2, 3, 4, 3, 2, 1};
+  std::mt19937 rng(42);
+  WeightedSampler<std::mt19937> sampler(&rng);
+  auto samples = sampler.sampleWithoutReplacement(5, weights);
+  EXPECT_LONGS_EQUAL(5, samples.size());
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h
index 557500e73..972bb45f7 100644
--- a/gtsam/base/timing.h
+++ b/gtsam/base/timing.h
@@ -196,7 +196,7 @@ namespace gtsam {
     /**
      * Small class that calls internal::tic at construction, and internol::toc when destroyed
      */
-    class AutoTicToc {
+    class GTSAM_EXPORT AutoTicToc {
      private:
       size_t id_;
       const char* label_;
diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h
index 9b2dae3d0..b52d44e2a 100644
--- a/gtsam/base/treeTraversal/parallelTraversalTasks.h
+++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h
@@ -22,11 +22,8 @@
 #include <boost/make_shared.hpp>
 
 #ifdef GTSAM_USE_TBB
-#  include <tbb/tbb.h>
-#  include <tbb/scalable_allocator.h>
-#  undef max // TBB seems to include windows.h and we don't want these macros
-#  undef min
-#  undef ERROR
+#include <tbb/task.h>               // tbb::task, tbb::task_list
+#include <tbb/scalable_allocator.h> // tbb::scalable_allocator
 
 namespace gtsam {
 
@@ -35,28 +32,6 @@ namespace gtsam {
 
     namespace internal {
 
-      /* ************************************************************************* */
-      template<typename NODE, typename DATA, typename VISITOR_POST>
-      class PostOrderTask : public tbb::task
-      {
-      public:
-        const boost::shared_ptr<NODE>& treeNode;
-        boost::shared_ptr<DATA> myData;
-        VISITOR_POST& visitorPost;
-
-        PostOrderTask(const boost::shared_ptr<NODE>& treeNode,
-                      const boost::shared_ptr<DATA>& myData, VISITOR_POST& visitorPost)
-            : treeNode(treeNode), myData(myData), visitorPost(visitorPost) {}
-
-        tbb::task* execute()
-        {
-          // Run the post-order visitor
-          (void) visitorPost(treeNode, *myData);
-
-          return NULL;
-        }
-      };
-
       /* ************************************************************************* */
       template<typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
       class PreOrderTask : public tbb::task
@@ -88,7 +63,7 @@ namespace gtsam {
           {
             // Run the post-order visitor since this task was recycled to run the post-order visitor
             (void) visitorPost(treeNode, *myData);
-            return NULL;
+            return nullptr;
           }
           else
           {
@@ -129,14 +104,14 @@ namespace gtsam {
               {
                 // Run the post-order visitor in this task if we have no children
                 (void) visitorPost(treeNode, *myData);
-                return NULL;
+                return nullptr;
               }
             }
             else
             {
               // Process this node and its children in this task
               processNodeRecursively(treeNode, *myData);
-              return NULL;
+              return nullptr;
             }
           }
         }
@@ -184,8 +159,8 @@ namespace gtsam {
           set_ref_count(1 + (int) roots.size());
           // Spawn tasks
           spawn_and_wait_for_all(tasks);
-          // Return NULL
-          return NULL;
+          // Return nullptr
+          return nullptr;
         }
       };
 
diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp
new file mode 100644
index 000000000..edc449b12
--- /dev/null
+++ b/gtsam/base/types.cpp
@@ -0,0 +1,67 @@
+/* ----------------------------------------------------------------------------
+
+ * 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     types.cpp
+ * @brief    Functions for handling type information
+ * @author   Varun Agrawal
+ * @date     May 18, 2020
+ * @addtogroup base
+ */
+
+#include <gtsam/base/types.h>
+
+#ifdef _WIN32
+#define WIN32_LEAN_AND_MEAN
+#include <windows.h>
+#include <DbgHelp.h>
+#endif
+
+#ifdef __GNUG__
+#include <cstdlib>
+#include <cxxabi.h>
+#include <string>
+#endif
+
+namespace gtsam {
+
+/// Pretty print Value type name
+std::string demangle(const char* name) {
+  // by default set to the original mangled name
+  std::string demangled_name = std::string(name);
+
+#ifdef __GNUG__
+  // g++ version of demangle
+  char* demangled = nullptr;
+  int status = -1; // some arbitrary value to eliminate the compiler warning
+  demangled = abi::__cxa_demangle(name, nullptr, nullptr, &status),
+  
+  demangled_name = (status == 0) ? std::string(demangled) : std::string(name);
+
+  std::free(demangled);
+
+#elif _WIN32
+    char undecorated_name[1024];
+    
+    if (UnDecorateSymbolName(
+        name, undecorated_name, sizeof(undecorated_name),
+        UNDNAME_COMPLETE))
+    {
+      // successful conversion, take value from: undecorated_name			                      
+      demangled_name = std::string(undecorated_name);               
+    }
+    // else keep using mangled name
+#endif
+
+  return demangled_name;
+}
+
+} /* namespace gtsam */
diff --git a/gtsam/base/types.h b/gtsam/base/types.h
index 3b6c9f337..aaada3cee 100644
--- a/gtsam/base/types.h
+++ b/gtsam/base/types.h
@@ -27,9 +27,10 @@
 #include <cstddef>
 #include <cstdint>
 
+#include <exception>
+#include <string>
+
 #ifdef GTSAM_USE_TBB
-#include <tbb/task_scheduler_init.h>
-#include <tbb/tbb_exception.h>
 #include <tbb/scalable_allocator.h>
 #endif
 
@@ -53,6 +54,9 @@
 
 namespace gtsam {
 
+  /// Function to demangle type name of variable, e.g. demangle(typeid(x).name())
+  std::string GTSAM_EXPORT demangle(const char* name);
+
   /// Integer nonlinear key type
   typedef std::uint64_t Key;
 
@@ -227,3 +231,50 @@ namespace std {
 #ifdef ERROR
 #undef ERROR
 #endif
+
+namespace gtsam {
+
+  /// Convenience void_t as we assume C++11, it will not conflict the std one in C++17 as this is in `gtsam::`
+  template<typename ...> using void_t = void;
+
+  /**
+   * A SFINAE trait to mark classes that need special alignment.
+   *
+   * This is required to make boost::make_shared and etc respect alignment, which is essential for the Python
+   * wrappers to work properly.
+   *
+   * Explanation
+   * =============
+   * When a GTSAM type is not declared with the type alias `_eigen_aligned_allocator_trait = void`, the first template
+   * will be taken so `needs_eigen_aligned_allocator` will be resolved to `std::false_type`.
+   *
+   * Otherwise, it will resolve to the second template, which will be resolved to `std::true_type`.
+   *
+   * Please refer to `gtsam/base/make_shared.h` for an example.
+   */
+  template<typename, typename = void_t<>>
+  struct needs_eigen_aligned_allocator : std::false_type {
+  };
+  template<typename T>
+  struct needs_eigen_aligned_allocator<T, void_t<typename T::_eigen_aligned_allocator_trait>> : std::true_type {
+  };
+
+}
+
+/**
+ * This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned
+ * memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug.
+ * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.
+ */
+#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
+  using _eigen_aligned_allocator_trait = void;
+
+/**
+ * This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned
+ * memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug.
+ * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.
+ */
+#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
+  using _eigen_aligned_allocator_trait = void;
diff --git a/gtsam/config.h.in b/gtsam/config.h.in
index 4b8bd180d..b480996ec 100644
--- a/gtsam/config.h.in
+++ b/gtsam/config.h.in
@@ -42,6 +42,9 @@
 // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake)
 #cmakedefine GTSAM_USE_TBB
 
+// Whether we are using a TBB version higher than 2020
+#cmakedefine TBB_GREATER_EQUAL_2020
+
 // Whether we are using system-Eigen or our own patched version
 #cmakedefine GTSAM_USE_SYSTEM_EIGEN
 
diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h
index 041d4c206..9cc55ed6a 100644
--- a/gtsam/discrete/AlgebraicDecisionTree.h
+++ b/gtsam/discrete/AlgebraicDecisionTree.h
@@ -101,7 +101,7 @@ namespace gtsam {
     /** Create a new function splitting on a variable */
     template<typename Iterator>
     AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) :
-        Super(NULL) {
+        Super(nullptr) {
       this->root_ = compose(begin, end, label);
     }
 
diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h
index dcc336f89..237caf745 100644
--- a/gtsam/discrete/DiscreteBayesNet.h
+++ b/gtsam/discrete/DiscreteBayesNet.h
@@ -20,13 +20,14 @@
 #include <vector>
 #include <map>
 #include <boost/shared_ptr.hpp>
+#include <gtsam/inference/BayesNet.h>
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/discrete/DiscreteConditional.h>
 
 namespace gtsam {
 
 /** A Bayes net made from linear-Discrete densities */
-  class GTSAM_EXPORT DiscreteBayesNet: public FactorGraph<DiscreteConditional>
+  class GTSAM_EXPORT DiscreteBayesNet: public BayesNet<DiscreteConditional>
   {
   public:
 
diff --git a/gtsam/discrete/DiscreteBayesTree.cpp b/gtsam/discrete/DiscreteBayesTree.cpp
index bed50a470..990d10dbe 100644
--- a/gtsam/discrete/DiscreteBayesTree.cpp
+++ b/gtsam/discrete/DiscreteBayesTree.cpp
@@ -29,13 +29,32 @@ namespace gtsam {
   template class BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph>;
   template class BayesTree<DiscreteBayesTreeClique>;
 
+  /* ************************************************************************* */
+  double DiscreteBayesTreeClique::evaluate(
+      const DiscreteConditional::Values& values) const {
+    // evaluate all conditionals and multiply
+    double result = (*conditional_)(values);
+    for (const auto& child : children) {
+      result *= child->evaluate(values);
+    }
+    return result;
+  }
 
   /* ************************************************************************* */
-  bool DiscreteBayesTree::equals(const This& other, double tol) const
-  {
+  bool DiscreteBayesTree::equals(const This& other, double tol) const {
     return Base::equals(other, tol);
   }
 
+  /* ************************************************************************* */
+  double DiscreteBayesTree::evaluate(
+      const DiscreteConditional::Values& values) const {
+    double result = 1.0;
+    for (const auto& root : roots_) {
+      result *= root->evaluate(values);
+    }
+    return result;
+  }
+
 } // \namespace gtsam
 
 
diff --git a/gtsam/discrete/DiscreteBayesTree.h b/gtsam/discrete/DiscreteBayesTree.h
index 0df6ab476..3d6e016fd 100644
--- a/gtsam/discrete/DiscreteBayesTree.h
+++ b/gtsam/discrete/DiscreteBayesTree.h
@@ -11,7 +11,8 @@
 
 /**
  * @file    DiscreteBayesTree.h
- * @brief   Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree
+ * @brief   Discrete Bayes Tree, the result of eliminating a
+ * DiscreteJunctionTree
  * @brief   DiscreteBayesTree
  * @author  Frank Dellaert
  * @author  Richard Roberts
@@ -22,45 +23,62 @@
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/inference/BayesTree.h>
+#include <gtsam/inference/Conditional.h>
 #include <gtsam/inference/BayesTreeCliqueBase.h>
 
+#include <string>
+
 namespace gtsam {
 
-  // Forward declarations
-  class DiscreteConditional;
-  class VectorValues;
+// Forward declarations
+class DiscreteConditional;
+class VectorValues;
 
-  /* ************************************************************************* */
-  /** A clique in a DiscreteBayesTree */
-  class GTSAM_EXPORT DiscreteBayesTreeClique :
-    public BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph>
-  {
-  public:
-    typedef DiscreteBayesTreeClique This;
-    typedef BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph> Base;
-    typedef boost::shared_ptr<This> shared_ptr;
-    typedef boost::weak_ptr<This> weak_ptr;
-    DiscreteBayesTreeClique() {}
-    DiscreteBayesTreeClique(const boost::shared_ptr<DiscreteConditional>& conditional) : Base(conditional) {}
-  };
+/* ************************************************************************* */
+/** A clique in a DiscreteBayesTree */
+class GTSAM_EXPORT DiscreteBayesTreeClique
+    : public BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph> {
+ public:
+  typedef DiscreteBayesTreeClique This;
+  typedef BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph>
+      Base;
+  typedef boost::shared_ptr<This> shared_ptr;
+  typedef boost::weak_ptr<This> weak_ptr;
+  DiscreteBayesTreeClique() {}
+  DiscreteBayesTreeClique(
+      const boost::shared_ptr<DiscreteConditional>& conditional)
+      : Base(conditional) {}
 
-  /* ************************************************************************* */
-  /** A Bayes tree representing a Discrete density */
-  class GTSAM_EXPORT DiscreteBayesTree :
-    public BayesTree<DiscreteBayesTreeClique>
-  {
-  private:
-    typedef BayesTree<DiscreteBayesTreeClique> Base;
+  /// print index signature only
+  void printSignature(
+      const std::string& s = "Clique: ",
+      const KeyFormatter& formatter = DefaultKeyFormatter) const {
+    conditional_->printSignature(s, formatter);
+  }
 
-  public:
-    typedef DiscreteBayesTree This;
-    typedef boost::shared_ptr<This> shared_ptr;
+  //** evaluate conditional probability of subtree for given Values */
+  double evaluate(const DiscreteConditional::Values& values) const;
+};
 
-    /** Default constructor, creates an empty Bayes tree */
-    DiscreteBayesTree() {}
+/* ************************************************************************* */
+/** A Bayes tree representing a Discrete density */
+class GTSAM_EXPORT DiscreteBayesTree
+    : public BayesTree<DiscreteBayesTreeClique> {
+ private:
+  typedef BayesTree<DiscreteBayesTreeClique> Base;
 
-    /** Check equality */
-    bool equals(const This& other, double tol = 1e-9) const;
-  };
+ public:
+  typedef DiscreteBayesTree This;
+  typedef boost::shared_ptr<This> shared_ptr;
 
-}
+  /** Default constructor, creates an empty Bayes tree */
+  DiscreteBayesTree() {}
+
+  /** Check equality */
+  bool equals(const This& other, double tol = 1e-9) const;
+
+  //** evaluate probability for given Values */
+  double evaluate(const DiscreteConditional::Values& values) const;
+};
+
+}  // namespace gtsam
diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index f12cd2567..ac7c58405 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -23,13 +23,12 @@
 #include <gtsam/base/debug.h>
 
 #include <boost/make_shared.hpp>
-#include <boost/random/mersenne_twister.hpp>
-#include <boost/random/uniform_real.hpp>
-#include <boost/random/variate_generator.hpp>
 
-#include <vector>
 #include <algorithm>
+#include <random>
 #include <stdexcept>
+#include <string>
+#include <vector>
 
 using namespace std;
 
@@ -46,29 +45,43 @@ DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
 
 /* ******************************************************************************** */
 DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
-    const DecisionTreeFactor& marginal, const boost::optional<Ordering>& orderedKeys) :
+    const DecisionTreeFactor& marginal) :
     BaseFactor(
         ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal), BaseConditional(
             joint.size()-marginal.size()) {
   if (ISDEBUG("DiscreteConditional::DiscreteConditional"))
     cout << (firstFrontalKey()) << endl; //TODO Print all keys
-  if (orderedKeys) {
-    keys_.clear();
-    keys_.insert(keys_.end(), orderedKeys->begin(), orderedKeys->end());
+}
+
+/* ******************************************************************************** */
+DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
+    const DecisionTreeFactor& marginal, const Ordering& orderedKeys) :
+    DiscreteConditional(joint, marginal) {
+  keys_.clear();
+  keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end());
+}
+
+/* ******************************************************************************** */
+DiscreteConditional::DiscreteConditional(const Signature& signature)
+    : BaseFactor(signature.discreteKeys(), signature.cpt()),
+      BaseConditional(1) {}
+
+/* ******************************************************************************** */
+void DiscreteConditional::print(const string& s,
+                                const KeyFormatter& formatter) const {
+  cout << s << " P( ";
+  for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
+    cout << formatter(*it) << " ";
   }
-}
-
-/* ******************************************************************************** */
-DiscreteConditional::DiscreteConditional(const Signature& signature) :
-        BaseFactor(signature.discreteKeysParentsFirst(), signature.cpt()), BaseConditional(
-            1) {
-}
-
-/* ******************************************************************************** */
-void DiscreteConditional::print(const std::string& s,
-    const KeyFormatter& formatter) const {
-  std::cout << s << std::endl;
-  Potentials::print(s);
+  if (nrParents()) {
+    cout << "| ";
+    for (const_iterator it = beginParents(); it != endParents(); ++it) {
+      cout << formatter(*it) << " ";
+    }
+  }
+  cout << ")";
+  Potentials::print("");
+  cout << endl;
 }
 
 /* ******************************************************************************** */
@@ -171,57 +184,28 @@ size_t DiscreteConditional::solve(const Values& parentsValues) const {
 
 /* ******************************************************************************** */
 size_t DiscreteConditional::sample(const Values& parentsValues) const {
-
-  using boost::uniform_real;
-  static boost::mt19937 gen(2); // random number generator
-
-  bool debug = ISDEBUG("DiscreteConditional::sample");
+  static mt19937 rng(2);  // random number generator
 
   // Get the correct conditional density
-  ADT pFS = choose(parentsValues); // P(F|S=parentsValues)
-  if (debug)
-    GTSAM_PRINT(pFS);
+  ADT pFS = choose(parentsValues);  // P(F|S=parentsValues)
 
-  // get cumulative distribution function (cdf)
-  // TODO, only works for one key now, seems horribly slow this way
+  // TODO(Duy): only works for one key now, seems horribly slow this way
   assert(nrFrontals() == 1);
-  Key j = (firstFrontalKey());
-  size_t nj = cardinality(j);
-  vector<double> cdf(nj);
+  Key key = firstFrontalKey();
+  size_t nj = cardinality(key);
+  vector<double> p(nj);
   Values frontals;
-  double sum = 0;
   for (size_t value = 0; value < nj; value++) {
-    frontals[j] = value;
-    double pValueS = pFS(frontals); // P(F=value|S=parentsValues)
-    sum += pValueS; // accumulate
-    if (debug)
-      cout << sum << " ";
-    if (pValueS == 1) {
-      if (debug)
-        cout << "--> " << value << endl;
-      return value; // shortcut exit
+    frontals[key] = value;
+    p[value] = pFS(frontals);  // P(F=value|S=parentsValues)
+    if (p[value] == 1.0) {
+      return value;  // shortcut exit
     }
-    cdf[value] = sum;
   }
-
-  // inspired by http://www.boost.org/doc/libs/1_46_1/doc/html/boost_random/tutorial.html
-  uniform_real<> dist(0, cdf.back());
-  boost::variate_generator<boost::mt19937&, uniform_real<> > die(gen, dist);
-  size_t sampled = lower_bound(cdf.begin(), cdf.end(), die()) - cdf.begin();
-  if (debug)
-    cout << "-> " << sampled << endl;
-
-  return sampled;
-
-  return 0;
+  std::discrete_distribution<size_t> distribution(p.begin(), p.end());
+  return distribution(rng);
 }
 
-/* ******************************************************************************** */
-//void DiscreteConditional::permuteWithInverse(
-//    const Permutation& inversePermutation) {
-//  IndexConditionalOrdered::permuteWithInverse(inversePermutation);
-//  Potentials::permuteWithInverse(inversePermutation);
-//}
 /* ******************************************************************************** */
 
 }// namespace
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 88c3e5620..225e6e1d3 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -24,6 +24,8 @@
 #include <boost/shared_ptr.hpp>
 #include <boost/make_shared.hpp>
 
+#include <string>
+
 namespace gtsam {
 
 /**
@@ -60,7 +62,11 @@ public:
 
   /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */
   DiscreteConditional(const DecisionTreeFactor& joint,
-      const DecisionTreeFactor& marginal, const boost::optional<Ordering>& orderedKeys = boost::none);
+      const DecisionTreeFactor& marginal);
+
+  /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */
+  DiscreteConditional(const DecisionTreeFactor& joint,
+      const DecisionTreeFactor& marginal, const Ordering& orderedKeys);
 
   /**
    * Combine several conditional into a single one.
@@ -88,6 +94,13 @@ public:
   /// @name Standard Interface
   /// @{
 
+  /// print index signature only
+  void printSignature(
+      const std::string& s = "Discrete Conditional: ",
+      const KeyFormatter& formatter = DefaultKeyFormatter) const {
+    static_cast<const BaseConditional*>(this)->print(s, formatter);
+  }
+
   /// Evaluate, just look up in AlgebraicDecisonTree
   virtual double operator()(const Values& values) const {
     return Potentials::operator()(values);
diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index af11d4b14..e41968d6b 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -71,7 +71,7 @@ namespace gtsam {
     for (size_t i = 0; i < factors_.size(); i++) {
       std::stringstream ss;
       ss << "factor " << i << ": ";
-      if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter);
+      if (factors_[i] != nullptr) factors_[i]->print(ss.str(), formatter);
     }
   }
 
diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp
index c4cdbe0ef..fe99ea975 100644
--- a/gtsam/discrete/Potentials.cpp
+++ b/gtsam/discrete/Potentials.cpp
@@ -15,50 +15,52 @@
  * @author Frank Dellaert
  */
 
-#include <gtsam/discrete/Potentials.h>
 #include <gtsam/discrete/DecisionTree-inl.h>
+#include <gtsam/discrete/Potentials.h>
+
 #include <boost/format.hpp>
 
+#include <string>
+
 using namespace std;
 
 namespace gtsam {
 
-  // explicit instantiation
-  template class DecisionTree<Key, double> ;
-  template class AlgebraicDecisionTree<Key> ;
+// explicit instantiation
+template class DecisionTree<Key, double>;
+template class AlgebraicDecisionTree<Key>;
 
-  /* ************************************************************************* */
-  double Potentials::safe_div(const double& a, const double& b) {
-    // cout << boost::format("%g / %g = %g\n") % a % b % ((a == 0) ? 0 : (a / b));
-    // The use for safe_div is when we divide the product factor by the sum factor.
-    // If the product or sum is zero, we accord zero probability to the event.
-    return (a == 0 || b == 0) ? 0 : (a / b);
-  }
+/* ************************************************************************* */
+double Potentials::safe_div(const double& a, const double& b) {
+  // cout << boost::format("%g / %g = %g\n") % a % b % ((a == 0) ? 0 : (a / b));
+  // The use for safe_div is when we divide the product factor by the sum
+  // factor. If the product or sum is zero, we accord zero probability to the
+  // event.
+  return (a == 0 || b == 0) ? 0 : (a / b);
+}
 
-  /* ******************************************************************************** */
-  Potentials::Potentials() :
-      ADT(1.0) {
-  }
+/* ********************************************************************************
+ */
+Potentials::Potentials() : ADT(1.0) {}
 
-  /* ******************************************************************************** */
-  Potentials::Potentials(const DiscreteKeys& keys, const ADT& decisionTree) :
-      ADT(decisionTree), cardinalities_(keys.cardinalities()) {
-  }
+/* ********************************************************************************
+ */
+Potentials::Potentials(const DiscreteKeys& keys, const ADT& decisionTree)
+    : ADT(decisionTree), cardinalities_(keys.cardinalities()) {}
 
-  /* ************************************************************************* */
-  bool Potentials::equals(const Potentials& other, double tol) const {
-    return ADT::equals(other, tol);
-  }
+/* ************************************************************************* */
+bool Potentials::equals(const Potentials& other, double tol) const {
+  return ADT::equals(other, tol);
+}
 
-  /* ************************************************************************* */
-  void Potentials::print(const string& s,
-      const KeyFormatter& formatter) const {
-    cout << s << "\n  Cardinalities: ";
-    for(const DiscreteKey& key: cardinalities_)
-      cout << formatter(key.first) << "=" << formatter(key.second) << " ";
-    cout << endl;
-    ADT::print(" ");
-  }
+/* ************************************************************************* */
+void Potentials::print(const string& s, const KeyFormatter& formatter) const {
+  cout << s << "\n  Cardinalities: {";
+  for (const DiscreteKey& key : cardinalities_)
+    cout << formatter(key.first) << ":" << key.second << ", ";
+  cout << "}" << endl;
+  ADT::print(" ");
+}
 //
 //  /* ************************************************************************* */
 //  template<class P>
@@ -95,4 +97,4 @@ namespace gtsam {
 
   /* ************************************************************************* */
 
-} // namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam/discrete/Signature.cpp b/gtsam/discrete/Signature.cpp
index 89e763703..94b160a29 100644
--- a/gtsam/discrete/Signature.cpp
+++ b/gtsam/discrete/Signature.cpp
@@ -122,28 +122,30 @@ namespace gtsam {
     key_(key) {
   }
 
-  DiscreteKeys Signature::discreteKeysParentsFirst() const {
+  DiscreteKeys Signature::discreteKeys() const {
     DiscreteKeys keys;
-    for(const DiscreteKey& key: parents_)
-      keys.push_back(key);
     keys.push_back(key_);
+    for (const DiscreteKey& key : parents_) keys.push_back(key);
     return keys;
   }
 
   KeyVector Signature::indices() const {
     KeyVector js;
     js.push_back(key_.first);
-    for(const DiscreteKey& key: parents_)
-      js.push_back(key.first);
+    for (const DiscreteKey& key : parents_) js.push_back(key.first);
     return js;
   }
 
   vector<double> Signature::cpt() const {
     vector<double> cpt;
     if (table_) {
-      for(const Row& row: *table_)
-              for(const double& x: row)
-                      cpt.push_back(x);
+      const size_t nrStates = table_->at(0).size();
+      for (size_t j = 0; j < nrStates; j++) {
+        for (const Row& row : *table_) {
+          assert(row.size() == nrStates);
+          cpt.push_back(row[j]);
+        }
+      }
     }
     return cpt;
   }
diff --git a/gtsam/discrete/Signature.h b/gtsam/discrete/Signature.h
index 587cd6b30..6c59b5bff 100644
--- a/gtsam/discrete/Signature.h
+++ b/gtsam/discrete/Signature.h
@@ -86,8 +86,8 @@ namespace gtsam {
       return parents_;
     }
 
-    /** All keys, with variable key last */
-    DiscreteKeys discreteKeysParentsFirst() const;
+    /** All keys, with variable key first */
+    DiscreteKeys discreteKeys() const;
 
     /** All key indices, with variable key first */
     KeyVector indices() const;
diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
index 9c3f4bd63..0261ef833 100644
--- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
+++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
@@ -24,7 +24,6 @@
 #include <gtsam/discrete/DecisionTree-inl.h> // for convert only
 #define DISABLE_TIMING
 
-#include <boost/timer.hpp>
 #include <boost/tokenizer.hpp>
 #include <boost/assign/std/map.hpp>
 #include <boost/assign/std/vector.hpp>
@@ -32,6 +31,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/discrete/Signature.h>
+#include <gtsam/base/timing.h>
 
 using namespace std;
 using namespace gtsam;
@@ -75,16 +75,15 @@ void dot(const T&f, const string& filename) {
 // instrumented operators
 /* ******************************************************************************** */
 size_t muls = 0, adds = 0;
-boost::timer timer;
+double elapsed;
 void resetCounts() {
   muls = 0;
   adds = 0;
-  timer.restart();
 }
 void printCounts(const string& s) {
 #ifndef DISABLE_TIMING
   cout << boost::format("%s: %3d muls, %3d adds, %g ms.") % s % muls % adds
-  % (1000 * timer.elapsed()) << endl;
+  % (1000 * elapsed) << endl;
 #endif
   resetCounts();
 }
@@ -133,7 +132,7 @@ TEST(ADT, example3)
 
 /** Convert Signature into CPT */
 ADT create(const Signature& signature) {
-  ADT p(signature.discreteKeysParentsFirst(), signature.cpt());
+  ADT p(signature.discreteKeys(), signature.cpt());
   static size_t count = 0;
   const DiscreteKey& key = signature.key();
   string dotfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str();
@@ -148,6 +147,7 @@ TEST(ADT, joint)
   DiscreteKey A(0, 2), S(1, 2), T(2, 2), L(3, 2), B(4, 2), E(5, 2), X(6, 2), D(7, 2);
 
   resetCounts();
+  gttic_(asiaCPTs);
   ADT pA = create(A % "99/1");
   ADT pS = create(S % "50/50");
   ADT pT = create(T | A = "99/1 95/5");
@@ -156,10 +156,15 @@ TEST(ADT, joint)
   ADT pE = create((E | T, L) = "F T T T");
   ADT pX = create(X | E = "95/5 2/98");
   ADT pD = create((D | E, B) = "9/1 2/8 3/7 1/9");
+  gttoc_(asiaCPTs);
+  tictoc_getNode(asiaCPTsNode, asiaCPTs);
+  elapsed = asiaCPTsNode->secs() + asiaCPTsNode->wall();
+  tictoc_reset_();
   printCounts("Asia CPTs");
 
   // Create joint
   resetCounts();
+  gttic_(asiaJoint);
   ADT joint = pA;
   dot(joint, "Asia-A");
   joint = apply(joint, pS, &mul);
@@ -176,15 +181,20 @@ TEST(ADT, joint)
   dot(joint, "Asia-ASTLBEX");
   joint = apply(joint, pD, &mul);
   dot(joint, "Asia-ASTLBEXD");
-  EXPECT_LONGS_EQUAL(346, (long)muls);
+  EXPECT_LONGS_EQUAL(346, muls);
+  gttoc_(asiaJoint);
+  tictoc_getNode(asiaJointNode, asiaJoint);
+  elapsed = asiaJointNode->secs() + asiaJointNode->wall();
+  tictoc_reset_();
   printCounts("Asia joint");
 
+  // Form P(A,S,T,L) = P(A) P(S) P(T|A) P(L|S)
   ADT pASTL = pA;
   pASTL = apply(pASTL, pS, &mul);
   pASTL = apply(pASTL, pT, &mul);
   pASTL = apply(pASTL, pL, &mul);
 
-  // test combine
+  // test combine to check that P(A) = \sum_{S,T,L} P(A,S,T,L)
   ADT fAa = pASTL.combine(L, &add_).combine(T, &add_).combine(S, &add_);
   EXPECT(assert_equal(pA, fAa));
   ADT fAb = pASTL.combine(S, &add_).combine(T, &add_).combine(L, &add_);
@@ -199,6 +209,7 @@ TEST(ADT, inference)
       B(2,2), L(3,2), E(4,2), S(5,2), T(6,2), X(7,2);
 
   resetCounts();
+  gttic_(infCPTs);
   ADT pA = create(A % "99/1");
   ADT pS = create(S % "50/50");
   ADT pT = create(T | A = "99/1 95/5");
@@ -207,10 +218,15 @@ TEST(ADT, inference)
   ADT pE = create((E | T, L) = "F T T T");
   ADT pX = create(X | E = "95/5 2/98");
   ADT pD = create((D | E, B) = "9/1 2/8 3/7 1/9");
+  gttoc_(infCPTs);
+  tictoc_getNode(infCPTsNode, infCPTs);
+  elapsed = infCPTsNode->secs() + infCPTsNode->wall();
+  tictoc_reset_();
   //  printCounts("Inference CPTs");
 
   // Create joint
   resetCounts();
+  gttic_(asiaProd);
   ADT joint = pA;
   dot(joint, "Joint-Product-A");
   joint = apply(joint, pS, &mul);
@@ -228,8 +244,14 @@ TEST(ADT, inference)
   joint = apply(joint, pD, &mul);
   dot(joint, "Joint-Product-ASTLBEXD");
   EXPECT_LONGS_EQUAL(370, (long)muls); // different ordering
+  gttoc_(asiaProd);
+  tictoc_getNode(asiaProdNode, asiaProd);
+  elapsed = asiaProdNode->secs() + asiaProdNode->wall();
+  tictoc_reset_();
   printCounts("Asia product");
 
+  resetCounts();
+  gttic_(asiaSum);
   ADT marginal = joint;
   marginal = marginal.combine(X, &add_);
   dot(marginal, "Joint-Sum-ADBLEST");
@@ -240,6 +262,10 @@ TEST(ADT, inference)
   marginal = marginal.combine(E, &add_);
   dot(marginal, "Joint-Sum-ADBL");
   EXPECT_LONGS_EQUAL(161, (long)adds);
+  gttoc_(asiaSum);
+  tictoc_getNode(asiaSumNode, asiaSum);
+  elapsed = asiaSumNode->secs() + asiaSumNode->wall();
+  tictoc_reset_();
   printCounts("Asia sum");
 }
 
@@ -249,6 +275,7 @@ TEST(ADT, factor_graph)
   DiscreteKey B(0,2), L(1,2), E(2,2), S(3,2), T(4,2), X(5,2);
 
   resetCounts();
+  gttic_(createCPTs);
   ADT pS = create(S % "50/50");
   ADT pT = create(T % "95/5");
   ADT pL = create(L | S = "99/1 90/10");
@@ -256,10 +283,15 @@ TEST(ADT, factor_graph)
   ADT pX = create(X | E = "95/5 2/98");
   ADT pD = create(B | E = "1/8 7/9");
   ADT pB = create(B | S = "70/30 40/60");
+  gttoc_(createCPTs);
+  tictoc_getNode(createCPTsNode, createCPTs);
+  elapsed = createCPTsNode->secs() + createCPTsNode->wall();
+  tictoc_reset_();
   //  printCounts("Create CPTs");
 
   // Create joint
   resetCounts();
+  gttic_(asiaFG);
   ADT fg = pS;
   fg = apply(fg, pT, &mul);
   fg = apply(fg, pL, &mul);
@@ -269,8 +301,14 @@ TEST(ADT, factor_graph)
   fg = apply(fg, pD, &mul);
   dot(fg, "FactorGraph");
   EXPECT_LONGS_EQUAL(158, (long)muls);
+  gttoc_(asiaFG);
+  tictoc_getNode(asiaFGNode, asiaFG);
+  elapsed = asiaFGNode->secs() + asiaFGNode->wall();
+  tictoc_reset_();
   printCounts("Asia FG");
 
+  resetCounts();
+  gttic_(marg);
   fg = fg.combine(X, &add_);
   dot(fg, "Marginalized-6X");
   fg = fg.combine(T, &add_);
@@ -282,49 +320,83 @@ TEST(ADT, factor_graph)
   fg = fg.combine(L, &add_);
   dot(fg, "Marginalized-2L");
   EXPECT(adds = 54);
+  gttoc_(marg);
+  tictoc_getNode(margNode, marg);
+  elapsed = margNode->secs() + margNode->wall();
+  tictoc_reset_();
   printCounts("marginalize");
 
   // BLESTX
 
   // Eliminate X
+  resetCounts();
+  gttic_(elimX);
   ADT fE = pX;
   dot(fE, "Eliminate-01-fEX");
   fE = fE.combine(X, &add_);
   dot(fE, "Eliminate-02-fE");
+  gttoc_(elimX);
+  tictoc_getNode(elimXNode, elimX);
+  elapsed = elimXNode->secs() + elimXNode->wall();
+  tictoc_reset_();
   printCounts("Eliminate X");
 
   // Eliminate T
+  resetCounts();
+  gttic_(elimT);
   ADT fLE = pT;
   fLE = apply(fLE, pE, &mul);
   dot(fLE, "Eliminate-03-fLET");
   fLE = fLE.combine(T, &add_);
   dot(fLE, "Eliminate-04-fLE");
+  gttoc_(elimT);
+  tictoc_getNode(elimTNode, elimT);
+  elapsed = elimTNode->secs() + elimTNode->wall();
+  tictoc_reset_();
   printCounts("Eliminate T");
 
   // Eliminate S
+  resetCounts();
+  gttic_(elimS);
   ADT fBL = pS;
   fBL = apply(fBL, pL, &mul);
   fBL = apply(fBL, pB, &mul);
   dot(fBL, "Eliminate-05-fBLS");
   fBL = fBL.combine(S, &add_);
   dot(fBL, "Eliminate-06-fBL");
+  gttoc_(elimS);
+  tictoc_getNode(elimSNode, elimS);
+  elapsed = elimSNode->secs() + elimSNode->wall();
+  tictoc_reset_();
   printCounts("Eliminate S");
 
   // Eliminate E
+  resetCounts();
+  gttic_(elimE);
   ADT fBL2 = fE;
   fBL2 = apply(fBL2, fLE, &mul);
   fBL2 = apply(fBL2, pD, &mul);
   dot(fBL2, "Eliminate-07-fBLE");
   fBL2 = fBL2.combine(E, &add_);
   dot(fBL2, "Eliminate-08-fBL2");
+  gttoc_(elimE);
+  tictoc_getNode(elimENode, elimE);
+  elapsed = elimENode->secs() + elimENode->wall();
+  tictoc_reset_();
   printCounts("Eliminate E");
 
   // Eliminate L
+  resetCounts();
+  gttic_(elimL);
   ADT fB = fBL;
   fB = apply(fB, fBL2, &mul);
   dot(fB, "Eliminate-09-fBL");
   fB = fB.combine(L, &add_);
   dot(fB, "Eliminate-10-fB");
+  gttoc_(elimL);
+  tictoc_getNode(elimLNode, elimL);
+  elapsed = elimLNode->secs() + elimLNode->wall();
+  tictoc_reset_();
   printCounts("Eliminate L");
 }
 
diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp
index 05223c67a..71cb4abe3 100644
--- a/gtsam/discrete/tests/testDecisionTree.cpp
+++ b/gtsam/discrete/tests/testDecisionTree.cpp
@@ -17,7 +17,6 @@
  * @date    Jan 30, 2012
  */
 
-#include <boost/timer.hpp>
 #include <boost/assign/std/vector.hpp>
 using namespace boost::assign;
 
diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp
index f8a1b6309..2b440e5a0 100644
--- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp
+++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp
@@ -18,110 +18,135 @@
 
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/base/Testable.h>
+#include <gtsam/discrete/DiscreteMarginals.h>
 #include <gtsam/base/debug.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/Vector.h>
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/assign/std/map.hpp>
+
 #include <boost/assign/list_inserter.hpp>
+#include <boost/assign/std/map.hpp>
 
 using namespace boost::assign;
 
 #include <iostream>
+#include <string>
+#include <vector>
 
 using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-TEST(DiscreteBayesNet, Asia)
-{
+TEST(DiscreteBayesNet, bayesNet) {
+  DiscreteBayesNet bayesNet;
+  DiscreteKey Parent(0, 2), Child(1, 2);
+
+  auto prior = boost::make_shared<DiscreteConditional>(Parent % "6/4");
+  CHECK(assert_equal(Potentials::ADT({Parent}, "0.6 0.4"),
+                     (Potentials::ADT)*prior));
+  bayesNet.push_back(prior);
+
+  auto conditional =
+      boost::make_shared<DiscreteConditional>(Child | Parent = "7/3 8/2");
+  EXPECT_LONGS_EQUAL(1, *(conditional->beginFrontals()));
+  Potentials::ADT expected(Child & Parent, "0.7 0.8 0.3 0.2");
+  CHECK(assert_equal(expected, (Potentials::ADT)*conditional));
+  bayesNet.push_back(conditional);
+
+  DiscreteFactorGraph fg(bayesNet);
+  LONGS_EQUAL(2, fg.back()->size());
+
+  // Check the marginals
+  const double expectedMarginal[2]{0.4, 0.6 * 0.3 + 0.4 * 0.2};
+  DiscreteMarginals marginals(fg);
+  for (size_t j = 0; j < 2; j++) {
+    Vector FT = marginals.marginalProbabilities(DiscreteKey(j, 2));
+    EXPECT_DOUBLES_EQUAL(expectedMarginal[j], FT[1], 1e-3);
+    EXPECT_DOUBLES_EQUAL(FT[0], 1.0 - FT[1], 1e-9);
+  }
+}
+
+/* ************************************************************************* */
+TEST(DiscreteBayesNet, Asia) {
   DiscreteBayesNet asia;
-//  DiscreteKey A("Asia"), S("Smoking"), T("Tuberculosis"), L("LungCancer"), B(
-//      "Bronchitis"), E("Either"), X("XRay"), D("Dyspnoea");
-  DiscreteKey A(0,2), S(4,2), T(3,2), L(6,2), B(7,2), E(5,2), X(2,2), D(1,2);
+  DiscreteKey Asia(0, 2), Smoking(4, 2), Tuberculosis(3, 2), LungCancer(6, 2),
+      Bronchitis(7, 2), Either(5, 2), XRay(2, 2), Dyspnea(1, 2);
 
-  // TODO: make a version that doesn't use the parser
-  asia.add(A % "99/1");
-  asia.add(S % "50/50");
+  asia.add(Asia % "99/1");
+  asia.add(Smoking % "50/50");
 
-  asia.add(T | A = "99/1 95/5");
-  asia.add(L | S = "99/1 90/10");
-  asia.add(B | S = "70/30 40/60");
+  asia.add(Tuberculosis | Asia = "99/1 95/5");
+  asia.add(LungCancer | Smoking = "99/1 90/10");
+  asia.add(Bronchitis | Smoking = "70/30 40/60");
 
-  asia.add((E | T, L) = "F T T T");
+  asia.add((Either | Tuberculosis, LungCancer) = "F T T T");
 
-  asia.add(X | E = "95/5 2/98");
-  // next lines are same as asia.add((D | E, B) = "9/1 2/8 3/7 1/9");
-  DiscreteConditional::shared_ptr actual =
-      boost::make_shared<DiscreteConditional>((D | E, B) = "9/1 2/8 3/7 1/9");
-  asia.push_back(actual);
-  //  GTSAM_PRINT(asia);
+  asia.add(XRay | Either = "95/5 2/98");
+  asia.add((Dyspnea | Either, Bronchitis) = "9/1 2/8 3/7 1/9");
 
   // Convert to factor graph
   DiscreteFactorGraph fg(asia);
-//    GTSAM_PRINT(fg);
-  LONGS_EQUAL(3,fg.back()->size());
-  Potentials::ADT expected(B & D & E, "0.9 0.3 0.1 0.7 0.2 0.1 0.8 0.9");
-  CHECK(assert_equal(expected,(Potentials::ADT)*actual));
+  LONGS_EQUAL(3, fg.back()->size());
+
+  // Check the marginals we know (of the parent-less nodes)
+  DiscreteMarginals marginals(fg);
+  Vector2 va(0.99, 0.01), vs(0.5, 0.5);
+  EXPECT(assert_equal(va, marginals.marginalProbabilities(Asia)));
+  EXPECT(assert_equal(vs, marginals.marginalProbabilities(Smoking)));
 
   // Create solver and eliminate
   Ordering ordering;
-  ordering += Key(0),Key(1),Key(2),Key(3),Key(4),Key(5),Key(6),Key(7);
+  ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7);
   DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering);
-//    GTSAM_PRINT(*chordal);
-  DiscreteConditional expected2(B % "11/9");
-  CHECK(assert_equal(expected2,*chordal->back()));
+  DiscreteConditional expected2(Bronchitis % "11/9");
+  EXPECT(assert_equal(expected2, *chordal->back()));
 
   // solve
   DiscreteFactor::sharedValues actualMPE = chordal->optimize();
   DiscreteFactor::Values expectedMPE;
-  insert(expectedMPE)(A.first, 0)(D.first, 0)(X.first, 0)(T.first, 0)(S.first,
-      0)(E.first, 0)(L.first, 0)(B.first, 0);
+  insert(expectedMPE)(Asia.first, 0)(Dyspnea.first, 0)(XRay.first, 0)(
+      Tuberculosis.first, 0)(Smoking.first, 0)(Either.first, 0)(
+      LungCancer.first, 0)(Bronchitis.first, 0);
   EXPECT(assert_equal(expectedMPE, *actualMPE));
 
-  // add evidence, we were in Asia and we have Dispnoea
-  fg.add(A, "0 1");
-  fg.add(D, "0 1");
-//  fg.product().dot("fg");
+  // add evidence, we were in Asia and we have dyspnea
+  fg.add(Asia, "0 1");
+  fg.add(Dyspnea, "0 1");
 
   // solve again, now with evidence
   DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering);
-//  GTSAM_PRINT(*chordal2);
   DiscreteFactor::sharedValues actualMPE2 = chordal2->optimize();
   DiscreteFactor::Values expectedMPE2;
-  insert(expectedMPE2)(A.first, 1)(D.first, 1)(X.first, 0)(T.first, 0)(S.first,
-      1)(E.first, 0)(L.first, 0)(B.first, 1);
+  insert(expectedMPE2)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 0)(
+      Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 0)(
+      LungCancer.first, 0)(Bronchitis.first, 1);
   EXPECT(assert_equal(expectedMPE2, *actualMPE2));
 
   // now sample from it
   DiscreteFactor::Values expectedSample;
   SETDEBUG("DiscreteConditional::sample", false);
-  insert(expectedSample)(A.first, 1)(D.first, 1)(X.first, 0)(T.first, 0)(
-      S.first, 1)(E.first, 0)(L.first, 0)(B.first, 1);
+  insert(expectedSample)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 1)(
+      Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 1)(
+      LungCancer.first, 1)(Bronchitis.first, 0);
   DiscreteFactor::sharedValues actualSample = chordal2->sample();
   EXPECT(assert_equal(expectedSample, *actualSample));
 }
 
 /* ************************************************************************* */
-TEST_UNSAFE(DiscreteBayesNet, Sugar)
-{
-  DiscreteKey T(0,2), L(1,2), E(2,2), D(3,2), C(8,3), S(7,2);
+TEST_UNSAFE(DiscreteBayesNet, Sugar) {
+  DiscreteKey T(0, 2), L(1, 2), E(2, 2), C(8, 3), S(7, 2);
 
   DiscreteBayesNet bn;
 
-  // test some mistakes
-  //  add(bn, D);
-  //  add(bn, D | E);
-  //  add(bn, D | E = "blah");
-
   // try logic
   bn.add((E | T, L) = "OR");
   bn.add((E | T, L) = "AND");
 
-  //  // try multivalued
- bn.add(C % "1/1/2");
- bn.add(C | S = "1/1/2 5/2/3");
+  // try multivalued
+  bn.add(C % "1/1/2");
+  bn.add(C | S = "1/1/2 5/2/3");
 }
 
 /* ************************************************************************* */
@@ -130,4 +155,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-
diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp
index 93126f642..ecf485036 100644
--- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp
+++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp
@@ -1,261 +1,228 @@
-///* ----------------------------------------------------------------------------
-//
-// * 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 testDiscreteBayesTree.cpp
-// * @date sept 15, 2012
-// * @author Frank Dellaert
-// */
-//
-//#include <gtsam/discrete/DiscreteBayesNet.h>
-//#include <gtsam/discrete/DiscreteBayesTree.h>
-//#include <gtsam/discrete/DiscreteFactorGraph.h>
-//
-//#include <boost/assign/std/vector.hpp>
-//using namespace boost::assign;
-//
+/* ----------------------------------------------------------------------------
+
+* GTSAM Copyright 2010-2020, 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 testDiscreteBayesTree.cpp
+ * @date sept 15, 2012
+ * @author Frank Dellaert
+ */
+
+#include <gtsam/base/Vector.h>
+#include <gtsam/discrete/DiscreteBayesNet.h>
+#include <gtsam/discrete/DiscreteBayesTree.h>
+#include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/inference/BayesNet.h>
+
+#include <boost/assign/std/vector.hpp>
+using namespace boost::assign;
+
 #include <CppUnitLite/TestHarness.h>
-//
-//using namespace std;
-//using namespace gtsam;
-//
-//static bool debug = false;
-//
-///**
-// * Custom clique class to debug shortcuts
-// */
-////class Clique: public BayesTreeCliqueBaseOrdered<Clique, DiscreteConditional> {
-////
-////protected:
-////
-////public:
-////
-////  typedef BayesTreeCliqueBaseOrdered<Clique, DiscreteConditional> Base;
-////  typedef boost::shared_ptr<Clique> shared_ptr;
-////
-////  // Constructors
-////  Clique() {
-////  }
-////  Clique(const DiscreteConditional::shared_ptr& conditional) :
-////      Base(conditional) {
-////  }
-////  Clique(
-////      const std::pair<DiscreteConditional::shared_ptr,
-////          DiscreteConditional::FactorType::shared_ptr>& result) :
-////      Base(result) {
-////  }
-////
-////  /// print index signature only
-////  void printSignature(const std::string& s = "Clique: ",
-////      const KeyFormatter& indexFormatter = DefaultKeyFormatter) const {
-////    ((IndexConditionalOrdered::shared_ptr) conditional_)->print(s, indexFormatter);
-////  }
-////
-////  /// evaluate value of sub-tree
-////  double evaluate(const DiscreteConditional::Values & values) {
-////    double result = (*(this->conditional_))(values);
-////    // evaluate all children and multiply into result
-////    for(boost::shared_ptr<Clique> c: children_)
-////      result *= c->evaluate(values);
-////    return result;
-////  }
-////
-////};
-//
-////typedef BayesTreeOrdered<DiscreteConditional, Clique> DiscreteBayesTree;
-////
-/////* ************************************************************************* */
-////double evaluate(const DiscreteBayesTree& tree,
-////    const DiscreteConditional::Values & values) {
-////  return tree.root()->evaluate(values);
-////}
-//
-///* ************************************************************************* */
-//
-//TEST_UNSAFE( DiscreteBayesTree, thinTree ) {
-//
-//  const int nrNodes = 15;
-//  const size_t nrStates = 2;
-//
-//  // define variables
-//  vector<DiscreteKey> key;
-//  for (int i = 0; i < nrNodes; i++) {
-//    DiscreteKey key_i(i, nrStates);
-//    key.push_back(key_i);
-//  }
-//
-//  // create a thin-tree Bayesnet, a la Jean-Guillaume
-//  DiscreteBayesNet bayesNet;
-//  bayesNet.add(key[14] % "1/3");
-//
-//  bayesNet.add(key[13] | key[14] = "1/3 3/1");
-//  bayesNet.add(key[12] | key[14] = "3/1 3/1");
-//
-//  bayesNet.add((key[11] | key[13], key[14]) = "1/4 2/3 3/2 4/1");
-//  bayesNet.add((key[10] | key[13], key[14]) = "1/4 3/2 2/3 4/1");
-//  bayesNet.add((key[9] | key[12], key[14]) = "4/1 2/3 F 1/4");
-//  bayesNet.add((key[8] | key[12], key[14]) = "T 1/4 3/2 4/1");
-//
-//  bayesNet.add((key[7] | key[11], key[13]) = "1/4 2/3 3/2 4/1");
-//  bayesNet.add((key[6] | key[11], key[13]) = "1/4 3/2 2/3 4/1");
-//  bayesNet.add((key[5] | key[10], key[13]) = "4/1 2/3 3/2 1/4");
-//  bayesNet.add((key[4] | key[10], key[13]) = "2/3 1/4 3/2 4/1");
-//
-//  bayesNet.add((key[3] | key[9], key[12]) = "1/4 2/3 3/2 4/1");
-//  bayesNet.add((key[2] | key[9], key[12]) = "1/4 8/2 2/3 4/1");
-//  bayesNet.add((key[1] | key[8], key[12]) = "4/1 2/3 3/2 1/4");
-//  bayesNet.add((key[0] | key[8], key[12]) = "2/3 1/4 3/2 4/1");
-//
-////  if (debug) {
-////    GTSAM_PRINT(bayesNet);
-////    bayesNet.saveGraph("/tmp/discreteBayesNet.dot");
-////  }
-//
-//  // create a BayesTree out of a Bayes net
-//  DiscreteBayesTree bayesTree(bayesNet);
-//  if (debug) {
-//    GTSAM_PRINT(bayesTree);
-//    bayesTree.saveGraph("/tmp/discreteBayesTree.dot");
-//  }
-//
-//  // Check whether BN and BT give the same answer on all configurations
-//  // Also calculate all some marginals
-//  Vector marginals = zero(15);
-//  double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint_8_12 = 0,
-//      joint82 = 0, joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0,
-//      joint_4_11 = 0;
-//  vector<DiscreteFactor::Values> allPosbValues = cartesianProduct(
-//      key[0] & key[1] & key[2] & key[3] & key[4] & key[5] & key[6] & key[7]
-//          & key[8] & key[9] & key[10] & key[11] & key[12] & key[13] & key[14]);
-//  for (size_t i = 0; i < allPosbValues.size(); ++i) {
-//    DiscreteFactor::Values x = allPosbValues[i];
-//    double expected = evaluate(bayesNet, x);
-//    double actual = evaluate(bayesTree, x);
-//    DOUBLES_EQUAL(expected, actual, 1e-9);
-//    // collect marginals
-//    for (size_t i = 0; i < 15; i++)
-//      if (x[i])
-//        marginals[i] += actual;
-//    // calculate shortcut 8 and 0
-//    if (x[12] && x[14])
-//      joint_12_14 += actual;
-//    if (x[9] && x[12] & x[14])
-//      joint_9_12_14 += actual;
-//    if (x[8] && x[12] & x[14])
-//      joint_8_12_14 += actual;
-//    if (x[8] && x[12])
-//      joint_8_12 += actual;
-//    if (x[8] && x[2])
-//      joint82 += actual;
-//    if (x[1] && x[2])
-//      joint12 += actual;
-//    if (x[2] && x[4])
-//      joint24 += actual;
-//    if (x[4] && x[5])
-//      joint45 += actual;
-//    if (x[4] && x[6])
-//      joint46 += actual;
-//    if (x[4] && x[11])
-//      joint_4_11 += actual;
-//  }
-//  DiscreteFactor::Values all1 = allPosbValues.back();
-//
-//  Clique::shared_ptr R = bayesTree.root();
-//
-//  // check separator marginal P(S0)
-//  Clique::shared_ptr c = bayesTree[0];
-//  DiscreteFactorGraph separatorMarginal0 = c->separatorMarginal(R,
-//      EliminateDiscrete);
-//  EXPECT_DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9);
-//
-//  // check separator marginal P(S9), should be P(14)
-//  c = bayesTree[9];
-//  DiscreteFactorGraph separatorMarginal9 = c->separatorMarginal(R,
-//      EliminateDiscrete);
-//  EXPECT_DOUBLES_EQUAL(marginals[14], separatorMarginal9(all1), 1e-9);
-//
-//  // check separator marginal of root, should be empty
-//  c = bayesTree[11];
-//  DiscreteFactorGraph separatorMarginal11 = c->separatorMarginal(R,
-//      EliminateDiscrete);
-//  EXPECT_LONGS_EQUAL(0, separatorMarginal11.size());
-//
-//  // check shortcut P(S9||R) to root
-//  c = bayesTree[9];
-//  DiscreteBayesNet shortcut = c->shortcut(R, EliminateDiscrete);
-//  EXPECT_LONGS_EQUAL(0, shortcut.size());
-//
-//  // check shortcut P(S8||R) to root
-//  c = bayesTree[8];
-//  shortcut = c->shortcut(R, EliminateDiscrete);
-//  EXPECT_DOUBLES_EQUAL(joint_12_14/marginals[14], evaluate(shortcut,all1),
-//      1e-9);
-//
-//  // check shortcut P(S2||R) to root
-//  c = bayesTree[2];
-//  shortcut = c->shortcut(R, EliminateDiscrete);
-//  EXPECT_DOUBLES_EQUAL(joint_9_12_14/marginals[14], evaluate(shortcut,all1),
-//      1e-9);
-//
-//  // check shortcut P(S0||R) to root
-//  c = bayesTree[0];
-//  shortcut = c->shortcut(R, EliminateDiscrete);
-//  EXPECT_DOUBLES_EQUAL(joint_8_12_14/marginals[14], evaluate(shortcut,all1),
-//      1e-9);
-//
-//  // calculate all shortcuts to root
-//  DiscreteBayesTree::Nodes cliques = bayesTree.nodes();
-//  for(Clique::shared_ptr c: cliques) {
-//    DiscreteBayesNet shortcut = c->shortcut(R, EliminateDiscrete);
-//    if (debug) {
-//      c->printSignature();
-//      shortcut.print("shortcut:");
-//    }
-//  }
-//
-//  // Check all marginals
-//  DiscreteFactor::shared_ptr marginalFactor;
-//  for (size_t i = 0; i < 15; i++) {
-//    marginalFactor = bayesTree.marginalFactor(i, EliminateDiscrete);
-//    double actual = (*marginalFactor)(all1);
-//    EXPECT_DOUBLES_EQUAL(marginals[i], actual, 1e-9);
-//  }
-//
-//  DiscreteBayesNet::shared_ptr actualJoint;
-//
-//  // Check joint P(8,2) TODO: not disjoint !
-////  actualJoint = bayesTree.jointBayesNet(8, 2, EliminateDiscrete);
-////  EXPECT_DOUBLES_EQUAL(joint82, evaluate(*actualJoint,all1), 1e-9);
-//
-//  // Check joint P(1,2) TODO: not disjoint !
-////  actualJoint = bayesTree.jointBayesNet(1, 2, EliminateDiscrete);
-////  EXPECT_DOUBLES_EQUAL(joint12, evaluate(*actualJoint,all1), 1e-9);
-//
-//  // Check joint P(2,4)
-//  actualJoint = bayesTree.jointBayesNet(2, 4, EliminateDiscrete);
-//  EXPECT_DOUBLES_EQUAL(joint24, evaluate(*actualJoint,all1), 1e-9);
-//
-//  // Check joint P(4,5) TODO: not disjoint !
-////  actualJoint = bayesTree.jointBayesNet(4, 5, EliminateDiscrete);
-////  EXPECT_DOUBLES_EQUAL(joint46, evaluate(*actualJoint,all1), 1e-9);
-//
-//  // Check joint P(4,6) TODO: not disjoint !
-////  actualJoint = bayesTree.jointBayesNet(4, 6, EliminateDiscrete);
-////  EXPECT_DOUBLES_EQUAL(joint46, evaluate(*actualJoint,all1), 1e-9);
-//
-//  // Check joint P(4,11)
-//  actualJoint = bayesTree.jointBayesNet(4, 11, EliminateDiscrete);
-//  EXPECT_DOUBLES_EQUAL(joint_4_11, evaluate(*actualJoint,all1), 1e-9);
-//
-//}
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+static bool debug = false;
+
+/* ************************************************************************* */
+
+TEST_UNSAFE(DiscreteBayesTree, ThinTree) {
+  const int nrNodes = 15;
+  const size_t nrStates = 2;
+
+  // define variables
+  vector<DiscreteKey> key;
+  for (int i = 0; i < nrNodes; i++) {
+    DiscreteKey key_i(i, nrStates);
+    key.push_back(key_i);
+  }
+
+  // create a thin-tree Bayesnet, a la Jean-Guillaume
+  DiscreteBayesNet bayesNet;
+  bayesNet.add(key[14] % "1/3");
+
+  bayesNet.add(key[13] | key[14] = "1/3 3/1");
+  bayesNet.add(key[12] | key[14] = "3/1 3/1");
+
+  bayesNet.add((key[11] | key[13], key[14]) = "1/4 2/3 3/2 4/1");
+  bayesNet.add((key[10] | key[13], key[14]) = "1/4 3/2 2/3 4/1");
+  bayesNet.add((key[9] | key[12], key[14]) = "4/1 2/3 F 1/4");
+  bayesNet.add((key[8] | key[12], key[14]) = "T 1/4 3/2 4/1");
+
+  bayesNet.add((key[7] | key[11], key[13]) = "1/4 2/3 3/2 4/1");
+  bayesNet.add((key[6] | key[11], key[13]) = "1/4 3/2 2/3 4/1");
+  bayesNet.add((key[5] | key[10], key[13]) = "4/1 2/3 3/2 1/4");
+  bayesNet.add((key[4] | key[10], key[13]) = "2/3 1/4 3/2 4/1");
+
+  bayesNet.add((key[3] | key[9], key[12]) = "1/4 2/3 3/2 4/1");
+  bayesNet.add((key[2] | key[9], key[12]) = "1/4 8/2 2/3 4/1");
+  bayesNet.add((key[1] | key[8], key[12]) = "4/1 2/3 3/2 1/4");
+  bayesNet.add((key[0] | key[8], key[12]) = "2/3 1/4 3/2 4/1");
+
+  if (debug) {
+    GTSAM_PRINT(bayesNet);
+    bayesNet.saveGraph("/tmp/discreteBayesNet.dot");
+  }
+
+  // create a BayesTree out of a Bayes net
+  auto bayesTree = DiscreteFactorGraph(bayesNet).eliminateMultifrontal();
+  if (debug) {
+    GTSAM_PRINT(*bayesTree);
+    bayesTree->saveGraph("/tmp/discreteBayesTree.dot");
+  }
+
+  // Check frontals and parents
+  for (size_t i : {13, 14, 9, 3, 2, 8, 1, 0, 10, 5, 4}) {
+    auto clique_i = (*bayesTree)[i];
+    EXPECT_LONGS_EQUAL(i, *(clique_i->conditional_->beginFrontals()));
+  }
+
+  auto R = bayesTree->roots().front();
+
+  // Check whether BN and BT give the same answer on all configurations
+  vector<DiscreteFactor::Values> allPosbValues = cartesianProduct(
+      key[0] & key[1] & key[2] & key[3] & key[4] & key[5] & key[6] & key[7] &
+      key[8] & key[9] & key[10] & key[11] & key[12] & key[13] & key[14]);
+  for (size_t i = 0; i < allPosbValues.size(); ++i) {
+    DiscreteFactor::Values x = allPosbValues[i];
+    double expected = bayesNet.evaluate(x);
+    double actual = bayesTree->evaluate(x);
+    DOUBLES_EQUAL(expected, actual, 1e-9);
+  }
+
+  // Calculate all some marginals for Values==all1
+  Vector marginals = Vector::Zero(15);
+  double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint_8_12 = 0,
+         joint82 = 0, joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0,
+         joint_4_11 = 0, joint_11_13 = 0, joint_11_13_14 = 0,
+         joint_11_12_13_14 = 0, joint_9_11_12_13 = 0, joint_8_11_12_13 = 0;
+  for (size_t i = 0; i < allPosbValues.size(); ++i) {
+    DiscreteFactor::Values x = allPosbValues[i];
+    double px = bayesTree->evaluate(x);
+    for (size_t i = 0; i < 15; i++)
+      if (x[i]) marginals[i] += px;
+    if (x[12] && x[14]) {
+      joint_12_14 += px;
+      if (x[9]) joint_9_12_14 += px;
+      if (x[8]) joint_8_12_14 += px;
+    }
+    if (x[8] && x[12]) joint_8_12 += px;
+    if (x[2]) {
+      if (x[8]) joint82 += px;
+      if (x[1]) joint12 += px;
+    }
+    if (x[4]) {
+      if (x[2]) joint24 += px;
+      if (x[5]) joint45 += px;
+      if (x[6]) joint46 += px;
+      if (x[11]) joint_4_11 += px;
+    }
+    if (x[11] && x[13]) {
+      joint_11_13 += px;
+      if (x[8] && x[12]) joint_8_11_12_13 += px;
+      if (x[9] && x[12]) joint_9_11_12_13 += px;
+      if (x[14]) {
+        joint_11_13_14 += px;
+        if (x[12]) {
+          joint_11_12_13_14 += px;
+        }
+      }
+    }
+  }
+  DiscreteFactor::Values all1 = allPosbValues.back();
+
+  // check separator marginal P(S0)
+  auto clique = (*bayesTree)[0];
+  DiscreteFactorGraph separatorMarginal0 =
+      clique->separatorMarginal(EliminateDiscrete);
+  DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9);
+
+  // check separator marginal P(S9), should be P(14)
+  clique = (*bayesTree)[9];
+  DiscreteFactorGraph separatorMarginal9 =
+      clique->separatorMarginal(EliminateDiscrete);
+  DOUBLES_EQUAL(marginals[14], separatorMarginal9(all1), 1e-9);
+
+  // check separator marginal of root, should be empty
+  clique = (*bayesTree)[11];
+  DiscreteFactorGraph separatorMarginal11 =
+      clique->separatorMarginal(EliminateDiscrete);
+  LONGS_EQUAL(0, separatorMarginal11.size());
+
+  // check shortcut P(S9||R) to root
+  clique = (*bayesTree)[9];
+  DiscreteBayesNet shortcut = clique->shortcut(R, EliminateDiscrete);
+  LONGS_EQUAL(1, shortcut.size());
+  DOUBLES_EQUAL(joint_11_13_14 / joint_11_13, shortcut.evaluate(all1), 1e-9);
+
+  // check shortcut P(S8||R) to root
+  clique = (*bayesTree)[8];
+  shortcut = clique->shortcut(R, EliminateDiscrete);
+  DOUBLES_EQUAL(joint_11_12_13_14 / joint_11_13, shortcut.evaluate(all1), 1e-9);
+
+  // check shortcut P(S2||R) to root
+  clique = (*bayesTree)[2];
+  shortcut = clique->shortcut(R, EliminateDiscrete);
+  DOUBLES_EQUAL(joint_9_11_12_13 / joint_11_13, shortcut.evaluate(all1), 1e-9);
+
+  // check shortcut P(S0||R) to root
+  clique = (*bayesTree)[0];
+  shortcut = clique->shortcut(R, EliminateDiscrete);
+  DOUBLES_EQUAL(joint_8_11_12_13 / joint_11_13, shortcut.evaluate(all1), 1e-9);
+
+  // calculate all shortcuts to root
+  DiscreteBayesTree::Nodes cliques = bayesTree->nodes();
+  for (auto clique : cliques) {
+    DiscreteBayesNet shortcut = clique.second->shortcut(R, EliminateDiscrete);
+    if (debug) {
+      clique.second->conditional_->printSignature();
+      shortcut.print("shortcut:");
+    }
+  }
+
+  // Check all marginals
+  DiscreteFactor::shared_ptr marginalFactor;
+  for (size_t i = 0; i < 15; i++) {
+    marginalFactor = bayesTree->marginalFactor(i, EliminateDiscrete);
+    double actual = (*marginalFactor)(all1);
+    DOUBLES_EQUAL(marginals[i], actual, 1e-9);
+  }
+
+  DiscreteBayesNet::shared_ptr actualJoint;
+
+  // Check joint P(8, 2)
+  actualJoint = bayesTree->jointBayesNet(8, 2, EliminateDiscrete);
+  DOUBLES_EQUAL(joint82, actualJoint->evaluate(all1), 1e-9);
+
+  // Check joint P(1, 2)
+  actualJoint = bayesTree->jointBayesNet(1, 2, EliminateDiscrete);
+  DOUBLES_EQUAL(joint12, actualJoint->evaluate(all1), 1e-9);
+
+  // Check joint P(2, 4)
+  actualJoint = bayesTree->jointBayesNet(2, 4, EliminateDiscrete);
+  DOUBLES_EQUAL(joint24, actualJoint->evaluate(all1), 1e-9);
+
+  // Check joint P(4, 5)
+  actualJoint = bayesTree->jointBayesNet(4, 5, EliminateDiscrete);
+  DOUBLES_EQUAL(joint45, actualJoint->evaluate(all1), 1e-9);
+
+  // Check joint P(4, 6)
+  actualJoint = bayesTree->jointBayesNet(4, 6, EliminateDiscrete);
+  DOUBLES_EQUAL(joint46, actualJoint->evaluate(all1), 1e-9);
+
+  // Check joint P(4, 11)
+  actualJoint = bayesTree->jointBayesNet(4, 11, EliminateDiscrete);
+  DOUBLES_EQUAL(joint_4_11, actualJoint->evaluate(all1), 1e-9);
+}
 
 /* ************************************************************************* */
 int main() {
@@ -263,4 +230,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-
diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.pdf b/gtsam/discrete/tests/testDiscreteBayesTree.pdf
new file mode 100644
index 000000000..e8167d455
Binary files /dev/null and b/gtsam/discrete/tests/testDiscreteBayesTree.pdf differ
diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp
index 888bf76df..3ac3ffc9e 100644
--- a/gtsam/discrete/tests/testDiscreteConditional.cpp
+++ b/gtsam/discrete/tests/testDiscreteConditional.cpp
@@ -16,9 +16,9 @@
  * @date Feb 14, 2011
  */
 
-#include <boost/make_shared.hpp>
 #include <boost/assign/std/map.hpp>
 #include <boost/assign/std/vector.hpp>
+#include <boost/make_shared.hpp>
 using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
@@ -36,6 +36,11 @@ TEST( DiscreteConditional, constructors)
   DiscreteConditional::shared_ptr expected1 = //
       boost::make_shared<DiscreteConditional>(X | Y = "1/1 2/3 1/4");
   EXPECT(expected1);
+  EXPECT_LONGS_EQUAL(0, *(expected1->beginFrontals()));
+  EXPECT_LONGS_EQUAL(2, *(expected1->beginParents()));
+  EXPECT(expected1->endParents() == expected1->end());
+  EXPECT(expected1->endFrontals() == expected1->beginParents());
+  
   DecisionTreeFactor f1(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8");
   DiscreteConditional actual1(1, f1);
   EXPECT(assert_equal(*expected1, actual1, 1e-9));
@@ -43,71 +48,68 @@ TEST( DiscreteConditional, constructors)
   DecisionTreeFactor f2(X & Y & Z,
       "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
   DiscreteConditional actual2(1, f2);
-  DecisionTreeFactor::shared_ptr actual2factor = actual2.toFactor();
-//  EXPECT(assert_equal(f2, *actual2factor, 1e-9));
+  EXPECT(assert_equal(f2 / *f2.sum(1), *actual2.toFactor(), 1e-9));
 }
 
 /* ************************************************************************* */
-TEST( DiscreteConditional, constructors_alt_interface)
-{
-  DiscreteKey X(0, 2), Y(2, 3), Z(1, 2); // watch ordering !
+TEST(DiscreteConditional, constructors_alt_interface) {
+  DiscreteKey X(0, 2), Y(2, 3), Z(1, 2);  // watch ordering !
 
   Signature::Table table;
   Signature::Row r1, r2, r3;
-  r1 += 1.0, 1.0; r2 += 2.0, 3.0; r3 += 1.0, 4.0;
+  r1 += 1.0, 1.0;
+  r2 += 2.0, 3.0;
+  r3 += 1.0, 4.0;
   table += r1, r2, r3;
-  DiscreteConditional::shared_ptr expected1 = //
-      boost::make_shared<DiscreteConditional>(X | Y = table);
-  EXPECT(expected1);
+  auto actual1 = boost::make_shared<DiscreteConditional>(X | Y = table);
+  EXPECT(actual1);
   DecisionTreeFactor f1(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8");
-  DiscreteConditional actual1(1, f1);
-  EXPECT(assert_equal(*expected1, actual1, 1e-9));
+  DiscreteConditional expected1(1, f1);
+  EXPECT(assert_equal(expected1, *actual1, 1e-9));
 
-  DecisionTreeFactor f2(X & Y & Z,
-      "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
+  DecisionTreeFactor f2(
+      X & Y & Z, "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
   DiscreteConditional actual2(1, f2);
-  DecisionTreeFactor::shared_ptr actual2factor = actual2.toFactor();
-//  EXPECT(assert_equal(f2, *actual2factor, 1e-9));
+  EXPECT(assert_equal(f2 / *f2.sum(1), *actual2.toFactor(), 1e-9));
 }
 
 /* ************************************************************************* */
-TEST( DiscreteConditional, constructors2)
-{
+TEST(DiscreteConditional, constructors2) {
   // Declare keys and ordering
-  DiscreteKey C(0,2), B(1,2);
-  DecisionTreeFactor expected(C & B, "0.8 0.75 0.2 0.25");
+  DiscreteKey C(0, 2), B(1, 2);
+  DecisionTreeFactor actual(C & B, "0.8 0.75 0.2 0.25");
   Signature signature((C | B) = "4/1 3/1");
-  DiscreteConditional actual(signature);
-  DecisionTreeFactor::shared_ptr actualFactor = actual.toFactor();
-  EXPECT(assert_equal(expected, *actualFactor));
+  DiscreteConditional expected(signature);
+  DecisionTreeFactor::shared_ptr expectedFactor = expected.toFactor();
+  EXPECT(assert_equal(*expectedFactor, actual));
 }
 
 /* ************************************************************************* */
-TEST( DiscreteConditional, constructors3)
-{
+TEST(DiscreteConditional, constructors3) {
   // Declare keys and ordering
-  DiscreteKey C(0,2), B(1,2), A(2,2);
-  DecisionTreeFactor expected(C & B & A, "0.8 0.5 0.5 0.2 0.2 0.5 0.5 0.8");
+  DiscreteKey C(0, 2), B(1, 2), A(2, 2);
+  DecisionTreeFactor actual(C & B & A, "0.8 0.5 0.5 0.2 0.2 0.5 0.5 0.8");
   Signature signature((C | B, A) = "4/1 1/1 1/1 1/4");
-  DiscreteConditional actual(signature);
-  DecisionTreeFactor::shared_ptr actualFactor = actual.toFactor();
-  EXPECT(assert_equal(expected, *actualFactor));
+  DiscreteConditional expected(signature);
+  DecisionTreeFactor::shared_ptr expectedFactor = expected.toFactor();
+  EXPECT(assert_equal(*expectedFactor, actual));
 }
 
 /* ************************************************************************* */
-TEST( DiscreteConditional, Combine) {
+TEST(DiscreteConditional, Combine) {
   DiscreteKey A(0, 2), B(1, 2);
   vector<DiscreteConditional::shared_ptr> c;
   c.push_back(boost::make_shared<DiscreteConditional>(A | B = "1/2 2/1"));
   c.push_back(boost::make_shared<DiscreteConditional>(B % "1/2"));
   DecisionTreeFactor factor(A & B, "0.111111 0.444444 0.222222 0.222222");
-  DiscreteConditional expected(2, factor);
-  DiscreteConditional::shared_ptr actual = DiscreteConditional::Combine(
-      c.begin(), c.end());
-  EXPECT(assert_equal(expected, *actual,1e-5));
+  DiscreteConditional actual(2, factor);
+  auto expected = DiscreteConditional::Combine(c.begin(), c.end());
+  EXPECT(assert_equal(*expected, actual, 1e-5));
 }
 
 /* ************************************************************************* */
-int main() {  TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */
-
diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
index 0fbf44097..1defd5acf 100644
--- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
+++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteEliminationTree.h>
 #include <gtsam/discrete/DiscreteBayesTree.h>
+#include <gtsam/inference/BayesNet.h>
 
 #include <CppUnitLite/TestHarness.h>
 
diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp
index 4e9f956b6..e1eb92af3 100644
--- a/gtsam/discrete/tests/testDiscreteMarginals.cpp
+++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp
@@ -146,8 +146,7 @@ TEST_UNSAFE( DiscreteMarginals, truss ) {
 
 /* ************************************************************************* */
 // Second truss example with non-trivial factors
-TEST_UNSAFE( DiscreteMarginals, truss2 ) {
-
+TEST_UNSAFE(DiscreteMarginals, truss2) {
   const int nrNodes = 5;
   const size_t nrStates = 2;
 
@@ -160,40 +159,39 @@ TEST_UNSAFE( DiscreteMarginals, truss2 ) {
 
   // create graph and add three truss potentials
   DiscreteFactorGraph graph;
-  graph.add(key[0] & key[2] & key[4],"1 2 3 4 5 6 7 8");
-  graph.add(key[1] & key[3] & key[4],"1 2 3 4 5 6 7 8");
-  graph.add(key[2] & key[3] & key[4],"1 2 3 4 5 6 7 8");
+  graph.add(key[0] & key[2] & key[4], "1 2 3 4 5 6 7 8");
+  graph.add(key[1] & key[3] & key[4], "1 2 3 4 5 6 7 8");
+  graph.add(key[2] & key[3] & key[4], "1 2 3 4 5 6 7 8");
 
   // Calculate the marginals by brute force
-  vector<DiscreteFactor::Values> allPosbValues = cartesianProduct(
-      key[0] & key[1] & key[2] & key[3] & key[4]);
+  vector<DiscreteFactor::Values> allPosbValues =
+      cartesianProduct(key[0] & key[1] & key[2] & key[3] & key[4]);
   Vector T = Z_5x1, F = Z_5x1;
   for (size_t i = 0; i < allPosbValues.size(); ++i) {
     DiscreteFactor::Values x = allPosbValues[i];
     double px = graph(x);
-    for (size_t j=0;j<5;j++)
-      if (x[j]) T[j]+=px; else F[j]+=px;
-    // cout << x[0] << " " << x[1] << " "<< x[2] << " " << x[3] << " " << x[4] << " :\t" << px << endl;
+    for (size_t j = 0; j < 5; j++)
+      if (x[j])
+        T[j] += px;
+      else
+        F[j] += px;
   }
 
   // Check all marginals given by a sequential solver and Marginals
-//  DiscreteSequentialSolver solver(graph);
+  //  DiscreteSequentialSolver solver(graph);
   DiscreteMarginals marginals(graph);
-  for (size_t j=0;j<5;j++) {
-    double sum = T[j]+F[j];
-    T[j]/=sum;
-    F[j]/=sum;
-
-//    // solver
-//    Vector actualV = solver.marginalProbabilities(key[j]);
-//    EXPECT(assert_equal((Vector(2) << F[j], T[j]), actualV));
+  for (size_t j = 0; j < 5; j++) {
+    double sum = T[j] + F[j];
+    T[j] /= sum;
+    F[j] /= sum;
 
     // Marginals
     vector<double> table;
-    table += F[j],T[j];
-    DecisionTreeFactor expectedM(key[j],table);
+    table += F[j], T[j];
+    DecisionTreeFactor expectedM(key[j], table);
     DiscreteFactor::shared_ptr actualM = marginals(j);
-    EXPECT(assert_equal(expectedM, *boost::dynamic_pointer_cast<DecisionTreeFactor>(actualM)));
+    EXPECT(assert_equal(
+        expectedM, *boost::dynamic_pointer_cast<DecisionTreeFactor>(actualM)));
   }
 }
 
diff --git a/gtsam/discrete/tests/testSignature.cpp b/gtsam/discrete/tests/testSignature.cpp
index de47a00f3..049c455f7 100644
--- a/gtsam/discrete/tests/testSignature.cpp
+++ b/gtsam/discrete/tests/testSignature.cpp
@@ -11,36 +11,43 @@
 
 /**
  * @file testSignature
- * @brief Tests focusing on the details of Signatures to evaluate boost compliance
+ * @brief Tests focusing on the details of Signatures to evaluate boost
+ * compliance
  * @author Alex Cunningham
  * @date Sept 19th 2011
  */
 
-#include <boost/assign/std/vector.hpp>
 #include <CppUnitLite/TestHarness.h>
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/discrete/Signature.h>
 
+#include <boost/assign/std/vector.hpp>
+#include <vector>
+
 using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 
-DiscreteKey X(0,2), Y(1,3), Z(2,2);
+DiscreteKey X(0, 2), Y(1, 3), Z(2, 2);
 
 /* ************************************************************************* */
 TEST(testSignature, simple_conditional) {
   Signature sig(X | Y = "1/1 2/3 1/4");
+  Signature::Table table = *sig.table();
+  vector<double> row[3]{{0.5, 0.5}, {0.4, 0.6}, {0.2, 0.8}};
+  CHECK(row[0] == table[0]);
+  CHECK(row[1] == table[1]);
+  CHECK(row[2] == table[2]);
   DiscreteKey actKey = sig.key();
-  LONGS_EQUAL((long)X.first, (long)actKey.first);
+  LONGS_EQUAL(X.first, actKey.first);
 
-  DiscreteKeys actKeys = sig.discreteKeysParentsFirst();
-  LONGS_EQUAL(2, (long)actKeys.size());
-  LONGS_EQUAL((long)Y.first, (long)actKeys.front().first);
-  LONGS_EQUAL((long)X.first, (long)actKeys.back().first);
+  DiscreteKeys actKeys = sig.discreteKeys();
+  LONGS_EQUAL(2, actKeys.size());
+  LONGS_EQUAL(X.first, actKeys.front().first);
+  LONGS_EQUAL(Y.first, actKeys.back().first);
 
   vector<double> actCpt = sig.cpt();
-  EXPECT_LONGS_EQUAL(6, (long)actCpt.size());
+  EXPECT_LONGS_EQUAL(6, actCpt.size());
 }
 
 /* ************************************************************************* */
@@ -54,17 +61,20 @@ TEST(testSignature, simple_conditional_nonparser) {
 
   Signature sig(X | Y = table);
   DiscreteKey actKey = sig.key();
-  EXPECT_LONGS_EQUAL((long)X.first, (long)actKey.first);
+  EXPECT_LONGS_EQUAL(X.first, actKey.first);
 
-  DiscreteKeys actKeys = sig.discreteKeysParentsFirst();
-  LONGS_EQUAL(2, (long)actKeys.size());
-  LONGS_EQUAL((long)Y.first, (long)actKeys.front().first);
-  LONGS_EQUAL((long)X.first, (long)actKeys.back().first);
+  DiscreteKeys actKeys = sig.discreteKeys();
+  LONGS_EQUAL(2, actKeys.size());
+  LONGS_EQUAL(X.first, actKeys.front().first);
+  LONGS_EQUAL(Y.first, actKeys.back().first);
 
   vector<double> actCpt = sig.cpt();
-  EXPECT_LONGS_EQUAL(6, (long)actCpt.size());
+  EXPECT_LONGS_EQUAL(6, actCpt.size());
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */
diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h
index 8214b0727..8db7abffe 100644
--- a/gtsam/geometry/BearingRange.h
+++ b/gtsam/geometry/BearingRange.h
@@ -162,7 +162,7 @@ private:
     NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
   };
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 // Declare this to be both Testable and a Manifold
@@ -187,8 +187,8 @@ struct HasBearing {
 };
 
 // Similar helper class for to implement Range traits for classes with a range method
-// For classes with overloaded range methods, such as SimpleCamera, this can even be templated:
-//   template <typename T> struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
+// For classes with overloaded range methods, such as PinholeCamera, this can even be templated:
+//   template <typename T> struct Range<PinholeCamera, T> : HasRange<PinholeCamera, T, double> {};
 template <class A1, typename A2, class RT>
 struct HasRange {
   typedef RT result_type;
diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index 223bcc242..0d64d0184 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -116,7 +116,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
 
   if (iteration >= maxIterations)
     throw std::runtime_error(
-        "Cal3DS2::calibrate fails to converge. need a better initialization");
+        "Cal3Bundler::calibrate fails to converge. need a better initialization");
 
   return pn;
 }
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index d95c47f7b..4787f565b 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -117,7 +117,7 @@ public:
   Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
       OptionalJacobian<2, 2> Dp = boost::none) const;
 
-  /// Conver a pixel coordinate to ideal coordinate
+  /// Convert a pixel coordinate to ideal coordinate
   Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index 4009a1921..3e62f758d 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -76,10 +76,10 @@ public:
   Vector localCoordinates(const Cal3DS2& T2) const ;
 
   /// Return dimensions of calibration manifold object
-  virtual size_t dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
+  virtual size_t dim() const { return dimension ; }
 
   /// Return dimensions of calibration manifold object
-  static size_t Dim() { return 9; }  //TODO: make a final dimension variable
+  static size_t Dim() { return dimension; }
 
   /// @}
   /// @name Clone
diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp
new file mode 100644
index 000000000..f7794fafb
--- /dev/null
+++ b/gtsam/geometry/Cal3Fisheye.cpp
@@ -0,0 +1,185 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Cal3Fisheye.cpp
+ * @date Apr 8, 2020
+ * @author ghaggin
+ */
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3Fisheye.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+Cal3Fisheye::Cal3Fisheye(const Vector9& v)
+    : fx_(v[0]),
+      fy_(v[1]),
+      s_(v[2]),
+      u0_(v[3]),
+      v0_(v[4]),
+      k1_(v[5]),
+      k2_(v[6]),
+      k3_(v[7]),
+      k4_(v[8]) {}
+
+/* ************************************************************************* */
+Vector9 Cal3Fisheye::vector() const {
+  Vector9 v;
+  v << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_;
+  return v;
+}
+
+/* ************************************************************************* */
+Matrix3 Cal3Fisheye::K() const {
+  Matrix3 K;
+  K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
+  return K;
+}
+
+/* ************************************************************************* */
+double Cal3Fisheye::Scaling(double r) {
+  static constexpr double threshold = 1e-8;
+  if (r > threshold || r < -threshold) {
+    return atan(r) / r;
+  } else {
+    // Taylor expansion close to 0
+    double r2 = r * r, r4 = r2 * r2;
+    return 1.0 - r2 / 3 + r4 / 5;
+  }
+}
+
+/* ************************************************************************* */
+Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
+                                OptionalJacobian<2, 2> H2) const {
+  const double xi = p.x(), yi = p.y();
+  const double r2 = xi * xi + yi * yi, r = sqrt(r2);
+  const double t = atan(r);
+  const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4;
+  Vector5 K, T;
+  K << 1, k1_, k2_, k3_, k4_;
+  T << 1, t2, t4, t6, t8;
+  const double scaling = Scaling(r);
+  const double s = scaling * K.dot(T);
+  const double xd = s * xi, yd = s * yi;
+  Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_);
+
+  Matrix2 DK;
+  if (H1 || H2) DK << fx_, s_, 0.0, fy_;
+
+  // Derivative for calibration parameters (2 by 9)
+  if (H1) {
+    Matrix25 DR1;
+    // order: fx, fy, s, u0, v0
+    DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0;
+
+    // order: k1, k2, k3, k4
+    Matrix24 DR2;
+    auto T4 = T.tail<4>().transpose();
+    DR2 << xi * T4, yi * T4;
+    *H1 << DR1, DK * scaling * DR2;
+  }
+
+  // Derivative for points in intrinsic coords (2 by 2)
+  if (H2) {
+    const double dtd_dt =
+        1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8;
+    const double dt_dr = 1 / (1 + r2);
+    const double rinv = 1 / r;
+    const double dr_dxi = xi * rinv;
+    const double dr_dyi = yi * rinv;
+    const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
+    const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
+
+    const double td = t * K.dot(T);
+    const double rrinv = 1 / r2;
+    const double dxd_dxi =
+        dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi;
+    const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi;
+    const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi;
+    const double dyd_dyi =
+        dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi;
+
+    Matrix2 DR;
+    DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
+
+    *H2 = DK * DR;
+  }
+
+  return uv;
+}
+
+/* ************************************************************************* */
+Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) 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,
+  // throw exception if max iterations are reached
+  const int maxIterations = 10;
+  int iteration;
+  for (iteration = 0; iteration < maxIterations; ++iteration) {
+    Matrix2 jac;
+
+    // Calculate the current estimate (uv_hat) and the jacobian
+    const Point2 uv_hat = uncalibrate(pi, boost::none, jac);
+
+    // Test convergence
+    if ((uv_hat - uv).norm() < tol) break;
+
+    // Newton's method update step
+    pi = pi - jac.inverse() * (uv_hat - uv);
+  }
+
+  if (iteration >= maxIterations)
+    throw std::runtime_error(
+        "Cal3Fisheye::calibrate fails to converge. need a better "
+        "initialization");
+
+  return pi;
+}
+
+/* ************************************************************************* */
+void Cal3Fisheye::print(const std::string& s_) const {
+  gtsam::print((Matrix)K(), s_ + ".K");
+  gtsam::print(Vector(k()), s_ + ".k");
+}
+
+/* ************************************************************************* */
+bool Cal3Fisheye::equals(const Cal3Fisheye& K, double tol) const {
+  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol ||
+      std::abs(s_ - K.s_) > tol || std::abs(u0_ - K.u0_) > tol ||
+      std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
+      std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol ||
+      std::abs(k4_ - K.k4_) > tol)
+    return false;
+  return true;
+}
+
+/* ************************************************************************* */
+Cal3Fisheye Cal3Fisheye::retract(const Vector& d) const {
+  return Cal3Fisheye(vector() + d);
+}
+
+/* ************************************************************************* */
+Vector Cal3Fisheye::localCoordinates(const Cal3Fisheye& T2) const {
+  return T2.vector() - vector();
+}
+
+}  // namespace gtsam
+/* ************************************************************************* */
diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h
new file mode 100644
index 000000000..e24fe074f
--- /dev/null
+++ b/gtsam/geometry/Cal3Fisheye.h
@@ -0,0 +1,209 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Cal3Fisheye.h
+ * @brief Calibration of a fisheye camera
+ * @date Apr 8, 2020
+ * @author ghaggin
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Point2.h>
+
+#include <string>
+
+namespace gtsam {
+
+/**
+ * @brief Calibration of a fisheye camera
+ * @addtogroup geometry
+ * \nosubgrouping
+ *
+ * Uses same distortionmodel as OpenCV, with
+ * https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html
+ * 3D point in camera frame
+ *   p = (x, y, z)
+ * Intrinsic coordinates:
+ *   [x_i;y_i] = [x/z; y/z]
+ * Distorted coordinates:
+ *   r^2 = (x_i)^2 + (y_i)^2
+ *   th = atan(r)
+ *   th_d = th(1 + k1*th^2 + k2*th^4 + k3*th^6 + k4*th^8)
+ *   [x_d; y_d] = (th_d / r)*[x_i; y_i]
+ * Pixel coordinates:
+ *   K = [fx s u0; 0 fy v0 ;0 0 1]
+ *   [u; v; 1] = K*[x_d; y_d; 1]
+ */
+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
+
+ public:
+  enum { dimension = 9 };
+  typedef boost::shared_ptr<Cal3Fisheye>
+      shared_ptr;  ///< shared pointer to fisheye calibration object
+
+  /// @name Standard Constructors
+  /// @{
+
+  /// 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) {}
+
+  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)
+      : fx_(fx),
+        fy_(fy),
+        s_(s),
+        u0_(u0),
+        v0_(v0),
+        k1_(k1),
+        k2_(k2),
+        k3_(k3),
+        k4_(k4) {}
+
+  virtual ~Cal3Fisheye() {}
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+
+  explicit Cal3Fisheye(const Vector9& v);
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /// focal length x
+  inline double fx() const { return fx_; }
+
+  /// focal length x
+  inline double fy() const { return fy_; }
+
+  /// skew
+  inline double skew() const { return s_; }
+
+  /// image center in x
+  inline double px() const { return u0_; }
+
+  /// image center in y
+  inline double py() const { return v0_; }
+
+  /// First distortion coefficient
+  inline double k1() const { return k1_; }
+
+  /// Second distortion coefficient
+  inline double k2() const { return k2_; }
+
+  /// First tangential distortion coefficient
+  inline double k3() const { return k3_; }
+
+  /// Second tangential distortion coefficient
+  inline double k4() const { return k4_; }
+
+  /// return calibration matrix
+  Matrix3 K() const;
+
+  /// return distortion parameter vector
+  Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
+
+  /// Return all parameters as a vector
+  Vector9 vector() const;
+
+  /// Helper function that calculates atan(r)/r
+  static double Scaling(double r);
+
+  /**
+   * @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image
+   * coordinates [u; v]
+   * @param p point in intrinsic coordinates
+   * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters (fx, fy, ...,
+   * k4)
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
+   * @return point in (distorted) image coordinates
+   */
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
+
+  /// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
+  /// y_i]
+  Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// print with optional string
+  virtual void print(const std::string& s = "") const;
+
+  /// assert equality up to a tolerance
+  bool equals(const Cal3Fisheye& K, double tol = 10e-9) const;
+
+  /// @}
+  /// @name Manifold
+  /// @{
+
+  /// Given delta vector, update calibration
+  Cal3Fisheye retract(const Vector& d) const;
+
+  /// Given a different calibration, calculate update to obtain it
+  Vector localCoordinates(const Cal3Fisheye& T2) const;
+
+  /// Return dimensions of calibration manifold object
+  virtual size_t dim() const { return 9; }
+
+  /// Return dimensions of calibration manifold object
+  static size_t Dim() { return 9; }
+
+  /// @}
+  /// @name Clone
+  /// @{
+
+  /// @return a deep copy of this object
+  virtual boost::shared_ptr<Cal3Fisheye> clone() const {
+    return boost::shared_ptr<Cal3Fisheye>(new Cal3Fisheye(*this));
+  }
+
+  /// @}
+
+ private:
+  /// @name Advanced Interface
+  /// @{
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_NVP(fx_);
+    ar& BOOST_SERIALIZATION_NVP(fy_);
+    ar& BOOST_SERIALIZATION_NVP(s_);
+    ar& BOOST_SERIALIZATION_NVP(u0_);
+    ar& BOOST_SERIALIZATION_NVP(v0_);
+    ar& BOOST_SERIALIZATION_NVP(k1_);
+    ar& BOOST_SERIALIZATION_NVP(k2_);
+    ar& BOOST_SERIALIZATION_NVP(k3_);
+    ar& BOOST_SERIALIZATION_NVP(k4_);
+  }
+
+  /// @}
+};
+
+template <>
+struct traits<Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
+
+template <>
+struct traits<const Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
+
+}  // namespace gtsam
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index 9982cec47..ae75c3916 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -118,10 +118,10 @@ public:
   Vector10 localCoordinates(const Cal3Unified& T2) const ;
 
   /// Return dimensions of calibration manifold object
-  virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
+  virtual size_t dim() const { return dimension ; }
 
   /// Return dimensions of calibration manifold object
-  static size_t Dim() { return 10; }  //TODO: make a final dimension variable
+  static size_t Dim() { return dimension; }
 
   /// Return all parameters as a vector
   Vector10 vector() const ;
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index 747f53fe1..f2848d0a3 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -193,14 +193,10 @@ public:
   /// @{
 
   /// return DOF, dimensionality of tangent space
-  inline size_t dim() const {
-    return 5;
-  }
+  inline size_t dim() const { return dimension; }
 
   /// return DOF, dimensionality of tangent space
-  static size_t Dim() {
-    return 5;
-  }
+  static size_t Dim() { return dimension; }
 
   /// Given 5-dim tangent vector, create new calibration
   inline Cal3_S2 retract(const Vector& d) const {
diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h
index 51692dc82..a6eb41b60 100644
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@@ -111,14 +111,10 @@ namespace gtsam {
     /// @{
 
     /// return DOF, dimensionality of tangent space
-    inline size_t dim() const {
-      return 6;
-    }
+    inline size_t dim() const { return dimension; }
 
     /// return DOF, dimensionality of tangent space
-    static size_t Dim() {
-      return 6;
-    }
+    static size_t Dim() { return dimension; }
 
     /// Given 6-dim tangent vector, create new calibration
     inline Cal3_S2Stereo retract(const Vector& d) const {
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index 9d9b37d7a..eff747eb5 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -326,12 +326,12 @@ public:
 
   /// @deprecated
   inline size_t dim() const {
-    return 6;
+    return dimension;
   }
 
   /// @deprecated
   inline static size_t Dim() {
-    return 6;
+    return dimension;
   }
 
   /// @}
diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index ecf9a572d..feab8e0fa 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -319,7 +319,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 template<class CAMERA>
diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h
index 3235fdedd..ca719eb37 100644
--- a/gtsam/geometry/EssentialMatrix.h
+++ b/gtsam/geometry/EssentialMatrix.h
@@ -212,7 +212,7 @@ class EssentialMatrix {
   /// @}
 
  public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 template<>
diff --git a/gtsam/geometry/Line3.cpp b/gtsam/geometry/Line3.cpp
new file mode 100644
index 000000000..e3b4841e0
--- /dev/null
+++ b/gtsam/geometry/Line3.cpp
@@ -0,0 +1,120 @@
+#include <gtsam/geometry/Line3.h>
+
+namespace gtsam {
+
+Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4, 4> Dp, OptionalJacobian<4, 4> Dv) const {
+  Vector3 w;
+  w << v[0], v[1], 0;
+  Rot3 incR;
+
+  if (Dp) {
+    Dp->setIdentity();
+    incR = Rot3::Expmap(w);
+    Dp->block<2, 2>(0, 0) = ((incR.matrix()).transpose()).block<2, 2>(0, 0);
+  }
+  if (Dv) {
+    Matrix3 Dw;
+    incR = Rot3::Expmap(w, Dw);
+    Dv->setIdentity();
+    Dv->block<2, 2>(0, 0) = Dw.block<2, 2>(0, 0);
+  } else {
+    incR = Rot3::Expmap(w);
+  }
+  Rot3 Rt = R_ * incR;
+  return Line3(Rt, a_ + v[2], b_ + v[3]);
+}
+
+Vector4 Line3::localCoordinates(const Line3 &q, OptionalJacobian<4, 4> Dp,
+                                OptionalJacobian<4, 4> Dq) const {
+  Vector3 omega;
+  Matrix3 D_log;
+  omega = Rot3::Logmap(R_.inverse() * q.R_, D_log);
+  if (Dp) {
+    Matrix3 D_log_wp = -((q.R_).matrix()).transpose() * R_.matrix();
+    Matrix3 Dwp = D_log * D_log_wp;
+    Dp->setIdentity();
+    Dp->block<2, 2>(0, 0) = Dwp.block<2, 2>(0, 0);
+    (*Dp)(2, 2) = -1;
+    (*Dp)(3, 3) = -1;
+  }
+  if (Dq) {
+    Dq->setIdentity();
+    Dq->block<2, 2>(0, 0) = D_log.block<2, 2>(0, 0);
+  }
+  Vector4 local;
+  local << omega[0], omega[1], q.a_ - a_, q.b_ - b_;
+  return local;
+}
+
+void Line3::print(const std::string &s) const {
+  std::cout << s << std::endl;
+  R_.print("R:\n");
+  std::cout << "a: " << a_ << ", b: " << b_ << std::endl;
+}
+
+bool Line3::equals(const Line3 &l2, double tol) const {
+  Vector4 diff = localCoordinates(l2);
+  return fabs(diff[0]) < tol && fabs(diff[1]) < tol
+      && fabs(diff[2]) < tol && fabs(diff[3]) < tol;
+}
+
+Unit3 Line3::project(OptionalJacobian<2, 4> Dline) const {
+  Vector3 V_0;
+  V_0 << -b_, a_, 0.0;
+
+  Unit3 l;
+  if (Dline) {
+    // Jacobian of the normalized Unit3 projected line with respect to
+    // un-normalized Vector3 projected line in homogeneous coordinates.
+    Matrix23 D_unit_line;
+    l = Unit3::FromPoint3(Point3(R_ * V_0), D_unit_line);
+    // Jacobian of the un-normalized Vector3 line with respect to
+    // input 3D line
+    Matrix34 D_vec_line = Matrix34::Zero();
+    D_vec_line.col(0) = a_ * R_.r3();
+    D_vec_line.col(1) = b_ * R_.r3();
+    D_vec_line.col(2) = R_.r2();
+    D_vec_line.col(3) = -R_.r1();
+    // Jacobian of output wrt input is the product of the two.
+    *Dline = D_unit_line * D_vec_line;
+  } else {
+    l = Unit3::FromPoint3(Point3(R_ * V_0));
+  }
+  return l;
+}
+
+Point3 Line3::point(double distance) const {
+  // defining "center" of the line to be the point where it
+  // intersects rotated XY axis
+  Point3 center(a_, b_, 0);
+  Point3 rotated_center = R_ * center;
+  return rotated_center + distance * R_.r3();
+}
+
+Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
+                  OptionalJacobian<4, 6> Dpose, OptionalJacobian<4, 4> Dline) {
+  Rot3 wRc = wTc.rotation();
+  Rot3 cRw = wRc.inverse();
+  Rot3 cRl = cRw * wL.R_;
+
+  Vector2 w_ab;
+  Vector3 t = ((wL.R_).transpose() * wTc.translation());
+  Vector2 c_ab(wL.a_ - t[0], wL.b_ - t[1]);
+
+  if (Dpose) {
+    Matrix3 lRc = (cRl.matrix()).transpose();
+    Dpose->setZero();
+    // rotation
+    Dpose->block<2, 3>(0, 0) = -lRc.block<2, 3>(0, 0);
+    // translation
+    Dpose->block<2, 3>(2, 3) = -lRc.block<2, 3>(0, 0);
+  }
+  if (Dline) {
+    Dline->setIdentity();
+    (*Dline)(0, 3) = -t[2];
+    (*Dline)(1, 2) = t[2];
+  }
+  return Line3(cRl, c_ab[0], c_ab[1]);
+}
+
+}
\ No newline at end of file
diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h
new file mode 100644
index 000000000..1c7ed2f4c
--- /dev/null
+++ b/gtsam/geometry/Line3.h
@@ -0,0 +1,135 @@
+/* ----------------------------------------------------------------------------
+ * 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    Line3.h
+ * @brief   4 dimensional manifold of 3D lines
+ * @author  Akshay Krishnan
+ * @author  Frank Dellaert
+ */
+// \callgraph
+
+#pragma once
+
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Pose3.h>
+
+namespace gtsam {
+
+/**
+ * A 3D line (R,a,b) : (Rot3,Scalar,Scalar)
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class Line3 {
+ private:
+  Rot3 R_;    // Rotation of line about x and y in world frame
+  double a_, b_;  // Intersection of line with the world x-y plane rotated by R_
+  // Also the closest point on line to origin
+ public:
+  enum { dimension = 4 };
+
+  /** Default constructor is the Z axis **/
+  Line3() :
+      a_(0), b_(0) {}
+
+  /** Constructor for general line from (R, a, b) **/
+  Line3(const Rot3 &R, const double a, const double b) :
+      R_(R), a_(a), b_(b) {}
+
+  /**
+   * The retract method maps from the tangent space back to the manifold.
+   * The method q = p + v, where p is this line, v is an increment along
+   * the tangent space and q is the resulting line.
+   * The tangent space for the rotation of a line is only two dimensional -
+   * rotation about x and y
+   * @param v: increment in tangent space
+   * @param Dp: increment of retraction with respect to this line
+   * @param Dv: Jacobian of retraction with respect to the increment
+   * @return q: resulting line after adding the increment and mapping to the manifold
+   */
+  Line3 retract(const Vector4 &v,
+                OptionalJacobian<4, 4> Dp = boost::none,
+                OptionalJacobian<4, 4> Dv = boost::none) const;
+
+  /**
+   * The localCoordinates method is the inverse of retract and finds the difference
+   * between two lines in the tangent space. It computes v = q - p where q is an input
+   * line, p is this line and v is their difference in the tangent space.
+   * @param q: Line3 on manifold
+   * @param Dp: OptionalJacobian of localCoordinates with respect to this line
+   * @param Dq: OptionalJacobian of localCoordinates with respect to this line
+   * @return v: difference in the tangent space
+   */
+  Vector4 localCoordinates(const Line3 &q,
+                           OptionalJacobian<4, 4> Dp = boost::none,
+                           OptionalJacobian<4, 4> Dq = boost::none) const;
+
+  /**
+   * Print R, a, b
+   * @param s: optional starting string
+   */
+  void print(const std::string &s = "") const;
+
+  /**
+   * Check if two lines are equal
+   * @param l2 - line to be compared
+   * @param tol : optional tolerance
+   * @return boolean - true if lines are equal
+   */
+  bool equals(const Line3 &l2, double tol = 10e-9) const;
+
+  /**
+   * Projecting a line to the image plane. Assumes this line is in camera frame.
+   * @param Dline: OptionalJacobian of projected line with respect to this line
+   * @return Unit3 - projected line in image plane, in homogenous coordinates.
+   * We use Unit3 since it is a manifold with the right dimension.
+   */
+  Unit3 project(OptionalJacobian<2, 4> Dline = boost::none) const;
+
+  /**
+   * Returns point on the line that is at a certain distance starting from the
+   * point where the rotated XY axis intersects the line.
+   * @param distance (can be positive or negative) - positive is the positive
+   * direction of the rotated z axis that forms the line. The default value of zero
+   * returns the point where the rotated XY axis intersects the line.
+   * @return Point on the line
+   */
+  Point3 point(double distance = 0) const;
+
+  /**
+   * Transform a line from world to camera frame
+   * @param wTc - Pose3 of camera in world frame
+   * @param wL - Line3 in world frame
+   * @param Dpose - OptionalJacobian of transformed line with respect to p
+   * @param Dline -  OptionalJacobian of transformed line with respect to l
+   * @return Transformed line in camera frame
+   */
+  friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
+                           OptionalJacobian<4, 6> Dpose,
+                           OptionalJacobian<4, 4> Dline);
+};
+
+/**
+ * Transform a line from world to camera frame
+ * @param wTc - Pose3 of camera in world frame
+ * @param wL - Line3 in world frame
+ * @param Dpose - OptionalJacobian of transformed line with respect to p
+ * @param Dline -  OptionalJacobian of transformed line with respect to l
+ * @return Transformed line in camera frame
+ */
+Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
+                  OptionalJacobian<4, 6> Dpose = boost::none,
+                  OptionalJacobian<4, 4> Dline = boost::none);
+
+template<>
+struct traits<Line3> : public internal::Manifold<Line3> {};
+
+template<>
+struct traits<const Line3> : public internal::Manifold<Line3> {};
+}
\ No newline at end of file
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index c52127a45..9a80b937b 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -325,7 +325,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 // manifold traits
diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h
index 935962423..79dbb9ce9 100644
--- a/gtsam/geometry/PinholePose.h
+++ b/gtsam/geometry/PinholePose.h
@@ -107,9 +107,9 @@ public:
 
     // If needed, apply chain rule
     if (Dpose)
-    *Dpose = Dpi_pn * *Dpose;
+      *Dpose = Dpi_pn * *Dpose;
     if (Dpoint)
-    *Dpoint = Dpi_pn * *Dpoint;
+      *Dpoint = Dpi_pn * *Dpoint;
 
     return pi;
   }
@@ -222,7 +222,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // end of class PinholeBaseK
 
@@ -425,7 +425,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // end of class PinholePose
 
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 388318827..2a1f108ca 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -317,7 +317,7 @@ public:
 
 public:
   // Align for Point2, which is either derived from, or is typedef, of Vector2
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 }; // Pose2
 
 /** specialization for pose2 wedge function (generic template in Lie.h) */
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 01611d739..1b9285100 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -19,8 +19,10 @@
 #include <gtsam/geometry/concepts.h>
 #include <gtsam/base/concepts.h>
 
-#include <iostream>
 #include <cmath>
+#include <iostream>
+#include <limits>
+#include <string>
 
 using namespace std;
 
@@ -36,10 +38,10 @@ Pose3::Pose3(const Pose2& pose2) :
 }
 
 /* ************************************************************************* */
-Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> H1,
-                    OptionalJacobian<6, 3> H2) {
-  if (H1) *H1 << I_3x3, Z_3x3;
-  if (H2) *H2 << Z_3x3, R.transpose();
+Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR,
+                    OptionalJacobian<6, 3> Ht) {
+  if (HR) *HR << I_3x3, Z_3x3;
+  if (Ht) *Ht << Z_3x3, R.transpose();
   return Pose3(R, t);
 }
 
@@ -52,7 +54,6 @@ Pose3 Pose3::inverse() const {
 /* ************************************************************************* */
 // Calculate Adjoint map
 // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
-// Experimental - unit tests of derivatives based on it do not check out yet
 Matrix6 Pose3::AdjointMap() const {
   const Matrix3 R = R_.matrix();
   Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
@@ -73,15 +74,15 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {
 
 /* ************************************************************************* */
 Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
-    OptionalJacobian<6,6> H) {
-  if (H) {
-    H->setZero();
+    OptionalJacobian<6, 6> Hxi) {
+  if (Hxi) {
+    Hxi->setZero();
     for (int i = 0; i < 6; ++i) {
       Vector6 dxi;
       dxi.setZero();
       dxi(i) = 1.0;
       Matrix6 Gi = adjointMap(dxi);
-      H->col(i) = Gi * y;
+      Hxi->col(i) = Gi * y;
     }
   }
   return adjointMap(xi) * y;
@@ -89,15 +90,15 @@ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
 
 /* ************************************************************************* */
 Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
-    OptionalJacobian<6,6> H) {
-  if (H) {
-    H->setZero();
+    OptionalJacobian<6, 6> Hxi) {
+  if (Hxi) {
+    Hxi->setZero();
     for (int i = 0; i < 6; ++i) {
       Vector6 dxi;
       dxi.setZero();
       dxi(i) = 1.0;
       Matrix6 GTi = adjointMap(dxi).transpose();
-      H->col(i) = GTi * y;
+      Hxi->col(i) = GTi * y;
     }
   }
   return adjointMap(xi).transpose() * y;
@@ -107,7 +108,7 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
 void Pose3::print(const string& s) const {
   cout << s;
   R_.print("R:\n");
-  cout << '[' << t_.x() << ", " << t_.y() << ", " << t_.z() << "]\';";
+  cout << t_ << ";" << endl;
 }
 
 /* ************************************************************************* */
@@ -117,8 +118,8 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
 
 /* ************************************************************************* */
 /** Modified from Murray94book version (which assumes w and v normalized?) */
-Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
-  if (H) *H = ExpmapDerivative(xi);
+Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
+  if (Hxi) *Hxi = ExpmapDerivative(xi);
 
   // get angular velocity omega and translational velocity v from twist xi
   Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
@@ -126,8 +127,8 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
   Rot3 R = Rot3::Expmap(omega);
   double theta2 = omega.dot(omega);
   if (theta2 > std::numeric_limits<double>::epsilon()) {
-    Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis
-    Vector3 omega_cross_v = omega.cross(v);    // points towards axis
+    Vector3 t_parallel = omega * omega.dot(v);  // translation parallel to axis
+    Vector3 omega_cross_v = omega.cross(v);     // points towards axis
     Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
     return Pose3(R, t);
   } else {
@@ -136,10 +137,10 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
 }
 
 /* ************************************************************************* */
-Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) {
-  if (H) *H = LogmapDerivative(p);
-  const Vector3 w = Rot3::Logmap(p.rotation());
-  const Vector3 T = p.translation();
+Vector6 Pose3::Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose) {
+  if (Hpose) *Hpose = LogmapDerivative(pose);
+  const Vector3 w = Rot3::Logmap(pose.rotation());
+  const Vector3 T = pose.translation();
   const double t = w.norm();
   if (t < 1e-10) {
     Vector6 log;
@@ -159,33 +160,33 @@ Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) {
 }
 
 /* ************************************************************************* */
-Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
+Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian Hxi) {
 #ifdef GTSAM_POSE3_EXPMAP
-  return Expmap(xi, H);
+  return Expmap(xi, Hxi);
 #else
   Matrix3 DR;
-  Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0);
-  if (H) {
-    *H = I_6x6;
-    H->topLeftCorner<3,3>() = DR;
+  Rot3 R = Rot3::Retract(xi.head<3>(), Hxi ? &DR : 0);
+  if (Hxi) {
+    *Hxi = I_6x6;
+    Hxi->topLeftCorner<3, 3>() = DR;
   }
   return Pose3(R, Point3(xi.tail<3>()));
 #endif
 }
 
 /* ************************************************************************* */
-Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
+Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 #ifdef GTSAM_POSE3_EXPMAP
-  return Logmap(T, H);
+  return Logmap(pose, Hpose);
 #else
   Matrix3 DR;
-  Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0);
-  if (H) {
-    *H = I_6x6;
-    H->topLeftCorner<3,3>() = DR;
+  Vector3 omega = Rot3::LocalCoordinates(pose.rotation(), Hpose ? &DR : 0);
+  if (Hpose) {
+    *Hpose = I_6x6;
+    Hpose->topLeftCorner<3, 3>() = DR;
   }
   Vector6 xi;
-  xi << omega, T.translation();
+  xi << omega, pose.translation();
   return xi;
 #endif
 }
@@ -262,16 +263,16 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
 }
 
 /* ************************************************************************* */
-const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const {
-  if (H) *H << Z_3x3, rotation().matrix();
+const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const {
+  if (Hself) *Hself << Z_3x3, rotation().matrix();
   return t_;
 }
 
 /* ************************************************************************* */
 
-const Rot3& Pose3::rotation(OptionalJacobian<3, 6> H) const {
-  if (H) {
-    *H << I_3x3, Z_3x3;
+const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const {
+  if (Hself) {
+    *Hself << I_3x3, Z_3x3;
   }
   return R_;
 }
@@ -285,9 +286,10 @@ Matrix4 Pose3::matrix() const {
 }
 
 /* ************************************************************************* */
-Pose3 Pose3::transformPoseFrom(const Pose3& aTb) const {
+Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself,
+                                                 OptionalJacobian<6, 6> HaTb) const {
   const Pose3& wTa = *this;
-  return wTa * aTb;
+  return wTa.compose(aTb, Hself, HaTb);
 }
 
 /* ************************************************************************* */
@@ -300,101 +302,101 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
 #endif
 
 /* ************************************************************************* */
-Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> H1,
-                                               OptionalJacobian<6, 6> H2) const {
-  if (H1) *H1 = -wTb.inverse().AdjointMap() * AdjointMap();
-  if (H2) *H2 = I_6x6;
+Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
+                                               OptionalJacobian<6, 6> HwTb) const {
+  if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap();
+  if (HwTb) *HwTb = I_6x6;
   const Pose3& wTa = *this;
   return wTa.inverse() * wTb;
 }
 
 /* ************************************************************************* */
-Point3 Pose3::transformFrom(const Point3& p, OptionalJacobian<3,6> Dpose,
-    OptionalJacobian<3,3> Dpoint) const {
+Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself,
+    OptionalJacobian<3, 3> Hpoint) const {
   // Only get matrix once, to avoid multiple allocations,
   // as well as multiple conversions in the Quaternion case
   const Matrix3 R = R_.matrix();
-  if (Dpose) {
-    Dpose->leftCols<3>() = R * skewSymmetric(-p.x(), -p.y(), -p.z());
-    Dpose->rightCols<3>() = R;
+  if (Hself) {
+    Hself->leftCols<3>() = R * skewSymmetric(-point.x(), -point.y(), -point.z());
+    Hself->rightCols<3>() = R;
   }
-  if (Dpoint) {
-    *Dpoint = R;
+  if (Hpoint) {
+    *Hpoint = R;
   }
-  return R_ * p + t_;
+  return R_ * point + t_;
 }
 
 /* ************************************************************************* */
-Point3 Pose3::transformTo(const Point3& p, OptionalJacobian<3,6> Dpose,
-    OptionalJacobian<3,3> Dpoint) const {
+Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
+    OptionalJacobian<3, 3> Hpoint) const {
   // Only get transpose once, to avoid multiple allocations,
   // as well as multiple conversions in the Quaternion case
   const Matrix3 Rt = R_.transpose();
-  const Point3 q(Rt*(p - t_));
-  if (Dpose) {
+  const Point3 q(Rt*(point - t_));
+  if (Hself) {
     const double wx = q.x(), wy = q.y(), wz = q.z();
-    (*Dpose) <<
+    (*Hself) <<
         0.0, -wz, +wy,-1.0, 0.0, 0.0,
         +wz, 0.0, -wx, 0.0,-1.0, 0.0,
         -wy, +wx, 0.0, 0.0, 0.0,-1.0;
   }
-  if (Dpoint) {
-    *Dpoint = Rt;
+  if (Hpoint) {
+    *Hpoint = Rt;
   }
   return q;
 }
 
 /* ************************************************************************* */
-double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
-                    OptionalJacobian<1, 3> H2) const {
+double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself,
+                    OptionalJacobian<1, 3> Hpoint) const {
   Matrix36 D_local_pose;
   Matrix3 D_local_point;
-  Point3 local = transformTo(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
-  if (!H1 && !H2) {
+  Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0);
+  if (!Hself && !Hpoint) {
     return local.norm();
   } else {
     Matrix13 D_r_local;
     const double r = norm3(local, D_r_local);
-    if (H1) *H1 = D_r_local * D_local_pose;
-    if (H2) *H2 = D_r_local * D_local_point;
+    if (Hself) *Hself = D_r_local * D_local_pose;
+    if (Hpoint) *Hpoint = D_r_local * D_local_point;
     return r;
   }
 }
 
 /* ************************************************************************* */
-double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> H1,
-                    OptionalJacobian<1, 6> H2) const {
+double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> Hself,
+                    OptionalJacobian<1, 6> Hpose) const {
   Matrix13 D_local_point;
-  double r = range(pose.translation(), H1, H2 ? &D_local_point : 0);
-  if (H2) *H2 << Matrix13::Zero(), D_local_point * pose.rotation().matrix();
+  double r = range(pose.translation(), Hself, Hpose ? &D_local_point : 0);
+  if (Hpose) *Hpose << Matrix13::Zero(), D_local_point * pose.rotation().matrix();
   return r;
 }
 
 /* ************************************************************************* */
-Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
-                     OptionalJacobian<2, 3> H2) const {
+Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> Hself,
+                     OptionalJacobian<2, 3> Hpoint) const {
   Matrix36 D_local_pose;
   Matrix3 D_local_point;
-  Point3 local = transformTo(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
-  if (!H1 && !H2) {
+  Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0);
+  if (!Hself && !Hpoint) {
     return Unit3(local);
   } else {
     Matrix23 D_b_local;
     Unit3 b = Unit3::FromPoint3(local, D_b_local);
-    if (H1) *H1 = D_b_local * D_local_pose;
-    if (H2) *H2 = D_b_local * D_local_point;
+    if (Hself) *Hself = D_b_local * D_local_pose;
+    if (Hpoint) *Hpoint = D_b_local * D_local_point;
     return b;
   }
 }
 
 /* ************************************************************************* */
-Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> H1,
-                     OptionalJacobian<2, 6> H2) const {
-  if (H2) {
-    H2->setZero();
-    return bearing(pose.translation(), H1, H2.cols<3>(3));
+Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself,
+                     OptionalJacobian<2, 6> Hpose) const {
+  if (Hpose) {
+    Hpose->setZero();
+    return bearing(pose.translation(), Hself, Hpose.cols<3>(3));
   }
-  return bearing(pose.translation(), H1, boost::none);
+  return bearing(pose.translation(), Hself, boost::none);
 }
 
 /* ************************************************************************* */
@@ -418,24 +420,11 @@ boost::optional<Pose3> Pose3::Align(const std::vector<Point3Pair>& abPointPairs)
   for(const Point3Pair& abPair: abPointPairs) {
     Point3 da = abPair.first - aCentroid;
     Point3 db = abPair.second - bCentroid;
-    H += db * da.transpose();
+    H += da * db.transpose();
     }
 
-  // Compute SVD
-  Eigen::JacobiSVD<Matrix> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
-  Matrix U = svd.matrixU();
-  Vector S = svd.singularValues();
-  Matrix V = svd.matrixV();
-
-  // Check rank
-  if (S[1] < 1e-10)
-    return boost::none;
-
-  // Recover transform with correction from Eggert97machinevisionandapplications
-  Matrix3 UVtranspose = U * V.transpose();
-  Matrix3 detWeighting = I_3x3;
-  detWeighting(2, 2) = UVtranspose.determinant();
-  Rot3 aRb(Matrix(V * detWeighting * U.transpose()));
+  // ClosestTo finds rotation matrix closest to H in Frobenius sense
+  Rot3 aRb = Rot3::ClosestTo(H);
   Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid);
   return Pose3(aRb, aTb);
 }
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index e90f91127..3825b6241 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -44,7 +44,7 @@ public:
 private:
 
   Rot3 R_; ///< Rotation gRp, between global and pose frame
-  Point3 t_; ///< Translation gTp, from global origin to pose frame origin
+  Point3 t_; ///< Translation gPp, from global origin to pose frame origin
 
 public:
 
@@ -75,8 +75,8 @@ public:
 
   /// Named constructor with derivatives
   static Pose3 Create(const Rot3& R, const Point3& t,
-                      OptionalJacobian<6, 3> H1 = boost::none,
-                      OptionalJacobian<6, 3> H2 = boost::none);
+                      OptionalJacobian<6, 3> HR = boost::none,
+                      OptionalJacobian<6, 3> Ht = boost::none);
 
   /**
    *  Create Pose3 by aligning two point pairs
@@ -117,10 +117,10 @@ public:
   /// @{
 
   /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
-  static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> H = boost::none);
+  static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = boost::none);
 
   /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation
-  static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none);
+  static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none);
 
   /**
    * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame
@@ -157,7 +157,7 @@ public:
    * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
    */
   static Vector6 adjoint(const Vector6 &xi, const Vector6 &y,
-      OptionalJacobian<6, 6> = boost::none);
+      OptionalJacobian<6, 6> Hxi = boost::none);
 
   // temporary fix for wrappers until case issue is resolved
   static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
@@ -167,7 +167,7 @@ public:
    * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
    */
   static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
-      OptionalJacobian<6, 6> H = boost::none);
+      OptionalJacobian<6, 6> Hxi = boost::none);
 
   /// Derivative of Expmap
   static Matrix6 ExpmapDerivative(const Vector6& xi);
@@ -177,8 +177,8 @@ public:
 
   // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
   struct ChartAtOrigin {
-    static Pose3 Retract(const Vector6& v, ChartJacobian H = boost::none);
-    static Vector6 Local(const Pose3& r, ChartJacobian H = boost::none);
+    static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none);
+    static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
   };
 
   using LieGroup<Pose3, 6>::inverse; // version with derivative
@@ -201,38 +201,38 @@ public:
 
   /**
    * @brief takes point in Pose coordinates and transforms it to world coordinates
-   * @param p point in Pose coordinates
-   * @param Dpose optional 3*6 Jacobian wrpt this pose
-   * @param Dpoint optional 3*3 Jacobian wrpt point
+   * @param point point in Pose coordinates
+   * @param Hself optional 3*6 Jacobian wrpt this pose
+   * @param Hpoint optional 3*3 Jacobian wrpt point
    * @return point in world coordinates
    */
-  Point3 transformFrom(const Point3& p, OptionalJacobian<3, 6> Dpose =
-      boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const;
+  Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself =
+      boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
 
   /** syntactic sugar for transformFrom */
-  inline Point3 operator*(const Point3& p) const {
-    return transformFrom(p);
+  inline Point3 operator*(const Point3& point) const {
+    return transformFrom(point);
   }
 
   /**
    * @brief takes point in world coordinates and transforms it to Pose coordinates
-   * @param p point in world coordinates
-   * @param Dpose optional 3*6 Jacobian wrpt this pose
-   * @param Dpoint optional 3*3 Jacobian wrpt point
+   * @param point point in world coordinates
+   * @param Hself optional 3*6 Jacobian wrpt this pose
+   * @param Hpoint optional 3*3 Jacobian wrpt point
    * @return point in Pose coordinates
    */
-  Point3 transformTo(const Point3& p, OptionalJacobian<3, 6> Dpose =
-      boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const;
+  Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself =
+      boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
 
   /// @}
   /// @name Standard Interface
   /// @{
 
   /// get rotation
-  const Rot3& rotation(OptionalJacobian<3, 6> H = boost::none) const;
+  const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const;
 
   /// get translation
-  const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const;
+  const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) const;
 
   /// get x
   double x() const {
@@ -252,36 +252,44 @@ public:
   /** convert to 4*4 matrix */
   Matrix4 matrix() const;
 
-  /** receives a pose in local coordinates and transforms it to world coordinates */
-  Pose3 transformPoseFrom(const Pose3& pose) const;
+  /** 
+    * Assuming self == wTa, takes a pose aTb in local coordinates 
+    * and transforms it to world coordinates wTb = wTa * aTb.
+    * This is identical to compose.
+    */
+  Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none,
+                                            OptionalJacobian<6, 6> HaTb = boost::none) const;
 
-  /** receives a pose in world coordinates and transforms it to local coordinates */
-  Pose3 transformPoseTo(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none,
-                                           OptionalJacobian<6, 6> H2 = boost::none) const;
+  /** 
+   *  Assuming self == wTa, takes a pose wTb in world coordinates 
+   * and transforms it to local coordinates aTb = inv(wTa) * wTb 
+   */
+  Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none,
+                                          OptionalJacobian<6, 6> HwTb = boost::none) const;
 
   /**
    * Calculate range to a landmark
    * @param point 3D location of landmark
    * @return range (double)
    */
-  double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none,
-      OptionalJacobian<1, 3> H2 = boost::none) const;
+  double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none,
+      OptionalJacobian<1, 3> Hpoint = boost::none) const;
 
   /**
    * Calculate range to another pose
    * @param pose Other SO(3) pose
    * @return range (double)
    */
-  double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
-      OptionalJacobian<1, 6> H2 = boost::none) const;
+  double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none,
+      OptionalJacobian<1, 6> Hpose = boost::none) const;
 
   /**
    * Calculate bearing to a landmark
    * @param point 3D location of landmark
    * @return bearing (Unit3)
    */
-  Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none,
-      OptionalJacobian<2, 3> H2 = boost::none) const;
+  Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none,
+      OptionalJacobian<2, 3> Hpoint = boost::none) const;
 
   /**
    * Calculate bearing to another pose
@@ -289,8 +297,8 @@ public:
    * information is ignored.
    * @return bearing (Unit3)
    */
-  Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> H1 = boost::none,
-      OptionalJacobian<2, 6> H2 = boost::none) const;
+  Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none,
+      OptionalJacobian<2, 6> Hpose = boost::none) const;
 
   /// @}
   /// @name Advanced Interface
@@ -321,20 +329,20 @@ public:
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
   /// @name Deprecated
   /// @{
-  Point3 transform_from(const Point3& p,
-                        OptionalJacobian<3, 6> Dpose = boost::none,
-                        OptionalJacobian<3, 3> Dpoint = boost::none) const {
-    return transformFrom(p, Dpose, Dpoint);
+  Point3 transform_from(const Point3& point,
+                        OptionalJacobian<3, 6> Hself = boost::none,
+                        OptionalJacobian<3, 3> Hpoint = boost::none) const {
+    return transformFrom(point, Hself, Hpoint);
   }
-  Point3 transform_to(const Point3& p,
-                      OptionalJacobian<3, 6> Dpose = boost::none,
-                      OptionalJacobian<3, 3> Dpoint = boost::none) const {
-    return transformTo(p, Dpose, Dpoint);
+  Point3 transform_to(const Point3& point,
+                      OptionalJacobian<3, 6> Hself = boost::none,
+                      OptionalJacobian<3, 3> Hpoint = boost::none) const {
+    return transformTo(point, Hself, Hpoint);
   }
-  Pose3 transform_pose_to(const Pose3& pose, 
-                          OptionalJacobian<6, 6> H1 = boost::none,
-                          OptionalJacobian<6, 6> H2 = boost::none) const {
-    return transformPoseTo(pose, H1, H2);
+  Pose3 transform_pose_to(const Pose3& pose,
+                          OptionalJacobian<6, 6> Hself = boost::none,
+                          OptionalJacobian<6, 6> Hpose = boost::none) const {
+    return transformPoseTo(pose, Hself, Hpose);
   }
   /** 
   * @deprecated: this function is neither here not there. */
@@ -355,7 +363,7 @@ public:
 #ifdef GTSAM_USE_QUATERNIONS
   // Align if we are using Quaternions
   public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
 };
 // Pose3 class
diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index c2e711a17..01f62b8cb 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -16,13 +16,15 @@
  * @author  Christian Potthast
  * @author  Frank Dellaert
  * @author  Richard Roberts
+ * @author  Varun Agrawal
  */
 
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/SO3.h>
 #include <boost/math/constants/constants.hpp>
-#include <boost/random.hpp>
+
 #include <cmath>
+#include <random>
 
 using namespace std;
 
@@ -34,10 +36,9 @@ void Rot3::print(const std::string& s) const {
 }
 
 /* ************************************************************************* */
-Rot3 Rot3::Random(boost::mt19937& rng) {
-  // TODO allow any engine without including all of boost :-(
+Rot3 Rot3::Random(std::mt19937& rng) {
   Unit3 axis = Unit3::Random(rng);
-  boost::uniform_real<double> randomAngle(-M_PI, M_PI);
+  uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
   double angle = randomAngle(rng);
   return AxisAngle(axis, angle);
 }
@@ -184,6 +185,12 @@ Vector Rot3::quaternion() const {
   return v;
 }
 
+/* ************************************************************************* */
+pair<Unit3, double> Rot3::axisAngle() const {
+  const Vector3 omega = Rot3::Logmap(*this);
+  return std::pair<Unit3, double>(Unit3(omega), omega.norm());
+}
+
 /* ************************************************************************* */
 Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
   return SO3::ExpmapDerivative(x);
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 985c521a2..8f24f07c8 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -17,6 +17,7 @@
  * @author  Frank Dellaert
  * @author  Richard Roberts
  * @author  Luca Carlone
+ * @author  Varun Agrawal
  */
 // \callgraph
 
@@ -28,6 +29,8 @@
 #include <gtsam/base/concepts.h>
 #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
 
+#include <random>
+
 // You can override the default coordinate mode using this flag
 #ifndef ROT3_DEFAULT_COORDINATES_MODE
   #ifdef GTSAM_USE_QUATERNIONS
@@ -61,7 +64,7 @@ namespace gtsam {
     /** Internal Eigen Quaternion */
     gtsam::Quaternion quaternion_;
 #else
-    Matrix3 rot_;
+    SO3 rot_;
 #endif
 
   public:
@@ -92,26 +95,34 @@ namespace gtsam {
      * allow assignment/construction from a generic matrix.
      * See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top
      */
-    template<typename Derived>
-    inline explicit Rot3(const Eigen::MatrixBase<Derived>& R) {
-      #ifdef GTSAM_USE_QUATERNIONS
-        quaternion_=Matrix3(R);
-      #else
-        rot_ = R;
-      #endif
+    template <typename Derived>
+#ifdef GTSAM_USE_QUATERNIONS
+    explicit Rot3(const Eigen::MatrixBase<Derived>& R) {
+      quaternion_ = Matrix3(R);
     }
+#else
+    explicit Rot3(const Eigen::MatrixBase<Derived>& R) : rot_(R) {
+    }
+#endif
 
     /**
      * Constructor from a rotation matrix
      * Overload version for Matrix3 to avoid casting in quaternion mode.
      */
-    inline explicit Rot3(const Matrix3& R) {
-      #ifdef GTSAM_USE_QUATERNIONS
-        quaternion_=R;
-      #else
-        rot_ = R;
-      #endif
-    }
+#ifdef GTSAM_USE_QUATERNIONS
+    explicit Rot3(const Matrix3& R) : quaternion_(R) {}
+#else
+    explicit Rot3(const Matrix3& R) : rot_(R) {}
+#endif
+
+    /**
+     * Constructor from an SO3 instance
+     */
+#ifdef GTSAM_USE_QUATERNIONS
+    explicit Rot3(const SO3& R) : quaternion_(R.matrix()) {}
+#else
+    explicit Rot3(const SO3& R) : rot_(R) {}
+#endif
 
     /** Constructor from a quaternion.  This can also be called using a plain
      * Vector, due to implicit conversion from Vector to Quaternion
@@ -120,8 +131,13 @@ namespace gtsam {
     Rot3(const Quaternion& q);
     Rot3(double w, double x, double y, double z) : Rot3(Quaternion(w, x, y, z)) {}
 
-    /// Random, generates a random axis, then random angle \in [-p,pi]
-    static Rot3 Random(boost::mt19937 & rng);
+    /**
+     * Random, generates a random axis, then random angle \in [-p,pi]
+     * Example:
+     *   std::mt19937 engine(42);
+     *   Unit3 unit = Unit3::Random(engine);
+     */
+    static Rot3 Random(std::mt19937 & rng);
 
     /** Virtual destructor */
     virtual ~Rot3() {}
@@ -179,22 +195,24 @@ namespace gtsam {
 
     /**
      * Convert from axis/angle representation
-     * @param  axisw is the rotation axis, unit length
-     * @param   angle rotation angle
+     * @param  axis is the rotation axis, unit length
+     * @param  angle rotation angle
      * @return incremental rotation
      */
     static Rot3 AxisAngle(const Point3& axis, double angle) {
+      // Convert to unit vector.
+      Vector3 unitAxis = Unit3(axis).unitVector();
 #ifdef GTSAM_USE_QUATERNIONS
-      return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis));
+      return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, unitAxis));
 #else
-      return Rot3(SO3::AxisAngle(axis,angle));
+      return Rot3(SO3::AxisAngle(unitAxis,angle));
 #endif
     }
 
     /**
      * Convert from axis/angle representation
-     * @param   axis is the rotation axis
-     * @param   angle rotation angle
+     * @param  axis is the rotation axis
+     * @param  angle rotation angle
      * @return incremental rotation
      */
     static Rot3 AxisAngle(const Unit3& axis, double angle) {
@@ -228,6 +246,9 @@ namespace gtsam {
     static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p,  //
                               const Unit3& a_q, const Unit3& b_q);
 
+    /// Static, named constructor that finds Rot3 element closest to M in Frobenius norm.
+    static Rot3 ClosestTo(const Matrix3& M) { return Rot3(SO3::ClosestTo(M)); }
+
     /// @}
     /// @name Testable
     /// @{
@@ -250,9 +271,13 @@ namespace gtsam {
     /// Syntatic sugar for composing two rotations
     Rot3 operator*(const Rot3& R2) const;
 
-    /// inverse of a rotation, TODO should be different for M/Q
+    /// inverse of a rotation
     Rot3 inverse() const {
-      return Rot3(Matrix3(transpose()));
+#ifdef GTSAM_USE_QUATERNIONS
+      return Rot3(quaternion_.inverse());
+#else
+      return Rot3(rot_.matrix().transpose());
+#endif
     }
 
     /**
@@ -388,7 +413,6 @@ namespace gtsam {
      * Return 3*3 transpose (inverse) rotation matrix
      */
     Matrix3 transpose() const;
-    // TODO: const Eigen::Transpose<const Matrix3> transpose() const;
 
     /// @deprecated, this is base 1, and was just confusing
     Point3 column(int index) const;
@@ -411,7 +435,7 @@ namespace gtsam {
 
     /**
      * Use RQ to calculate roll-pitch-yaw angle representation
-     * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r)
+     * @return a vector containing rpy s.t. R = Rot3::Ypr(y,p,r)
      */
     Vector3 rpy() const;
 
@@ -421,7 +445,7 @@ namespace gtsam {
      * you should instead use xyz() or ypr()
      * TODO: make this more efficient
      */
-    inline double roll() const  { return ypr()(2); }
+    inline double roll() const  { return xyz()(0); }
 
     /**
      * Accessor to get to component of angle representations
@@ -429,7 +453,7 @@ namespace gtsam {
      * you should instead use xyz() or ypr()
      * TODO: make this more efficient
      */
-    inline double pitch() const { return ypr()(1); }
+    inline double pitch() const { return xyz()(1); }
 
     /**
      * Accessor to get to component of angle representations
@@ -437,12 +461,22 @@ namespace gtsam {
      * you should instead use xyz() or ypr()
      * TODO: make this more efficient
      */
-    inline double yaw() const   { return ypr()(0); }
+    inline double yaw() const   { return xyz()(2); }
 
     /// @}
     /// @name Advanced Interface
     /// @{
 
+    /**
+      * Compute the Euler axis and angle (in radians) representation
+      * of this rotation.
+      * The angle is in the range [0, π]. If the angle is not in the range,
+      * the axis is flipped around accordingly so that the returned angle is
+      * within the specified range.
+      * @return pair consisting of Unit3 axis and angle in radians
+      */
+    std::pair<Unit3, double> axisAngle() const;
+
     /** Compute the quaternion representation of this rotation.
      * @return The quaternion
      */
@@ -483,35 +517,34 @@ namespace gtsam {
   /// @}
 #endif
 
-  private:
-
+   private:
     /** Serialization function */
     friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int /*version*/)
-    {
+    template <class ARCHIVE>
+    void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
 #ifndef GTSAM_USE_QUATERNIONS
-       ar & boost::serialization::make_nvp("rot11", rot_(0,0));
-       ar & boost::serialization::make_nvp("rot12", rot_(0,1));
-       ar & boost::serialization::make_nvp("rot13", rot_(0,2));
-       ar & boost::serialization::make_nvp("rot21", rot_(1,0));
-       ar & boost::serialization::make_nvp("rot22", rot_(1,1));
-       ar & boost::serialization::make_nvp("rot23", rot_(1,2));
-       ar & boost::serialization::make_nvp("rot31", rot_(2,0));
-       ar & boost::serialization::make_nvp("rot32", rot_(2,1));
-       ar & boost::serialization::make_nvp("rot33", rot_(2,2));
+      Matrix3& M = rot_.matrix_;
+      ar& boost::serialization::make_nvp("rot11", M(0, 0));
+      ar& boost::serialization::make_nvp("rot12", M(0, 1));
+      ar& boost::serialization::make_nvp("rot13", M(0, 2));
+      ar& boost::serialization::make_nvp("rot21", M(1, 0));
+      ar& boost::serialization::make_nvp("rot22", M(1, 1));
+      ar& boost::serialization::make_nvp("rot23", M(1, 2));
+      ar& boost::serialization::make_nvp("rot31", M(2, 0));
+      ar& boost::serialization::make_nvp("rot32", M(2, 1));
+      ar& boost::serialization::make_nvp("rot33", M(2, 2));
 #else
-      ar & boost::serialization::make_nvp("w", quaternion_.w());
-      ar & boost::serialization::make_nvp("x", quaternion_.x());
-      ar & boost::serialization::make_nvp("y", quaternion_.y());
-      ar & boost::serialization::make_nvp("z", quaternion_.z());
+      ar& boost::serialization::make_nvp("w", quaternion_.w());
+      ar& boost::serialization::make_nvp("x", quaternion_.x());
+      ar& boost::serialization::make_nvp("y", quaternion_.y());
+      ar& boost::serialization::make_nvp("z", quaternion_.z());
 #endif
     }
 
 #ifdef GTSAM_USE_QUATERNIONS
   // only align if quaternion, Matrix3 has no alignment requirements
   public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
   };
 
diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp
index 529c64973..46a07e50a 100644
--- a/gtsam/geometry/Rot3M.cpp
+++ b/gtsam/geometry/Rot3M.cpp
@@ -36,18 +36,17 @@ Rot3::Rot3() : rot_(I_3x3) {}
 
 /* ************************************************************************* */
 Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
-  rot_.col(0) = col1;
-  rot_.col(1) = col2;
-  rot_.col(2) = col3;
+  Matrix3 R;
+  R << col1, col2, col3;
+  rot_ = SO3(R);
 }
 
 /* ************************************************************************* */
-Rot3::Rot3(double R11, double R12, double R13,
-    double R21, double R22, double R23,
-    double R31, double R32, double R33) {
-    rot_ << R11, R12, R13,
-        R21, R22, R23,
-        R31, R32, R33;
+Rot3::Rot3(double R11, double R12, double R13, double R21, double R22,
+           double R23, double R31, double R32, double R33) {
+  Matrix3 R;
+  R << R11, R12, R13, R21, R22, R23, R31, R32, R33;
+  rot_ = SO3(R);
 }
 
 /* ************************************************************************* */
@@ -107,21 +106,20 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) {
 
 /* ************************************************************************* */
 Rot3 Rot3::operator*(const Rot3& R2) const {
-  return Rot3(Matrix3(rot_*R2.rot_));
+  return Rot3(rot_*R2.rot_);
 }
 
 /* ************************************************************************* */
-// TODO const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
 Matrix3 Rot3::transpose() const {
-  return rot_.transpose();
+  return rot_.matrix().transpose();
 }
 
 /* ************************************************************************* */
 Point3 Rot3::rotate(const Point3& p,
     OptionalJacobian<3,3> H1,  OptionalJacobian<3,3> H2) const {
-  if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
-  if (H2) *H2 = rot_;
-  return rot_ * p;
+  if (H1) *H1 = rot_.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
+  if (H2) *H2 = rot_.matrix();
+  return rot_.matrix() * p;
 }
 
 /* ************************************************************************* */
@@ -178,21 +176,21 @@ Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
 
 /* ************************************************************************* */
 Matrix3 Rot3::matrix() const {
-  return rot_;
+  return rot_.matrix();
 }
 
 /* ************************************************************************* */
-Point3 Rot3::r1() const { return Point3(rot_.col(0)); }
+Point3 Rot3::r1() const { return Point3(rot_.matrix().col(0)); }
 
 /* ************************************************************************* */
-Point3 Rot3::r2() const { return Point3(rot_.col(1)); }
+Point3 Rot3::r2() const { return Point3(rot_.matrix().col(1)); }
 
 /* ************************************************************************* */
-Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
+Point3 Rot3::r3() const { return Point3(rot_.matrix().col(2)); }
 
 /* ************************************************************************* */
 gtsam::Quaternion Rot3::toQuaternion() const {
-  return gtsam::Quaternion(rot_);
+  return gtsam::Quaternion(rot_.matrix());
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp
index 8af9a7144..d609c289c 100644
--- a/gtsam/geometry/Rot3Q.cpp
+++ b/gtsam/geometry/Rot3Q.cpp
@@ -19,8 +19,8 @@
 
 #ifdef GTSAM_USE_QUATERNIONS
 
-#include <boost/math/constants/constants.hpp>
 #include <gtsam/geometry/Rot3.h>
+#include <boost/math/constants/constants.hpp>
 #include <cmath>
 
 using namespace std;
@@ -79,9 +79,13 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  // TODO: Could we do this? It works in Rot3M but not here, probably because
-  // here we create an intermediate value by calling matrix()
-  // const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
+  // TODO: Maybe use return type `const Eigen::Transpose<const Matrix3>`?
+  // It works in Rot3M but not here, because of some weird behavior
+  // due to Eigen's expression templates which needs more investigation.
+  // For example, if we use matrix(), the value returned has a 1e-10,
+  // and if we use quaternion_.toRotationMatrix(), the matrix is arbitrary.
+  // Using eval() here doesn't help, it only helps if we use it in
+  // the downstream code.
   Matrix3 Rot3::transpose() const {
     return matrix().transpose();
   }
diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 034956e99..c86b9b860 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -18,18 +18,38 @@
  * @date    December 2014
  */
 
-#include <gtsam/geometry/SO3.h>
 #include <gtsam/base/concepts.h>
+#include <gtsam/geometry/SO3.h>
+
+#include <Eigen/SVD>
+
 #include <cmath>
-#include <limits>
 #include <iostream>
+#include <limits>
 
 namespace gtsam {
 
+//******************************************************************************
 namespace so3 {
 
+GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
+  Matrix99 H;
+  auto R = Q.matrix();
+  H << I_3x3 * R(0, 0), I_3x3 * R(1, 0), I_3x3 * R(2, 0),  //
+      I_3x3 * R(0, 1), I_3x3 * R(1, 1), I_3x3 * R(2, 1),   //
+      I_3x3 * R(0, 2), I_3x3 * R(1, 2), I_3x3 * R(2, 2);
+  return H;
+}
+
+GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
+  Matrix3 MR = M * R.matrix();
+  if (H) *H = Dcompose(R);
+  return MR;
+}
+
 void ExpmapFunctor::init(bool nearZeroApprox) {
-  nearZero = nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
+  nearZero =
+      nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
   if (!nearZero) {
     sin_theta = std::sin(theta);
     const double s2 = std::sin(theta / 2.0);
@@ -48,7 +68,8 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
   }
 }
 
-ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox)
+ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
+                             bool nearZeroApprox)
     : theta2(angle * angle), theta(angle) {
   const double ax = axis.x(), ay = axis.y(), az = axis.z();
   K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
@@ -61,16 +82,16 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApp
 
 SO3 ExpmapFunctor::expmap() const {
   if (nearZero)
-    return I_3x3 + W;
+    return SO3(I_3x3 + W);
   else
-    return I_3x3 + sin_theta * K + one_minus_cos * KK;
+    return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
 }
 
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
-  if (nearZero)
+  if (nearZero) {
     dexp_ = I_3x3 - 0.5 * W;
-  else {
+  } else {
     a = one_minus_cos / theta;
     b = 1.0 - sin_theta / theta;
     dexp_ = I_3x3 - a * K + b * KK;
@@ -103,7 +124,7 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
   if (H1) {
     Matrix3 D_dexpv_omega;
     applyDexp(c, D_dexpv_omega);  // get derivative H of forward mapping
-    *H1 = -invDexp* D_dexpv_omega;
+    *H1 = -invDexp * D_dexpv_omega;
   }
   if (H2) *H2 = invDexp;
   return c;
@@ -111,39 +132,127 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
 
 }  // namespace so3
 
-/* ************************************************************************* */
+//******************************************************************************
+template <>
+GTSAM_EXPORT
 SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
   return so3::ExpmapFunctor(axis, theta).expmap();
 }
 
-/* ************************************************************************* */
-void SO3::print(const std::string& s) const {
-   std::cout << s << *this << std::endl;
- }
+//******************************************************************************
+template <>
+GTSAM_EXPORT
+SO3 SO3::ClosestTo(const Matrix3& M) {
+  Eigen::JacobiSVD<Matrix3> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
+  const auto& U = svd.matrixU();
+  const auto& V = svd.matrixV();
+  const double det = (U * V.transpose()).determinant();
+  return SO3(U * Vector3(1, 1, det).asDiagonal() * V.transpose());
+}
 
-/* ************************************************************************* */
+//******************************************************************************
+template <>
+GTSAM_EXPORT
+SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
+  // See Hartley13ijcv:
+  // Cost function C(R) = \sum sqr(|R-R_i|_F)
+  // Closed form solution = ClosestTo(C_e), where C_e = \sum R_i !!!!
+  Matrix3 C_e{Z_3x3};
+  for (const auto& R_i : rotations) {
+    C_e += R_i.matrix();
+  }
+  return ClosestTo(C_e);
+}
+
+//******************************************************************************
+template <>
+GTSAM_EXPORT
+Matrix3 SO3::Hat(const Vector3& xi) {
+  // skew symmetric matrix X = xi^
+  Matrix3 Y = Z_3x3;
+  Y(0, 1) = -xi(2);
+  Y(0, 2) = +xi(1);
+  Y(1, 2) = -xi(0);
+  return Y - Y.transpose();
+}
+
+//******************************************************************************
+template <>
+GTSAM_EXPORT
+Vector3 SO3::Vee(const Matrix3& X) {
+  Vector3 xi;
+  xi(0) = -X(1, 2);
+  xi(1) = +X(0, 2);
+  xi(2) = -X(0, 1);
+  return xi;
+}
+
+//******************************************************************************
+template <>
+GTSAM_EXPORT
+Matrix3 SO3::AdjointMap() const {
+  return matrix_;
+}
+
+//******************************************************************************
+template <>
+GTSAM_EXPORT
 SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
   if (H) {
     so3::DexpFunctor impl(omega);
     *H = impl.dexp();
     return impl.expmap();
-  } else
+  } else {
     return so3::ExpmapFunctor(omega).expmap();
+  }
 }
 
+template <>
+GTSAM_EXPORT
 Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
   return so3::DexpFunctor(omega).dexp();
 }
 
-/* ************************************************************************* */
-Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
-  using std::sqrt;
+//******************************************************************************
+/* Right Jacobian for Log map in SO(3) - equation (10.86) and following
+ equations in G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie
+ Groups", Volume 2, 2008.
+
+   logmap( Rhat * expmap(omega) ) \approx logmap(Rhat) + Jrinv * omega
+
+ where Jrinv = LogmapDerivative(omega). This maps a perturbation on the
+ manifold (expmap(omega)) to a perturbation in the tangent space (Jrinv *
+ omega)
+ */
+template <>
+GTSAM_EXPORT
+Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
+  using std::cos;
   using std::sin;
 
+  double theta2 = omega.dot(omega);
+  if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
+  double theta = std::sqrt(theta2);  // rotation angle
+
+  // element of Lie algebra so(3): W = omega^
+  const Matrix3 W = Hat(omega);
+  return I_3x3 + 0.5 * W +
+         (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) *
+             W * W;
+}
+
+//******************************************************************************
+template <>
+GTSAM_EXPORT
+Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
+  using std::sin;
+  using std::sqrt;
+
   // note switch to base 1
-  const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
-  const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
-  const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
+  const Matrix3& R = Q.matrix();
+  const double &R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
+  const double &R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
+  const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
 
   // Get trace(R)
   const double tr = R.trace();
@@ -152,17 +261,17 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
 
   // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
   // we do something special
-  if (std::abs(tr + 1.0) < 1e-10) {
-    if (std::abs(R33 + 1.0) > 1e-10)
+  if (tr + 1.0 < 1e-10) {
+    if (std::abs(R33 + 1.0) > 1e-5)
       omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
-    else if (std::abs(R22 + 1.0) > 1e-10)
+    else if (std::abs(R22 + 1.0) > 1e-5)
       omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
     else
-      // if(std::abs(R.r1_.x()+1.0) > 1e-10)  This is implicit
+      // if(std::abs(R.r1_.x()+1.0) > 1e-5)  This is implicit
       omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
   } else {
     double magnitude;
-    const double tr_3 = tr - 3.0; // always negative
+    const double tr_3 = tr - 3.0;  // always negative
     if (tr_3 < -1e-7) {
       double theta = acos((tr - 1.0) / 2.0);
       magnitude = theta / (2.0 * sin(theta));
@@ -174,32 +283,52 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
     omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
   }
 
-  if(H) *H = LogmapDerivative(omega);
+  if (H) *H = LogmapDerivative(omega);
   return omega;
 }
 
-/* ************************************************************************* */
-Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
-  using std::cos;
-  using std::sin;
+//******************************************************************************
+// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
 
-  double theta2 = omega.dot(omega);
-  if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
-  double theta = std::sqrt(theta2);  // rotation angle
-  /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
-   * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
-   * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
-   * where Jrinv = LogmapDerivative(omega);
-   * This maps a perturbation on the manifold (expmap(omega))
-   * to a perturbation in the tangent space (Jrinv * omega)
-   */
-  const Matrix3 W = skewSymmetric(omega); // element of Lie algebra so(3): W = omega^
-  return I_3x3 + 0.5 * W +
-         (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) *
-             W * W;
+template <>
+GTSAM_EXPORT
+SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
+  return Expmap(omega, H);
 }
 
-/* ************************************************************************* */
+template <>
+GTSAM_EXPORT
+Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H) {
+  return Logmap(R, H);
+}
 
-} // end namespace gtsam
+//******************************************************************************
+// local vectorize
+static Vector9 vec3(const Matrix3& R) {
+  return Eigen::Map<const Vector9>(R.data());
+}
 
+// so<3> generators
+static std::vector<Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
+                                SO3::Hat(Vector3::Unit(1)),
+                                SO3::Hat(Vector3::Unit(2))});
+
+// vectorized generators
+static const Matrix93 P3 =
+    (Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished();
+
+//******************************************************************************
+template <>
+GTSAM_EXPORT
+Vector9 SO3::vec(OptionalJacobian<9, 3> H) const {
+  const Matrix3& R = matrix_;
+  if (H) {
+    // As Luca calculated (for SO4), this is (I3 \oplus R) * P3
+    *H << R * P3.block<3, 3>(0, 0), R * P3.block<3, 3>(3, 0),
+        R * P3.block<3, 3>(6, 0);
+  }
+  return gtsam::vec3(R);
+}
+//******************************************************************************
+
+}  // end namespace gtsam
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index 5f1c7d1bf..0463bc2e8 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -20,120 +20,116 @@
 
 #pragma once
 
-#include <gtsam/base/Matrix.h>
+#include <gtsam/geometry/SOn.h>
+
 #include <gtsam/base/Lie.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/dllexport.h>
 
 #include <cmath>
-#include <iosfwd>
+#include <vector>
 
 namespace gtsam {
 
+using SO3 = SO<3>;
+
+// Below are all declarations of SO<3> specializations.
+// They are *defined* in SO3.cpp.
+
+template <>
+GTSAM_EXPORT
+SO3 SO3::AxisAngle(const Vector3& axis, double theta);
+
+template <>
+GTSAM_EXPORT
+SO3 SO3::ClosestTo(const Matrix3& M);
+
+template <>
+GTSAM_EXPORT
+SO3 SO3::ChordalMean(const std::vector<SO3>& rotations);
+
+template <>
+GTSAM_EXPORT
+Matrix3 SO3::Hat(const Vector3& xi);  ///< make skew symmetric matrix
+
+template <>
+GTSAM_EXPORT
+Vector3 SO3::Vee(const Matrix3& X);  ///< inverse of Hat
+
+/// Adjoint map
+template <>
+Matrix3 SO3::AdjointMap() const;
+
 /**
- *  True SO(3), i.e., 3*3 matrix subgroup
- *  We guarantee (all but first) constructors only generate from sub-manifold.
- *  However, round-off errors in repeated composition could move off it...
+ * Exponential map at identity - create a rotation from canonical coordinates
+ * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
  */
-class SO3: public Matrix3, public LieGroup<SO3, 3> {
+template <>
+GTSAM_EXPORT
+SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
 
-protected:
+/// Derivative of Expmap
+template <>
+GTSAM_EXPORT
+Matrix3 SO3::ExpmapDerivative(const Vector3& omega);
 
-public:
-  enum {
-    dimension = 3
-  };
+/**
+ * Log map at identity - returns the canonical coordinates
+ * \f$ [R_x,R_y,R_z] \f$ of this rotation
+ */
+template <>
+GTSAM_EXPORT
+Vector3 SO3::Logmap(const SO3& R, ChartJacobian H);
 
-  /// @name Constructors
-  /// @{
+/// Derivative of Logmap
+template <>
+GTSAM_EXPORT
+Matrix3 SO3::LogmapDerivative(const Vector3& omega);
 
-  /// Constructor from AngleAxisd
-  SO3() :
-      Matrix3(I_3x3) {
-  }
+// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
+template <>
+GTSAM_EXPORT
+SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H);
 
-  /// Constructor from Eigen Matrix
-  template<typename Derived>
-  SO3(const MatrixBase<Derived>& R) :
-      Matrix3(R.eval()) {
-  }
+template <>
+GTSAM_EXPORT
+Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H);
 
-  /// Constructor from AngleAxisd
-  SO3(const Eigen::AngleAxisd& angleAxis) :
-      Matrix3(angleAxis) {
-  }
+template <>
+GTSAM_EXPORT
+Vector9 SO3::vec(OptionalJacobian<9, 3> H) const;
 
-  /// Static, named constructor TODO think about relation with above
-  GTSAM_EXPORT static SO3 AxisAngle(const Vector3& axis, double theta);
+/** Serialization function */
+template <class Archive>
+void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) {
+  Matrix3& M = R.matrix_;
+  ar& boost::serialization::make_nvp("R11", M(0, 0));
+  ar& boost::serialization::make_nvp("R12", M(0, 1));
+  ar& boost::serialization::make_nvp("R13", M(0, 2));
+  ar& boost::serialization::make_nvp("R21", M(1, 0));
+  ar& boost::serialization::make_nvp("R22", M(1, 1));
+  ar& boost::serialization::make_nvp("R23", M(1, 2));
+  ar& boost::serialization::make_nvp("R31", M(2, 0));
+  ar& boost::serialization::make_nvp("R32", M(2, 1));
+  ar& boost::serialization::make_nvp("R33", M(2, 2));
+}
 
-  /// @}
-  /// @name Testable
-  /// @{
-
-  GTSAM_EXPORT void print(const std::string& s) const;
-
-  bool equals(const SO3 & R, double tol) const {
-    return equal_with_abs_tol(*this, R, tol);
-  }
-
-  /// @}
-  /// @name Group
-  /// @{
-
-  /// identity rotation for group operation
-  static SO3 identity() {
-    return I_3x3;
-  }
-
-  /// inverse of a rotation = transpose
-  SO3 inverse() const {
-    return this->Matrix3::inverse();
-  }
-
-  /// @}
-  /// @name Lie Group
-  /// @{
-
-  /**
-   * Exponential map at identity - create a rotation from canonical coordinates
-   * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
-   */
-  GTSAM_EXPORT static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
-
-  /// Derivative of Expmap
-  GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& omega);
-
-  /**
-   * Log map at identity - returns the canonical coordinates
-   * \f$ [R_x,R_y,R_z] \f$ of this rotation
-   */
-  GTSAM_EXPORT static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
-
-  /// Derivative of Logmap
-  GTSAM_EXPORT static Matrix3 LogmapDerivative(const Vector3& omega);
-
-  Matrix3 AdjointMap() const {
-    return *this;
-  }
-
-  // Chart at origin
-  struct ChartAtOrigin {
-    static SO3 Retract(const Vector3& omega, ChartJacobian H = boost::none) {
-      return Expmap(omega, H);
-    }
-    static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) {
-      return Logmap(R, H);
-    }
-  };
-
-  using LieGroup<SO3, 3>::inverse;
-
-  /// @}
-};
-
-// This namespace exposes two functors that allow for saving computation when
-// exponential map and its derivatives are needed at the same location in so<3>
-// The second functor also implements dedicated methods to apply dexp and/or inv(dexp)
 namespace so3 {
 
+/**
+ * Compose general matrix with an SO(3) element.
+ * We only provide the 9*9 derivative in the first argument M.
+ */
+GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
+                OptionalJacobian<9, 9> H = boost::none);
+
+/// (constant) Jacobian of compose wrpt M
+GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
+
+// Below are two functors that allow for saving computation when exponential map
+// and its derivatives are needed at the same location in so<3>. The second
+// functor also implements dedicated methods to apply dexp and/or inv(dexp).
+
 /// Functor implementing Exponential map
 class GTSAM_EXPORT ExpmapFunctor {
  protected:
@@ -146,7 +142,7 @@ class GTSAM_EXPORT ExpmapFunctor {
 
  public:
   /// Constructor with element of Lie algebra so(3)
-  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
+  explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
 
   /// Constructor with axis-angle
   ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
@@ -163,7 +159,7 @@ class DexpFunctor : public ExpmapFunctor {
 
  public:
   /// Constructor with element of Lie algebra so(3)
-  GTSAM_EXPORT DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
+  GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
 
   // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
   // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
@@ -184,12 +180,14 @@ class DexpFunctor : public ExpmapFunctor {
 };
 }  //  namespace so3
 
-template<>
-struct traits<SO3> : public internal::LieGroup<SO3> {
-};
+/*
+ * Define the traits. internal::LieGroup provides both Lie group and Testable
+ */
 
-template<>
-struct traits<const SO3> : public internal::LieGroup<SO3> {
-};
-} // end namespace gtsam
+template <>
+struct traits<SO3> : public internal::LieGroup<SO3> {};
 
+template <>
+struct traits<const SO3> : public internal::LieGroup<SO3> {};
+
+}  // end namespace gtsam
diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp
new file mode 100644
index 000000000..8a78bb83f
--- /dev/null
+++ b/gtsam/geometry/SO4.cpp
@@ -0,0 +1,234 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, 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    SO4.cpp
+ * @brief   4*4 matrix representation of SO(4)
+ * @author  Frank Dellaert
+ * @author  Luca Carlone
+ */
+
+#include <gtsam/base/concepts.h>
+#include <gtsam/base/timing.h>
+#include <gtsam/geometry/SO4.h>
+#include <gtsam/geometry/Unit3.h>
+
+#include <Eigen/Eigenvalues>
+
+#include <algorithm>
+#include <cmath>
+#include <iostream>
+#include <random>
+#include <vector>
+
+using namespace std;
+
+namespace gtsam {
+
+// TODO(frank): is this better than SOn::Random?
+// /* *************************************************************************
+// */ static Vector3 randomOmega(boost::mt19937 &rng) {
+//   static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
+//   return Unit3::Random(rng).unitVector() * randomAngle(rng);
+// }
+
+// /* *************************************************************************
+// */
+// // Create random SO(4) element using direct product of lie algebras.
+// SO4 SO4::Random(boost::mt19937 &rng) {
+//   Vector6 delta;
+//   delta << randomOmega(rng), randomOmega(rng);
+//   return SO4::Expmap(delta);
+// }
+
+//******************************************************************************
+template <>
+GTSAM_EXPORT
+Matrix4 SO4::Hat(const Vector6& xi) {
+  // skew symmetric matrix X = xi^
+  // Unlike Luca, makes upper-left the SO(3) subgroup.
+  Matrix4 Y = Z_4x4;
+  Y(0, 1) = -xi(5);
+  Y(0, 2) = +xi(4);
+  Y(1, 2) = -xi(3);
+  Y(0, 3) = +xi(2);
+  Y(1, 3) = -xi(1);
+  Y(2, 3) = +xi(0);
+  return Y - Y.transpose();
+}
+
+//******************************************************************************
+template <>
+GTSAM_EXPORT
+Vector6 SO4::Vee(const Matrix4& X) {
+  Vector6 xi;
+  xi(5) = -X(0, 1);
+  xi(4) = +X(0, 2);
+  xi(3) = -X(1, 2);
+  xi(2) = +X(0, 3);
+  xi(1) = -X(1, 3);
+  xi(0) = +X(2, 3);
+  return xi;
+}
+
+//******************************************************************************
+/* Exponential map, porting MATLAB implementation by Luca, which follows
+ * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
+ * Ramona-Andreaa Rohan */
+template <>
+GTSAM_EXPORT
+SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
+  using namespace std;
+  if (H) throw std::runtime_error("SO4::Expmap Jacobian");
+
+  // skew symmetric matrix X = xi^
+  const Matrix4 X = Hat(xi);
+
+  // do eigen-decomposition
+  auto eig = Eigen::EigenSolver<Matrix4>(X);
+  Eigen::Vector4cd e = eig.eigenvalues();
+  using std::abs;
+  sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
+    return abs(a.imag()) > abs(b.imag());
+  });
+
+  // Get a and b from eigenvalues +/i ai and +/- bi
+  double a = e[0].imag(), b = e[2].imag();
+  if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
+    throw runtime_error("SO4::Expmap: wrong eigenvalues.");
+  }
+
+  // Build expX = exp(xi^)
+  Matrix4 expX;
+  using std::cos;
+  using std::sin;
+  const auto X2 = X * X;
+  const auto X3 = X2 * X;
+  double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b;
+  if (a != 0 && b == 0) {
+    double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3;
+    return SO4(I_4x4 + X + c2 * X2 + c3 * X3);
+  } else if (a == b && b != 0) {
+    double sin_a = sin(a), cos_a = cos(a);
+    double c0 = (a * sin_a + 2 * cos_a) / 2,
+           c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a),
+           c3 = (sin_a - a * cos_a) / (2 * a3);
+    return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
+  } else if (a != b) {
+    double sin_a = sin(a), cos_a = cos(a);
+    double sin_b = sin(b), cos_b = cos(b);
+    double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2),
+           c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)),
+           c2 = (cos_a - cos_b) / (b2 - a2),
+           c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2));
+    return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
+  } else {
+    return SO4();
+  }
+}
+
+//******************************************************************************
+// local vectorize
+static SO4::VectorN2 vec4(const Matrix4& Q) {
+  return Eigen::Map<const SO4::VectorN2>(Q.data());
+}
+
+// so<4> generators
+static std::vector<Matrix4, Eigen::aligned_allocator<Matrix4> > G4(
+    {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)),
+     SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)),
+     SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});
+
+// vectorized generators
+static const Eigen::Matrix<double, 16, 6> P4 =
+    (Eigen::Matrix<double, 16, 6>() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]),
+     vec4(G4[3]), vec4(G4[4]), vec4(G4[5]))
+        .finished();
+
+//******************************************************************************
+template <>
+GTSAM_EXPORT
+Matrix6 SO4::AdjointMap() const {
+  // Elaborate way of calculating the AdjointMap
+  // TODO(frank): find a closed form solution. In SO(3) is just R :-/
+  const Matrix4& Q = matrix_;
+  const Matrix4 Qt = Q.transpose();
+  Matrix6 A;
+  for (size_t i = 0; i < 6; i++) {
+    // Calculate column i of linear map for coeffcient of Gi
+    A.col(i) = SO4::Vee(Q * G4[i] * Qt);
+  }
+  return A;
+}
+
+//******************************************************************************
+template <>
+GTSAM_EXPORT
+SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const {
+  const Matrix& Q = matrix_;
+  if (H) {
+    // As Luca calculated, this is (I4 \oplus Q) * P4
+    *H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0),
+        Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0);
+  }
+  return gtsam::vec4(Q);
+}
+
+///******************************************************************************
+template <>
+GTSAM_EXPORT
+SO4 SO4::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
+  if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
+  gttic(SO4_Retract);
+  const Matrix4 X = Hat(xi / 2);
+  return SO4((I_4x4 + X) * (I_4x4 - X).inverse());
+}
+
+//******************************************************************************
+template <>
+GTSAM_EXPORT
+Vector6 SO4::ChartAtOrigin::Local(const SO4& Q, ChartJacobian H) {
+  if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
+  const Matrix4& R = Q.matrix();
+  const Matrix4 X = (I_4x4 - R) * (I_4x4 + R).inverse();
+  return -2 * Vee(X);
+}
+
+//******************************************************************************
+GTSAM_EXPORT Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) {
+  const Matrix4& R = Q.matrix();
+  const Matrix3 M = R.topLeftCorner<3, 3>();
+  if (H) {
+    const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
+                  q = R.topRightCorner<3, 1>();
+    *H << Z_3x1, Z_3x1, -q, Z_3x1, -m3, m2,  //
+        Z_3x1, q, Z_3x1, m3, Z_3x1, -m1,     //
+        -q, Z_3x1, Z_3x1, -m2, m1, Z_3x1;
+  }
+  return M;
+}
+
+//******************************************************************************
+GTSAM_EXPORT Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) {
+  const Matrix4& R = Q.matrix();
+  const Matrix43 M = R.leftCols<3>();
+  if (H) {
+    const auto &m1 = R.col(0), m2 = R.col(1), m3 = R.col(2), q = R.col(3);
+    *H << Z_4x1, Z_4x1, -q, Z_4x1, -m3, m2,  //
+        Z_4x1, q, Z_4x1, m3, Z_4x1, -m1,     //
+        -q, Z_4x1, Z_4x1, -m2, m1, Z_4x1;
+  }
+  return M;
+}
+
+//******************************************************************************
+
+}  // end namespace gtsam
diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h
new file mode 100644
index 000000000..5014414c2
--- /dev/null
+++ b/gtsam/geometry/SO4.h
@@ -0,0 +1,116 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, 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    SO4.h
+ * @brief   4*4 matrix representation of SO(4)
+ * @author  Frank Dellaert
+ * @author  Luca Carlone
+ * @date    March 2019
+ */
+
+#pragma once
+
+#include <gtsam/geometry/SOn.h>
+
+#include <gtsam/base/Group.h>
+#include <gtsam/base/Lie.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/dllexport.h>
+
+#include <string>
+
+namespace gtsam {
+
+using SO4 = SO<4>;
+
+// /// Random SO(4) element (no big claims about uniformity)
+// static SO4 Random(boost::mt19937 &rng);
+
+// Below are all declarations of SO<4> specializations.
+// They are *defined* in SO4.cpp.
+
+template <>
+GTSAM_EXPORT
+Matrix4 SO4::Hat(const TangentVector &xi);
+
+template <>
+GTSAM_EXPORT
+Vector6 SO4::Vee(const Matrix4 &X);
+
+template <>
+GTSAM_EXPORT
+SO4 SO4::Expmap(const Vector6 &xi, ChartJacobian H);
+
+template <>
+GTSAM_EXPORT
+Matrix6 SO4::AdjointMap() const;
+
+template <>
+GTSAM_EXPORT
+SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const;
+
+template <>
+GTSAM_EXPORT
+SO4 SO4::ChartAtOrigin::Retract(const Vector6 &omega, ChartJacobian H);
+
+template <>
+GTSAM_EXPORT
+Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H);
+
+/**
+ * Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3).
+ */
+GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::none);
+
+/**
+ * Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q)
+ * -> S \in St(3,4).
+ */
+GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none);
+
+/** Serialization function */
+template <class Archive>
+void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) {
+  Matrix4 &M = Q.matrix_;
+  ar &boost::serialization::make_nvp("Q11", M(0, 0));
+  ar &boost::serialization::make_nvp("Q12", M(0, 1));
+  ar &boost::serialization::make_nvp("Q13", M(0, 2));
+  ar &boost::serialization::make_nvp("Q14", M(0, 3));
+
+  ar &boost::serialization::make_nvp("Q21", M(1, 0));
+  ar &boost::serialization::make_nvp("Q22", M(1, 1));
+  ar &boost::serialization::make_nvp("Q23", M(1, 2));
+  ar &boost::serialization::make_nvp("Q24", M(1, 3));
+
+  ar &boost::serialization::make_nvp("Q31", M(2, 0));
+  ar &boost::serialization::make_nvp("Q32", M(2, 1));
+  ar &boost::serialization::make_nvp("Q33", M(2, 2));
+  ar &boost::serialization::make_nvp("Q34", M(2, 3));
+
+  ar &boost::serialization::make_nvp("Q41", M(3, 0));
+  ar &boost::serialization::make_nvp("Q42", M(3, 1));
+  ar &boost::serialization::make_nvp("Q43", M(3, 2));
+  ar &boost::serialization::make_nvp("Q44", M(3, 3));
+}
+
+/*
+ * Define the traits. internal::LieGroup provides both Lie group and Testable
+ */
+
+template <>
+struct traits<SO4> : public internal::LieGroup<SO4> {};
+
+template <>
+struct traits<const SO4> : public internal::LieGroup<SO4> {};
+
+}  // end namespace gtsam
diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h
new file mode 100644
index 000000000..0d7f3e108
--- /dev/null
+++ b/gtsam/geometry/SOn-inl.h
@@ -0,0 +1,121 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, 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
+
+ * -------------------------------------------------------------------------- */
+
+#pragma once
+
+/**
+ * @file    SOn-inl.h
+ * @brief   Template implementations for SO(n)
+ * @author  Frank Dellaert
+ * @date    March 2019
+ */
+
+#include <gtsam/base/Matrix.h>
+
+#include <iostream>
+
+using namespace std;
+
+namespace gtsam {
+
+// Implementation for N>5 just uses dynamic version
+template <int N>
+typename SO<N>::MatrixNN SO<N>::Hat(const TangentVector& xi) {
+  return SOn::Hat(xi);
+}
+
+// Implementation for N>5 just uses dynamic version
+template <int N>
+typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) {
+  return SOn::Vee(X);
+}
+
+template <int N>
+SO<N> SO<N>::ChartAtOrigin::Retract(const TangentVector& xi, ChartJacobian H) {
+  if (H) throw std::runtime_error("SO<N>::Retract jacobian not implemented.");
+  const Matrix X = Hat(xi / 2.0);
+  size_t n = AmbientDim(xi.size());
+  const auto I = Eigen::MatrixXd::Identity(n, n);
+  // https://pdfs.semanticscholar.org/6165/0347b2ccac34b5f423081d1ce4dbc4d09475.pdf
+  return SO((I + X) * (I - X).inverse());
+}
+
+template <int N>
+typename SO<N>::TangentVector SO<N>::ChartAtOrigin::Local(const SO& R,
+                                                          ChartJacobian H) {
+  if (H) throw std::runtime_error("SO<N>::Local jacobian not implemented.");
+  const size_t n = R.rows();
+  const auto I = Eigen::MatrixXd::Identity(n, n);
+  const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse();
+  return -2 * Vee(X);
+}
+
+template <int N>
+typename SO<N>::MatrixDD SO<N>::AdjointMap() const {
+  throw std::runtime_error(
+      "SO<N>::AdjointMap only implemented for SO3 and SO4.");
+}
+
+template <int N>
+SO<N> SO<N>::Expmap(const TangentVector& omega, ChartJacobian H) {
+  throw std::runtime_error("SO<N>::Expmap only implemented for SO3 and SO4.");
+}
+
+template <int N>
+typename SO<N>::MatrixDD SO<N>::ExpmapDerivative(const TangentVector& omega) {
+  throw std::runtime_error("SO<N>::ExpmapDerivative only implemented for SO3.");
+}
+
+template <int N>
+typename SO<N>::TangentVector SO<N>::Logmap(const SO& R, ChartJacobian H) {
+  throw std::runtime_error("SO<N>::Logmap only implemented for SO3.");
+}
+
+template <int N>
+typename SO<N>::MatrixDD SO<N>::LogmapDerivative(const TangentVector& omega) {
+  throw std::runtime_error("O<N>::LogmapDerivative only implemented for SO3.");
+}
+
+template <int N>
+typename SO<N>::VectorN2 SO<N>::vec(
+    OptionalJacobian<internal::NSquaredSO(N), dimension> H) const {
+  const size_t n = rows();
+  const size_t n2 = n * n;
+
+  // Vectorize
+  VectorN2 X(n2);
+  X << Eigen::Map<const Matrix>(matrix_.data(), n2, 1);
+
+  // If requested, calculate H as (I \oplus Q) * P,
+  // where Q is the N*N rotation matrix, and P is calculated below.
+  if (H) {
+    // Calculate P matrix of vectorized generators
+    // TODO(duy): Should we refactor this as the jacobian of Hat?
+    const size_t d = dim();
+    Matrix P(n2, d);
+    for (size_t j = 0; j < d; j++) {
+      const auto X = Hat(Eigen::VectorXd::Unit(d, j));
+      P.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
+    }
+    H->resize(n2, d);
+    for (size_t i = 0; i < n; i++) {
+      H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);
+    }
+  }
+  return X;
+}
+
+template <int N>
+void SO<N>::print(const std::string& s) const {
+    cout << s << matrix_ << endl;
+}
+
+}  // namespace gtsam
diff --git a/gtsam/geometry/SOn.cpp b/gtsam/geometry/SOn.cpp
new file mode 100644
index 000000000..37b6c1784
--- /dev/null
+++ b/gtsam/geometry/SOn.cpp
@@ -0,0 +1,102 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, 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    SOn.cpp
+ * @brief   Definitions of dynamic specializations of SO(n)
+ * @author  Frank Dellaert
+ * @author  Varun Agrawal
+ * @date    March 2019
+ */
+
+#include <gtsam/geometry/SOn.h>
+
+namespace gtsam {
+
+template <>
+GTSAM_EXPORT
+Matrix SOn::Hat(const Vector& xi) {
+  size_t n = AmbientDim(xi.size());
+  if (n < 2) throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
+
+  Matrix X(n, n);  // allocate space for n*n skew-symmetric matrix
+  X.setZero();
+  if (n == 2) {
+    // Handle SO(2) case as recursion bottom
+    assert(xi.size() == 1);
+    X << 0, -xi(0), xi(0), 0;
+  } else {
+    // Recursively call SO(n-1) call for top-left block
+    const size_t dmin = (n - 1) * (n - 2) / 2;
+    X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin));
+
+    // determine sign of last element (signs alternate)
+    double sign = pow(-1.0, xi.size());
+    // Now fill last row and column
+    for (size_t i = 0; i < n - 1; i++) {
+      const size_t j = n - 2 - i;
+      X(n - 1, j) = -sign * xi(i);
+      X(j, n - 1) = -X(n - 1, j);
+      sign = -sign;
+    }
+  }
+  return X;
+}
+
+template <>
+GTSAM_EXPORT
+Vector SOn::Vee(const Matrix& X) {
+  const size_t n = X.rows();
+  if (n < 2) throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
+
+  if (n == 2) {
+    // Handle SO(2) case as recursion bottom
+    Vector xi(1);
+    xi(0) = X(1, 0);
+    return xi;
+  } else {
+    // Calculate dimension and allocate space
+    const size_t d = n * (n - 1) / 2;
+    Vector xi(d);
+
+    // Fill first n-1 spots from last row of X
+    double sign = pow(-1.0, xi.size());
+    for (size_t i = 0; i < n - 1; i++) {
+      const size_t j = n - 2 - i;
+      xi(i) = -sign * X(n - 1, j);
+      sign = -sign;
+    }
+
+    // Recursively call Vee to fill remainder of x
+    const size_t dmin = (n - 1) * (n - 2) / 2;
+    xi.tail(dmin) = Vee(X.topLeftCorner(n - 1, n - 1));
+    return xi;
+  }
+}
+
+template <>
+SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1,
+                                           DynamicJacobian H2) const {
+  if (H1) *H1 = g.inverse().AdjointMap();
+  if (H2) *H2 = SOn::IdentityJacobian(g.rows());
+  return derived() * g;
+}
+
+template <>
+SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
+                                           DynamicJacobian H2) const {
+  SOn result = derived().inverse() * g;
+  if (H1) *H1 = -result.inverse().AdjointMap();
+  if (H2) *H2 = SOn::IdentityJacobian(g.rows());
+  return result;
+}
+
+}  // namespace gtsam
diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h
new file mode 100644
index 000000000..a6392c2f9
--- /dev/null
+++ b/gtsam/geometry/SOn.h
@@ -0,0 +1,358 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, 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    SOn.h
+ * @brief   N*N matrix representation of SO(N). N can be Eigen::Dynamic
+ * @author  Frank Dellaert
+ * @date    March 2019
+ */
+
+#pragma once
+
+#include <gtsam/base/Lie.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/make_shared.h>
+#include <gtsam/dllexport.h>
+#include <Eigen/Core>
+
+#include <iostream> // TODO(frank): how to avoid?
+#include <string>
+#include <type_traits>
+#include <vector>
+#include <random>
+
+namespace gtsam {
+
+namespace internal {
+/// Calculate dimensionality of SO<N> manifold, or return Dynamic if so
+constexpr int DimensionSO(int N) {
+  return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2;
+}
+
+// Calculate N^2 at compile time, or return Dynamic if so
+constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; }
+}  // namespace internal
+
+/**
+ * Manifold of special orthogonal rotation matrices SO<N>.
+ * Template paramater N can be a fixed integer or can be Eigen::Dynamic
+ */
+template <int N>
+class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
+ public:
+  enum { dimension = internal::DimensionSO(N) };
+  using MatrixNN = Eigen::Matrix<double, N, N>;
+  using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
+  using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
+
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true)
+
+ protected:
+  MatrixNN matrix_;  ///< Rotation matrix
+
+  // enable_if_t aliases, used to specialize constructors/methods, see
+  // https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty-sfinae/
+  template <int N_>
+  using IsDynamic = typename std::enable_if<N_ == Eigen::Dynamic, void>::type;
+  template <int N_>
+  using IsFixed = typename std::enable_if<N_ >= 2, void>::type;
+  template <int N_>
+  using IsSO3 = typename std::enable_if<N_ == 3, void>::type;
+
+ public:
+  /// @name Constructors
+  /// @{
+
+  /// Construct SO<N> identity for N >= 2
+  template <int N_ = N, typename = IsFixed<N_>>
+  SO() : matrix_(MatrixNN::Identity()) {}
+
+  /// Construct SO<N> identity for N == Eigen::Dynamic
+  template <int N_ = N, typename = IsDynamic<N_>>
+  explicit SO(size_t n = 0) {
+    // We allow for n=0 as the default constructor, needed for serialization,
+    // wrappers etc.
+    matrix_ = Eigen::MatrixXd::Identity(n, n);
+  }
+
+  /// Constructor from Eigen Matrix, dynamic version
+  template <typename Derived>
+  explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
+
+  /// Named constructor from Eigen Matrix
+  template <typename Derived>
+  static SO FromMatrix(const Eigen::MatrixBase<Derived>& R) {
+    return SO(R);
+  }
+
+  /// Named constructor from lower dimensional matrix
+  template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
+  static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
+    Matrix Q = Matrix::Identity(n, n);
+    size_t p = R.rows();
+    assert(p < n && R.cols() == p);
+    Q.topLeftCorner(p, p) = R;
+    return SO(Q);
+  }
+
+  /// Construct dynamic SO(n) from Fixed SO<M>
+  template <int M, int N_ = N, typename = IsDynamic<N_>>
+  explicit SO(const SO<M>& R) : matrix_(R.matrix()) {}
+
+  /// Constructor from AngleAxisd
+  template <int N_ = N, typename = IsSO3<N_>>
+  explicit SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {}
+
+  /// Constructor from axis and angle. Only defined for SO3
+  static SO AxisAngle(const Vector3& axis, double theta);
+
+  /// Named constructor that finds SO(n) matrix closest to M in Frobenius norm,
+  /// currently only defined for SO3.
+  static SO ClosestTo(const MatrixNN& M);
+
+  /// Named constructor that finds chordal mean = argmin_R \sum sqr(|R-R_i|_F),
+  /// currently only defined for SO3.
+  static SO ChordalMean(const std::vector<SO>& rotations);
+
+  /// Random SO(n) element (no big claims about uniformity). SO(3) is specialized in SO3.cpp
+  template <int N_ = N, typename = IsDynamic<N_>>
+  static SO Random(std::mt19937& rng, size_t n = 0) {
+    if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
+    // TODO(frank): this might need to be re-thought
+    static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
+    const size_t d = SO::Dimension(n);
+    Vector xi(d);
+    for (size_t j = 0; j < d; j++) {
+      xi(j) = randomAngle(rng);
+    }
+    return SO::Retract(xi);
+  }
+
+  /// Random SO(N) element (no big claims about uniformity)
+  template <int N_ = N, typename = IsFixed<N_>>
+  static SO Random(std::mt19937& rng) {
+    // By default, use dynamic implementation above. Specialized for SO(3).
+    return SO(SO<Eigen::Dynamic>::Random(rng, N).matrix());
+  }
+
+  /// @}
+  /// @name Standard methods
+  /// @{
+
+  /// Return matrix
+  const MatrixNN& matrix() const { return matrix_; }
+
+  size_t rows() const { return matrix_.rows(); }
+  size_t cols() const { return matrix_.cols(); }
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  void print(const std::string& s = std::string()) const;
+
+  bool equals(const SO& other, double tol) const {
+    return equal_with_abs_tol(matrix_, other.matrix_, tol);
+  }
+
+  /// @}
+  /// @name Group
+  /// @{
+
+  /// Multiplication
+  SO operator*(const SO& other) const {
+    assert(dim() == other.dim());
+    return SO(matrix_ * other.matrix_);
+  }
+
+  /// SO<N> identity for N >= 2
+  template <int N_ = N, typename = IsFixed<N_>>
+  static SO identity() {
+    return SO();
+  }
+
+  /// SO<N> identity for N == Eigen::Dynamic
+  template <int N_ = N, typename = IsDynamic<N_>>
+  static SO identity(size_t n = 0) {
+    return SO(n);
+  }
+
+  /// inverse of a rotation = transpose
+  SO inverse() const { return SO(matrix_.transpose()); }
+
+  /// @}
+  /// @name Manifold
+  /// @{
+
+  using TangentVector = Eigen::Matrix<double, dimension, 1>;
+  using ChartJacobian = OptionalJacobian<dimension, dimension>;
+
+  /// Return compile-time dimensionality: fixed size N or Eigen::Dynamic
+  static int Dim() { return dimension; }
+
+  // Calculate manifold dimensionality for SO(n).
+  // Available as dimension or Dim() for fixed N.
+  static size_t Dimension(size_t n) { return n * (n - 1) / 2; }
+
+  // Calculate ambient dimension n from manifold dimensionality d.
+  static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; }
+
+  // Calculate run-time dimensionality of manifold.
+  // Available as dimension or Dim() for fixed N.
+  size_t dim() const { return Dimension(matrix_.rows()); }
+
+  /**
+   * Hat operator creates Lie algebra element corresponding to d-vector, where d
+   * is the dimensionality of the manifold. This function is implemented
+   * recursively, and the d-vector is assumed to laid out such that the last
+   * element corresponds to so(2), the last 3 to so(3), the last 6 to so(4)
+   * etc... For example, the vector-space isomorphic to so(5) is laid out as:
+   *   a b c d | u v w | x y | z
+   * where the latter elements correspond to "telescoping" sub-algebras:
+   *   0 -z  y  w -d
+   *   z  0 -x -v  c
+   *  -y  x  0  u -b
+   *  -w  v -u  0  a
+   *   d -c  b -a  0
+   * This scheme behaves exactly as expected for SO(2) and SO(3).
+   */
+  static MatrixNN Hat(const TangentVector& xi);
+
+  /**
+   * Inverse of Hat. See note about xi element order in Hat.
+   */
+  static TangentVector Vee(const MatrixNN& X);
+
+  // Chart at origin
+  struct ChartAtOrigin {
+    /**
+     * Retract uses Cayley map. See note about xi element order in Hat.
+     * Deafault implementation has no Jacobian implemented
+     */
+    static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none);
+
+    /**
+     * Inverse of Retract. See note about xi element order in Hat.
+     */
+    static TangentVector Local(const SO& R, ChartJacobian H = boost::none);
+  };
+
+  // Return dynamic identity DxD Jacobian for given SO(n)
+  template <int N_ = N, typename = IsDynamic<N_>>
+  static MatrixDD IdentityJacobian(size_t n) {
+    const size_t d = Dimension(n);
+    return MatrixDD::Identity(d, d);
+  }
+
+  /// @}
+  /// @name Lie Group
+  /// @{
+
+  /// Adjoint map
+  MatrixDD AdjointMap() const;
+
+  /**
+   * Exponential map at identity - create a rotation from canonical coordinates
+   */
+  static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none);
+
+  /// Derivative of Expmap, currently only defined for SO3
+  static MatrixDD ExpmapDerivative(const TangentVector& omega);
+
+  /**
+   * Log map at identity - returns the canonical coordinates of this rotation
+   */
+  static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none);
+
+  /// Derivative of Logmap, currently only defined for SO3
+  static MatrixDD LogmapDerivative(const TangentVector& omega);
+
+  // inverse with optional derivative
+  using LieGroup<SO<N>, internal::DimensionSO(N)>::inverse;
+
+  /// @}
+  /// @name Other methods
+  /// @{
+
+  /**
+   * Return vectorized rotation matrix in column order.
+   * Will use dynamic matrices as intermediate results, but returns a fixed size
+   * X and fixed-size Jacobian if dimension is known at compile time.
+   * */
+  VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
+                   boost::none) const;
+  /// @}
+
+  template <class Archive>
+  friend void save(Archive&, SO&, const unsigned int);
+  template <class Archive>
+  friend void load(Archive&, SO&, const unsigned int);
+  template <class Archive>
+  friend void serialize(Archive&, SO&, const unsigned int);
+  friend class boost::serialization::access;
+  friend class Rot3;  // for serialize
+};
+
+using SOn = SO<Eigen::Dynamic>;
+
+/*
+ * Specialize dynamic Hat and Vee, because recursion depends on dynamic nature.
+ * The definition is in SOn.cpp. Fixed-size SO3 and SO4 have their own version,
+ * and implementation for other fixed N is in SOn-inl.h.
+ */
+
+template <>
+GTSAM_EXPORT
+Matrix SOn::Hat(const Vector& xi);
+
+template <>
+GTSAM_EXPORT
+Vector SOn::Vee(const Matrix& X);
+
+/*
+ * Specialize dynamic compose and between, because the derivative is unknowable
+ * by the LieGroup implementations, who return a fixed-size matrix for H2.
+ */
+
+using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
+
+template <>
+SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1,
+                                           DynamicJacobian H2) const;
+
+template <>
+SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
+                                           DynamicJacobian H2) const;
+
+/** Serialization function */
+template<class Archive>
+void serialize(
+  Archive& ar, SOn& Q,
+  const unsigned int file_version
+) {
+  Matrix& M = Q.matrix_;
+  ar& BOOST_SERIALIZATION_NVP(M);
+}
+
+/*
+ * Define the traits. internal::LieGroup provides both Lie group and Testable
+ */
+
+template <int N>
+struct traits<SO<N>> : public internal::LieGroup<SO<N>> {};
+
+template <int N>
+struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {};
+
+}  // namespace gtsam
+
+#include "SOn-inl.h"
diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp
old mode 100755
new mode 100644
index 533ee500e..6f70d840e
--- a/gtsam/geometry/Unit3.cpp
+++ b/gtsam/geometry/Unit3.cpp
@@ -23,16 +23,6 @@
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/config.h>  // for GTSAM_USE_TBB
 
-#ifdef __clang__
-#  pragma clang diagnostic push
-#  pragma clang diagnostic ignored "-Wunused-variable"
-#endif
-#include <boost/random/uniform_on_sphere.hpp>
-#ifdef __clang__
-#  pragma clang diagnostic pop
-#endif
-
-#include <boost/random/variate_generator.hpp>
 #include <iostream>
 #include <limits>
 #include <cmath>
@@ -54,15 +44,19 @@ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2, 3> H) {
 }
 
 /* ************************************************************************* */
-Unit3 Unit3::Random(boost::mt19937 & rng) {
-  // TODO(dellaert): allow any engine without including all of boost :-(
-  boost::uniform_on_sphere<double> randomDirection(3);
-  // This variate_generator object is required for versions of boost somewhere
-  // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
-  boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> > generator(
-      rng, randomDirection);
-  const vector<double> d = generator();
-  return Unit3(d[0], d[1], d[2]);
+Unit3 Unit3::Random(std::mt19937& rng) {
+  // http://mathworld.wolfram.com/SpherePointPicking.html
+  // Adapted from implementation in boost, but using std <random>
+  std::uniform_real_distribution<double> uniform(-1.0, 1.0);
+  double sqsum;
+  double x, y;
+  do {
+    x = uniform(rng);
+    y = uniform(rng);
+    sqsum = x * x + y * y;
+  } while (sqsum > 1);
+  const double mult = 2 * sqrt(1 - sqsum);
+  return Unit3(x * mult, y * mult, 2 * sqsum - 1);
 }
 
 /* ************************************************************************* */
@@ -84,7 +78,7 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
   // NOTE(hayk): At some point it seemed like this reproducably resulted in
   // deadlock. However, I don't know why and I can no longer reproduce it.
   // It either was a red herring or there is still a latent bug left to debug.
-  tbb::mutex::scoped_lock lock(B_mutex_);
+  std::unique_lock<std::mutex> lock(B_mutex_);
 #endif
 
   const bool cachedBasis = static_cast<bool>(B_);
diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h
index 211698806..27d41a014 100644
--- a/gtsam/geometry/Unit3.h
+++ b/gtsam/geometry/Unit3.h
@@ -27,13 +27,13 @@
 #include <gtsam/dllexport.h>
 
 #include <boost/optional.hpp>
-#include <boost/random/mersenne_twister.hpp>
 #include <boost/serialization/nvp.hpp>
 
+#include <random>
 #include <string>
 
 #ifdef GTSAM_USE_TBB
-#include <tbb/mutex.h>
+#include <mutex> // std::mutex
 #endif
 
 namespace gtsam {
@@ -48,7 +48,7 @@ private:
   mutable boost::optional<Matrix62> H_B_; ///< Cached basis derivative
 
 #ifdef GTSAM_USE_TBB
-  mutable tbb::mutex B_mutex_; ///< Mutex to protect the cached basis.
+  mutable std::mutex B_mutex_; ///< Mutex to protect the cached basis.
 #endif
 
 public:
@@ -90,6 +90,8 @@ public:
   /// Copy assignment
   Unit3& operator=(const Unit3 & u) {
     p_ = u.p_;
+    B_ = u.B_;
+    H_B_ = u.H_B_;
     return *this;
   }
 
@@ -97,8 +99,13 @@ public:
   GTSAM_EXPORT static Unit3 FromPoint3(const Point3& point, //
       OptionalJacobian<2, 3> H = boost::none);
 
-  /// Random direction, using boost::uniform_on_sphere
-  GTSAM_EXPORT static Unit3 Random(boost::mt19937 & rng);
+  /**
+   * Random direction, using boost::uniform_on_sphere
+   * Example:
+   *   std::mt19937 engine(42);
+   *   Unit3 unit = Unit3::Random(engine);
+   */
+  GTSAM_EXPORT static Unit3 Random(std::mt19937 & rng);
 
   /// @}
 
@@ -209,7 +216,7 @@ private:
   /// @}
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 // Define GTSAM traits
diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp
new file mode 100644
index 000000000..9317fb737
--- /dev/null
+++ b/gtsam/geometry/tests/testCal3DFisheye.cpp
@@ -0,0 +1,189 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testCal3DFisheye.cpp
+ * @brief Unit tests for fisheye calibration class
+ * @author ghaggin
+ */
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3Fisheye.h>
+#include <gtsam/geometry/Point3.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace gtsam;
+
+GTSAM_CONCEPT_TESTABLE_INST(Cal3Fisheye)
+GTSAM_CONCEPT_MANIFOLD_INST(Cal3Fisheye)
+
+static const double fx = 250, fy = 260, s = 0.1, u0 = 320, v0 = 240;
+static Cal3Fisheye K(fx, fy, s, u0, v0, -0.013721808247486035,
+                     0.020727425669427896, -0.012786476702685545,
+                     0.0025242267320687625);
+static Point2 kTestPoint2(2, 3);
+
+/* ************************************************************************* */
+TEST(Cal3Fisheye, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
+
+/* ************************************************************************* */
+TEST(Cal3Fisheye, retract) {
+  Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4,
+                       K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8,
+                       K.k4() + 9);
+  Vector d(9);
+  d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
+  Cal3Fisheye actual = K.retract(d);
+  CHECK(assert_equal(expected, actual, 1e-7));
+  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
+}
+
+/* ************************************************************************* */
+TEST(Cal3Fisheye, uncalibrate1) {
+  // Calculate the solution
+  const double xi = kTestPoint2.x(), yi = kTestPoint2.y();
+  const double r = sqrt(xi * xi + yi * yi);
+  const double t = atan(r);
+  const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
+  const double td =
+      t * (1 + K.k1() * tt + K.k2() * t4 + K.k3() * t6 + K.k4() * t8);
+  Vector3 pd(td / r * xi, td / r * yi, 1);
+  Vector3 v = K.K() * pd;
+
+  Point2 uv_sol(v[0] / v[2], v[1] / v[2]);
+
+  Point2 uv = K.uncalibrate(kTestPoint2);
+  CHECK(assert_equal(uv, uv_sol));
+}
+
+/* ************************************************************************* */
+// For numerical derivatives
+Point2 f(const Cal3Fisheye& k, const Point2& pt) { return k.uncalibrate(pt); }
+
+/* ************************************************************************* */
+TEST(Cal3Fisheye, Derivatives) {
+  Matrix H1, H2;
+  K.uncalibrate(kTestPoint2, H1, H2);
+  CHECK(assert_equal(numericalDerivative21(f, K, kTestPoint2, 1e-7), H1, 1e-5));
+  CHECK(assert_equal(numericalDerivative22(f, K, kTestPoint2, 1e-7), H2, 1e-5));
+}
+
+/* ************************************************************************* */
+// Check that a point at (0,0) projects to the image center.
+TEST(Cal3Fisheye, uncalibrate2) {
+  Point2 pz(0, 0);
+  Matrix H1, H2;
+  auto uv = K.uncalibrate(pz, H1, H2);
+  CHECK(assert_equal(uv, Point2(u0, v0)));
+  CHECK(assert_equal(numericalDerivative21(f, K, pz, 1e-7), H1, 1e-5));
+  // TODO(frank): the second jacobian is all NaN for the image center!
+  // CHECK(assert_equal(numericalDerivative22(f, K, pz, 1e-7), H2, 1e-5));
+}
+
+/* ************************************************************************* */
+//  This test uses cv2::fisheye::projectPoints to test that uncalibrate
+//  properly projects a point into the image plane.  One notable difference
+//  between opencv and the Cal3Fisheye::uncalibrate function is the skew
+//  parameter. The equivalence is alpha = s/fx.
+//
+// Python script to project points with fisheye model in OpenCv
+// (script run with OpenCv version 4.2.0 and Numpy version 1.18.2)
+// clang-format off
+/*
+===========================================================
+
+import numpy as np
+import cv2
+
+objpts = np.float64([[23,27,31]]).reshape(1,-1,3)
+
+cameraMatrix = np.float64([
+    [250, 0, 320],
+    [0, 260, 240],
+    [0,0,1]
+])
+alpha = 0.1/250
+distCoeffs = np.float64([-0.013721808247486035, 0.020727425669427896,-0.012786476702685545, 0.0025242267320687625]) 
+
+rvec = np.float64([[0.,0.,0.]])
+tvec = np.float64([[0.,0.,0.]]);
+imagePoints, jacobian = cv2.fisheye.projectPoints(objpts, rvec, tvec, cameraMatrix, distCoeffs, alpha=alpha) 
+np.set_printoptions(precision=14) 
+print(imagePoints)
+
+===========================================================
+ * Script output: [[[457.82638130304935 408.18905848512986]]]
+ */
+// clang-format on
+TEST(Cal3Fisheye, uncalibrate3) {
+  Point3 p3(23, 27, 31);
+  Point2 xi(p3.x() / p3.z(), p3.y() / p3.z());
+  auto uv = K.uncalibrate(xi);
+  CHECK(assert_equal(uv, Point2(457.82638130304935, 408.18905848512986)));
+}
+
+/* ************************************************************************* */
+TEST(Cal3Fisheye, calibrate1) {
+  Point2 pi;
+  Point2 uv;
+  Point2 pi_hat;
+
+  pi = Point2(0.5, 0.5);     // point in intrinsic coordinates
+  uv = K.uncalibrate(pi);    // map intrinsic coord to image plane (pi)
+  pi_hat = K.calibrate(uv);  // map image coords (pi) back to intrinsic coords
+  CHECK(traits<Point2>::Equals(pi, pi_hat,
+                               1e-5));  // check that the inv mapping works
+
+  pi = Point2(-0.7, -1.2);
+  uv = K.uncalibrate(pi);
+  pi_hat = K.calibrate(uv);
+  CHECK(traits<Point2>::Equals(pi, pi_hat, 1e-5));
+
+  pi = Point2(-3, 5);
+  uv = K.uncalibrate(pi);
+  pi_hat = K.calibrate(uv);
+  CHECK(traits<Point2>::Equals(pi, pi_hat, 1e-5));
+
+  pi = Point2(7, -12);
+  uv = K.uncalibrate(pi);
+  pi_hat = K.calibrate(uv);
+  CHECK(traits<Point2>::Equals(pi, pi_hat, 1e-5));
+}
+
+/* ************************************************************************* */
+// Check that calibrate returns (0,0) for the image center
+TEST(Cal3Fisheye, calibrate2) {
+  Point2 uv(u0, v0);
+  auto xi_hat = K.calibrate(uv);
+  CHECK(assert_equal(xi_hat, Point2(0, 0)))
+}
+
+/* ************************************************************************* */
+// Run calibrate on OpenCv test from uncalibrate3
+//  (script shown above)
+// 3d point: (23, 27, 31)
+// 2d point in image plane: (457.82638130304935, 408.18905848512986)
+TEST(Cal3Fisheye, calibrate3) {
+  Point3 p3(23, 27, 31);
+  Point2 xi(p3.x() / p3.z(), p3.y() / p3.z());
+  Point2 uv(457.82638130304935, 408.18905848512986);
+  auto xi_hat = K.calibrate(uv);
+  CHECK(assert_equal(xi_hat, xi));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp
index c923e398b..86a498cdc 100644
--- a/gtsam/geometry/tests/testEssentialMatrix.cpp
+++ b/gtsam/geometry/tests/testEssentialMatrix.cpp
@@ -159,7 +159,7 @@ TEST (EssentialMatrix, rotate) {
   Matrix actH1, actH2;
   try {
     bodyE.rotate(cRb, actH1, actH2);
-  } catch (exception e) {
+  } catch (exception& e) {
   } // avoid exception
   Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), //
   expH2 = numericalDerivative22(rotate_, bodyE, cRb);
diff --git a/gtsam/geometry/tests/testLine3.cpp b/gtsam/geometry/tests/testLine3.cpp
new file mode 100644
index 000000000..9ed12aef7
--- /dev/null
+++ b/gtsam/geometry/tests/testLine3.cpp
@@ -0,0 +1,155 @@
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Line3.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/expressionTesting.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/expressions.h>
+
+using namespace gtsam;
+
+GTSAM_CONCEPT_TESTABLE_INST(Line3)
+GTSAM_CONCEPT_MANIFOLD_INST(Line3)
+
+static const Line3 l(Rot3(), 1, 1);
+
+// Testing equals function of Line3
+TEST(Line3, equals) {
+  Line3 l_same = l;
+  EXPECT(l.equals(l_same));
+  Line3 l2(Rot3(), 1, 2);
+  EXPECT(!l.equals(l2));
+}
+
+// testing localCoordinates along 4 dimensions
+TEST(Line3, localCoordinates) {
+  // l1 and l differ only in a_
+  Line3 l1(Rot3(), 2, 1);
+  Vector4 v1(0, 0, -1, 0);
+  CHECK(assert_equal(l1.localCoordinates(l), v1));
+
+  // l2 and l differ only in b_
+  Line3 l2(Rot3(), 1, 2);
+  Vector4 v2(0, 0, 0, -1);
+  CHECK(assert_equal(l2.localCoordinates(l), v2));
+
+  // l3 and l differ in R_x
+  Rot3 r3 = Rot3::Expmap(Vector3(M_PI / 4, 0, 0));
+  Line3 l3(r3, 1, 1);
+  Vector4 v3(-M_PI / 4, 0, 0, 0);
+  CHECK(assert_equal(l3.localCoordinates(l), v3));
+
+  // l4 and l differ in R_y
+  Rot3 r4 = Rot3::Expmap(Vector3(0, M_PI / 4, 0));
+  Line3 l4(r4, 1, 1);
+  Vector4 v4(0, -M_PI / 4, 0, 0);
+  CHECK(assert_equal(l4.localCoordinates(l), v4));
+
+  // Jacobians
+  Line3 l5(Rot3::Expmap(Vector3(M_PI / 3, -M_PI / 4, 0)), -0.8, 2);
+  Values val;
+  val.insert(1, l);
+  val.insert(2, l5);
+  Line3_ l_(1);
+  Line3_ l5_(2);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1);
+  Vector4_ local_(l5_, &Line3::localCoordinates, l_);
+  ExpressionFactor<Vector4> f(model, l5.localCoordinates(l), local_);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
+}
+
+// testing retract along 4 dimensions
+TEST(Line3, retract) {
+  // l1 and l differ only in a_
+  Vector4 v1(0, 0, 0, 1);
+  Line3 l1(Rot3(), 1, 2);
+  EXPECT(l1.equals(l.retract(v1)));
+
+  // l2 and l differ only in b_
+  Vector4 v2(0, 0, 1, 0);
+  Line3 l2(Rot3(), 2, 1);
+  EXPECT(l2.equals(l.retract(v2)));
+
+  // l3 and l differ in R_x
+  Vector4 v3(M_PI / 4, 0, 0, 0);
+  Rot3 r3;
+  r3 = r3.Expmap(Vector3(M_PI / 4, 0, 0));
+  Line3 l3(r3, 1, 1);
+  EXPECT(l3.equals(l.retract(v3)));
+
+  // l4 and l differ in R_y
+  Vector4 v4(0, M_PI / 4, 0, 0);
+  Rot3 r4 = Rot3::Expmap(Vector3(0, M_PI / 4, 0));
+  Line3 l4(r4, 1, 1);
+  EXPECT(l4.equals(l.retract(v4)));
+
+  // Jacobians
+  Vector4 v5(M_PI / 3, -M_PI / 4, -0.4, 1.2); // arbitrary vector
+  Values val;
+  val.insert(1, l);
+  val.insert(2, v5);
+  Line3_ l_(1);
+  Vector4_ v5_(2);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1);
+  Line3_ retract_(l_, &Line3::retract, v5_);
+  ExpressionFactor<Line3> f(model, l.retract(v5), retract_);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
+}
+
+// testing manifold property - Retract(p, Local(p,q)) == q
+TEST(Line3, retractOfLocalCoordinates) {
+  Rot3 r2 = Rot3::Expmap(Vector3(M_PI / 4, M_PI / 3, 0));
+  Line3 l2(r2, 5, 9);
+  EXPECT(assert_equal(l.retract(l.localCoordinates(l2)), l2));
+}
+
+// testing manifold property - Local(p, Retract(p, v)) == v
+TEST(Line3, localCoordinatesOfRetract) {
+  Vector4 r2(2.3, 0.987, -3, 4);
+  EXPECT(assert_equal(l.localCoordinates(l.retract(r2)), r2));
+}
+
+// transform from world to camera test
+TEST(Line3, transformToExpressionJacobians) {
+  Rot3 r = Rot3::Expmap(Vector3(0, M_PI / 3, 0));
+  Vector3 t(0, 0, 0);
+  Pose3 p(r, t);
+
+  Line3 l_c(r.inverse(), 1, 1);
+  Line3 l_w(Rot3(), 1, 1);
+  EXPECT(l_c.equals(transformTo(p, l_w)));
+
+  Line3_ l_(1);
+  Pose3_ p_(2);
+  Values val;
+  val.insert(1, l_w);
+  val.insert(2, p);
+
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1);
+  ExpressionFactor<Line3> f(model, transformTo(p, l_w), transformTo(p_, l_));
+  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
+}
+
+// projection in camera frame test
+TEST(Line3, projection) {
+  Rot3 r = Rot3::Expmap(Vector3(0, 0, 0));
+  Line3 wL(r, 1, 1);
+
+  Unit3 expected = Unit3::FromPoint3(Point3(-1, 1, 0));
+  EXPECT(expected.equals(wL.project()));
+
+  Values val;
+  val.insert(1, wL);
+  Line3_ wL_(1);
+
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.1);
+  Unit3_ projected_(wL_, &Line3::project);
+  ExpressionFactor<Unit3> f(model, expected, projected_);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5);
+}
+
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp
index 54cf84ea8..dc5851319 100644
--- a/gtsam/geometry/tests/testOrientedPlane3.cpp
+++ b/gtsam/geometry/tests/testOrientedPlane3.cpp
@@ -66,8 +66,8 @@ TEST (OrientedPlane3, transform) {
   OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose,
       none, none);
   OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none);
-  EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-9));
-  EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-9));
+  EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-5));
+  EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-5));
 
   // Test the jacobians of transform
   Matrix actualH1, expectedH1, actualH2, expectedH2;
@@ -75,18 +75,18 @@ TEST (OrientedPlane3, transform) {
     // because the Transform class uses a wrong order of Jacobians in interface
     OrientedPlane3::Transform(plane, pose, actualH1, none);
     expectedH1 = numericalDerivative22(Transform_, plane, pose);
-    EXPECT(assert_equal(expectedH1, actualH1, 1e-9));
+    EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
     OrientedPlane3::Transform(plane, pose, none, actualH2);
     expectedH2 = numericalDerivative21(Transform_, plane, pose);
-    EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
+    EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
   }
   {
     plane.transform(pose, actualH1, none);
     expectedH1 = numericalDerivative21(transform_, plane, pose);
-    EXPECT(assert_equal(expectedH1, actualH1, 1e-9));
+    EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
     plane.transform(pose, none, actualH2);
     expectedH2 = numericalDerivative22(Transform_, plane, pose);
-    EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
+    EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
   }
 }
 
@@ -157,8 +157,8 @@ TEST (OrientedPlane3, error2) {
       boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
   expectedH1 = numericalDerivative21(f, plane1, plane2);
   expectedH2 = numericalDerivative22(f, plane1, plane2);
-  EXPECT(assert_equal(expectedH1, actualH1, 1e-9));
-  EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
+  EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
+  EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
 }
 
 //*******************************************************************************
@@ -171,19 +171,19 @@ TEST (OrientedPlane3, jacobian_retract) {
       Vector3 v (-0.1, 0.2, 0.3);
       plane.retract(v, H_actual);
       Matrix H_expected_numerical = numericalDerivative11(f, v);
-      EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9));
+      EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5));
   }
   {
       Vector3 v (0, 0, 0);
       plane.retract(v, H_actual);
       Matrix H_expected_numerical = numericalDerivative11(f, v);
-      EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9));
+      EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5));
   }
 }
 
 /* ************************************************************************* */
 int main() {
-  srand(time(NULL));
+  srand(time(nullptr));
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }
diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp
index b8b3fc52c..3d821502d 100644
--- a/gtsam/geometry/tests/testPose2.cpp
+++ b/gtsam/geometry/tests/testPose2.cpp
@@ -835,6 +835,81 @@ TEST(Pose2 , ChartDerivatives) {
   CHECK_CHART_DERIVATIVES(T2,T1);
 }
 
+//******************************************************************************
+#include "testPoseAdjointMap.h"
+
+TEST(Pose2 , TransformCovariance3) {
+  // Use simple covariance matrices and transforms to create tests that can be
+  // validated with simple computations.
+  using namespace test_pose_adjoint_map;
+
+  // rotate
+  {
+    auto cov = FullCovarianceFromSigmas<Pose2>({0.1, 0.3, 0.7});
+    auto transformed = TransformCovariance<Pose2>{{0., 0., 90 * degree}}(cov);
+    // interchange x and y axes
+    EXPECT(assert_equal(
+      Vector3{cov(1, 1), cov(0, 0), cov(2, 2)},
+      Vector3{transformed.diagonal()}));
+    EXPECT(assert_equal(
+      Vector3{-cov(1, 0), -cov(2, 1), cov(2, 0)},
+      Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)}));
+  }
+
+  // translate along x with uncertainty in x
+  {
+    auto cov = SingleVariableCovarianceFromSigma<Pose2>(0, 0.1);
+    auto transformed = TransformCovariance<Pose2>{{20., 0., 0.}}(cov);
+    // No change
+    EXPECT(assert_equal(cov, transformed));
+  }
+
+  // translate along x with uncertainty in y
+  {
+    auto cov = SingleVariableCovarianceFromSigma<Pose2>(1, 0.1);
+    auto transformed = TransformCovariance<Pose2>{{20., 0., 0.}}(cov);
+    // No change
+    EXPECT(assert_equal(cov, transformed));
+  }
+
+  // translate along x with uncertainty in theta
+  {
+    auto cov = SingleVariableCovarianceFromSigma<Pose2>(2, 0.1);
+    auto transformed = TransformCovariance<Pose2>{{20., 0., 0.}}(cov);
+    EXPECT(assert_equal(
+      Vector3{0., 0.1 * 0.1 * 20. * 20., 0.1 * 0.1},
+      Vector3{transformed.diagonal()}));
+    EXPECT(assert_equal(
+      Vector3{0., 0., -0.1 * 0.1 * 20.},
+      Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)}));
+  }
+
+  // rotate and translate along x with uncertainty in x
+  {
+    auto cov = SingleVariableCovarianceFromSigma<Pose2>(0, 0.1);
+    auto transformed = TransformCovariance<Pose2>{{20., 0., 90 * degree}}(cov);
+    // interchange x and y axes
+    EXPECT(assert_equal(
+      Vector3{cov(1, 1), cov(0, 0), cov(2, 2)},
+      Vector3{transformed.diagonal()}));
+    EXPECT(assert_equal(
+      Vector3{Z_3x1},
+      Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)}));
+  }
+
+  // rotate and translate along x with uncertainty in theta
+  {
+    auto cov = SingleVariableCovarianceFromSigma<Pose2>(2, 0.1);
+    auto transformed = TransformCovariance<Pose2>{{20., 0., 90 * degree}}(cov);
+    EXPECT(assert_equal(
+      Vector3{0., 0.1 * 0.1 * 20. * 20., 0.1 * 0.1},
+      Vector3{transformed.diagonal()}));
+    EXPECT(assert_equal(
+      Vector3{0., 0., -0.1 * 0.1 * 20.},
+      Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)}));
+  }
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 0b2f59773..a169c833c 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -418,6 +418,29 @@ TEST(Pose3, transform_to_rotate) {
   EXPECT(assert_equal(expected, actual, 0.001));
 }
 
+/* ************************************************************************* */
+// Check transformPoseFrom and its pushforward
+Pose3 transformPoseFrom_(const Pose3& wTa, const Pose3& aTb) {
+  return wTa.transformPoseFrom(aTb);
+}
+
+TEST(Pose3, transformPoseFrom)
+{
+  Matrix actual = (T2*T2).matrix();
+  Matrix expected = T2.matrix()*T2.matrix();
+  EXPECT(assert_equal(actual, expected, 1e-8));
+
+  Matrix H1, H2;
+  T2.transformPoseFrom(T2, H1, H2);
+
+  Matrix numericalH1 = numericalDerivative21(transformPoseFrom_, T2, T2);
+  EXPECT(assert_equal(numericalH1, H1, 5e-3));
+  EXPECT(assert_equal(T2.inverse().AdjointMap(), H1, 5e-3));
+
+  Matrix numericalH2 = numericalDerivative22(transformPoseFrom_, T2, T2);
+  EXPECT(assert_equal(numericalH2, H2, 1e-4));
+}
+
 /* ************************************************************************* */
 TEST(Pose3, transformTo) {
   Pose3 transform(Rot3::Rodrigues(0, 0, -1.570796), Point3(2, 4, 0));
@@ -649,8 +672,8 @@ TEST(Pose3, Bearing) {
   // Check numerical derivatives
   expectedH1 = numericalDerivative21(bearing_proxy, x1, l1);
   expectedH2 = numericalDerivative22(bearing_proxy, x1, l1);
-  EXPECT(assert_equal(expectedH1, actualH1));
-  EXPECT(assert_equal(expectedH2, actualH2));
+  EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
+  EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
 }
 
 TEST(Pose3, Bearing2) {
@@ -660,8 +683,8 @@ TEST(Pose3, Bearing2) {
   // Check numerical derivatives
   expectedH1 = numericalDerivative21(bearing_proxy, x2, l4);
   expectedH2 = numericalDerivative22(bearing_proxy, x2, l4);
-  EXPECT(assert_equal(expectedH1, actualH1));
-  EXPECT(assert_equal(expectedH2, actualH2));
+  EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
+  EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
 }
 
 TEST(Pose3, PoseToPoseBearing) {
@@ -681,8 +704,8 @@ TEST(Pose3, PoseToPoseBearing) {
   expectedH2.setZero();
   expectedH2.block<2, 3>(0, 3) = H2block;
 
-  EXPECT(assert_equal(expectedH1, actualH1));
-  EXPECT(assert_equal(expectedH2, actualH2));
+  EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
+  EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
 }
 
 /* ************************************************************************* */
@@ -878,6 +901,105 @@ TEST(Pose3 , ChartDerivatives) {
   }
 }
 
+//******************************************************************************
+#include "testPoseAdjointMap.h"
+
+TEST(Pose3, TransformCovariance6MapTo2d) {
+  // Create 3d scenarios that map to 2d configurations and compare with Pose2 results.
+  using namespace test_pose_adjoint_map;
+
+  Vector3 s2{0.1, 0.3, 0.7};
+  Pose2 p2{1.1, 1.5, 31. * degree};
+  auto cov2 = FullCovarianceFromSigmas<Pose2>(s2);
+  auto transformed2 = TransformCovariance<Pose2>{p2}(cov2);
+
+  auto match_cov3_to_cov2 = [&](int spatial_axis0, int spatial_axis1, int r_axis,
+                                const Pose2::Jacobian &cov2, const Pose3::Jacobian &cov3) -> void
+  {
+    EXPECT(assert_equal(
+      Vector3{cov2.diagonal()},
+      Vector3{cov3(spatial_axis0, spatial_axis0), cov3(spatial_axis1, spatial_axis1), cov3(r_axis, r_axis)}));
+    EXPECT(assert_equal(
+      Vector3{cov2(1, 0), cov2(2, 0), cov2(2, 1)},
+      Vector3{cov3(spatial_axis1, spatial_axis0), cov3(r_axis, spatial_axis0), cov3(r_axis, spatial_axis1)}));
+  };
+
+  // rotate around x axis
+  {
+    auto cov3 = FullCovarianceFromSigmas<Pose3>((Vector6{} << s2(2), 0., 0., 0., s2(0), s2(1)).finished());
+    auto transformed3 = TransformCovariance<Pose3>{{Rot3::RzRyRx(p2.theta(), 0., 0.), {0., p2.x(), p2.y()}}}(cov3);
+    match_cov3_to_cov2(4, 5, 0, transformed2, transformed3);
+  }
+
+  // rotate around y axis
+  {
+    auto cov3 = FullCovarianceFromSigmas<Pose3>((Vector6{} << 0., s2(2), 0., s2(1), 0., s2(0)).finished());
+    auto transformed3 = TransformCovariance<Pose3>{{Rot3::RzRyRx(0., p2.theta(), 0.), {p2.y(), 0., p2.x()}}}(cov3);
+    match_cov3_to_cov2(5, 3, 1, transformed2, transformed3);
+  }
+
+  // rotate around z axis
+  {
+    auto cov3 = FullCovarianceFromSigmas<Pose3>((Vector6{} << 0., 0., s2(2), s2(0), s2(1), 0.).finished());
+    auto transformed3 = TransformCovariance<Pose3>{{Rot3::RzRyRx(0., 0., p2.theta()), {p2.x(), p2.y(), 0.}}}(cov3);
+    match_cov3_to_cov2(3, 4, 2, transformed2, transformed3);
+  }
+}
+
+/* ************************************************************************* */
+TEST(Pose3, TransformCovariance6) {
+  // Use simple covariance matrices and transforms to create tests that can be
+  // validated with simple computations.
+  using namespace test_pose_adjoint_map;
+
+  // rotate 90 around z axis and then 90 around y axis
+  {
+    auto cov = FullCovarianceFromSigmas<Pose3>((Vector6{} << 0.1, 0.2, 0.3, 0.5, 0.7, 1.1).finished());
+    auto transformed = TransformCovariance<Pose3>{{Rot3::RzRyRx(0., 90 * degree, 90 * degree), {0., 0., 0.}}}(cov);
+    // x from y, y from z, z from x
+    EXPECT(assert_equal(
+      (Vector6{} << cov(1, 1), cov(2, 2), cov(0, 0), cov(4, 4), cov(5, 5), cov(3, 3)).finished(),
+      Vector6{transformed.diagonal()}));
+    // Both the x and z axes are pointing in the negative direction.
+    EXPECT(assert_equal(
+      (Vector5{} << -cov(2, 1), cov(0, 1), cov(4, 1), -cov(5, 1), cov(3, 1)).finished(),
+      (Vector5{} << transformed(1, 0), transformed(2, 0), transformed(3, 0),
+        transformed(4, 0), transformed(5, 0)).finished()));
+  }
+
+  // translate along the x axis with uncertainty in roty and rotz
+  {
+    auto cov = TwoVariableCovarianceFromSigmas<Pose3>(1, 2, 0.7, 0.3);
+    auto transformed = TransformCovariance<Pose3>{{Rot3::RzRyRx(0., 0., 0.), {20., 0., 0.}}}(cov);
+    // The uncertainty in roty and rotz causes off-diagonal covariances
+    EXPECT(assert_equal(0.7 * 0.7 * 20., transformed(5, 1)));
+    EXPECT(assert_equal(0.7 * 0.7 * 20. * 20., transformed(5, 5)));
+    EXPECT(assert_equal(-0.3 * 0.3 * 20., transformed(4, 2)));
+    EXPECT(assert_equal(0.3 * 0.3 * 20. * 20., transformed(4, 4)));
+    EXPECT(assert_equal(-0.3 * 0.7 * 20., transformed(4, 1)));
+    EXPECT(assert_equal(0.3 * 0.7 * 20., transformed(5, 2)));
+    EXPECT(assert_equal(-0.3 * 0.7 * 20. * 20., transformed(5, 4)));
+  }
+
+  // rotate around x axis and translate along the x axis with uncertainty in rotx
+  {
+    auto cov = SingleVariableCovarianceFromSigma<Pose3>(0, 0.1);
+    auto transformed = TransformCovariance<Pose3>{{Rot3::RzRyRx(90 * degree, 0., 0.), {20., 0., 0.}}}(cov);
+    // No change
+    EXPECT(assert_equal(cov, transformed));
+  }
+
+  // rotate around x axis and translate along the x axis with uncertainty in roty
+  {
+    auto cov = SingleVariableCovarianceFromSigma<Pose3>(1, 0.1);
+    auto transformed = TransformCovariance<Pose3>{{Rot3::RzRyRx(90 * degree, 0., 0.), {20., 0., 0.}}}(cov);
+    // Uncertainty is spread to other dimensions.
+    EXPECT(assert_equal(
+      (Vector6{} << 0., 0., 0.1 * 0.1, 0., 0.1 * 0.1 * 20. * 20., 0.).finished(),
+      Vector6{transformed.diagonal()}));
+  }
+}
+
 /* ************************************************************************* */
 TEST(Pose3, interpolate) {
   EXPECT(assert_equal(T2, interpolate(T2,T3, 0.0)));
@@ -894,6 +1016,39 @@ TEST(Pose3, Create) {
   EXPECT(assert_equal(numericalDerivative22<Pose3,Rot3,Point3>(create, R, P2), actualH2, 1e-9));
 }
 
+/* ************************************************************************* */
+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);
+
+  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);
+
+#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
+  expected << "1\n"
+              "2\n"
+              "3;\n";
+#else
+  expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';";
+#endif
+
+  // reset cout to the original stream
+  std::cout.rdbuf(oldbuf);
+
+  // Get substring corresponding to translation part
+  std::string actual = redirectStream.str().substr(38, 11);
+
+  CHECK_EQUAL(expected.str(), actual);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;
diff --git a/gtsam/geometry/tests/testPoseAdjointMap.h b/gtsam/geometry/tests/testPoseAdjointMap.h
new file mode 100644
index 000000000..1d006c3d9
--- /dev/null
+++ b/gtsam/geometry/tests/testPoseAdjointMap.h
@@ -0,0 +1,59 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testPoseAdjointMap.h
+ * @brief   Support utilities for using AdjointMap for transforming Pose2 and Pose3 covariance matrices
+ * @author  Peter Mulllen
+ * @author  Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+
+namespace test_pose_adjoint_map
+{
+  const double degree = M_PI / 180;
+
+  // Return a covariance matrix for type T with non-zero values for every element.
+  // Use sigma_values^2 on the diagonal and fill in non-diagonal entries with
+  // correlation coefficient of 1. Note: a covariance matrix for T has the same
+  // dimensions as a Jacobian for T, the returned matrix is not a Jacobian.
+  template<class T>
+  typename T::Jacobian FullCovarianceFromSigmas(
+    const typename T::TangentVector &sigmas)
+  {
+    return sigmas * sigmas.transpose();
+  }
+
+  // Return a covariance matrix with one non-zero element on the diagonal.
+  template<class T>
+  typename T::Jacobian SingleVariableCovarianceFromSigma(int idx, double sigma)
+  {
+    typename T::Jacobian cov = T::Jacobian::Zero();
+    cov(idx, idx) = sigma * sigma;
+    return cov;
+  }
+
+  // Return a covariance matrix with two non-zero elements on the diagonal and
+  // a correlation of 1.0 between the two variables.
+  template<class T>
+  typename T::Jacobian TwoVariableCovarianceFromSigmas(int idx0, int idx1, double sigma0, double sigma1)
+  {
+    typename T::Jacobian cov = T::Jacobian::Zero();
+    cov(idx0, idx0) = sigma0 * sigma0;
+    cov(idx1, idx1) = sigma1 * sigma1;
+    cov(idx0, idx1) = cov(idx1, idx0) = sigma0 * sigma1;
+    return cov;
+  }
+}
\ No newline at end of file
diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index e468eaf55..598c57b24 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -14,6 +14,7 @@
  * @brief   Unit tests for Rot3 class - common between Matrix and Quaternion
  * @author  Alireza Fathi
  * @author  Frank Dellaert
+ * @author  Varun Agrawal
  */
 
 #include <gtsam/geometry/Point3.h>
@@ -115,6 +116,10 @@ TEST( Rot3, AxisAngle)
   CHECK(assert_equal(expected,actual,1e-5));
   Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*M_PI);
   CHECK(assert_equal(expected,actual2,1e-5));
+
+  axis = Vector3(0, 50, 0);
+  Rot3 actual3 = Rot3::AxisAngle(axis, angle);
+  CHECK(assert_equal(expected,actual3,1e-5));
 }
 
 /* ************************************************************************* */
@@ -176,63 +181,81 @@ TEST( Rot3, retract)
 }
 
 /* ************************************************************************* */
-TEST(Rot3, log)
-{
+TEST(Rot3, log) {
   static const double PI = boost::math::constants::pi<double>();
   Vector w;
   Rot3 R;
 
-#define CHECK_OMEGA(X,Y,Z) \
-  w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \
-  R = Rot3::Rodrigues(w); \
-  EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12));
+#define CHECK_OMEGA(X, Y, Z)             \
+  w = (Vector(3) << X, Y, Z).finished(); \
+  R = Rot3::Rodrigues(w);                \
+  EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12));
 
   // Check zero
-  CHECK_OMEGA(  0,   0,   0)
+  CHECK_OMEGA(0, 0, 0)
 
   // create a random direction:
-  double norm=sqrt(1.0+16.0+4.0);
-  double x=1.0/norm, y=4.0/norm, z=2.0/norm;
+  double norm = sqrt(1.0 + 16.0 + 4.0);
+  double x = 1.0 / norm, y = 4.0 / norm, z = 2.0 / norm;
 
   // Check very small rotation for Taylor expansion
   // Note that tolerance above is 1e-12, so Taylor is pretty good !
   double d = 0.0001;
-  CHECK_OMEGA(  d,   0,   0)
-  CHECK_OMEGA(  0,   d,   0)
-  CHECK_OMEGA(  0,   0,   d)
-  CHECK_OMEGA(x*d, y*d, z*d)
+  CHECK_OMEGA(d, 0, 0)
+  CHECK_OMEGA(0, d, 0)
+  CHECK_OMEGA(0, 0, d)
+  CHECK_OMEGA(x * d, y * d, z * d)
 
   // check normal rotation
   d = 0.1;
-  CHECK_OMEGA(  d,   0,   0)
-  CHECK_OMEGA(  0,   d,   0)
-  CHECK_OMEGA(  0,   0,   d)
-  CHECK_OMEGA(x*d, y*d, z*d)
+  CHECK_OMEGA(d, 0, 0)
+  CHECK_OMEGA(0, d, 0)
+  CHECK_OMEGA(0, 0, d)
+  CHECK_OMEGA(x * d, y * d, z * d)
 
   // Check 180 degree rotations
-  CHECK_OMEGA(  PI,   0,   0)
-  CHECK_OMEGA(   0,  PI,   0)
-  CHECK_OMEGA(   0,   0,  PI)
+  CHECK_OMEGA(PI, 0, 0)
+  CHECK_OMEGA(0, PI, 0)
+  CHECK_OMEGA(0, 0, PI)
 
   // Windows and Linux have flipped sign in quaternion mode
-#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS)
-  w = (Vector(3) << x*PI, y*PI, z*PI).finished();
+#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
+  w = (Vector(3) << x * PI, y * PI, z * PI).finished();
   R = Rot3::Rodrigues(w);
-  EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12));
+  EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12));
 #else
-  CHECK_OMEGA(x*PI,y*PI,z*PI)
+  CHECK_OMEGA(x * PI, y * PI, z * PI)
 #endif
 
   // Check 360 degree rotations
-#define CHECK_OMEGA_ZERO(X,Y,Z) \
-  w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \
-  R = Rot3::Rodrigues(w); \
-  EXPECT(assert_equal((Vector) Z_3x1, Rot3::Logmap(R)));
+#define CHECK_OMEGA_ZERO(X, Y, Z)        \
+  w = (Vector(3) << X, Y, Z).finished(); \
+  R = Rot3::Rodrigues(w);                \
+  EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R)));
 
-  CHECK_OMEGA_ZERO( 2.0*PI,      0,      0)
-  CHECK_OMEGA_ZERO(      0, 2.0*PI,      0)
-  CHECK_OMEGA_ZERO(      0,      0, 2.0*PI)
-  CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI)
+  CHECK_OMEGA_ZERO(2.0 * PI, 0, 0)
+  CHECK_OMEGA_ZERO(0, 2.0 * PI, 0)
+  CHECK_OMEGA_ZERO(0, 0, 2.0 * PI)
+  CHECK_OMEGA_ZERO(x * 2. * PI, y * 2. * PI, z * 2. * PI)
+
+  // Check problematic case from Lund dataset vercingetorix.g2o
+  // This is an almost rotation with determinant not *quite* 1.
+  Rot3 Rlund(-0.98582676, -0.03958746, -0.16303092,  //
+             -0.03997006, -0.88835923, 0.45740671,   //
+             -0.16293753, 0.45743998, 0.87418537);
+
+  // Rot3's Logmap returns different, but equivalent compacted
+  // axis-angle vectors depending on whether Rot3 is implemented
+  // by Quaternions or SO3.
+  #if defined(GTSAM_USE_QUATERNIONS)
+    // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees
+    EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211),
+                        (Vector)Rot3::Logmap(Rlund), 1e-8));
+  #else
+    // SO3 does not bound angle resulting in ~180.1 degrees
+    EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314),
+                        (Vector)Rot3::Logmap(Rlund), 1e-8));
+  #endif
 }
 
 /* ************************************************************************* */
@@ -376,7 +399,7 @@ TEST( Rot3, inverse )
   Rot3 actual = R.inverse(actualH);
   CHECK(assert_equal(I,R*actual));
   CHECK(assert_equal(I,actual*R));
-  CHECK(assert_equal((Matrix)actual.matrix(), R.transpose()));
+  CHECK(assert_equal(actual.matrix(), R.transpose()));
 
   Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
   CHECK(assert_equal(numericalH,actualH));
@@ -531,16 +554,15 @@ TEST( Rot3, logmapStability ) {
 TEST(Rot3, quaternion) {
   // NOTE: This is also verifying the ability to convert Vector to Quaternion
   Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
-  Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) <<
-      0.271018623057411,   0.278786459830371,   0.921318086098018,
-      0.578529366719085,   0.717799701969298,  -0.387385285854279,
-     -0.769319620053772,   0.637998195662053,   0.033250932803219).finished());
+  Rot3 R1(0.271018623057411, 0.278786459830371, 0.921318086098018,
+          0.578529366719085, 0.717799701969298, -0.387385285854279,
+          -0.769319620053772, 0.637998195662053, 0.033250932803219);
 
-  Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
-  Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) <<
-      -0.207341903877828,   0.250149415542075,   0.945745528564780,
-       0.881304914479026,  -0.371869043667957,   0.291573424846290,
-       0.424630407073532,   0.893945571198514,  -0.143353873763946).finished());
+  Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335,
+                0.599136268678053);
+  Rot3 R2(-0.207341903877828, 0.250149415542075, 0.945745528564780,
+          0.881304914479026, -0.371869043667957, 0.291573424846290,
+          0.424630407073532, 0.893945571198514, -0.143353873763946);
 
   // Check creating Rot3 from quaternion
   EXPECT(assert_equal(R1, Rot3(q1)));
@@ -645,6 +667,81 @@ TEST(Rot3 , ChartDerivatives) {
   }
 }
 
+/* ************************************************************************* */
+TEST(Rot3, ClosestTo) {
+  Matrix3 M;
+  M << 0.79067393, 0.6051136, -0.0930814,   //
+      0.4155925, -0.64214347, -0.64324489,  //
+      -0.44948549, 0.47046326, -0.75917576;
+
+  Matrix expected(3, 3);
+  expected << 0.790687, 0.605096, -0.0931312,  //
+      0.415746, -0.642355, -0.643844,          //
+      -0.449411, 0.47036, -0.759468;
+
+  auto actual = Rot3::ClosestTo(3*M);
+  EXPECT(assert_equal(expected, actual.matrix(), 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(Rot3, axisAngle) {
+  Unit3 actualAxis;
+  double actualAngle;
+
+// not a lambda as otherwise we can't trace error easily
+#define CHECK_AXIS_ANGLE(expectedAxis, expectedAngle, rotation) \
+  std::tie(actualAxis, actualAngle) = rotation.axisAngle();     \
+  EXPECT(assert_equal(expectedAxis, actualAxis, 1e-9));         \
+  EXPECT_DOUBLES_EQUAL(expectedAngle, actualAngle, 1e-9);       \
+  EXPECT(assert_equal(rotation, Rot3::AxisAngle(expectedAxis, expectedAngle)))
+
+  // CHECK R defined at top = Rot3::Rodrigues(0.1, 0.4, 0.2)
+  Vector3 omega(0.1, 0.4, 0.2);
+  Unit3 axis(omega), _axis(-omega);
+  CHECK_AXIS_ANGLE(axis, omega.norm(), R);
+
+  // rotate by 90
+  CHECK_AXIS_ANGLE(Unit3(1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, M_PI_2))
+  CHECK_AXIS_ANGLE(Unit3(0, 1, 0), M_PI_2, Rot3::Ypr(0, M_PI_2, 0))
+  CHECK_AXIS_ANGLE(Unit3(0, 0, 1), M_PI_2, Rot3::Ypr(M_PI_2, 0, 0))
+  CHECK_AXIS_ANGLE(axis, M_PI_2, Rot3::AxisAngle(axis, M_PI_2))
+
+  // rotate by -90
+  CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, -M_PI_2))
+  CHECK_AXIS_ANGLE(Unit3(0, -1, 0), M_PI_2, Rot3::Ypr(0, -M_PI_2, 0))
+  CHECK_AXIS_ANGLE(Unit3(0, 0, -1), M_PI_2, Rot3::Ypr(-M_PI_2, 0, 0))
+  CHECK_AXIS_ANGLE(_axis, M_PI_2, Rot3::AxisAngle(axis, -M_PI_2))
+
+  // rotate by 270
+  const double theta270 = M_PI + M_PI / 2;
+  CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, theta270))
+  CHECK_AXIS_ANGLE(Unit3(0, -1, 0), M_PI_2, Rot3::Ypr(0, theta270, 0))
+  CHECK_AXIS_ANGLE(Unit3(0, 0, -1), M_PI_2, Rot3::Ypr(theta270, 0, 0))
+  CHECK_AXIS_ANGLE(_axis, M_PI_2, Rot3::AxisAngle(axis, theta270))
+
+  // rotate by -270
+  const double theta_270 = -(M_PI + M_PI / 2);  // 90 (or -270) degrees
+  CHECK_AXIS_ANGLE(Unit3(1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, theta_270))
+  CHECK_AXIS_ANGLE(Unit3(0, 1, 0), M_PI_2, Rot3::Ypr(0, theta_270, 0))
+  CHECK_AXIS_ANGLE(Unit3(0, 0, 1), M_PI_2, Rot3::Ypr(theta_270, 0, 0))
+  CHECK_AXIS_ANGLE(axis, M_PI_2, Rot3::AxisAngle(axis, theta_270))
+
+  const double theta195 = 195 * M_PI / 180;
+  const double theta165 = 165 * M_PI / 180;
+
+  /// Non-trivial angle 165
+  CHECK_AXIS_ANGLE(Unit3(1, 0, 0), theta165, Rot3::Ypr(0, 0, theta165))
+  CHECK_AXIS_ANGLE(Unit3(0, 1, 0), theta165, Rot3::Ypr(0, theta165, 0))
+  CHECK_AXIS_ANGLE(Unit3(0, 0, 1), theta165, Rot3::Ypr(theta165, 0, 0))
+  CHECK_AXIS_ANGLE(axis, theta165, Rot3::AxisAngle(axis, theta165))
+
+  /// Non-trivial angle 195
+  CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), theta165, Rot3::Ypr(0, 0, theta195))
+  CHECK_AXIS_ANGLE(Unit3(0, -1, 0), theta165, Rot3::Ypr(0, theta195, 0))
+  CHECK_AXIS_ANGLE(Unit3(0, 0, -1), theta165, Rot3::Ypr(theta195, 0, 0))
+  CHECK_AXIS_ANGLE(_axis, theta165, Rot3::AxisAngle(axis, theta195))
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index 56751fa06..b2c781b35 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -10,19 +10,31 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file   testQuaternion.cpp
+ * @file   testSO3.cpp
  * @brief  Unit tests for SO3, as a GTSAM-adapted Lie Group
  * @author Frank Dellaert
  **/
 
 #include <gtsam/geometry/SO3.h>
-#include <gtsam/base/testLie.h>
+
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/testLie.h>
+
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
 using namespace gtsam;
 
+//******************************************************************************
+TEST(SO3, Identity) {
+  const SO3 R;
+  EXPECT_LONGS_EQUAL(3, R.rows());
+  EXPECT_LONGS_EQUAL(3, SO3::dimension);
+  EXPECT_LONGS_EQUAL(3, SO3::Dim());
+  EXPECT_LONGS_EQUAL(3, R.dim());
+  EXPECT_LONGS_EQUAL(3, traits<SO3>::GetDimension(R));
+}
+
 //******************************************************************************
 TEST(SO3, Concept) {
   BOOST_CONCEPT_ASSERT((IsGroup<SO3>));
@@ -31,14 +43,59 @@ TEST(SO3, Concept) {
 }
 
 //******************************************************************************
-TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); }
+TEST(SO3, Constructors) {
+  const Vector3 axis(0, 0, 1);
+  const double angle = 2.5;
+  SO3 Q(Eigen::AngleAxisd(angle, axis));
+  SO3 R = SO3::AxisAngle(axis, angle);
+  EXPECT(assert_equal(Q, R));
+}
+
+/* ************************************************************************* */
+TEST(SO3, ClosestTo) {
+  Matrix3 M;
+  M << 0.79067393, 0.6051136, -0.0930814,   //
+      0.4155925, -0.64214347, -0.64324489,  //
+      -0.44948549, 0.47046326, -0.75917576;
+
+  Matrix expected(3, 3);
+  expected << 0.790687, 0.605096, -0.0931312,  //
+      0.415746, -0.642355, -0.643844,          //
+      -0.449411, 0.47036, -0.759468;
+
+  auto actual = SO3::ClosestTo(3 * M);
+  EXPECT(assert_equal(expected, actual.matrix(), 1e-6));
+}
 
 //******************************************************************************
 SO3 id;
-Vector3 z_axis(0, 0, 1);
+Vector3 z_axis(0, 0, 1), v2(1, 2, 0), v3(1, 2, 3);
 SO3 R1(Eigen::AngleAxisd(0.1, z_axis));
 SO3 R2(Eigen::AngleAxisd(0.2, z_axis));
 
+/* ************************************************************************* */
+TEST(SO3, ChordalMean) {
+  std::vector<SO3> rotations = {R1, R1.inverse()};
+  EXPECT(assert_equal(SO3(), SO3::ChordalMean(rotations)));
+}
+
+//******************************************************************************
+TEST(SO3, Hat) {
+  // Check that Hat specialization is equal to dynamic version
+  EXPECT(assert_equal(SO3::Hat(z_axis), SOn::Hat(z_axis)));
+  EXPECT(assert_equal(SO3::Hat(v2), SOn::Hat(v2)));
+  EXPECT(assert_equal(SO3::Hat(v3), SOn::Hat(v3)));
+}
+
+//******************************************************************************
+TEST(SO3, Vee) {
+  // Check that Hat specialization is equal to dynamic version
+  auto X1 = SOn::Hat(z_axis), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
+  EXPECT(assert_equal(SO3::Vee(X1), SOn::Vee(X1)));
+  EXPECT(assert_equal(SO3::Vee(X2), SOn::Vee(X2)));
+  EXPECT(assert_equal(SO3::Vee(X3), SOn::Vee(X3)));
+}
+
 //******************************************************************************
 TEST(SO3, Local) {
   Vector3 expected(0, 0, 0.1);
@@ -50,7 +107,21 @@ TEST(SO3, Local) {
 TEST(SO3, Retract) {
   Vector3 v(0, 0, 0.1);
   SO3 actual = traits<SO3>::Retract(R1, v);
-  EXPECT(actual.isApprox(R2));
+  EXPECT(assert_equal(R2, actual));
+}
+
+//******************************************************************************
+TEST(SO3, Logmap) {
+  Vector3 expected(0, 0, 0.1);
+  Vector3 actual = SO3::Logmap(R1.between(R2));
+  EXPECT(assert_equal((Vector)expected, actual));
+}
+
+//******************************************************************************
+TEST(SO3, Expmap) {
+  Vector3 v(0, 0, 0.1);
+  SO3 actual = R1 * SO3::Expmap(v);
+  EXPECT(assert_equal(R2, actual));
 }
 
 //******************************************************************************
@@ -83,7 +154,7 @@ TEST(SO3, ChartDerivatives) {
 }
 
 /* ************************************************************************* */
-TEST(SO3, Expmap) {
+TEST(SO3, ExpmapFunctor) {
   Vector axis = Vector3(0., 1., 0.);  // rotation around Y
   double angle = 3.14 / 4.0;
   Matrix expected(3,3);
@@ -134,7 +205,7 @@ TEST(SO3, ExpmapDerivative) {
   EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, ExpmapDerivative2) {
   const Vector3 theta(0.1, 0, 0.1);
   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
@@ -145,7 +216,7 @@ TEST(SO3, ExpmapDerivative2) {
                      SO3::ExpmapDerivative(-theta)));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, ExpmapDerivative3) {
   const Vector3 theta(10, 20, 30);
   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
@@ -156,7 +227,7 @@ TEST(SO3, ExpmapDerivative3) {
                      SO3::ExpmapDerivative(-theta)));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, ExpmapDerivative4) {
   // Iserles05an (Lie-group Methods) says:
   // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
@@ -184,7 +255,7 @@ TEST(SO3, ExpmapDerivative4) {
   }
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, ExpmapDerivative5) {
   auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
   auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
@@ -200,7 +271,7 @@ TEST(SO3, ExpmapDerivative5) {
   }
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, ExpmapDerivative6) {
   const Vector3 thetahat(0.1, 0, 0.1);
   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
@@ -210,7 +281,7 @@ TEST(SO3, ExpmapDerivative6) {
   EXPECT(assert_equal(Jexpected, Jactual));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, LogmapDerivative) {
   const Vector3 thetahat(0.1, 0, 0.1);
   const SO3 R = SO3::Expmap(thetahat);  // some rotation
@@ -220,7 +291,7 @@ TEST(SO3, LogmapDerivative) {
   EXPECT(assert_equal(Jexpected, Jactual));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, JacobianLogmap) {
   const Vector3 thetahat(0.1, 0, 0.1);
   const SO3 R = SO3::Expmap(thetahat);  // some rotation
@@ -231,7 +302,7 @@ TEST(SO3, JacobianLogmap) {
   EXPECT(assert_equal(Jexpected, Jactual));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, ApplyDexp) {
   Matrix aH1, aH2;
   for (bool nearZeroApprox : {true, false}) {
@@ -254,7 +325,7 @@ TEST(SO3, ApplyDexp) {
   }
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, ApplyInvDexp) {
   Matrix aH1, aH2;
   for (bool nearZeroApprox : {true, false}) {
@@ -278,6 +349,33 @@ TEST(SO3, ApplyInvDexp) {
   }
 }
 
+//******************************************************************************
+TEST(SO3, vec) {
+  const Vector9 expected = Eigen::Map<const Vector9>(R2.matrix().data());
+  Matrix actualH;
+  const Vector9 actual = R2.vec(actualH);
+  CHECK(assert_equal(expected, actual));
+  boost::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
+  const Matrix numericalH = numericalDerivative11(f, R2, 1e-5);
+  CHECK(assert_equal(numericalH, actualH));
+}
+
+//******************************************************************************
+TEST(Matrix, compose) {
+  Matrix3 M;
+  M << 1, 2, 3, 4, 5, 6, 7, 8, 9;
+  SO3 R = SO3::Expmap(Vector3(1, 2, 3));
+  const Matrix3 expected = M * R.matrix();
+  Matrix actualH;
+  const Matrix3 actual = so3::compose(M, R, actualH);
+  CHECK(assert_equal(expected, actual));
+  boost::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
+    return so3::compose(M, R);
+  };
+  Matrix numericalH = numericalDerivative11(f, M, 1e-2);
+  CHECK(assert_equal(numericalH, actualH));
+}
+
 //******************************************************************************
 int main() {
   TestResult tr;
diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp
new file mode 100644
index 000000000..f771eea5f
--- /dev/null
+++ b/gtsam/geometry/tests/testSO4.cpp
@@ -0,0 +1,207 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testSO4.cpp
+ * @brief  Unit tests for SO4, as a GTSAM-adapted Lie Group
+ * @author Frank Dellaert
+ **/
+
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/lieProxies.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/SO4.h>
+#include <gtsam/geometry/SO3.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <iostream>
+#include <random>
+
+using namespace std;
+using namespace gtsam;
+
+//******************************************************************************
+TEST(SO4, Identity) {
+  const SO4 R;
+  EXPECT_LONGS_EQUAL(4, R.rows());
+  EXPECT_LONGS_EQUAL(6, SO4::dimension);
+  EXPECT_LONGS_EQUAL(6, SO4::Dim());
+  EXPECT_LONGS_EQUAL(6, R.dim());
+  EXPECT_LONGS_EQUAL(6, traits<SO4>::GetDimension(R));
+}
+
+//******************************************************************************
+TEST(SO4, Concept) {
+  BOOST_CONCEPT_ASSERT((IsGroup<SO4>));
+  BOOST_CONCEPT_ASSERT((IsManifold<SO4>));
+  BOOST_CONCEPT_ASSERT((IsLieGroup<SO4>));
+}
+
+//******************************************************************************
+SO4 id;
+Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
+SO4 Q1 = SO4::Expmap(v1);
+Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished();
+SO4 Q2 = SO4::Expmap(v2);
+Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished();
+SO4 Q3 = SO4::Expmap(v3);
+
+//******************************************************************************
+TEST(SO4, Random) {
+  std::mt19937 rng(42);
+  auto Q = SO4::Random(rng);
+  EXPECT_LONGS_EQUAL(4, Q.matrix().rows());
+}
+//******************************************************************************
+TEST(SO4, Expmap) {
+  // If we do exponential map in SO(3) subgroup, topleft should be equal to R1.
+  auto R1 = SO3::Expmap(v1.tail<3>()).matrix();
+  EXPECT((Q1.matrix().topLeftCorner<3, 3>().isApprox(R1)));
+
+  // Same here
+  auto R2 = SO3::Expmap(v2.tail<3>()).matrix();
+  EXPECT((Q2.matrix().topLeftCorner<3, 3>().isApprox(R2)));
+
+  // Check commutative subgroups
+  for (size_t i = 0; i < 6; i++) {
+    Vector6 xi = Vector6::Zero();
+    xi[i] = 2;
+    SO4 Q1 = SO4::Expmap(xi);
+    xi[i] = 3;
+    SO4 Q2 = SO4::Expmap(xi);
+    EXPECT(assert_equal(Q1 * Q2, Q2 * Q1));
+  }
+}
+
+//******************************************************************************
+TEST(SO4, Hat) {
+  // Check that Hat specialization is equal to dynamic version
+  EXPECT(assert_equal(SO4::Hat(v1), SOn::Hat(v1)));
+  EXPECT(assert_equal(SO4::Hat(v2), SOn::Hat(v2)));
+  EXPECT(assert_equal(SO4::Hat(v3), SOn::Hat(v3)));
+}
+
+//******************************************************************************
+TEST(SO4, Vee) {
+  // Check that Hat specialization is equal to dynamic version
+  auto X1 = SOn::Hat(v1), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
+  EXPECT(assert_equal(SO4::Vee(X1), SOn::Vee(X1)));
+  EXPECT(assert_equal(SO4::Vee(X2), SOn::Vee(X2)));
+  EXPECT(assert_equal(SO4::Vee(X3), SOn::Vee(X3)));
+}
+
+//******************************************************************************
+TEST(SO4, Retract) {
+  // Check that Cayley yields the same as Expmap for small values
+  EXPECT(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100)));
+  EXPECT(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100)));
+
+  // Check that Cayley is identical to dynamic version
+  EXPECT(assert_equal(SOn(id.retract(v1 / 100)), SOn(4).retract(v1 / 100)));
+  EXPECT(assert_equal(SOn(id.retract(v2 / 100)), SOn(4).retract(v2 / 100)));
+
+  auto v = Vector6::Zero();
+  SO4 actual = traits<SO4>::Retract(id, v);
+  EXPECT(assert_equal(id, actual));
+}
+
+//******************************************************************************
+TEST(SO4, Local) {
+  // Check that Cayley is identical to dynamic version
+  EXPECT(
+      assert_equal(id.localCoordinates(Q1), SOn(4).localCoordinates(SOn(Q1))));
+  EXPECT(
+      assert_equal(id.localCoordinates(Q2), SOn(4).localCoordinates(SOn(Q2))));
+
+  auto v0 = Vector6::Zero();
+  Vector6 actual = traits<SO4>::Local(id, id);
+  EXPECT(assert_equal((Vector)v0, actual));
+}
+
+//******************************************************************************
+TEST(SO4, Invariants) {
+  EXPECT(check_group_invariants(id, id));
+  EXPECT(check_group_invariants(id, Q1));
+  EXPECT(check_group_invariants(Q2, id));
+  EXPECT(check_group_invariants(Q2, Q1));
+  EXPECT(check_group_invariants(Q1, Q2));
+
+  EXPECT(check_manifold_invariants(id, id));
+  EXPECT(check_manifold_invariants(id, Q1));
+  EXPECT(check_manifold_invariants(Q2, id));
+  EXPECT(check_manifold_invariants(Q2, Q1));
+  EXPECT(check_manifold_invariants(Q1, Q2));
+}
+
+//******************************************************************************
+TEST(SO4, compose) {
+  SO4 expected = Q1 * Q2;
+  Matrix actualH1, actualH2;
+  SO4 actual = Q1.compose(Q2, actualH1, actualH2);
+  EXPECT(assert_equal(expected, actual));
+
+  Matrix numericalH1 =
+      numericalDerivative21(testing::compose<SO4>, Q1, Q2, 1e-2);
+  EXPECT(assert_equal(numericalH1, actualH1));
+
+  Matrix numericalH2 =
+      numericalDerivative22(testing::compose<SO4>, Q1, Q2, 1e-2);
+  EXPECT(assert_equal(numericalH2, actualH2));
+}
+
+//******************************************************************************
+TEST(SO4, vec) {
+  using Vector16 = SO4::VectorN2;
+  const Vector16 expected = Eigen::Map<const Vector16>(Q2.matrix().data());
+  Matrix actualH;
+  const Vector16 actual = Q2.vec(actualH);
+  EXPECT(assert_equal(expected, actual));
+  boost::function<Vector16(const SO4&)> f = [](const SO4& Q) {
+    return Q.vec();
+  };
+  const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5);
+  EXPECT(assert_equal(numericalH, actualH));
+}
+
+//******************************************************************************
+TEST(SO4, topLeft) {
+  const Matrix3 expected = Q3.matrix().topLeftCorner<3, 3>();
+  Matrix actualH;
+  const Matrix3 actual = topLeft(Q3, actualH);
+  EXPECT(assert_equal(expected, actual));
+  boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
+    return topLeft(Q3);
+  };
+  const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
+  EXPECT(assert_equal(numericalH, actualH));
+}
+
+//******************************************************************************
+TEST(SO4, stiefel) {
+  const Matrix43 expected = Q3.matrix().leftCols<3>();
+  Matrix actualH;
+  const Matrix43 actual = stiefel(Q3, actualH);
+  EXPECT(assert_equal(expected, actual));
+  boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
+    return stiefel(Q3);
+  };
+  const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
+  EXPECT(assert_equal(numericalH, actualH));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************
diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp
new file mode 100644
index 000000000..1cf8caed2
--- /dev/null
+++ b/gtsam/geometry/tests/testSOn.cpp
@@ -0,0 +1,179 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testSOn.cpp
+ * @brief  Unit tests for dynamic SO(n) classes.
+ * @author Frank Dellaert
+ **/
+
+#include <gtsam/geometry/SO3.h>
+#include <gtsam/geometry/SO4.h>
+#include <gtsam/geometry/SOn.h>
+
+#include <gtsam/base/Lie.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/lieProxies.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/testLie.h>
+#include <gtsam/nonlinear/Values.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <iostream>
+#include <random>
+#include <stdexcept>
+#include <type_traits>
+
+using namespace std;
+using namespace gtsam;
+
+//******************************************************************************
+// Test dhynamic with n=0
+TEST(SOn, SO0) {
+  const auto R = SOn(0);
+  EXPECT_LONGS_EQUAL(0, R.rows());
+  EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
+  EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim());
+  EXPECT_LONGS_EQUAL(0, R.dim());
+  EXPECT_LONGS_EQUAL(0, traits<SOn>::GetDimension(R));
+}
+
+//******************************************************************************
+TEST(SOn, SO5) {
+  const auto R = SOn(5);
+  EXPECT_LONGS_EQUAL(5, R.rows());
+  EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
+  EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim());
+  EXPECT_LONGS_EQUAL(10, R.dim());
+  EXPECT_LONGS_EQUAL(10, traits<SOn>::GetDimension(R));
+}
+
+//******************************************************************************
+TEST(SOn, Concept) {
+  BOOST_CONCEPT_ASSERT((IsGroup<SOn>));
+  BOOST_CONCEPT_ASSERT((IsManifold<SOn>));
+  BOOST_CONCEPT_ASSERT((IsLieGroup<SOn>));
+}
+
+//******************************************************************************
+TEST(SOn, CopyConstructor) {
+  const auto R = SOn(5);
+  const auto B(R);
+  EXPECT_LONGS_EQUAL(5, B.rows());
+  EXPECT_LONGS_EQUAL(10, B.dim());
+}
+
+//******************************************************************************
+TEST(SOn, Values) {
+  const auto R = SOn(5);
+  Values values;
+  const Key key(0);
+  values.insert(key, R);
+  const auto B = values.at<SOn>(key);
+  EXPECT_LONGS_EQUAL(5, B.rows());
+  EXPECT_LONGS_EQUAL(10, B.dim());
+}
+
+//******************************************************************************
+TEST(SOn, Random) {
+  static std::mt19937 rng(42);
+  EXPECT_LONGS_EQUAL(3, SOn::Random(rng, 3).rows());
+  EXPECT_LONGS_EQUAL(4, SOn::Random(rng, 4).rows());
+  EXPECT_LONGS_EQUAL(5, SOn::Random(rng, 5).rows());
+}
+
+//******************************************************************************
+TEST(SOn, HatVee) {
+  Vector10 v;
+  v << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
+
+  Matrix expected2(2, 2);
+  expected2 << 0, -1, 1, 0;
+  const auto actual2 = SOn::Hat(v.head<1>());
+  EXPECT(assert_equal(expected2, actual2));
+  EXPECT(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2)));
+
+  Matrix expected3(3, 3);
+  expected3 << 0, -3,  2, //
+               3,  0, -1, //
+              -2,  1,  0;
+  const auto actual3 = SOn::Hat(v.head<3>());
+  EXPECT(assert_equal(expected3, actual3));
+  EXPECT(assert_equal(skewSymmetric(1, 2, 3), actual3));
+  EXPECT(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3)));
+
+  Matrix expected4(4, 4);
+  expected4 << 0, -6,  5,  3, //
+               6,  0, -4, -2, //
+              -5,  4,  0,  1, //
+              -3,  2, -1,  0;
+  const auto actual4 = SOn::Hat(v.head<6>());
+  EXPECT(assert_equal(expected4, actual4));
+  EXPECT(assert_equal((Vector)v.head<6>(), SOn::Vee(actual4)));
+
+  Matrix expected5(5, 5);
+  expected5 << 0,-10,  9,  7, -4, //
+              10,  0, -8, -6,  3, //
+              -9,  8,  0,  5, -2, //
+              -7,  6, -5,  0,  1, //
+               4, -3,  2, -1,  0;
+  const auto actual5 = SOn::Hat(v);
+  EXPECT(assert_equal(expected5, actual5));
+  EXPECT(assert_equal((Vector)v, SOn::Vee(actual5)));
+}
+
+//******************************************************************************
+TEST(SOn, RetractLocal) {
+  Vector6 v1 = (Vector(6) << 0, 0, 0, 1, 0, 0).finished() / 10000;
+  Vector6 v2 = (Vector(6) << 0, 0, 0, 1, 2, 3).finished() / 10000;
+  Vector6 v3 = (Vector(6) << 3, 2, 1, 1, 2, 3).finished() / 10000;
+
+  // Check that Cayley yields the same as Expmap for small values
+  SOn id(4);
+  EXPECT(assert_equal(id.retract(v1), SOn(SO4::Expmap(v1))));
+  EXPECT(assert_equal(id.retract(v2), SOn(SO4::Expmap(v2))));
+  EXPECT(assert_equal(id.retract(v3), SOn(SO4::Expmap(v3))));
+
+  // Same for SO3:
+  SOn I3(3);
+  EXPECT(
+      assert_equal(I3.retract(v1.tail<3>()), SOn(SO3::Expmap(v1.tail<3>()))));
+  EXPECT(
+      assert_equal(I3.retract(v2.tail<3>()), SOn(SO3::Expmap(v2.tail<3>()))));
+
+  // If we do expmap in SO(3) subgroup, topleft should be equal to R1.
+  Matrix R1 = SO3().retract(v1.tail<3>()).matrix();
+  SOn Q1 = SOn::Retract(v1);
+  CHECK(assert_equal(R1, Q1.matrix().block(0, 0, 3, 3), 1e-7));
+  CHECK(assert_equal(v1, SOn::ChartAtOrigin::Local(Q1), 1e-7));
+}
+
+//******************************************************************************
+TEST(SOn, vec) {
+  Vector10 v;
+  v << 0, 0, 0, 0, 1, 2, 3, 4, 5, 6;
+  SOn Q = SOn::ChartAtOrigin::Retract(v);
+  Matrix actualH;
+  const Vector actual = Q.vec(actualH);
+  boost::function<Vector(const SOn&)> h = [](const SOn& Q) { return Q.vec(); };
+  const Matrix H = numericalDerivative11<Vector, SOn, 10>(h, Q, 1e-5);
+  CHECK(assert_equal(H, actualH));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************
diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp
index 97412f94d..4f71a48da 100644
--- a/gtsam/geometry/tests/testTriangulation.cpp
+++ b/gtsam/geometry/tests/testTriangulation.cpp
@@ -17,7 +17,7 @@
  */
 
 #include <gtsam/geometry/triangulation.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/StereoCamera.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/Cal3Bundler.h>
@@ -151,7 +151,7 @@ TEST( triangulation, fourPoses) {
 
   // 3. Add a slightly rotated third camera above, again with measurement noise
   Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
-  SimpleCamera camera3(pose3, *sharedCal);
+  PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
   Point2 z3 = camera3.project(landmark);
 
   poses += pose3;
@@ -168,7 +168,7 @@ TEST( triangulation, fourPoses) {
 
   // 4. Test failure: Add a 4th camera facing the wrong way
   Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
-  SimpleCamera camera4(pose4, *sharedCal);
+  PinholeCamera<Cal3_S2> camera4(pose4, *sharedCal);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
@@ -185,17 +185,17 @@ TEST( triangulation, fourPoses) {
 TEST( triangulation, fourPoses_distinct_Ks) {
   Cal3_S2 K1(1500, 1200, 0, 640, 480);
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  SimpleCamera camera1(pose1, K1);
+  PinholeCamera<Cal3_S2> camera1(pose1, K1);
 
   // create second camera 1 meter to the right of first camera
   Cal3_S2 K2(1600, 1300, 0, 650, 440);
-  SimpleCamera camera2(pose2, K2);
+  PinholeCamera<Cal3_S2> camera2(pose2, K2);
 
   // 1. Project two landmarks into two cameras and triangulate
   Point2 z1 = camera1.project(landmark);
   Point2 z2 = camera2.project(landmark);
 
-  CameraSet<SimpleCamera> cameras;
+  CameraSet<PinholeCamera<Cal3_S2> > cameras;
   Point2Vector measurements;
 
   cameras += camera1, camera2;
@@ -216,7 +216,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
   // 3. Add a slightly rotated third camera above, again with measurement noise
   Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
   Cal3_S2 K3(700, 500, 0, 640, 480);
-  SimpleCamera camera3(pose3, K3);
+  PinholeCamera<Cal3_S2> camera3(pose3, K3);
   Point2 z3 = camera3.project(landmark);
 
   cameras += camera3;
@@ -234,7 +234,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
   // 4. Test failure: Add a 4th camera facing the wrong way
   Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   Cal3_S2 K4(700, 500, 0, 640, 480);
-  SimpleCamera camera4(pose4, K4);
+  PinholeCamera<Cal3_S2> camera4(pose4, K4);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
@@ -250,17 +250,17 @@ TEST( triangulation, fourPoses_distinct_Ks) {
 TEST( triangulation, outliersAndFarLandmarks) {
   Cal3_S2 K1(1500, 1200, 0, 640, 480);
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  SimpleCamera camera1(pose1, K1);
+  PinholeCamera<Cal3_S2> camera1(pose1, K1);
 
   // create second camera 1 meter to the right of first camera
   Cal3_S2 K2(1600, 1300, 0, 650, 440);
-  SimpleCamera camera2(pose2, K2);
+  PinholeCamera<Cal3_S2> camera2(pose2, K2);
 
   // 1. Project two landmarks into two cameras and triangulate
   Point2 z1 = camera1.project(landmark);
   Point2 z2 = camera2.project(landmark);
 
-  CameraSet<SimpleCamera> cameras;
+  CameraSet<PinholeCamera<Cal3_S2> > cameras;
   Point2Vector measurements;
 
   cameras += camera1, camera2;
@@ -280,7 +280,7 @@ TEST( triangulation, outliersAndFarLandmarks) {
   // 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER)
   Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
   Cal3_S2 K3(700, 500, 0, 640, 480);
-  SimpleCamera camera3(pose3, K3);
+  PinholeCamera<Cal3_S2> camera3(pose3, K3);
   Point2 z3 = camera3.project(landmark);
 
   cameras += camera3;
@@ -302,7 +302,7 @@ TEST( triangulation, outliersAndFarLandmarks) {
 //******************************************************************************
 TEST( triangulation, twoIdenticalPoses) {
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  SimpleCamera camera1(pose1, *sharedCal);
+  PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
 
   // 1. Project two landmarks into two cameras and triangulate
   Point2 z1 = camera1.project(landmark);
@@ -382,8 +382,8 @@ TEST( triangulation, StereotriangulateNonlinear ) {
     graph.emplace_shared<StereoFactor>(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK);
 
     const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9);
-    graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x',1), Pose3(m1), posePrior);
-    graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x',2), Pose3(m2), posePrior);
+    graph.addPrior(Symbol('x',1), Pose3(m1), posePrior);
+    graph.addPrior(Symbol('x',2), Pose3(m2), posePrior);
 
     LevenbergMarquardtOptimizer optimizer(graph, values);
     Values result = optimizer.optimize();
diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp
index 9d53a30a6..ddf60a256 100644
--- a/gtsam/geometry/tests/testUnit3.cpp
+++ b/gtsam/geometry/tests/testUnit3.cpp
@@ -27,15 +27,15 @@
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/slam/PriorFactor.h>
 
 
 #include <CppUnitLite/TestHarness.h>
 
 #include <boost/bind.hpp>
-#include <boost/random.hpp>
 #include <boost/assign/std/vector.hpp>
+
 #include <cmath>
+#include <random>
 
 using namespace boost::assign;
 using namespace gtsam;
@@ -58,8 +58,8 @@ TEST(Unit3, point3) {
   for(Point3 p: ps) {
     Unit3 s(p);
     expectedH = numericalDerivative11<Point3, Unit3>(point3_, s);
-    EXPECT(assert_equal(p, s.point3(actualH), 1e-8));
-    EXPECT(assert_equal(expectedH, actualH, 1e-9));
+    EXPECT(assert_equal(p, s.point3(actualH), 1e-5));
+    EXPECT(assert_equal(expectedH, actualH, 1e-5));
   }
 }
 
@@ -73,18 +73,18 @@ TEST(Unit3, rotate) {
   Unit3 p(1, 0, 0);
   Unit3 expected = Unit3(R.column(1));
   Unit3 actual = R * p;
-  EXPECT(assert_equal(expected, actual, 1e-8));
+  EXPECT(assert_equal(expected, actual, 1e-5));
   Matrix actualH, expectedH;
   // Use numerical derivatives to calculate the expected Jacobian
   {
     expectedH = numericalDerivative21(rotate_, R, p);
     R.rotate(p, actualH, boost::none);
-    EXPECT(assert_equal(expectedH, actualH, 1e-9));
+    EXPECT(assert_equal(expectedH, actualH, 1e-5));
   }
   {
     expectedH = numericalDerivative22(rotate_, R, p);
     R.rotate(p, boost::none, actualH);
-    EXPECT(assert_equal(expectedH, actualH, 1e-9));
+    EXPECT(assert_equal(expectedH, actualH, 1e-5));
   }
 }
 
@@ -98,19 +98,19 @@ TEST(Unit3, unrotate) {
   Unit3 p(1, 0, 0);
   Unit3 expected = Unit3(1, 1, 0);
   Unit3 actual = R.unrotate(p);
-  EXPECT(assert_equal(expected, actual, 1e-8));
+  EXPECT(assert_equal(expected, actual, 1e-5));
 
   Matrix actualH, expectedH;
   // Use numerical derivatives to calculate the expected Jacobian
   {
     expectedH = numericalDerivative21(unrotate_, R, p);
     R.unrotate(p, actualH, boost::none);
-    EXPECT(assert_equal(expectedH, actualH, 1e-9));
+    EXPECT(assert_equal(expectedH, actualH, 1e-5));
   }
   {
     expectedH = numericalDerivative22(unrotate_, R, p);
     R.unrotate(p, boost::none, actualH);
-    EXPECT(assert_equal(expectedH, actualH, 1e-9));
+    EXPECT(assert_equal(expectedH, actualH, 1e-5));
   }
 }
 
@@ -119,7 +119,7 @@ TEST(Unit3, dot) {
   Unit3 q = p.retract(Vector2(0.5, 0));
   Unit3 r = p.retract(Vector2(0.8, 0));
   Unit3 t = p.retract(Vector2(0, 0.3));
-  EXPECT(assert_equal(1.0, p.dot(p), 1e-8));
+  EXPECT(assert_equal(1.0, p.dot(p), 1e-5));
   EXPECT(assert_equal(0.877583, p.dot(q), 1e-5));
   EXPECT(assert_equal(0.696707, p.dot(r), 1e-5));
   EXPECT(assert_equal(0.955336, p.dot(t), 1e-5));
@@ -130,18 +130,18 @@ TEST(Unit3, dot) {
                                                                       boost::none, boost::none);
   {
     p.dot(q, H1, H2);
-    EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-9));
-    EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, q), H2, 1e-9));
+    EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-5));
+    EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, q), H2, 1e-5));
   }
   {
     p.dot(r, H1, H2);
-    EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, r), H1, 1e-9));
-    EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, r), H2, 1e-9));
+    EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, r), H1, 1e-5));
+    EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, r), H2, 1e-5));
   }
   {
     p.dot(t, H1, H2);
-    EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, t), H1, 1e-9));
-    EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, t), H2, 1e-9));
+    EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, t), H1, 1e-5));
+    EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, t), H2, 1e-5));
   }
 }
 
@@ -149,7 +149,7 @@ TEST(Unit3, dot) {
 TEST(Unit3, error) {
   Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
   r = p.retract(Vector2(0.8, 0));
-  EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-8));
+  EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-5));
   EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5));
   EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));
 
@@ -159,13 +159,13 @@ TEST(Unit3, error) {
     expected = numericalDerivative11<Vector2,Unit3>(
         boost::bind(&Unit3::error, &p, _1, boost::none), q);
     p.error(q, actual);
-    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
+    EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
   }
   {
     expected = numericalDerivative11<Vector2,Unit3>(
         boost::bind(&Unit3::error, &p, _1, boost::none), r);
     p.error(r, actual);
-    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
+    EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
   }
 }
 
@@ -176,7 +176,7 @@ TEST(Unit3, error2) {
   Unit3 r = p.retract(Vector2(0.8, 0));
 
   // Hard-coded as simple regression values
-  EXPECT(assert_equal((Vector)(Vector2(0.0, 0.0)), p.errorVector(p), 1e-8));
+  EXPECT(assert_equal((Vector)(Vector2(0.0, 0.0)), p.errorVector(p), 1e-5));
   EXPECT(assert_equal((Vector)(Vector2(0.198337495, -0.0991687475)), p.errorVector(q), 1e-5));
   EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.errorVector(r), 1e-5));
 
@@ -186,25 +186,25 @@ TEST(Unit3, error2) {
     expected = numericalDerivative21<Vector2, Unit3, Unit3>(
         boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
     p.errorVector(q, actual, boost::none);
-    EXPECT(assert_equal(expected, actual, 1e-9));
+    EXPECT(assert_equal(expected, actual, 1e-5));
   }
   {
     expected = numericalDerivative21<Vector2, Unit3, Unit3>(
         boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
     p.errorVector(r, actual, boost::none);
-    EXPECT(assert_equal(expected, actual, 1e-9));
+    EXPECT(assert_equal(expected, actual, 1e-5));
   }
   {
     expected = numericalDerivative22<Vector2, Unit3, Unit3>(
         boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
     p.errorVector(q, boost::none, actual);
-    EXPECT(assert_equal(expected, actual, 1e-9));
+    EXPECT(assert_equal(expected, actual, 1e-5));
   }
   {
     expected = numericalDerivative22<Vector2, Unit3, Unit3>(
         boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
     p.errorVector(r, boost::none, actual);
-    EXPECT(assert_equal(expected, actual, 1e-9));
+    EXPECT(assert_equal(expected, actual, 1e-5));
   }
 }
 
@@ -212,9 +212,9 @@ TEST(Unit3, error2) {
 TEST(Unit3, distance) {
   Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
   r = p.retract(Vector2(0.8, 0));
-  EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
-  EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-8);
-  EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-8);
+  EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-5);
+  EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-5);
+  EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-5);
 
   Matrix actual, expected;
   // Use numerical derivatives to calculate the expected Jacobian
@@ -222,13 +222,13 @@ TEST(Unit3, distance) {
     expected = numericalGradient<Unit3>(
         boost::bind(&Unit3::distance, &p, _1, boost::none), q);
     p.distance(q, actual);
-    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
+    EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
   }
   {
     expected = numericalGradient<Unit3>(
         boost::bind(&Unit3::distance, &p, _1, boost::none), r);
     p.distance(r, actual);
-    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
+    EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
   }
 }
 
@@ -236,7 +236,7 @@ TEST(Unit3, distance) {
 TEST(Unit3, localCoordinates0) {
   Unit3 p;
   Vector actual = p.localCoordinates(p);
-  EXPECT(assert_equal(Z_2x1, actual, 1e-8));
+  EXPECT(assert_equal(Z_2x1, actual, 1e-5));
 }
 
 TEST(Unit3, localCoordinates) {
@@ -244,46 +244,46 @@ TEST(Unit3, localCoordinates) {
     Unit3 p, q;
     Vector2 expected = Vector2::Zero();
     Vector2 actual = p.localCoordinates(q);
-    EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8));
-    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
+    EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-5));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-5));
   }
   {
     Unit3 p, q(1, 6.12385e-21, 0);
     Vector2 expected = Vector2::Zero();
     Vector2 actual = p.localCoordinates(q);
-    EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8));
-    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
+    EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-5));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-5));
   }
   {
     Unit3 p, q(-1, 0, 0);
     Vector2 expected(M_PI, 0);
     Vector2 actual = p.localCoordinates(q);
-    EXPECT(assert_equal(expected, actual, 1e-8));
-    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
+    EXPECT(assert_equal(expected, actual, 1e-5));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-5));
   }
   {
     Unit3 p, q(0, 1, 0);
     Vector2 expected(0,-M_PI_2);
     Vector2 actual = p.localCoordinates(q);
-    EXPECT(assert_equal(expected, actual, 1e-8));
-    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
+    EXPECT(assert_equal(expected, actual, 1e-5));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-5));
   }
   {
     Unit3 p, q(0, -1, 0);
     Vector2 expected(0, M_PI_2);
     Vector2 actual = p.localCoordinates(q);
-    EXPECT(assert_equal(expected, actual, 1e-8));
-    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
+    EXPECT(assert_equal(expected, actual, 1e-5));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-5));
   }
   {
     Unit3 p(0,1,0), q(0,-1,0);
     Vector2 actual = p.localCoordinates(q);
-    EXPECT(assert_equal(q, p.retract(actual), 1e-8));
+    EXPECT(assert_equal(q, p.retract(actual), 1e-5));
   }
   {
     Unit3 p(0,0,1), q(0,0,-1);
     Vector2 actual = p.localCoordinates(q);
-    EXPECT(assert_equal(q, p.retract(actual), 1e-8));
+    EXPECT(assert_equal(q, p.retract(actual), 1e-5));
   }
 
   double twist = 1e-4;
@@ -328,18 +328,18 @@ TEST(Unit3, basis) {
 
   // with H, first time
   EXPECT(assert_equal(expected, p.basis(actualH), 1e-6));
-  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+  EXPECT(assert_equal(expectedH, actualH, 1e-5));
 
   // with H, cached
   EXPECT(assert_equal(expected, p.basis(actualH), 1e-6));
-  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+  EXPECT(assert_equal(expectedH, actualH, 1e-5));
 }
 
 //*******************************************************************************
 /// Check the basis derivatives of a bunch of random Unit3s.
 TEST(Unit3, basis_derivatives) {
   int num_tests = 100;
-  boost::mt19937 rng(42);
+  std::mt19937 rng(42);
   for (int i = 0; i < num_tests; i++) {
     Unit3 p = Unit3::Random(rng);
 
@@ -348,7 +348,7 @@ TEST(Unit3, basis_derivatives) {
 
     Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
                            boost::bind(BasisTest, _1, boost::none), p);
-    EXPECT(assert_equal(expectedH, actualH, 1e-8));
+    EXPECT(assert_equal(expectedH, actualH, 1e-5));
   }
 }
 
@@ -360,14 +360,14 @@ TEST(Unit3, retract) {
     Unit3 expected(0.877583, 0, 0.479426);
     Unit3 actual = p.retract(v);
     EXPECT(assert_equal(expected, actual, 1e-6));
-    EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
+    EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-5));
   }
   {
     Unit3 p;
     Vector2 v(0, 0);
     Unit3 actual = p.retract(v);
     EXPECT(assert_equal(p, actual, 1e-6));
-    EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
+    EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-5));
   }
 }
 
@@ -381,13 +381,13 @@ TEST (Unit3, jacobian_retract) {
       Vector2 v (-0.2, 0.1);
       p.retract(v, H);
       Matrix H_expected_numerical = numericalDerivative11(f, v);
-      EXPECT(assert_equal(H_expected_numerical, H, 1e-9));
+      EXPECT(assert_equal(H_expected_numerical, H, 1e-5));
   }
   {
       Vector2 v (0, 0);
       p.retract(v, H);
       Matrix H_expected_numerical = numericalDerivative11(f, v);
-      EXPECT(assert_equal(H_expected_numerical, H, 1e-9));
+      EXPECT(assert_equal(H_expected_numerical, H, 1e-5));
   }
 }
 
@@ -397,13 +397,13 @@ TEST(Unit3, retract_expmap) {
   Vector2 v((M_PI / 2.0), 0);
   Unit3 expected(Point3(0, 0, 1));
   Unit3 actual = p.retract(v);
-  EXPECT(assert_equal(expected, actual, 1e-8));
-  EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
+  EXPECT(assert_equal(expected, actual, 1e-5));
+  EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-5));
 }
 
 //*******************************************************************************
 TEST(Unit3, Random) {
-  boost::mt19937 rng(42);
+  std::mt19937 rng(42);
   // Check that means are all zero at least
   Point3 expectedMean(0,0,0), actualMean(0,0,0);
   for (size_t i = 0; i < 100; i++)
@@ -415,7 +415,7 @@ TEST(Unit3, Random) {
 //*******************************************************************************
 // New test that uses Unit3::Random
 TEST(Unit3, localCoordinates_retract) {
-  boost::mt19937 rng(42);
+  std::mt19937 rng(42);
   size_t numIterations = 10000;
 
   for (size_t i = 0; i < numIterations; i++) {
@@ -428,7 +428,7 @@ TEST(Unit3, localCoordinates_retract) {
     // Check if the local coordinates and retract return consistent results.
     Vector v12 = s1.localCoordinates(s2);
     Unit3 actual_s2 = s1.retract(v12);
-    EXPECT(assert_equal(s2, actual_s2, 1e-9));
+    EXPECT(assert_equal(s2, actual_s2, 1e-5));
   }
 }
 
@@ -437,10 +437,10 @@ TEST (Unit3, FromPoint3) {
   Matrix actualH;
   Point3 point(1, -2, 3); // arbitrary point
   Unit3 expected(point);
-  EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-8));
+  EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5));
   Matrix expectedH = numericalDerivative11<Unit3, Point3>(
       boost::bind(Unit3::FromPoint3, _1, boost::none), point);
-  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+  EXPECT(assert_equal(expectedH, actualH, 1e-5));
 }
 
 //*******************************************************************************
@@ -455,7 +455,7 @@ TEST(Unit3, ErrorBetweenFactor) {
   // Add prior factors.
   SharedNoiseModel R_prior = noiseModel::Unit::Create(2);
   for (size_t i = 0; i < data.size(); i++) {
-    graph.emplace_shared<PriorFactor<Unit3> >(U(i), data[i], R_prior);
+    graph.addPrior(U(i), data[i], R_prior);
   }
 
   // Add process factors using the dot product error function.
@@ -484,6 +484,15 @@ TEST(Unit3, ErrorBetweenFactor) {
   }
 }
 
+TEST(Unit3, CopyAssign) {
+  Unit3 p{1, 0.2, 0.3};
+
+  EXPECT(p.error(p).isZero());
+
+  p = Unit3{-1, 2, 8};
+  EXPECT(p.error(p).isZero());
+}
+
 /* ************************************************************************* */
 TEST(actualH, Serialization) {
   Unit3 p(0, 1, 0);
@@ -494,7 +503,7 @@ TEST(actualH, Serialization) {
 
 /* ************************************************************************* */
 int main() {
-  srand(time(NULL));
+  srand(time(nullptr));
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index ed61c75b5..8cdf0fdc0 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -22,7 +22,6 @@
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/slam/TriangulationFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/inference/Symbol.h>
 
@@ -124,7 +123,17 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
   return std::make_pair(graph, values);
 }
 
-/// PinholeCamera specific version // TODO: (chris) why does this exist?
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+/// DEPRECATED: PinholeCamera specific version
+template<class CALIBRATION>
+Point3 triangulateNonlinear(
+    const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
+    const Point2Vector& measurements, const Point3& initialEstimate) {
+  return triangulateNonlinear<PinholeCamera<CALIBRATION> > //
+  (cameras, measurements, initialEstimate);
+}
+
+/// DEPRECATED: PinholeCamera specific version
 template<class CALIBRATION>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
     const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
@@ -133,6 +142,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
   return triangulationGraph<PinholeCamera<CALIBRATION> > //
   (cameras, measurements, landmarkKey, initialEstimate);
 }
+#endif
 
 /**
  * Optimize for triangulation
@@ -187,15 +197,6 @@ Point3 triangulateNonlinear(
   return optimize(graph, values, Symbol('p', 0));
 }
 
-/// PinholeCamera specific version  // TODO: (chris) why does this exist?
-template<class CALIBRATION>
-Point3 triangulateNonlinear(
-    const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
-    const Point2Vector& measurements, const Point3& initialEstimate) {
-  return triangulateNonlinear<PinholeCamera<CALIBRATION> > //
-  (cameras, measurements, initialEstimate);
-}
-
 /**
  * Create a 3*4 camera projection matrix from calibration and pose.
  * Functor for partial application on calibration
@@ -214,7 +215,7 @@ struct CameraProjectionMatrix {
 private:
   const Matrix3 K_;
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /**
diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h
index a69fb9b8c..0597ece98 100644
--- a/gtsam/inference/BayesNet.h
+++ b/gtsam/inference/BayesNet.h
@@ -69,4 +69,6 @@ namespace gtsam {
     void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
   };
 
-}
\ No newline at end of file
+}
+
+#include <gtsam/inference/BayesNet-inst.h>
diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h
index 4df234004..5b53a5719 100644
--- a/gtsam/inference/BayesTree-inst.h
+++ b/gtsam/inference/BayesTree-inst.h
@@ -125,7 +125,7 @@ namespace gtsam {
   void BayesTree<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
     for(Key j: clique->conditional()->frontals())
       nodes_[j] = clique;
-    if (parent_clique != NULL) {
+    if (parent_clique != nullptr) {
       clique->parent_ = parent_clique;
       parent_clique->children.push_back(clique);
     } else {
@@ -262,7 +262,7 @@ namespace gtsam {
 
     // Now, marginalize out everything that is not variable j
     BayesNetType marginalBN = *cliqueMarginal.marginalMultifrontalBayesNet(
-      Ordering(cref_list_of<1,Key>(j)), boost::none, function);
+      Ordering(cref_list_of<1,Key>(j)), function);
 
     // The Bayes net should contain only one conditional for variable j, so return it
     return marginalBN.front();
@@ -383,7 +383,7 @@ namespace gtsam {
     }
 
     // now, marginalize out everything that is not variable j1 or j2
-    return p_BC1C2.marginalMultifrontalBayesNet(Ordering(cref_list_of<2,Key>(j1)(j2)), boost::none, function);
+    return p_BC1C2.marginalMultifrontalBayesNet(Ordering(cref_list_of<2,Key>(j1)(j2)), function);
   }
 
   /* ************************************************************************* */
@@ -430,7 +430,7 @@ namespace gtsam {
   template <class CLIQUE>
   void BayesTree<CLIQUE>::removePath(sharedClique clique, BayesNetType* bn,
                                      Cliques* orphans) {
-    // base case is NULL, if so we do nothing and return empties above
+    // base case is nullptr, if so we do nothing and return empties above
     if (clique) {
       // remove the clique from orphans in case it has been added earlier
       orphans->remove(clique);
diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h
index 6bcfb434d..a02fe274e 100644
--- a/gtsam/inference/BayesTreeCliqueBase-inst.h
+++ b/gtsam/inference/BayesTreeCliqueBase-inst.h
@@ -136,57 +136,61 @@ namespace gtsam {
     }
   }
 
-  /* ************************************************************************* */
+  /* *********************************************************************** */
   // separator marginal, uses separator marginal of parent recursively
   // P(C) = P(F|S) P(S)
-  /* ************************************************************************* */
-  template<class DERIVED, class FACTORGRAPH>
+  /* *********************************************************************** */
+  template <class DERIVED, class FACTORGRAPH>
   typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
-    BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separatorMarginal(Eliminate function) const
-  {
+  BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separatorMarginal(
+      Eliminate function) const {
     gttic(BayesTreeCliqueBase_separatorMarginal);
     // Check if the Separator marginal was already calculated
-    if (!cachedSeparatorMarginal_)
-    {
+    if (!cachedSeparatorMarginal_) {
       gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
+
       // If this is the root, there is no separator
-      if (parent_.expired() /*(if we're the root)*/)
-      {
+      if (parent_.expired() /*(if we're the root)*/) {
         // we are root, return empty
         FactorGraphType empty;
         cachedSeparatorMarginal_ = empty;
-      }
-      else
-      {
+      } else {
+        // Flatten recursion in timing outline
+        gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss);
+        gttoc(BayesTreeCliqueBase_separatorMarginal);
+
         // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
         // initialize P(Cp) with the parent separator marginal
         derived_ptr parent(parent_.lock());
-        gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); // Flatten recursion in timing outline
-        gttoc(BayesTreeCliqueBase_separatorMarginal);
-        FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp)
+        FactorGraphType p_Cp(parent->separatorMarginal(function));  // P(Sp)
+
         gttic(BayesTreeCliqueBase_separatorMarginal);
         gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
+
         // now add the parent conditional
-        p_Cp += parent->conditional_; // P(Fp|Sp)
+        p_Cp += parent->conditional_;  // P(Fp|Sp)
 
         // The variables we want to keepSet are exactly the ones in S
-        KeyVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents());
-        cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function);
+        KeyVector indicesS(this->conditional()->beginParents(),
+                           this->conditional()->endParents());
+        auto separatorMarginal =
+            p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function);
+        cachedSeparatorMarginal_.reset(*separatorMarginal);
       }
     }
 
     // return the shortcut P(S||B)
-    return *cachedSeparatorMarginal_; // return the cached version
+    return *cachedSeparatorMarginal_;  // return the cached version
   }
 
-  /* ************************************************************************* */
-  // marginal2, uses separator marginal of parent recursively
+  /* *********************************************************************** */
+  // marginal2, uses separator marginal of parent
   // P(C) = P(F|S) P(S)
-  /* ************************************************************************* */
-  template<class DERIVED, class FACTORGRAPH>
+  /* *********************************************************************** */
+  template <class DERIVED, class FACTORGRAPH>
   typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
-    BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::marginal2(Eliminate function) const
-  {
+  BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::marginal2(
+      Eliminate function) const {
     gttic(BayesTreeCliqueBase_marginal2);
     // initialize with separator marginal P(S)
     FactorGraphType p_C = this->separatorMarginal(function);
diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h
index 317ba1c44..6266e961c 100644
--- a/gtsam/inference/BayesTreeCliqueBase.h
+++ b/gtsam/inference/BayesTreeCliqueBase.h
@@ -86,6 +86,8 @@ namespace gtsam {
     FastVector<derived_ptr> children;
     int problemSize_;
 
+    bool is_root = false;
+
     /// Fill the elimination result produced during elimination.  Here this just stores the
     /// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique
     /// to also cache the remaining factor.
@@ -165,8 +167,14 @@ namespace gtsam {
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+      if(!parent_.lock()) {
+        is_root = true;
+      }
+      ar & BOOST_SERIALIZATION_NVP(is_root);
       ar & BOOST_SERIALIZATION_NVP(conditional_);
-      ar & BOOST_SERIALIZATION_NVP(parent_);
+      if (!is_root) { // TODO(fan): Workaround for boost/serialization #119
+        ar & BOOST_SERIALIZATION_NVP(parent_);
+      }
       ar & BOOST_SERIALIZATION_NVP(children);
     }
 
diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h
index 1d486030c..295122879 100644
--- a/gtsam/inference/Conditional.h
+++ b/gtsam/inference/Conditional.h
@@ -65,6 +65,8 @@ namespace gtsam {
     Conditional(size_t nrFrontals) : nrFrontals_(nrFrontals) {}
 
     /// @}
+
+  public:
     /// @name Testable
     /// @{
 
@@ -76,7 +78,6 @@ namespace gtsam {
 
     /// @}
 
-  public:
     /// @name Standard Interface
     /// @{
 
diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h
index af2a91257..81f4047a1 100644
--- a/gtsam/inference/EliminateableFactorGraph-inst.h
+++ b/gtsam/inference/EliminateableFactorGraph-inst.h
@@ -28,33 +28,19 @@ namespace gtsam {
   template<class FACTORGRAPH>
   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
     EliminateableFactorGraph<FACTORGRAPH>::eliminateSequential(
-    OptionalOrdering ordering, const Eliminate& function,
-    OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
-  {
-    if(ordering && variableIndex) {
-      gttic(eliminateSequential);
-      // Do elimination
-      EliminationTreeType etree(asDerived(), *variableIndex, *ordering);
-      boost::shared_ptr<BayesNetType> bayesNet;
-      boost::shared_ptr<FactorGraphType> factorGraph;
-      boost::tie(bayesNet,factorGraph) = etree.eliminate(function);
-      // If any factors are remaining, the ordering was incomplete
-      if(!factorGraph->empty())
-        throw InconsistentEliminationRequested();
-      // Return the Bayes net
-      return bayesNet;
-    }
-    else if(!variableIndex) {
+    OptionalOrderingType orderingType, const Eliminate& function,
+    OptionalVariableIndex variableIndex) const {
+    if(!variableIndex) {
       // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
       // for no variable index first so that it's always computed if we need to call COLAMD because
-      // no Ordering is provided.
+      // no Ordering is provided.  When removing optional from VariableIndex, create VariableIndex
+      // before creating ordering.
       VariableIndex computedVariableIndex(asDerived());
-      return eliminateSequential(ordering, function, computedVariableIndex, orderingType);
+      return eliminateSequential(function, computedVariableIndex, orderingType);
     }
-    else /*if(!ordering)*/ {
-      // If no Ordering provided, compute one and call this function again.  We are guaranteed to
-      // have a VariableIndex already here because we computed one if needed in the previous 'else'
-      // block.
+    else {
+      // Compute an ordering and call this function again.  We are guaranteed to have a 
+      // VariableIndex already here because we computed one if needed in the previous 'if' block.
       if (orderingType == Ordering::METIS) {
         Ordering computedOrdering = Ordering::Metis(asDerived());
         return eliminateSequential(computedOrdering, function, variableIndex, orderingType);
@@ -65,17 +51,75 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************* */
+  template<class FACTORGRAPH>
+  boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
+    EliminateableFactorGraph<FACTORGRAPH>::eliminateSequential(
+    const Ordering& ordering, const Eliminate& function,
+    OptionalVariableIndex variableIndex) const
+  {
+    if(!variableIndex) {
+      // If no VariableIndex provided, compute one and call this function again
+      VariableIndex computedVariableIndex(asDerived());
+      return eliminateSequential(ordering, function, computedVariableIndex);
+    } else {
+      gttic(eliminateSequential);
+      // Do elimination
+      EliminationTreeType etree(asDerived(), *variableIndex, ordering);
+      boost::shared_ptr<BayesNetType> bayesNet;
+      boost::shared_ptr<FactorGraphType> factorGraph;
+      boost::tie(bayesNet,factorGraph) = etree.eliminate(function);
+      // If any factors are remaining, the ordering was incomplete
+      if(!factorGraph->empty())
+        throw InconsistentEliminationRequested();
+      // Return the Bayes net
+      return bayesNet;
+    }
+  }
+
   /* ************************************************************************* */
   template<class FACTORGRAPH>
   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
     EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
-    OptionalOrdering ordering, const Eliminate& function,
-    OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
+    OptionalOrderingType orderingType, const Eliminate& function,
+    OptionalVariableIndex variableIndex) const
   {
-    if(ordering && variableIndex) {
+    if(!variableIndex) {
+      // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
+      // for no variable index first so that it's always computed if we need to call COLAMD because
+      // no Ordering is provided.  When removing optional from VariableIndex, create VariableIndex
+      // before creating ordering.
+      VariableIndex computedVariableIndex(asDerived());
+      return eliminateMultifrontal(function, computedVariableIndex, orderingType);
+    }
+    else {
+      // Compute an ordering and call this function again.  We are guaranteed to have a
+      // VariableIndex already here because we computed one if needed in the previous 'if' block.
+      if (orderingType == Ordering::METIS) {
+        Ordering computedOrdering = Ordering::Metis(asDerived());
+        return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType);
+      } else {
+        Ordering computedOrdering = Ordering::Colamd(*variableIndex);
+        return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType);
+      }
+    }
+  }
+
+  /* ************************************************************************* */
+  template<class FACTORGRAPH>
+  boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
+    EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
+    const Ordering& ordering, const Eliminate& function,
+    OptionalVariableIndex variableIndex) const
+  {
+    if(!variableIndex) {
+      // If no VariableIndex provided, compute one and call this function again
+      VariableIndex computedVariableIndex(asDerived());
+      return eliminateMultifrontal(ordering, function, computedVariableIndex);
+    } else {
       gttic(eliminateMultifrontal);
       // Do elimination with given ordering
-      EliminationTreeType etree(asDerived(), *variableIndex, *ordering);
+      EliminationTreeType etree(asDerived(), *variableIndex, ordering);
       JunctionTreeType junctionTree(etree);
       boost::shared_ptr<BayesTreeType> bayesTree;
       boost::shared_ptr<FactorGraphType> factorGraph;
@@ -86,25 +130,6 @@ namespace gtsam {
       // Return the Bayes tree
       return bayesTree;
     }
-    else if(!variableIndex) {
-      // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
-      // for no variable index first so that it's always computed if we need to call COLAMD because
-      // no Ordering is provided.
-      VariableIndex computedVariableIndex(asDerived());
-      return eliminateMultifrontal(ordering, function, computedVariableIndex, orderingType);
-    }
-    else /*if(!ordering)*/ {
-      // If no Ordering provided, compute one and call this function again.  We are guaranteed to
-      // have a VariableIndex already here because we computed one if needed in the previous 'else'
-      // block.
-      if (orderingType == Ordering::METIS) {
-        Ordering computedOrdering = Ordering::Metis(asDerived());
-        return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType);
-      } else {
-        Ordering computedOrdering = Ordering::Colamd(*variableIndex);
-        return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType);
-      }
-    }
   }
 
   /* ************************************************************************* */
@@ -191,57 +216,65 @@ namespace gtsam {
   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
     EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
     boost::variant<const Ordering&, const KeyVector&> variables,
-    OptionalOrdering marginalizedVariableOrdering,
     const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
-    if(variableIndex)
-    {
-      if(marginalizedVariableOrdering)
-      {
-        gttic(marginalMultifrontalBayesNet);
-        // An ordering was provided for the marginalized variables, so we can first eliminate them
-        // in the order requested.
-        boost::shared_ptr<BayesTreeType> bayesTree;
-        boost::shared_ptr<FactorGraphType> factorGraph;
-        boost::tie(bayesTree,factorGraph) =
-          eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex);
-
-        if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
-        {
-          // An ordering was also provided for the unmarginalized variables, so we can also
-          // eliminate them in the order requested.
-          return factorGraph->eliminateSequential(*varsAsOrdering, function);
-        }
-        else
-        {
-          // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
-          return factorGraph->eliminateSequential(boost::none, function);
-        }
-      }
-      else
-      {
-        // No ordering was provided for the marginalized variables, so order them using constrained
-        // COLAMD.
-        bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
-        const KeyVector* variablesOrOrdering =
-          unmarginalizedAreOrdered ?
-          boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
-
-        Ordering totalOrdering =
-          Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
-
-        // Split up ordering
-        const size_t nVars = variablesOrOrdering->size();
-        Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
-        Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
-
-        // Call this function again with the computed orderings
-        return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
-      }
+    if(!variableIndex) {
+      // If no variable index is provided, compute one and call this function again
+      VariableIndex index(asDerived());
+      return marginalMultifrontalBayesNet(variables, function, index);
     } else {
+      // No ordering was provided for the marginalized variables, so order them using constrained
+      // COLAMD.
+      bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
+      const KeyVector* variablesOrOrdering =
+        unmarginalizedAreOrdered ?
+        boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
+
+      Ordering totalOrdering =
+        Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
+
+      // Split up ordering
+      const size_t nVars = variablesOrOrdering->size();
+      Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
+      Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
+
+      // Call this function again with the computed orderings
+      return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
+    }
+  }
+
+  /* ************************************************************************* */
+  template<class FACTORGRAPH>
+  boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
+    EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
+    boost::variant<const Ordering&, const KeyVector&> variables,
+    const Ordering& marginalizedVariableOrdering,
+    const Eliminate& function, OptionalVariableIndex variableIndex) const
+  {
+    if(!variableIndex) {
       // If no variable index is provided, compute one and call this function again
       VariableIndex index(asDerived());
       return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index);
+    } else {
+      gttic(marginalMultifrontalBayesNet);
+      // An ordering was provided for the marginalized variables, so we can first eliminate them
+      // in the order requested.
+      boost::shared_ptr<BayesTreeType> bayesTree;
+      boost::shared_ptr<FactorGraphType> factorGraph;
+      boost::tie(bayesTree,factorGraph) =
+        eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex);
+
+      if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
+      {
+        // An ordering was also provided for the unmarginalized variables, so we can also
+        // eliminate them in the order requested.
+        return factorGraph->eliminateSequential(*varsAsOrdering, function);
+      }
+      else
+      {
+        // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
+        return factorGraph->eliminateSequential(function);
+      }
     }
   }
 
@@ -250,57 +283,65 @@ namespace gtsam {
   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
     EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
     boost::variant<const Ordering&, const KeyVector&> variables,
-    OptionalOrdering marginalizedVariableOrdering,
     const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
-    if(variableIndex)
-    {
-      if(marginalizedVariableOrdering)
-      {
-        gttic(marginalMultifrontalBayesTree);
-        // An ordering was provided for the marginalized variables, so we can first eliminate them
-        // in the order requested.
-        boost::shared_ptr<BayesTreeType> bayesTree;
-        boost::shared_ptr<FactorGraphType> factorGraph;
-        boost::tie(bayesTree,factorGraph) =
-          eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex);
-
-        if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
-        {
-          // An ordering was also provided for the unmarginalized variables, so we can also
-          // eliminate them in the order requested.
-          return factorGraph->eliminateMultifrontal(*varsAsOrdering, function);
-        }
-        else
-        {
-          // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
-          return factorGraph->eliminateMultifrontal(boost::none, function);
-        }
-      }
-      else
-      {
-        // No ordering was provided for the marginalized variables, so order them using constrained
-        // COLAMD.
-        bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
-        const KeyVector* variablesOrOrdering =
-          unmarginalizedAreOrdered ?
-          boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
-
-        Ordering totalOrdering =
-          Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
-
-        // Split up ordering
-        const size_t nVars = variablesOrOrdering->size();
-        Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
-        Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
-
-        // Call this function again with the computed orderings
-        return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
-      }
+    if(!variableIndex) {
+      // If no variable index is provided, compute one and call this function again
+      VariableIndex computedVariableIndex(asDerived());
+      return marginalMultifrontalBayesTree(variables, function, computedVariableIndex);
     } else {
+      // No ordering was provided for the marginalized variables, so order them using constrained
+      // COLAMD.
+      bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
+      const KeyVector* variablesOrOrdering =
+        unmarginalizedAreOrdered ?
+        boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
+
+      Ordering totalOrdering =
+        Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
+
+      // Split up ordering
+      const size_t nVars = variablesOrOrdering->size();
+      Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
+      Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
+
+      // Call this function again with the computed orderings
+      return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
+    }
+  }
+
+  /* ************************************************************************* */
+  template<class FACTORGRAPH>
+  boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
+    EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
+    boost::variant<const Ordering&, const KeyVector&> variables,
+    const Ordering& marginalizedVariableOrdering,
+    const Eliminate& function, OptionalVariableIndex variableIndex) const
+  {
+    if(!variableIndex) {
       // If no variable index is provided, compute one and call this function again
       VariableIndex computedVariableIndex(asDerived());
       return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex);
+    } else {
+      gttic(marginalMultifrontalBayesTree);
+      // An ordering was provided for the marginalized variables, so we can first eliminate them
+      // in the order requested.
+      boost::shared_ptr<BayesTreeType> bayesTree;
+      boost::shared_ptr<FactorGraphType> factorGraph;
+      boost::tie(bayesTree,factorGraph) =
+        eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex);
+
+      if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
+      {
+        // An ordering was also provided for the unmarginalized variables, so we can also
+        // eliminate them in the order requested.
+        return factorGraph->eliminateMultifrontal(*varsAsOrdering, function);
+      }
+      else
+      {
+        // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
+        return factorGraph->eliminateMultifrontal(function);
+      }
     }
   }
 
diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h
index 8084aa75b..316ca8ee4 100644
--- a/gtsam/inference/EliminateableFactorGraph.h
+++ b/gtsam/inference/EliminateableFactorGraph.h
@@ -88,9 +88,6 @@ namespace gtsam {
     /// The function type that does a single dense elimination step on a subgraph.
     typedef boost::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
 
-    /// Typedef for an optional ordering as an argument to elimination functions
-    typedef boost::optional<const Ordering&> OptionalOrdering;
-
     /// Typedef for an optional variable index as an argument to elimination functions
     typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
 
@@ -108,24 +105,38 @@ namespace gtsam {
      *  <b> Example - METIS ordering for elimination
      *  \code
      *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(OrderingType::METIS);
-     *
-     *  <b> Example - Full QR elimination in specified order:
-     *  \code
-     *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, myOrdering);
      *  \endcode
      *
      *  <b> Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: </b>
      *  \code
      *  VariableIndex varIndex(graph); // Build variable index
      *  Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index
-     *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, boost::none, varIndex);
+     *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, varIndex, boost::none);
      *  \endcode
      *  */
     boost::shared_ptr<BayesNetType> eliminateSequential(
-      OptionalOrdering ordering = boost::none,
+      OptionalOrderingType orderingType = boost::none,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
-      OptionalVariableIndex variableIndex = boost::none,
-      OptionalOrderingType orderingType = boost::none) const;
+      OptionalVariableIndex variableIndex = boost::none) const;
+
+    /** Do sequential elimination of all variables to produce a Bayes net.
+     *
+     *  <b> Example - Full QR elimination in specified order:
+     *  \code
+     *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(myOrdering, EliminateQR);
+     *  \endcode
+     *
+     *  <b> Example - Reusing an existing VariableIndex to improve performance: </b>
+     *  \code
+     *  VariableIndex varIndex(graph); // Build variable index
+     *  Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index
+     *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(myOrdering, EliminateQR, varIndex, boost::none);
+     *  \endcode
+     *  */
+    boost::shared_ptr<BayesNetType> eliminateSequential(
+      const Ordering& ordering,
+      const Eliminate& function = EliminationTraitsType::DefaultEliminate,
+      OptionalVariableIndex variableIndex = boost::none) const;
 
     /** Do multifrontal elimination of all variables to produce a Bayes tree.  If an ordering is not
      *  provided, the ordering will be computed using either COLAMD or METIS, dependeing on
@@ -136,11 +147,6 @@ namespace gtsam {
      *  boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateCholesky);
      *  \endcode
      *
-     *  <b> Example - Full QR elimination in specified order:
-     *  \code
-     *  boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, myOrdering);
-     *  \endcode
-     *
      *  <b> Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: </b>
      *  \code
      *  VariableIndex varIndex(graph); // Build variable index
@@ -149,10 +155,23 @@ namespace gtsam {
      *  \endcode
      *  */
     boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
-      OptionalOrdering ordering = boost::none,
+      OptionalOrderingType orderingType = boost::none,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
-      OptionalVariableIndex variableIndex = boost::none,
-      OptionalOrderingType orderingType = boost::none) const;
+      OptionalVariableIndex variableIndex = boost::none) const;
+
+    /** Do multifrontal elimination of all variables to produce a Bayes tree.  If an ordering is not
+     *  provided, the ordering will be computed using either COLAMD or METIS, dependeing on
+     *  the parameter orderingType (Ordering::COLAMD or Ordering::METIS)
+     *
+     *  <b> Example - Full QR elimination in specified order:
+     *  \code
+     *  boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, myOrdering);
+     *  \endcode
+     *  */
+    boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
+      const Ordering& ordering,
+      const Eliminate& function = EliminationTraitsType::DefaultEliminate,
+      OptionalVariableIndex variableIndex = boost::none) const;
 
     /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net
      *  and a remaining factor graph.  This computes the factorization \f$ p(X) = p(A|B) p(B) \f$,
@@ -194,20 +213,47 @@ namespace gtsam {
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none) const;
 
-    /** Compute the marginal of the requested variables and return the result as a Bayes net.
+    /** Compute the marginal of the requested variables and return the result as a Bayes net.  Uses
+     *  COLAMD marginalization ordering by default
      *  @param variables Determines the variables whose marginal to compute, if provided as an
      *         Ordering they will be ordered in the returned BayesNet as specified, and if provided
      *         as a KeyVector they will be ordered using constrained COLAMD.
-     *  @param marginalizedVariableOrdering Optional ordering for the variables being marginalized
-     *         out, i.e. all variables not in \c variables.  If this is boost::none, the ordering
-     *         will be computed with COLAMD.
      *  @param function Optional dense elimination function, if not provided the default will be
      *         used.
      *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
      *         provided one will be computed. */
     boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
       boost::variant<const Ordering&, const KeyVector&> variables,
-      OptionalOrdering marginalizedVariableOrdering = boost::none,
+      const Eliminate& function = EliminationTraitsType::DefaultEliminate,
+      OptionalVariableIndex variableIndex = boost::none) const;
+
+    /** Compute the marginal of the requested variables and return the result as a Bayes net.
+     *  @param variables Determines the variables whose marginal to compute, if provided as an
+     *         Ordering they will be ordered in the returned BayesNet as specified, and if provided
+     *         as a KeyVector they will be ordered using constrained COLAMD.
+     *  @param marginalizedVariableOrdering Ordering for the variables being marginalized out,
+     *         i.e. all variables not in \c variables.
+     *  @param function Optional dense elimination function, if not provided the default will be
+     *         used.
+     *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
+     *         provided one will be computed. */
+    boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
+      boost::variant<const Ordering&, const KeyVector&> variables,
+      const Ordering& marginalizedVariableOrdering,
+      const Eliminate& function = EliminationTraitsType::DefaultEliminate,
+      OptionalVariableIndex variableIndex = boost::none) const;
+
+    /** Compute the marginal of the requested variables and return the result as a Bayes tree.  Uses
+     *  COLAMD marginalization order by default
+     *  @param variables Determines the variables whose marginal to compute, if provided as an
+     *         Ordering they will be ordered in the returned BayesNet as specified, and if provided
+     *         as a KeyVector they will be ordered using constrained COLAMD.
+     *  @param function Optional dense elimination function, if not provided the default will be
+     *         used.
+     *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
+     *         provided one will be computed. */
+    boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
+      boost::variant<const Ordering&, const KeyVector&> variables,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none) const;
 
@@ -215,16 +261,15 @@ namespace gtsam {
      *  @param variables Determines the variables whose marginal to compute, if provided as an
      *         Ordering they will be ordered in the returned BayesNet as specified, and if provided
      *         as a KeyVector they will be ordered using constrained COLAMD.
-     *  @param marginalizedVariableOrdering Optional ordering for the variables being marginalized
-     *         out, i.e. all variables not in \c variables.  If this is boost::none, the ordering
-     *         will be computed with COLAMD.
+     *  @param marginalizedVariableOrdering Ordering for the variables being marginalized out,
+     *         i.e. all variables not in \c variables.
      *  @param function Optional dense elimination function, if not provided the default will be
      *         used.
      *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
      *         provided one will be computed. */
     boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
       boost::variant<const Ordering&, const KeyVector&> variables,
-      OptionalOrdering marginalizedVariableOrdering = boost::none,
+      const Ordering& marginalizedVariableOrdering,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none) const;
 
@@ -241,6 +286,59 @@ namespace gtsam {
 
     // Access the derived factor graph class
     FactorGraphType& asDerived() { return static_cast<FactorGraphType&>(*this); }
+
+  public:
+    /** \deprecated ordering and orderingType shouldn't both be specified */
+    boost::shared_ptr<BayesNetType> eliminateSequential(
+      const Ordering& ordering,
+      const Eliminate& function,
+      OptionalVariableIndex variableIndex,
+      OptionalOrderingType orderingType) const {
+        return eliminateSequential(ordering, function, variableIndex);
+      }
+    
+    /** \deprecated orderingType specified first for consistency */
+    boost::shared_ptr<BayesNetType> eliminateSequential(
+      const Eliminate& function,
+      OptionalVariableIndex variableIndex = boost::none,
+      OptionalOrderingType orderingType = boost::none) const {
+        return eliminateSequential(orderingType, function, variableIndex);
+      }
+
+    /** \deprecated ordering and orderingType shouldn't both be specified */
+    boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
+      const Ordering& ordering,
+      const Eliminate& function,
+      OptionalVariableIndex variableIndex,
+      OptionalOrderingType orderingType) const {
+        return eliminateMultifrontal(ordering, function, variableIndex);
+      }
+
+    /** \deprecated orderingType specified first for consistency */
+    boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
+      const Eliminate& function,
+      OptionalVariableIndex variableIndex = boost::none,
+      OptionalOrderingType orderingType = boost::none) const {
+        return eliminateMultifrontal(orderingType, function, variableIndex);
+      }
+
+    /** \deprecated */
+    boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
+      boost::variant<const Ordering&, const KeyVector&> variables,
+      boost::none_t,
+      const Eliminate& function = EliminationTraitsType::DefaultEliminate,
+      OptionalVariableIndex variableIndex = boost::none) const {
+          return marginalMultifrontalBayesNet(variables, function, variableIndex);
+      }
+
+    /** \deprecated */
+    boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
+      boost::variant<const Ordering&, const KeyVector&> variables,
+      boost::none_t,
+      const Eliminate& function = EliminationTraitsType::DefaultEliminate,
+      OptionalVariableIndex variableIndex = boost::none) const {
+          return marginalMultifrontalBayesTree(variables, function, variableIndex);
+      }
   };
 
 }
diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h
index 34ca8ab7f..df68019e1 100644
--- a/gtsam/inference/FactorGraph-inst.h
+++ b/gtsam/inference/FactorGraph-inst.h
@@ -55,8 +55,8 @@ bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
   // check whether the factors are the same, in same order.
   for (size_t i = 0; i < factors_.size(); i++) {
     sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
-    if (f1 == NULL && f2 == NULL) continue;
-    if (f1 == NULL || f2 == NULL) return false;
+    if (f1 == nullptr && f2 == nullptr) continue;
+    if (f1 == nullptr || f2 == nullptr) return false;
     if (!f1->equals(*f2, tol)) return false;
   }
   return true;
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index 600e3f9ed..2bc9578b2 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -353,7 +353,7 @@ class FactorGraph {
    */
   void resize(size_t size) { factors_.resize(size); }
 
-  /** delete factor without re-arranging indexes by inserting a NULL pointer
+  /** delete factor without re-arranging indexes by inserting a nullptr pointer
    */
   void remove(size_t i) { factors_[i].reset(); }
 
diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp
index 6d34883fa..e67c56e5e 100644
--- a/gtsam/inference/LabeledSymbol.cpp
+++ b/gtsam/inference/LabeledSymbol.cpp
@@ -123,7 +123,7 @@ boost::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c,
 }
 
 /* ************************************************************************* */
-std::ostream &operator<<(std::ostream &os, const LabeledSymbol &symbol) {
+GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const LabeledSymbol &symbol) {
   os << StreamedKey(symbol);
   return os;
 }
diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h
index 5b3ec8766..d46425bb4 100644
--- a/gtsam/inference/LabeledSymbol.h
+++ b/gtsam/inference/LabeledSymbol.h
@@ -107,7 +107,7 @@ public:
   LabeledSymbol newLabel(unsigned char label) const { return LabeledSymbol(c_, label, j_); }
 
   /// Output stream operator that can be used with key_formatter (see Key.h).
-  friend std::ostream &operator<<(std::ostream &, const LabeledSymbol &);
+  friend GTSAM_EXPORT std::ostream &operator<<(std::ostream &, const LabeledSymbol &);
 
 private:
 
diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp
index da61ca57e..266c54ca6 100644
--- a/gtsam/inference/Ordering.cpp
+++ b/gtsam/inference/Ordering.cpp
@@ -91,7 +91,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
 
   assert((size_t)count == variableIndex.nEntries());
 
-  //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
+  //double* knobs = nullptr; /* colamd arg 6: parameters (uses defaults if nullptr) */
   double knobs[CCOLAMD_KNOBS];
   ccolamd_set_defaults(knobs);
   knobs[CCOLAMD_DENSE_ROW] = -1;
@@ -213,11 +213,21 @@ Ordering Ordering::Metis(const MetisIndex& met) {
 #ifdef GTSAM_SUPPORT_NESTED_DISSECTION
   gttic(Ordering_METIS);
 
+  idx_t size = met.nValues();
+  if (size == 0)
+  {
+    return Ordering();
+  }
+
+  if (size == 1)
+  {
+    return Ordering(KeyVector(1, met.intToKey(0)));
+  }
+
   vector<idx_t> xadj = met.xadj();
   vector<idx_t> adj = met.adj();
   vector<idx_t> perm, iperm;
 
-  idx_t size = met.nValues();
   for (idx_t i = 0; i < size; i++) {
     perm.push_back(0);
     iperm.push_back(0);
@@ -225,7 +235,7 @@ Ordering Ordering::Metis(const MetisIndex& met) {
 
   int outputError;
 
-  outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0],
+  outputError = METIS_NodeND(&size, &xadj[0], &adj[0], nullptr, nullptr, &perm[0],
       &iperm[0]);
   Ordering result;
 
diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h
index f770c7da1..a2d165792 100644
--- a/gtsam/inference/Ordering.h
+++ b/gtsam/inference/Ordering.h
@@ -191,7 +191,10 @@ public:
 
   template<class FACTOR_GRAPH>
   static Ordering Metis(const FACTOR_GRAPH& graph) {
-    return Metis(MetisIndex(graph));
+    if (graph.empty())
+      return Ordering();
+    else
+      return Metis(MetisIndex(graph));
   }
 
   /// @}
diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp
index 5e41b3eac..e6a2beee0 100644
--- a/gtsam/inference/Symbol.cpp
+++ b/gtsam/inference/Symbol.cpp
@@ -66,7 +66,7 @@ boost::function<bool(Key)> Symbol::ChrTest(unsigned char c) {
   return bind(&Symbol::chr, bind(make, _1)) == c;
 }
 
-std::ostream &operator<<(std::ostream &os, const Symbol &symbol) {
+GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const Symbol &symbol) {
   os << StreamedKey(symbol);
   return os;
 }
diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h
index 86574f70d..5be195db3 100644
--- a/gtsam/inference/Symbol.h
+++ b/gtsam/inference/Symbol.h
@@ -113,7 +113,7 @@ public:
   static boost::function<bool(Key)> ChrTest(unsigned char c);
 
   /// Output stream operator that can be used with key_formatter (see Key.h).
-  friend std::ostream &operator<<(std::ostream &, const Symbol &);
+  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const Symbol &);
 
 private:
 
diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h
index f9297d300..a665890c2 100644
--- a/gtsam/inference/VariableSlots.h
+++ b/gtsam/inference/VariableSlots.h
@@ -99,7 +99,9 @@ VariableSlots::VariableSlots(const FG& factorGraph)
   // factor does not involve that variable.
   size_t jointFactorPos = 0;
   for(const typename FG::sharedFactor& factor: factorGraph) {
-    assert(factor);
+    if (!factor) {
+      continue;
+    }
     size_t factorVarSlot = 0;
     for(const Key involvedVariable: *factor) {
       // Set the slot in this factor for this variable.  If the
@@ -111,7 +113,7 @@ VariableSlots::VariableSlots(const FG& factorGraph)
       iterator thisVarSlots; bool inserted;
         boost::tie(thisVarSlots, inserted) = this->insert(std::make_pair(involvedVariable, FastVector<size_t>()));
       if(inserted)
-        thisVarSlots->second.resize(factorGraph.size(), Empty);
+        thisVarSlots->second.resize(factorGraph.nrFactors(), Empty);
       thisVarSlots->second[jointFactorPos] = factorVarSlot;
       if(debug) std::cout << "  var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl;
       ++ factorVarSlot;
diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp
index 0d23d9655..0305218af 100644
--- a/gtsam/inference/tests/testOrdering.cpp
+++ b/gtsam/inference/tests/testOrdering.cpp
@@ -294,6 +294,28 @@ TEST(Ordering, MetisLoop) {
 }
 #endif
 /* ************************************************************************* */
+#ifdef GTSAM_SUPPORT_NESTED_DISSECTION
+TEST(Ordering, MetisEmptyGraph) {
+  SymbolicFactorGraph symbolicGraph;
+
+  Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph);
+  Ordering expected;
+  EXPECT(assert_equal(expected, actual));
+}
+#endif
+/* ************************************************************************* */
+#ifdef GTSAM_SUPPORT_NESTED_DISSECTION
+TEST(Ordering, MetisSingleNode) {
+  // create graph with a single node
+  SymbolicFactorGraph symbolicGraph;
+  symbolicGraph.push_factor(7);
+
+  Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph);
+  Ordering expected = Ordering(list_of(7));
+  EXPECT(assert_equal(expected, actual));
+}
+#endif
+/* ************************************************************************* */
 TEST(Ordering, Create) {
 
   // create chain graph
diff --git a/gtsam/linear/Errors.cpp b/gtsam/linear/Errors.cpp
index 42164d540..3fe2f3307 100644
--- a/gtsam/linear/Errors.cpp
+++ b/gtsam/linear/Errors.cpp
@@ -43,7 +43,7 @@ void Errors::print(const std::string& s) const {
 }
 
 /* ************************************************************************* */
-struct equalsVector : public std::binary_function<const Vector&, const Vector&, bool> {
+struct equalsVector : public std::function<bool(const Vector&, const Vector&)> {
   double tol_;
   equalsVector(double tol = 1e-9) : tol_(tol) {}
   bool operator()(const Vector& expected, const Vector& actual) {
diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp
index bc96452b9..04094d593 100644
--- a/gtsam/linear/GaussianBayesNet.cpp
+++ b/gtsam/linear/GaussianBayesNet.cpp
@@ -156,16 +156,17 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  pair<Matrix, Vector> GaussianBayesNet::matrix(boost::optional<const Ordering&> ordering) const {
-    if (ordering) {
-      // Convert to a GaussianFactorGraph and use its machinery
-      GaussianFactorGraph factorGraph(*this);
-      return factorGraph.jacobian(ordering);
-    } else {
-      // recursively call with default ordering
-      const auto defaultOrdering = this->ordering();
-      return matrix(defaultOrdering);
-    }
+  pair<Matrix, Vector> GaussianBayesNet::matrix(const Ordering& ordering) const {
+    // Convert to a GaussianFactorGraph and use its machinery
+    GaussianFactorGraph factorGraph(*this);
+    return factorGraph.jacobian(ordering);
+  }
+
+  /* ************************************************************************* */
+  pair<Matrix, Vector> GaussianBayesNet::matrix() const {
+    // recursively call with default ordering
+    const auto defaultOrdering = this->ordering();
+    return matrix(defaultOrdering);
   }
 
   ///* ************************************************************************* */
@@ -187,16 +188,16 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  double GaussianBayesNet::logDeterminant() const
-  {
+  double GaussianBayesNet::logDeterminant() const {
     double logDet = 0.0;
-    for(const sharedConditional& cg: *this) {
-      if(cg->get_model()) {
+    for (const sharedConditional& cg : *this) {
+      if (cg->get_model()) {
         Vector diag = cg->R().diagonal();
         cg->get_model()->whitenInPlace(diag);
-        logDet += diag.unaryExpr(ptr_fun<double,double>(log)).sum();
+        logDet += diag.unaryExpr([](double x) { return log(x); }).sum();
       } else {
-        logDet += cg->R().diagonal().unaryExpr(ptr_fun<double,double>(log)).sum();
+        logDet +=
+            cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
       }
     }
     return logDet;
diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h
index 669c8a964..3f6d69e91 100644
--- a/gtsam/linear/GaussianBayesNet.h
+++ b/gtsam/linear/GaussianBayesNet.h
@@ -92,7 +92,14 @@ namespace gtsam {
      * Will return upper-triangular matrix only when using 'ordering' above.
      * In case Bayes net is incomplete zero columns are added to the end.
      */
-    std::pair<Matrix, Vector> matrix(boost::optional<const Ordering&> ordering = boost::none) const;
+    std::pair<Matrix, Vector> matrix(const Ordering& ordering) const;
+
+    /**
+     * Return (dense) upper-triangular matrix representation
+     * Will return upper-triangular matrix only when using 'ordering' above.
+     * In case Bayes net is incomplete zero columns are added to the end.
+     */
+    std::pair<Matrix, Vector> matrix() const;
 
     /**
      * Optimize along the gradient direction, with a closed-form computation to perform the line
@@ -136,7 +143,7 @@ namespace gtsam {
      *        allocateVectorValues */
     VectorValues gradientAtZero() const;
 
-    /** Mahalanobis norm error. */
+    /** 0.5 * sum of squared Mahalanobis distances. */
     double error(const VectorValues& x) const;
 
     /**
diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp
index 8d0fafb61..13c19bce6 100644
--- a/gtsam/linear/GaussianBayesTree.cpp
+++ b/gtsam/linear/GaussianBayesTree.cpp
@@ -40,11 +40,11 @@ namespace gtsam {
     parentSum += clique->conditional()
                      ->R()
                      .diagonal()
-                     .unaryExpr(std::ptr_fun<double, double>(log))
+                     .unaryExpr([](double x) { return log(x); })
                      .sum();
     return 0;
   }
-  }
+  }  // namespace internal
 
   /* ************************************************************************* */
   bool GaussianBayesTree::equals(const This& other, double tol) const
diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h
index fcc73f80e..b6f5acd52 100644
--- a/gtsam/linear/GaussianBayesTree.h
+++ b/gtsam/linear/GaussianBayesTree.h
@@ -106,7 +106,7 @@ namespace gtsam {
      * @return A VectorValues storing the gradient. */
     VectorValues gradientAtZero() const;
 
-    /** Mahalanobis norm error. */
+    /** 0.5 * sum of squared Mahalanobis distances. */
     double error(const VectorValues& x) const;
 
     /** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a
diff --git a/gtsam/linear/GaussianFactor.cpp b/gtsam/linear/GaussianFactor.cpp
new file mode 100644
index 000000000..0802deff4
--- /dev/null
+++ b/gtsam/linear/GaussianFactor.cpp
@@ -0,0 +1,33 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    GaussianFactor.cpp
+ * @brief   A factor with a quadratic error function - a Gaussian
+ * @brief   GaussianFactor
+ * @author  Fan Jiang
+ */
+
+// \callgraph
+
+#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/linear/VectorValues.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+  VectorValues GaussianFactor::hessianDiagonal() const {
+    VectorValues d;
+    hessianDiagonalAdd(d);
+    return d;
+  }
+
+}
diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h
index a80b4dfd4..9b4c5f940 100644
--- a/gtsam/linear/GaussianFactor.h
+++ b/gtsam/linear/GaussianFactor.h
@@ -100,7 +100,10 @@ namespace gtsam {
     virtual Matrix information() const = 0;
 
     /// Return the diagonal of the Hessian for this factor
-    virtual VectorValues hessianDiagonal() const = 0;
+    VectorValues hessianDiagonal() const;
+
+    /// Add the current diagonal to a VectorValues instance
+    virtual void hessianDiagonalAdd(VectorValues& d) const = 0;
 
     /// Raw memory access version of hessianDiagonal
     virtual void hessianDiagonal(double* d) const = 0;
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 9281c7e7a..69890c32d 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -190,33 +190,62 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Matrix GaussianFactorGraph::augmentedJacobian(
-      boost::optional<const Ordering&> optionalOrdering) const {
+      const Ordering& ordering) const {
     // combine all factors
-    JacobianFactor combined(*this, optionalOrdering);
+    JacobianFactor combined(*this, ordering);
+    return combined.augmentedJacobian();
+  }
+
+  /* ************************************************************************* */
+  Matrix GaussianFactorGraph::augmentedJacobian() const {
+    // combine all factors
+    JacobianFactor combined(*this);
     return combined.augmentedJacobian();
   }
 
   /* ************************************************************************* */
   pair<Matrix, Vector> GaussianFactorGraph::jacobian(
-      boost::optional<const Ordering&> optionalOrdering) const {
-    Matrix augmented = augmentedJacobian(optionalOrdering);
+      const Ordering& ordering) const {
+    Matrix augmented = augmentedJacobian(ordering);
+    return make_pair(augmented.leftCols(augmented.cols() - 1),
+        augmented.col(augmented.cols() - 1));
+  }
+
+  /* ************************************************************************* */
+  pair<Matrix, Vector> GaussianFactorGraph::jacobian() const {
+    Matrix augmented = augmentedJacobian();
     return make_pair(augmented.leftCols(augmented.cols() - 1),
         augmented.col(augmented.cols() - 1));
   }
 
   /* ************************************************************************* */
   Matrix GaussianFactorGraph::augmentedHessian(
-      boost::optional<const Ordering&> optionalOrdering) const {
+      const Ordering& ordering) const {
     // combine all factors and get upper-triangular part of Hessian
-    Scatter scatter(*this, optionalOrdering);
+    Scatter scatter(*this, ordering);
+    HessianFactor combined(*this, scatter);
+    return combined.info().selfadjointView();;
+  }
+
+  /* ************************************************************************* */
+  Matrix GaussianFactorGraph::augmentedHessian() const {
+    // combine all factors and get upper-triangular part of Hessian
+    Scatter scatter(*this);
     HessianFactor combined(*this, scatter);
     return combined.info().selfadjointView();;
   }
 
   /* ************************************************************************* */
   pair<Matrix, Vector> GaussianFactorGraph::hessian(
-      boost::optional<const Ordering&> optionalOrdering) const {
-    Matrix augmented = augmentedHessian(optionalOrdering);
+      const Ordering& ordering) const {
+    Matrix augmented = augmentedHessian(ordering);
+    size_t n = augmented.rows() - 1;
+    return make_pair(augmented.topLeftCorner(n, n), augmented.topRightCorner(n, 1));
+  }
+
+  /* ************************************************************************* */
+  pair<Matrix, Vector> GaussianFactorGraph::hessian() const {
+    Matrix augmented = augmentedHessian();
     size_t n = augmented.rows() - 1;
     return make_pair(augmented.topLeftCorner(n, n), augmented.topRightCorner(n, 1));
   }
@@ -226,8 +255,7 @@ namespace gtsam {
     VectorValues d;
     for (const sharedFactor& factor : *this) {
       if(factor){
-        VectorValues di = factor->hessianDiagonal();
-        d.addInPlace_(di);
+        factor->hessianDiagonalAdd(d);
       }
     }
     return d;
@@ -253,8 +281,13 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  VectorValues GaussianFactorGraph::optimize(OptionalOrdering ordering, const Eliminate& function) const
-  {
+  VectorValues GaussianFactorGraph::optimize(const Eliminate& function) const {
+    gttic(GaussianFactorGraph_optimize);
+    return BaseEliminateable::eliminateMultifrontal(function)->optimize();
+  }
+
+  /* ************************************************************************* */
+  VectorValues GaussianFactorGraph::optimize(const Ordering& ordering, const Eliminate& function) const {
     gttic(GaussianFactorGraph_optimize);
     return BaseEliminateable::eliminateMultifrontal(ordering, function)->optimize();
   }
@@ -460,4 +493,11 @@ namespace gtsam {
     return e;
   }
 
+  /* ************************************************************************* */
+  /** \deprecated */
+  VectorValues GaussianFactorGraph::optimize(boost::none_t,
+    const Eliminate& function) const {
+      return optimize(function);
+  }
+
 } // namespace gtsam
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 49cba482d..2b9e8e675 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -201,7 +201,16 @@ namespace gtsam {
      * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$.  See also
      * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
      */
-    Matrix augmentedJacobian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
+    Matrix augmentedJacobian(const Ordering& ordering) const;
+    
+    /**
+     * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$
+     * Jacobian matrix, augmented with b with the noise models baked
+     * into A and b.  The negative log-likelihood is
+     * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$.  See also
+     * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
+     */
+    Matrix augmentedJacobian() const;
 
     /**
      * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
@@ -210,7 +219,16 @@ namespace gtsam {
      * GaussianFactorGraph::augmentedJacobian and
      * GaussianFactorGraph::sparseJacobian.
      */
-    std::pair<Matrix,Vector> jacobian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
+    std::pair<Matrix,Vector> jacobian(const Ordering& ordering) const;
+
+    /**
+     * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
+     * with the noise models baked into A and b. The negative log-likelihood
+     * is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$.  See also
+     * GaussianFactorGraph::augmentedJacobian and
+     * GaussianFactorGraph::sparseJacobian.
+     */
+    std::pair<Matrix,Vector> jacobian() const;
 
     /**
      * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian
@@ -223,7 +241,20 @@ namespace gtsam {
      and the negative log-likelihood is
      \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
      */
-    Matrix augmentedHessian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
+    Matrix augmentedHessian(const Ordering& ordering) const;
+
+    /**
+     * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian
+     * matrix, augmented with the information vector \f$ \eta \f$.  The
+     * augmented Hessian is
+     \f[ \left[ \begin{array}{ccc}
+     \Lambda & \eta \\
+     \eta^T & c
+     \end{array} \right] \f]
+     and the negative log-likelihood is
+     \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
+     */
+    Matrix augmentedHessian() const;
 
     /**
      * Return the dense Hessian \f$ \Lambda \f$ and information vector
@@ -231,7 +262,15 @@ namespace gtsam {
      * is \frac{1}{2} x^T \Lambda x + \eta^T x + c.  See also
      * GaussianFactorGraph::augmentedHessian.
      */
-    std::pair<Matrix,Vector> hessian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
+    std::pair<Matrix,Vector> hessian(const Ordering& ordering) const;
+
+    /**
+     * Return the dense Hessian \f$ \Lambda \f$ and information vector
+     * \f$ \eta \f$, with the noise models baked in. The negative log-likelihood
+     * is \frac{1}{2} x^T \Lambda x + \eta^T x + c.  See also
+     * GaussianFactorGraph::augmentedHessian.
+     */
+    std::pair<Matrix,Vector> hessian() const;
 
     /** Return only the diagonal of the Hessian A'*A, as a VectorValues */
     virtual VectorValues hessianDiagonal() const;
@@ -243,7 +282,14 @@ namespace gtsam {
      *  the dense elimination function specified in \c function (default EliminatePreferCholesky),
      *  followed by back-substitution in the Bayes tree resulting from elimination.  Is equivalent
      *  to calling graph.eliminateMultifrontal()->optimize(). */
-    VectorValues optimize(OptionalOrdering ordering = boost::none,
+    VectorValues optimize(
+      const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
+
+    /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using
+     *  the dense elimination function specified in \c function (default EliminatePreferCholesky),
+     *  followed by back-substitution in the Bayes tree resulting from elimination.  Is equivalent
+     *  to calling graph.eliminateMultifrontal()->optimize(). */
+    VectorValues optimize(const Ordering&,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
 
     /**
@@ -329,6 +375,12 @@ namespace gtsam {
       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
     }
 
+  public:
+
+    /** \deprecated */
+    VectorValues optimize(boost::none_t,
+      const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
+
   };
 
   /**
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index c208259b8..7bf65353b 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -250,10 +250,10 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) :
 
 /* ************************************************************************* */
 HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
-    boost::optional<const Scatter&> scatter) {
+    const Scatter& scatter) {
   gttic(HessianFactor_MergeConstructor);
 
-  Allocate(scatter ? *scatter : Scatter(factors));
+  Allocate(scatter);
 
   // Form A' * A
   gttic(update);
@@ -302,12 +302,14 @@ Matrix HessianFactor::information() const {
 }
 
 /* ************************************************************************* */
-VectorValues HessianFactor::hessianDiagonal() const {
-  VectorValues d;
+void HessianFactor::hessianDiagonalAdd(VectorValues &d) const {
   for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
-    d.emplace(keys_[j], info_.diagonal(j));
+    auto result = d.emplace(keys_[j], info_.diagonal(j));
+    if(!result.second) {
+      // if emplace fails, it returns an iterator to the existing element, which we add to:
+      result.first->second += info_.diagonal(j);
+    }
   }
-  return d;
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h
index cb9da0f1a..64b764087 100644
--- a/gtsam/linear/HessianFactor.h
+++ b/gtsam/linear/HessianFactor.h
@@ -176,31 +176,38 @@ namespace gtsam {
 
     /** Combine a set of factors into a single dense HessianFactor */
     explicit HessianFactor(const GaussianFactorGraph& factors,
-      boost::optional<const Scatter&> scatter = boost::none);
+      const Scatter& scatter);
+
+    /** Combine a set of factors into a single dense HessianFactor */
+    explicit HessianFactor(const GaussianFactorGraph& factors)
+        : HessianFactor(factors, Scatter(factors)) {}
 
     /** Destructor */
     virtual ~HessianFactor() {}
 
     /** Clone this HessianFactor */
-    virtual GaussianFactor::shared_ptr clone() const {
+    GaussianFactor::shared_ptr clone() const override {
       return boost::make_shared<HessianFactor>(*this); }
 
     /** Print the factor for debugging and testing (implementing Testable) */
-    virtual void print(const std::string& s = "",
-        const KeyFormatter& formatter = DefaultKeyFormatter) const;
+    void print(const std::string& s = "",
+        const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
     /** Compare to another factor for testing (implementing Testable) */
-    virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
+    bool equals(const GaussianFactor& lf, double tol = 1e-9) const override;
 
-    /** Evaluate the factor error f(x), see above. */
-    virtual double error(const VectorValues& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */
+    /** 
+     * Evaluate the factor error f(x). 
+     * returns 0.5*[x -1]'*H*[x -1] (also see constructor documentation)
+     */
+    double error(const VectorValues& c) const override;
 
     /** Return the dimension of the variable pointed to by the given key iterator
      * todo: Remove this in favor of keeping track of dimensions with variables?
      * @param variable An iterator pointing to the slot in this factor.  You can
      * use, for example, begin() + 2 to get the 3rd variable in this factor.
      */
-    virtual DenseIndex getDim(const_iterator variable) const {
+    DenseIndex getDim(const_iterator variable) const override {
       return info_.getDim(std::distance(begin(), variable));
     }
 
@@ -212,10 +219,10 @@ namespace gtsam {
      * stored stored in this factor.
      * @return a HessianFactor with negated Hessian matrices
      */
-    virtual GaussianFactor::shared_ptr negate() const;
+    GaussianFactor::shared_ptr negate() const override;
 
     /** Check if the factor is empty.  TODO: How should this be defined? */
-    virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; }
+    bool empty() const override { return size() == 0 /*|| rows() == 0*/; }
 
     /** Return the constant term \f$ f \f$ as described above
      * @return The constant term \f$ f \f$
@@ -276,7 +283,7 @@ namespace gtsam {
      * representation of the augmented information matrix, which stores only the
      * upper triangle.
      */
-    virtual Matrix augmentedInformation() const;
+    Matrix augmentedInformation() const override;
 
     /// Return self-adjoint view onto the information matrix (NOT augmented).
     Eigen::SelfAdjointView<SymmetricBlockMatrix::constBlock, Eigen::Upper> informationView() const;
@@ -284,33 +291,36 @@ namespace gtsam {
     /** Return the non-augmented information matrix represented by this
      * GaussianFactor.
      */
-    virtual Matrix information() const;
+    Matrix information() const override;
 
-    /// Return the diagonal of the Hessian for this factor
-    virtual VectorValues hessianDiagonal() const;
+    /// Add the current diagonal to a VectorValues instance
+    void hessianDiagonalAdd(VectorValues& d) const override;
+
+    /// Using the base method
+    using Base::hessianDiagonal;
 
     /// Raw memory access version of hessianDiagonal
-    virtual void hessianDiagonal(double* d) const;
+    void hessianDiagonal(double* d) const override;
 
     /// Return the block diagonal of the Hessian for this factor
-    virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
+    std::map<Key,Matrix> hessianBlockDiagonal() const override;
 
     /// Return (dense) matrix associated with factor
-    virtual std::pair<Matrix, Vector> jacobian() const;
+    std::pair<Matrix, Vector> jacobian() const override;
 
     /**
      * Return (dense) matrix associated with factor
      * The returned system is an augmented matrix: [A b]
      * @param set weight to use whitening to bake in weights
      */
-    virtual Matrix augmentedJacobian() const;
+    Matrix augmentedJacobian() const override;
 
     /** Update an information matrix by adding the information corresponding to this factor
      * (used internally during elimination).
      * @param keys THe ordered vector of keys for the information matrix to be updated
      * @param info The information matrix to be updated
      */
-    void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const;
+    void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const override;
 
     /** Update another Hessian factor
      * @param other the HessianFactor to be updated
@@ -321,19 +331,19 @@ namespace gtsam {
     }
 
     /** y += alpha * A'*A*x */
-    void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const;
+    void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const override;
 
     /// eta for Hessian
-    VectorValues gradientAtZero() const;
+    VectorValues gradientAtZero() const override;
 
     /// Raw memory access version of gradientAtZero
-    virtual void gradientAtZero(double* d) const;
+    void gradientAtZero(double* d) const override;
 
     /**
      * Compute the gradient at a key:
      *      \grad f(x_i) = \sum_j G_ij*x_j - g_i
      */
-    Vector gradient(Key key, const VectorValues& x) const;
+    Vector gradient(Key key, const VectorValues& x) const override;
 
     /**
      *  In-place elimination that returns a conditional on (ordered) keys specified, and leaves
diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp
index c7d4e5405..c5007206d 100644
--- a/gtsam/linear/IterativeSolver.cpp
+++ b/gtsam/linear/IterativeSolver.cpp
@@ -115,8 +115,11 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) {
   size_t start = 0;
 
   for (size_t i = 0; i < n; ++i) {
-    const size_t key = ordering_[i];
-    const size_t dim = colspec.find(key)->second;
+    const Key key = ordering_[i];
+    const auto it_key = colspec.find(key);
+    if (it_key==colspec.end())
+      throw std::runtime_error("KeyInfo: Inconsistency in key-dim map");
+    const size_t dim = it_key->second;
     this->emplace(key, KeyInfoEntry(i, dim, start));
     start += dim;
   }
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 06f6b3bed..b8a08be9e 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -220,60 +220,13 @@ FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
 }
 
 /* ************************************************************************* */
-JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
-    boost::optional<const Ordering&> ordering,
-    boost::optional<const VariableSlots&> p_variableSlots) {
-  gttic(JacobianFactor_combine_constructor);
-
-  // Compute VariableSlots if one was not provided
-  // Binds reference, does not copy VariableSlots
-  const VariableSlots & variableSlots =
-      p_variableSlots ? p_variableSlots.get() : VariableSlots(graph);
+void JacobianFactor::JacobianFactorHelper(const GaussianFactorGraph& graph,
+    const FastVector<VariableSlots::const_iterator>& orderedSlots) {
 
   // Cast or convert to Jacobians
   FastVector<JacobianFactor::shared_ptr> jacobians = _convertOrCastToJacobians(
       graph);
 
-  gttic(Order_slots);
-  // Order variable slots - we maintain the vector of ordered slots, as well as keep a list
-  // 'unorderedSlots' of any variables discovered that are not in the ordering.  Those will then
-  // be added after all of the ordered variables.
-  FastVector<VariableSlots::const_iterator> orderedSlots;
-  orderedSlots.reserve(variableSlots.size());
-  if (ordering) {
-    // If an ordering is provided, arrange the slots first that ordering
-    FastList<VariableSlots::const_iterator> unorderedSlots;
-    size_t nOrderingSlotsUsed = 0;
-    orderedSlots.resize(ordering->size());
-    FastMap<Key, size_t> inverseOrdering = ordering->invert();
-    for (VariableSlots::const_iterator item = variableSlots.begin();
-        item != variableSlots.end(); ++item) {
-      FastMap<Key, size_t>::const_iterator orderingPosition =
-          inverseOrdering.find(item->first);
-      if (orderingPosition == inverseOrdering.end()) {
-        unorderedSlots.push_back(item);
-      } else {
-        orderedSlots[orderingPosition->second] = item;
-        ++nOrderingSlotsUsed;
-      }
-    }
-    if (nOrderingSlotsUsed != ordering->size())
-      throw std::invalid_argument(
-          "The ordering provided to the JacobianFactor combine constructor\n"
-              "contained extra variables that did not appear in the factors to combine.");
-    // Add the remaining slots
-    for(VariableSlots::const_iterator item: unorderedSlots) {
-      orderedSlots.push_back(item);
-    }
-  } else {
-    // If no ordering is provided, arrange the slots as they were, which will be sorted
-    // numerically since VariableSlots uses a map sorting on Key.
-    for (VariableSlots::const_iterator item = variableSlots.begin();
-        item != variableSlots.end(); ++item)
-      orderedSlots.push_back(item);
-  }
-  gttoc(Order_slots);
-
   // Count dimensions
   FastVector<DenseIndex> varDims;
   DenseIndex m, n;
@@ -344,6 +297,127 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
     this->setModel(anyConstrained, *sigmas);
 }
 
+/* ************************************************************************* */
+// Order variable slots - we maintain the vector of ordered slots, as well as keep a list
+// 'unorderedSlots' of any variables discovered that are not in the ordering.  Those will then
+// be added after all of the ordered variables.
+FastVector<VariableSlots::const_iterator> orderedSlotsHelper(
+    const Ordering& ordering,
+    const VariableSlots& variableSlots) {
+  gttic(Order_slots);
+  
+  FastVector<VariableSlots::const_iterator> orderedSlots;
+  orderedSlots.reserve(variableSlots.size());
+  
+  // If an ordering is provided, arrange the slots first that ordering
+  FastList<VariableSlots::const_iterator> unorderedSlots;
+  size_t nOrderingSlotsUsed = 0;
+  orderedSlots.resize(ordering.size());
+  FastMap<Key, size_t> inverseOrdering = ordering.invert();
+  for (VariableSlots::const_iterator item = variableSlots.begin();
+      item != variableSlots.end(); ++item) {
+    FastMap<Key, size_t>::const_iterator orderingPosition =
+        inverseOrdering.find(item->first);
+    if (orderingPosition == inverseOrdering.end()) {
+      unorderedSlots.push_back(item);
+    } else {
+      orderedSlots[orderingPosition->second] = item;
+      ++nOrderingSlotsUsed;
+    }
+  }
+  if (nOrderingSlotsUsed != ordering.size())
+    throw std::invalid_argument(
+        "The ordering provided to the JacobianFactor combine constructor\n"
+            "contained extra variables that did not appear in the factors to combine.");
+  // Add the remaining slots
+  for(VariableSlots::const_iterator item: unorderedSlots) {
+    orderedSlots.push_back(item);
+  }
+
+  gttoc(Order_slots);
+
+  return orderedSlots;
+}
+
+/* ************************************************************************* */
+JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph) {
+  gttic(JacobianFactor_combine_constructor);
+
+  // Compute VariableSlots if one was not provided
+  // Binds reference, does not copy VariableSlots
+  const VariableSlots & variableSlots = VariableSlots(graph);
+
+  gttic(Order_slots);
+  // Order variable slots - we maintain the vector of ordered slots, as well as keep a list
+  // 'unorderedSlots' of any variables discovered that are not in the ordering.  Those will then
+  // be added after all of the ordered variables.
+  FastVector<VariableSlots::const_iterator> orderedSlots;
+  orderedSlots.reserve(variableSlots.size());
+  
+  // If no ordering is provided, arrange the slots as they were, which will be sorted
+  // numerically since VariableSlots uses a map sorting on Key.
+  for (VariableSlots::const_iterator item = variableSlots.begin();
+      item != variableSlots.end(); ++item)
+    orderedSlots.push_back(item);
+  gttoc(Order_slots);
+
+  JacobianFactorHelper(graph, orderedSlots);
+}
+
+/* ************************************************************************* */
+JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
+    const VariableSlots& p_variableSlots) {
+  gttic(JacobianFactor_combine_constructor);
+
+  // Binds reference, does not copy VariableSlots
+  const VariableSlots & variableSlots = p_variableSlots;
+
+  gttic(Order_slots);
+  // Order variable slots - we maintain the vector of ordered slots, as well as keep a list
+  // 'unorderedSlots' of any variables discovered that are not in the ordering.  Those will then
+  // be added after all of the ordered variables.
+  FastVector<VariableSlots::const_iterator> orderedSlots;
+  orderedSlots.reserve(variableSlots.size());
+  
+  // If no ordering is provided, arrange the slots as they were, which will be sorted
+  // numerically since VariableSlots uses a map sorting on Key.
+  for (VariableSlots::const_iterator item = variableSlots.begin();
+      item != variableSlots.end(); ++item)
+    orderedSlots.push_back(item);
+  gttoc(Order_slots);
+
+  JacobianFactorHelper(graph, orderedSlots);
+}
+
+/* ************************************************************************* */
+JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
+    const Ordering& ordering) {
+  gttic(JacobianFactor_combine_constructor);
+  
+  // Compute VariableSlots if one was not provided
+  // Binds reference, does not copy VariableSlots
+  const VariableSlots & variableSlots = VariableSlots(graph);
+
+  // Order variable slots
+  FastVector<VariableSlots::const_iterator> orderedSlots =
+    orderedSlotsHelper(ordering, variableSlots);
+
+  JacobianFactorHelper(graph, orderedSlots);
+}
+
+/* ************************************************************************* */
+JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
+    const Ordering& ordering,
+    const VariableSlots& p_variableSlots) {
+  gttic(JacobianFactor_combine_constructor);
+  
+  // Order variable slots
+  FastVector<VariableSlots::const_iterator> orderedSlots =
+    orderedSlotsHelper(ordering, p_variableSlots);
+
+  JacobianFactorHelper(graph, orderedSlots);
+}
+
 /* ************************************************************************* */
 void JacobianFactor::print(const string& s,
     const KeyFormatter& formatter) const {
@@ -439,8 +513,10 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const {
 
 /* ************************************************************************* */
 double JacobianFactor::error(const VectorValues& c) const {
-  Vector weighted = error_vector(c);
-  return 0.5 * weighted.dot(weighted);
+  Vector e = unweighted_error(c);
+  // Use the noise model distance function to get the correct error if available.
+  if (model_) return 0.5 * model_->squaredMahalanobisDistance(e);
+  return 0.5 * e.dot(e);
 }
 
 /* ************************************************************************* */
@@ -466,21 +542,31 @@ Matrix JacobianFactor::information() const {
 }
 
 /* ************************************************************************* */
-VectorValues JacobianFactor::hessianDiagonal() const {
-  VectorValues d;
+void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const {
   for (size_t pos = 0; pos < size(); ++pos) {
     Key j = keys_[pos];
     size_t nj = Ab_(pos).cols();
-    Vector dj(nj);
+    auto result = d.emplace(j, nj);
+
+    Vector& dj = result.first->second;
+
     for (size_t k = 0; k < nj; ++k) {
-      Vector column_k = Ab_(pos).col(k);
-      if (model_)
-        model_->whitenInPlace(column_k);
-      dj(k) = dot(column_k, column_k);
+      Eigen::Ref<const Vector> column_k = Ab_(pos).col(k);
+      if (model_) {
+        Vector column_k_copy = column_k;
+        model_->whitenInPlace(column_k_copy);
+        if(!result.second)
+          dj(k) += dot(column_k_copy, column_k_copy);
+        else
+          dj(k) = dot(column_k_copy, column_k_copy);
+      } else {
+        if (!result.second)
+          dj(k) += dot(column_k, column_k);
+        else
+          dj(k) = dot(column_k, column_k);
+      }
     }
-    d.emplace(j, dj);
   }
-  return d;
 }
 
 /* ************************************************************************* */
@@ -764,7 +850,7 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFronta
 
   if (!model_) {
     throw std::invalid_argument(
-        "JacobianFactor::splitConditional cannot be  given a NULL noise model");
+        "JacobianFactor::splitConditional cannot be  given a nullptr noise model");
   }
 
   if (nrFrontals > size()) {
diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h
index 36d1e12da..49c47c7f0 100644
--- a/gtsam/linear/JacobianFactor.h
+++ b/gtsam/linear/JacobianFactor.h
@@ -22,8 +22,11 @@
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/base/VerticalBlockMatrix.h>
 #include <gtsam/global_includes.h>
+#include <gtsam/inference/VariableSlots.h>
 
 #include <boost/make_shared.hpp>
+#include <boost/serialization/version.hpp>
+#include <boost/serialization/split_member.hpp>
 
 namespace gtsam {
 
@@ -147,32 +150,55 @@ namespace gtsam {
     JacobianFactor(
       const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal());
 
+    /**
+     * Build a dense joint factor from all the factors in a factor graph.  If a VariableSlots
+     * structure computed for \c graph is already available, providing it will reduce the amount of
+     * computation performed. */
+    explicit JacobianFactor(
+      const GaussianFactorGraph& graph);
+
     /**
      * Build a dense joint factor from all the factors in a factor graph.  If a VariableSlots
      * structure computed for \c graph is already available, providing it will reduce the amount of
      * computation performed. */
     explicit JacobianFactor(
       const GaussianFactorGraph& graph,
-      boost::optional<const Ordering&> ordering = boost::none,
-      boost::optional<const VariableSlots&> p_variableSlots = boost::none);
+      const VariableSlots& p_variableSlots);
+    
+    /**
+     * Build a dense joint factor from all the factors in a factor graph.  If a VariableSlots
+     * structure computed for \c graph is already available, providing it will reduce the amount of
+     * computation performed. */
+    explicit JacobianFactor(
+      const GaussianFactorGraph& graph,
+      const Ordering& ordering);
+    
+    /**
+     * Build a dense joint factor from all the factors in a factor graph.  If a VariableSlots
+     * structure computed for \c graph is already available, providing it will reduce the amount of
+     * computation performed. */
+    explicit JacobianFactor(
+      const GaussianFactorGraph& graph,
+      const Ordering& ordering,
+      const VariableSlots& p_variableSlots);
 
     /** Virtual destructor */
     virtual ~JacobianFactor() {}
 
     /** Clone this JacobianFactor */
-    virtual GaussianFactor::shared_ptr clone() const {
+    GaussianFactor::shared_ptr clone() const override {
       return boost::static_pointer_cast<GaussianFactor>(
           boost::make_shared<JacobianFactor>(*this));
     }
 
     // Implementing Testable interface
-    virtual void print(const std::string& s = "",
-      const KeyFormatter& formatter = DefaultKeyFormatter) const;
-    virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
+    void print(const std::string& s = "",
+      const KeyFormatter& formatter = DefaultKeyFormatter) const override;
+    bool equals(const GaussianFactor& lf, double tol = 1e-9) const override;
 
     Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */
     Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */
-    virtual double error(const VectorValues& c) const; /**  0.5*(A*x-b)'*D*(A*x-b) */
+    double error(const VectorValues& c) const override; /**  0.5*(A*x-b)'*D*(A*x-b) */
 
     /** Return the augmented information matrix represented by this GaussianFactor.
      * The augmented information matrix contains the information matrix with an
@@ -182,26 +208,29 @@ namespace gtsam {
      * augmented information matrix is described in more detail in HessianFactor,
      * which in fact stores an augmented information matrix.
      */
-    virtual Matrix augmentedInformation() const;
+    Matrix augmentedInformation() const override;
 
     /** Return the non-augmented information matrix represented by this
      * GaussianFactor.
      */
-    virtual Matrix information() const;
+    Matrix information() const override;
 
-    /// Return the diagonal of the Hessian for this factor
-    virtual VectorValues hessianDiagonal() const;
+    /// Using the base method
+    using Base::hessianDiagonal;
+
+    /// Add the current diagonal to a VectorValues instance
+    void hessianDiagonalAdd(VectorValues& d) const override;
 
     /// Raw memory access version of hessianDiagonal
-    virtual void hessianDiagonal(double* d) const;
+    void hessianDiagonal(double* d) const override;
 
     /// Return the block diagonal of the Hessian for this factor
-    virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
+    std::map<Key,Matrix> hessianBlockDiagonal() const override;
 
     /**
      * @brief Returns (dense) A,b pair associated with factor, bakes in the weights
      */
-    virtual std::pair<Matrix, Vector> jacobian() const;
+    std::pair<Matrix, Vector> jacobian() const override;
 
     /**
      * @brief Returns (dense) A,b pair associated with factor, does not bake in weights
@@ -211,7 +240,7 @@ namespace gtsam {
     /** Return (dense) matrix associated with factor.  The returned system is an augmented matrix:
     *   [A b]
     *  weights are baked in */
-    virtual Matrix augmentedJacobian() const;
+    Matrix augmentedJacobian() const override;
 
     /** Return (dense) matrix associated with factor.  The returned system is an augmented matrix:
     *   [A b]
@@ -229,10 +258,10 @@ namespace gtsam {
      * stored stored in this factor.
      * @return a HessianFactor with negated Hessian matrices
      */
-    virtual GaussianFactor::shared_ptr negate() const;
+    GaussianFactor::shared_ptr negate() const override;
 
     /** Check if the factor is empty.  TODO: How should this be defined? */
-    virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; }
+    bool empty() const override { return size() == 0 /*|| rows() == 0*/; }
 
     /** is noise model constrained ? */
     bool isConstrained() const {
@@ -242,7 +271,9 @@ namespace gtsam {
     /** Return the dimension of the variable pointed to by the given key iterator
      * todo: Remove this in favor of keeping track of dimensions with variables?
      */
-    virtual DenseIndex getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); }
+    DenseIndex getDim(const_iterator variable) const override { 
+      return Ab_(variable - begin()).cols(); 
+    }
 
     /**
      * return the number of rows in the corresponding linear system
@@ -283,17 +314,19 @@ namespace gtsam {
      * @param scatter A mapping from variable index to slot index in this HessianFactor
      * @param info The information matrix to be updated
      */
-    void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const;
+    void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const override;
 
     /** Return A*x */
     Vector operator*(const VectorValues& x) const;
 
-    /** x += A'*e.  If x is initially missing any values, they are created and assumed to start as
-     *  zero vectors. */
-    void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const;
+    /** x += alpha * A'*e.  If x is initially missing any values, they are
+     * created and assumed to start as zero vectors. */
+    void transposeMultiplyAdd(double alpha, const Vector& e,
+                              VectorValues& x) const;
 
     /** y += alpha * A'*A*x */
-    void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const;
+    void multiplyHessianAdd(double alpha, const VectorValues& x,
+                            VectorValues& y) const override;
 
     /**
      * Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
@@ -307,13 +340,13 @@ namespace gtsam {
         const std::vector<size_t>& accumulatedDims) const;
 
     /// A'*b for Jacobian
-    VectorValues gradientAtZero() const;
+    VectorValues gradientAtZero() const override;
 
     /// A'*b for Jacobian (raw memory version)
-    virtual void gradientAtZero(double* d) const;
+    void gradientAtZero(double* d) const override;
 
     /// Compute the gradient wrt a key at any values
-    Vector gradient(Key key, const VectorValues& x) const;
+    Vector gradient(Key key, const VectorValues& x) const override;
 
     /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */
     JacobianFactor whiten() const;
@@ -356,11 +389,19 @@ namespace gtsam {
 
   private:
 
+    /**
+     * Helper function for public constructors:
+     * Build a dense joint factor from all the factors in a factor graph.  Takes in
+     * ordered variable slots */
+    void JacobianFactorHelper(
+      const GaussianFactorGraph& graph,
+      const FastVector<VariableSlots::const_iterator>& orderedSlots);
+
     /** Unsafe Constructor that creates an uninitialized Jacobian of right size
      *  @param keys in some order
      *  @param diemnsions of the variables in same order
      *  @param m output dimension
-     *  @param model noise model (default NULL)
+     *  @param model noise model (default nullptr)
      */
     template<class KEYS, class DIMENSIONS>
     JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
@@ -374,13 +415,41 @@ namespace gtsam {
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-      ar & BOOST_SERIALIZATION_NVP(Ab_);
-      ar & BOOST_SERIALIZATION_NVP(model_);
+    void save(ARCHIVE & ar, const unsigned int version) const {
+      // TODO(fan): This is a hack for Boost < 1.66
+      // We really need to introduce proper versioning in the archives
+      // As otherwise this will not read objects serialized by older
+      // versions of GTSAM
+      ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+      ar << BOOST_SERIALIZATION_NVP(Ab_);
+      bool model_null = false;
+      if(model_.get() == nullptr) {
+        model_null = true;
+        ar << boost::serialization::make_nvp("model_null", model_null);
+      } else {
+        ar << boost::serialization::make_nvp("model_null", model_null);
+        ar << BOOST_SERIALIZATION_NVP(model_);
+      }
     }
-  }; // JacobianFactor
 
+    template<class ARCHIVE>
+    void load(ARCHIVE & ar, const unsigned int version) {
+      // invoke serialization of the base class
+      ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+      ar >> BOOST_SERIALIZATION_NVP(Ab_);
+      if (version < 1) {
+        ar >> BOOST_SERIALIZATION_NVP(model_);
+      } else {
+        bool model_null;
+        ar >> BOOST_SERIALIZATION_NVP(model_null);
+        if (!model_null) {
+          ar >> BOOST_SERIALIZATION_NVP(model_);
+        }
+      }
+    }
+
+    BOOST_SERIALIZATION_SPLIT_MEMBER()
+  }; // JacobianFactor
 /// traits
 template<>
 struct traits<JacobianFactor> : public Testable<JacobianFactor> {
@@ -388,6 +457,8 @@ struct traits<JacobianFactor> : public Testable<JacobianFactor> {
 
 } // \ namespace gtsam
 
+BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1)
+
 #include <gtsam/linear/JacobianFactor-inl.h>
 
 
diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp
new file mode 100644
index 000000000..bf799a2ba
--- /dev/null
+++ b/gtsam/linear/LossFunctions.cpp
@@ -0,0 +1,424 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 NoiseModel.cpp
+ * @date Jan 13, 2010
+ * @author Richard Roberts
+ * @author Frank Dellaert
+ */
+
+#include <gtsam/linear/LossFunctions.h>
+
+#include <iostream>
+
+using namespace std;
+
+namespace gtsam {
+namespace noiseModel {
+
+/* ************************************************************************* */
+// M-Estimator
+/* ************************************************************************* */
+
+namespace mEstimator {
+
+Vector Base::weight(const Vector& error) const {
+  const size_t n = error.rows();
+  Vector w(n);
+  for (size_t i = 0; i < n; ++i)
+    w(i) = weight(error(i));
+  return w;
+}
+
+// The following three functions re-weight block matrices and a vector
+// according to their weight implementation
+
+void Base::reweight(Vector& error) const {
+  if (reweight_ == Block) {
+    const double w = sqrtWeight(error.norm());
+    error *= w;
+  } else {
+    error.array() *= weight(error).cwiseSqrt().array();
+  }
+}
+
+// Reweight n block matrices with one error vector
+void Base::reweight(vector<Matrix> &A, Vector &error) const {
+  if ( reweight_ == Block ) {
+    const double w = sqrtWeight(error.norm());
+    for(Matrix& Aj: A) {
+      Aj *= w;
+    }
+    error *= w;
+  }
+  else {
+    const Vector W = sqrtWeight(error);
+    for(Matrix& Aj: A) {
+      vector_scale_inplace(W,Aj);
+    }
+    error = W.cwiseProduct(error);
+  }
+}
+
+// Reweight one block matrix with one error vector
+void Base::reweight(Matrix &A, Vector &error) const {
+  if ( reweight_ == Block ) {
+    const double w = sqrtWeight(error.norm());
+    A *= w;
+    error *= w;
+  }
+  else {
+    const Vector W = sqrtWeight(error);
+    vector_scale_inplace(W,A);
+    error = W.cwiseProduct(error);
+  }
+}
+
+// Reweight two block matrix with one error vector
+void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const {
+  if ( reweight_ == Block ) {
+    const double w = sqrtWeight(error.norm());
+    A1 *= w;
+    A2 *= w;
+    error *= w;
+  }
+  else {
+    const Vector W = sqrtWeight(error);
+    vector_scale_inplace(W,A1);
+    vector_scale_inplace(W,A2);
+    error = W.cwiseProduct(error);
+  }
+}
+
+// Reweight three block matrix with one error vector
+void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const {
+  if ( reweight_ == Block ) {
+    const double w = sqrtWeight(error.norm());
+    A1 *= w;
+    A2 *= w;
+    A3 *= w;
+    error *= w;
+  }
+  else {
+    const Vector W = sqrtWeight(error);
+    vector_scale_inplace(W,A1);
+    vector_scale_inplace(W,A2);
+    vector_scale_inplace(W,A3);
+    error = W.cwiseProduct(error);
+  }
+}
+
+/* ************************************************************************* */
+// Null model
+/* ************************************************************************* */
+
+void Null::print(const std::string &s="") const
+{ cout << s << "null ()" << endl; }
+
+Null::shared_ptr Null::Create()
+{ return shared_ptr(new Null()); }
+
+/* ************************************************************************* */
+// Fair
+/* ************************************************************************* */
+
+Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) {
+  if (c_ <= 0) {
+    throw runtime_error("mEstimator Fair takes only positive double in constructor.");
+  }
+}
+
+double Fair::weight(double distance) const {
+  return 1.0 / (1.0 + std::abs(distance) / c_);
+}
+
+double Fair::loss(double distance) const {
+  const double absError = std::abs(distance);
+  const double normalizedError = absError / c_;
+  const double c_2 = c_ * c_;
+  return c_2 * (normalizedError - std::log1p(normalizedError));
+}
+
+void Fair::print(const std::string &s="") const
+{ cout << s << "fair (" << c_ << ")" << endl; }
+
+bool Fair::equals(const Base &expected, double tol) const {
+  const Fair* p = dynamic_cast<const Fair*> (&expected);
+  if (p == nullptr) return false;
+  return std::abs(c_ - p->c_ ) < tol;
+}
+
+Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight)
+{ return shared_ptr(new Fair(c, reweight)); }
+
+/* ************************************************************************* */
+// Huber
+/* ************************************************************************* */
+
+Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) {
+  if (k_ <= 0) {
+    throw runtime_error("mEstimator Huber takes only positive double in constructor.");
+  }
+}
+
+double Huber::weight(double distance) const {
+  const double absError = std::abs(distance);
+  return (absError <= k_) ? (1.0) : (k_ / absError);
+}
+
+double Huber::loss(double distance) const {
+  const double absError = std::abs(distance);
+  if (absError <= k_) {  // |x| <= k
+    return distance*distance / 2;
+  } else { // |x| > k
+    return k_ * (absError - (k_/2));
+  }
+}
+
+void Huber::print(const std::string &s="") const {
+  cout << s << "huber (" << k_ << ")" << endl;
+}
+
+bool Huber::equals(const Base &expected, double tol) const {
+  const Huber* p = dynamic_cast<const Huber*>(&expected);
+  if (p == nullptr) return false;
+  return std::abs(k_ - p->k_) < tol;
+}
+
+Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) {
+  return shared_ptr(new Huber(c, reweight));
+}
+
+/* ************************************************************************* */
+// Cauchy
+/* ************************************************************************* */
+
+Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), ksquared_(k * k) {
+  if (k <= 0) {
+    throw runtime_error("mEstimator Cauchy takes only positive double in constructor.");
+  }
+}
+
+double Cauchy::weight(double distance) const {
+  return ksquared_ / (ksquared_ + distance*distance);
+}
+
+double Cauchy::loss(double distance) const {
+  const double val = std::log1p(distance * distance / ksquared_);
+  return ksquared_ * val * 0.5;
+}
+
+void Cauchy::print(const std::string &s="") const {
+  cout << s << "cauchy (" << k_ << ")" << endl;
+}
+
+bool Cauchy::equals(const Base &expected, double tol) const {
+  const Cauchy* p = dynamic_cast<const Cauchy*>(&expected);
+  if (p == nullptr) return false;
+  return std::abs(ksquared_ - p->ksquared_) < tol;
+}
+
+Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) {
+  return shared_ptr(new Cauchy(c, reweight));
+}
+
+/* ************************************************************************* */
+// Tukey
+/* ************************************************************************* */
+
+Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {
+  if (c <= 0) {
+    throw runtime_error("mEstimator Tukey takes only positive double in constructor.");
+  }
+}
+
+double Tukey::weight(double distance) const {
+  if (std::abs(distance) <= c_) {
+    const double one_minus_xc2 = 1.0 - distance*distance/csquared_;
+    return one_minus_xc2 * one_minus_xc2;
+  }
+  return 0.0;
+}
+
+double Tukey::loss(double distance) const {
+  double absError = std::abs(distance);
+  if (absError <= c_) {
+    const double one_minus_xc2 = 1.0 - distance*distance/csquared_;
+    const double t = one_minus_xc2*one_minus_xc2*one_minus_xc2;
+    return csquared_ * (1 - t) / 6.0;
+  } else {
+    return csquared_ / 6.0;
+  }
+}
+
+void Tukey::print(const std::string &s="") const {
+  std::cout << s << ": Tukey (" << c_ << ")" << std::endl;
+}
+
+bool Tukey::equals(const Base &expected, double tol) const {
+  const Tukey* p = dynamic_cast<const Tukey*>(&expected);
+  if (p == nullptr) return false;
+  return std::abs(c_ - p->c_) < tol;
+}
+
+Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) {
+  return shared_ptr(new Tukey(c, reweight));
+}
+
+/* ************************************************************************* */
+// Welsch
+/* ************************************************************************* */
+
+Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
+
+double Welsch::weight(double distance) const {
+  const double xc2 = (distance*distance)/csquared_;
+  return std::exp(-xc2);
+}
+
+double Welsch::loss(double distance) const {
+  const double xc2 = (distance*distance)/csquared_;
+  return csquared_ * 0.5 * -std::expm1(-xc2);
+}
+
+void Welsch::print(const std::string &s="") const {
+  std::cout << s << ": Welsch (" << c_ << ")" << std::endl;
+}
+
+bool Welsch::equals(const Base &expected, double tol) const {
+  const Welsch* p = dynamic_cast<const Welsch*>(&expected);
+  if (p == nullptr) return false;
+  return std::abs(c_ - p->c_) < tol;
+}
+
+Welsch::shared_ptr Welsch::Create(double c, const ReweightScheme reweight) {
+  return shared_ptr(new Welsch(c, reweight));
+}
+
+/* ************************************************************************* */
+// GemanMcClure
+/* ************************************************************************* */
+GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight)
+  : Base(reweight), c_(c) {
+}
+
+double GemanMcClure::weight(double distance) const {
+  const double c2 = c_*c_;
+  const double c4 = c2*c2;
+  const double c2error = c2 + distance*distance;
+  return c4/(c2error*c2error);
+}
+
+double GemanMcClure::loss(double distance) const {
+  const double c2 = c_*c_;
+  const double error2 = distance*distance;
+  return 0.5 * (c2 * error2) / (c2 + error2);
+}
+
+void GemanMcClure::print(const std::string &s="") const {
+  std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl;
+}
+
+bool GemanMcClure::equals(const Base &expected, double tol) const {
+  const GemanMcClure* p = dynamic_cast<const GemanMcClure*>(&expected);
+  if (p == nullptr) return false;
+  return std::abs(c_ - p->c_) < tol;
+}
+
+GemanMcClure::shared_ptr GemanMcClure::Create(double c, const ReweightScheme reweight) {
+  return shared_ptr(new GemanMcClure(c, reweight));
+}
+
+/* ************************************************************************* */
+// DCS
+/* ************************************************************************* */
+DCS::DCS(double c, const ReweightScheme reweight)
+  : Base(reweight), c_(c) {
+}
+
+double DCS::weight(double distance) const {
+  const double e2 = distance*distance;
+  if (e2 > c_)
+  {
+    const double w = 2.0*c_/(c_ + e2);
+    return w*w;
+  }
+
+  return 1.0;
+}
+
+double DCS::loss(double distance) const {
+  // This is the simplified version of Eq 9 from (Agarwal13icra)
+  // after you simplify and cancel terms.
+  const double e2 = distance*distance;
+  const double e4 = e2*e2;
+  const double c2 = c_*c_;
+
+  return (c2*e2 + c_*e4) / ((e2 + c_)*(e2 + c_));
+}
+
+void DCS::print(const std::string &s="") const {
+  std::cout << s << ": DCS (" << c_ << ")" << std::endl;
+}
+
+bool DCS::equals(const Base &expected, double tol) const {
+  const DCS* p = dynamic_cast<const DCS*>(&expected);
+  if (p == nullptr) return false;
+  return std::abs(c_ - p->c_) < tol;
+}
+
+DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) {
+  return shared_ptr(new DCS(c, reweight));
+}
+
+/* ************************************************************************* */
+// L2WithDeadZone
+/* ************************************************************************* */
+
+L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight)
+ : Base(reweight), k_(k) {
+  if (k_ <= 0) {
+    throw runtime_error("mEstimator L2WithDeadZone takes only positive double in constructor.");
+  }
+}
+
+double L2WithDeadZone::weight(double distance) const {
+  // note that this code is slightly uglier than residual, because there are three distinct
+  // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two
+  // cases (deadzone, non-deadzone) in residual.
+  if (std::abs(distance) <= k_) return 0.0;
+  else if (distance > k_) return (-k_+distance)/distance;
+  else return (k_+distance)/distance;
+}
+
+double L2WithDeadZone::loss(double distance) const {
+  const double abs_error = std::abs(distance);
+  return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error);
+}
+
+void L2WithDeadZone::print(const std::string &s="") const {
+  std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl;
+}
+
+bool L2WithDeadZone::equals(const Base &expected, double tol) const {
+  const L2WithDeadZone* p = dynamic_cast<const L2WithDeadZone*>(&expected);
+  if (p == nullptr) return false;
+  return std::abs(k_ - p->k_) < tol;
+}
+
+L2WithDeadZone::shared_ptr L2WithDeadZone::Create(double k, const ReweightScheme reweight) {
+  return shared_ptr(new L2WithDeadZone(k, reweight));
+}
+
+} // namespace mEstimator
+} // namespace noiseModel
+} // gtsam
diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h
new file mode 100644
index 000000000..6a5dc5a26
--- /dev/null
+++ b/gtsam/linear/LossFunctions.h
@@ -0,0 +1,383 @@
+
+/* ----------------------------------------------------------------------------
+
+ * 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 NoiseModel.h
+ * @date  Jan 13, 2010
+ * @author Richard Roberts
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/dllexport.h>
+
+#include <boost/serialization/extended_type_info.hpp>
+#include <boost/serialization/nvp.hpp>
+#include <boost/serialization/optional.hpp>
+#include <boost/serialization/shared_ptr.hpp>
+#include <boost/serialization/singleton.hpp>
+
+namespace gtsam {
+namespace noiseModel {
+// clang-format off
+/**
+ * The mEstimator name space contains all robust error functions.
+ * It mirrors the exposition at
+ *  https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
+ * which talks about minimizing \sum \rho(r_i), where \rho is a loss function of choice.
+ *
+ * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples:
+ *
+ * Name        Symbol          Least-Squares   L1-norm    Huber
+ * Loss        \rho(x)         0.5*x^2         |x|        0.5*x^2 if |x|<k, 0.5*k^2 + k|x-k| otherwise
+ * Derivative  \phi(x)         x               sgn(x)     x       if |x|<k, k sgn(x)         otherwise
+ * Weight      w(x)=\phi(x)/x  1               1/|x|      1       if |x|<k, k/|x|            otherwise
+ *
+ * With these definitions, D(\rho(x), p) = \phi(x) D(x,p) = w(x) x D(x,p) = w(x) D(L2(x), p),
+ * and hence we can solve the equivalent weighted least squares problem \sum w(r_i) \rho(r_i)
+ *
+ * Each M-estimator in the mEstimator name space simply implements the above functions.
+ */
+// clang-format on
+namespace mEstimator {
+
+//---------------------------------------------------------------------------------------
+
+class GTSAM_EXPORT Base {
+ public:
+  enum ReweightScheme { Scalar, Block };
+  typedef boost::shared_ptr<Base> shared_ptr;
+
+ protected:
+  /** the rows can be weighted independently according to the error
+   * or uniformly with the norm of the right hand side */
+  ReweightScheme reweight_;
+
+ public:
+  Base(const ReweightScheme reweight = Block) : reweight_(reweight) {}
+  virtual ~Base() {}
+
+  /*
+   * This method is responsible for returning the total penalty for a given
+   * amount of error. For example, this method is responsible for implementing
+   * the quadratic function for an L2 penalty, the absolute value function for
+   * an L1 penalty, etc.
+   *
+   * TODO(mikebosse): When the loss function has as input the norm of the
+   * error vector, then it prevents implementations of asymmeric loss
+   * functions. It would be better for this function to accept the vector and
+   * internally call the norm if necessary.
+   */
+  virtual double loss(double distance) const { return 0; };
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+  virtual double residual(double distance) const { return loss(distance); };
+#endif
+
+  /*
+   * This method is responsible for returning the weight function for a given
+   * amount of error. The weight function is related to the analytic derivative
+   * of the loss function. See
+   *  https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
+   * for details. This method is required when optimizing cost functions with
+   * robust penalties using iteratively re-weighted least squares.
+   */
+  virtual double weight(double distance) const = 0;
+
+  virtual void print(const std::string &s) const = 0;
+  virtual bool equals(const Base &expected, double tol = 1e-8) const = 0;
+
+  double sqrtWeight(double distance) const { return std::sqrt(weight(distance)); }
+
+  /** produce a weight vector according to an error vector and the implemented
+   * robust function */
+  Vector weight(const Vector &error) const;
+
+  /** square root version of the weight function */
+  Vector sqrtWeight(const Vector &error) const {
+    return weight(error).cwiseSqrt();
+  }
+
+  /** reweight block matrices and a vector according to their weight
+   * implementation */
+  void reweight(Vector &error) const;
+  void reweight(std::vector<Matrix> &A, Vector &error) const;
+  void reweight(Matrix &A, Vector &error) const;
+  void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
+  void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_NVP(reweight_);
+  }
+};
+
+/// Null class should behave as Gaussian
+class GTSAM_EXPORT Null : public Base {
+ public:
+  typedef boost::shared_ptr<Null> shared_ptr;
+
+  Null(const ReweightScheme reweight = Block) : Base(reweight) {}
+  ~Null() {}
+  double weight(double /*error*/) const { return 1.0; }
+  double loss(double distance) const { return 0.5 * distance * distance; }
+  void print(const std::string &s) const;
+  bool equals(const Base & /*expected*/, double /*tol*/) const { return true; }
+  static shared_ptr Create();
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+  }
+};
+
+/// Fair implements the "Fair" robust error model (Zhang97ivc)
+class GTSAM_EXPORT Fair : public Base {
+ protected:
+  double c_;
+
+ public:
+  typedef boost::shared_ptr<Fair> shared_ptr;
+
+  Fair(double c = 1.3998, const ReweightScheme reweight = Block);
+  double weight(double distance) const override;
+  double loss(double distance) const override;
+  void print(const std::string &s) const override;
+  bool equals(const Base &expected, double tol = 1e-8) const override;
+  static shared_ptr Create(double c, const ReweightScheme reweight = Block);
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar &BOOST_SERIALIZATION_NVP(c_);
+  }
+};
+
+/// Huber implements the "Huber" robust error model (Zhang97ivc)
+class GTSAM_EXPORT Huber : public Base {
+ protected:
+  double k_;
+
+ public:
+  typedef boost::shared_ptr<Huber> shared_ptr;
+
+  Huber(double k = 1.345, const ReweightScheme reweight = Block);
+  double weight(double distance) const override;
+  double loss(double distance) const override;
+  void print(const std::string &s) const override;
+  bool equals(const Base &expected, double tol = 1e-8) const override;
+  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar &BOOST_SERIALIZATION_NVP(k_);
+  }
+};
+
+/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed
+/// by:
+///   Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
+///   Information Technology, Karlsruhe, Germany.
+///   oberlaender@fzi.de
+/// Thanks Jan!
+class GTSAM_EXPORT Cauchy : public Base {
+ protected:
+  double k_, ksquared_;
+
+ public:
+  typedef boost::shared_ptr<Cauchy> shared_ptr;
+
+  Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
+  double weight(double distance) const override;
+  double loss(double distance) const override;
+  void print(const std::string &s) const override;
+  bool equals(const Base &expected, double tol = 1e-8) const override;
+  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar &BOOST_SERIALIZATION_NVP(k_);
+  }
+};
+
+/// Tukey implements the "Tukey" robust error model (Zhang97ivc)
+class GTSAM_EXPORT Tukey : public Base {
+ protected:
+  double c_, csquared_;
+
+ public:
+  typedef boost::shared_ptr<Tukey> shared_ptr;
+
+  Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
+  double weight(double distance) const override;
+  double loss(double distance) const override;
+  void print(const std::string &s) const override;
+  bool equals(const Base &expected, double tol = 1e-8) const override;
+  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar &BOOST_SERIALIZATION_NVP(c_);
+  }
+};
+
+/// Welsch implements the "Welsch" robust error model (Zhang97ivc)
+class GTSAM_EXPORT Welsch : public Base {
+ protected:
+  double c_, csquared_;
+
+ public:
+  typedef boost::shared_ptr<Welsch> shared_ptr;
+
+  Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
+  double weight(double distance) const override;
+  double loss(double distance) const override;
+  void print(const std::string &s) const override;
+  bool equals(const Base &expected, double tol = 1e-8) const override;
+  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar &BOOST_SERIALIZATION_NVP(c_);
+  }
+};
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+/// @name Deprecated
+/// @{
+// Welsh implements the "Welsch" robust error model (Zhang97ivc)
+// This was misspelled in previous versions of gtsam and should be
+// removed in the future.
+using Welsh = Welsch;
+#endif
+
+/// GemanMcClure implements the "Geman-McClure" robust error model
+/// (Zhang97ivc).
+///
+/// Note that Geman-McClure weight function uses the parameter c == 1.0,
+/// but here it's allowed to use different values, so we actually have
+/// the generalized Geman-McClure from (Agarwal15phd).
+class GTSAM_EXPORT GemanMcClure : public Base {
+ public:
+  typedef boost::shared_ptr<GemanMcClure> shared_ptr;
+
+  GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
+  ~GemanMcClure() {}
+  double weight(double distance) const override;
+  double loss(double distance) const override;
+  void print(const std::string &s) const override;
+  bool equals(const Base &expected, double tol = 1e-8) const override;
+  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+
+ protected:
+  double c_;
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar &BOOST_SERIALIZATION_NVP(c_);
+  }
+};
+
+/// DCS implements the Dynamic Covariance Scaling robust error model
+/// from the paper Robust Map Optimization (Agarwal13icra).
+///
+/// Under the special condition of the parameter c == 1.0 and not
+/// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure.
+class GTSAM_EXPORT DCS : public Base {
+ public:
+  typedef boost::shared_ptr<DCS> shared_ptr;
+
+  DCS(double c = 1.0, const ReweightScheme reweight = Block);
+  ~DCS() {}
+  double weight(double distance) const override;
+  double loss(double distance) const override;
+  void print(const std::string &s) const override;
+  bool equals(const Base &expected, double tol = 1e-8) const override;
+  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+
+ protected:
+  double c_;
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar &BOOST_SERIALIZATION_NVP(c_);
+  }
+};
+
+/// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of
+/// width 2*k, centered at the origin. The resulting penalty within the dead
+/// zone is always zero, and grows quadratically outside the dead zone. In this
+/// sense, the L2WithDeadZone penalty is "robust to inliers", rather than being
+/// robust to outliers. This penalty can be used to create barrier functions in
+/// a general way.
+class GTSAM_EXPORT L2WithDeadZone : public Base {
+ protected:
+  double k_;
+
+ public:
+  typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
+
+  L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block);
+  double weight(double distance) const override;
+  double loss(double distance) const override;
+  void print(const std::string &s) const override;
+  bool equals(const Base &expected, double tol = 1e-8) const override;
+  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar &BOOST_SERIALIZATION_NVP(k_);
+  }
+};
+
+}  // namespace mEstimator
+}  // namespace noiseModel
+}  // namespace gtsam
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index a01233fad..f5ec95696 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -21,15 +21,12 @@
 
 #include <boost/format.hpp>
 #include <boost/make_shared.hpp>
-#include <boost/random/linear_congruential.hpp>
-#include <boost/random/normal_distribution.hpp>
-#include <boost/random/variate_generator.hpp>
 
-#include <limits>
-#include <iostream>
-#include <typeinfo>
-#include <stdexcept>
 #include <cmath>
+#include <iostream>
+#include <limits>
+#include <stdexcept>
+#include <typeinfo>
 
 using namespace std;
 
@@ -77,6 +74,13 @@ Vector Base::sigmas() const {
   throw("Base::sigmas: sigmas() not implemented for this noise model");
 }
 
+/* ************************************************************************* */
+double Base::squaredMahalanobisDistance(const Vector& v) const {
+  // Note: for Diagonal, which does ediv_, will be correct for constraints
+  Vector w = whiten(v);
+  return w.dot(w);
+}
+
 /* ************************************************************************* */
 Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
   size_t m = R.rows(), n = R.cols();
@@ -136,16 +140,25 @@ void Gaussian::print(const string& name) const {
 /* ************************************************************************* */
 bool Gaussian::equals(const Base& expected, double tol) const {
   const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
-  if (p == NULL) return false;
+  if (p == nullptr) return false;
   if (typeid(*this) != typeid(*p)) return false;
-  //if (!sqrt_information_) return true; // ALEX todo;
   return equal_with_abs_tol(R(), p->R(), sqrt(tol));
 }
 
+/* ************************************************************************* */
+Matrix Gaussian::covariance() const {
+  // Uses a fast version of `covariance = information().inverse();`
+  const Matrix& R = this->R();
+  Matrix I = Matrix::Identity(R.rows(), R.cols());
+  // Fast inverse of upper-triangular matrix R using forward-substitution
+  Matrix Rinv = R.triangularView<Eigen::Upper>().solve(I);
+  // (R' * R)^{-1} = R^{-1} * R^{-1}'
+  return Rinv * Rinv.transpose();
+}
+
 /* ************************************************************************* */
 Vector Gaussian::sigmas() const {
-  // TODO(frank): can this be done faster?
-  return Vector((thisR().transpose() * thisR()).inverse().diagonal()).cwiseSqrt();
+  return Vector(covariance().diagonal()).cwiseSqrt();
 }
 
 /* ************************************************************************* */
@@ -158,13 +171,6 @@ Vector Gaussian::unwhiten(const Vector& v) const {
   return backSubstituteUpper(thisR(), v);
 }
 
-/* ************************************************************************* */
-double Gaussian::Mahalanobis(const Vector& v) const {
-  // Note: for Diagonal, which does ediv_, will be correct for constraints
-  Vector w = whiten(v);
-  return w.dot(w);
-}
-
 /* ************************************************************************* */
 Matrix Gaussian::Whiten(const Matrix& H) const {
   return thisR() * H;
@@ -370,8 +376,19 @@ Vector Constrained::whiten(const Vector& v) const {
   return c;
 }
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 /* ************************************************************************* */
-double Constrained::distance(const Vector& v) const {
+double Constrained::error(const Vector& v) const {
+  Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
+  for (size_t i=0; i<dim_; ++i)  // add mu weights on constrained variables
+    if (constrained(i)) // whiten makes constrained variables zero
+      w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
+  return 0.5 * w.dot(w);
+}
+#endif
+
+/* ************************************************************************* */
+double Constrained::squaredMahalanobisDistance(const Vector& v) const {
   Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
   for (size_t i=0; i<dim_; ++i)  // add mu weights on constrained variables
     if (constrained(i)) // whiten makes constrained variables zero
@@ -575,7 +592,7 @@ void Isotropic::print(const string& name) const {
 }
 
 /* ************************************************************************* */
-double Isotropic::Mahalanobis(const Vector& v) const {
+double Isotropic::squaredMahalanobisDistance(const Vector& v) const {
   return v.dot(v) * invsigma_ * invsigma_;
 }
 
@@ -616,320 +633,6 @@ void Unit::print(const std::string& name) const {
   cout << name << "unit (" << dim_ << ") " << endl;
 }
 
-/* ************************************************************************* */
-// M-Estimator
-/* ************************************************************************* */
-
-namespace mEstimator {
-
-Vector Base::weight(const Vector& error) const {
-  const size_t n = error.rows();
-  Vector w(n);
-  for (size_t i = 0; i < n; ++i)
-    w(i) = weight(error(i));
-  return w;
-}
-
-// The following three functions re-weight block matrices and a vector
-// according to their weight implementation
-
-void Base::reweight(Vector& error) const {
-  if (reweight_ == Block) {
-    const double w = sqrtWeight(error.norm());
-    error *= w;
-  } else {
-    error.array() *= weight(error).cwiseSqrt().array();
-  }
-}
-
-// Reweight n block matrices with one error vector
-void Base::reweight(vector<Matrix> &A, Vector &error) const {
-  if ( reweight_ == Block ) {
-    const double w = sqrtWeight(error.norm());
-    for(Matrix& Aj: A) {
-      Aj *= w;
-    }
-    error *= w;
-  }
-  else {
-    const Vector W = sqrtWeight(error);
-    for(Matrix& Aj: A) {
-      vector_scale_inplace(W,Aj);
-    }
-    error = W.cwiseProduct(error);
-  }
-}
-
-// Reweight one block matrix with one error vector
-void Base::reweight(Matrix &A, Vector &error) const {
-  if ( reweight_ == Block ) {
-    const double w = sqrtWeight(error.norm());
-    A *= w;
-    error *= w;
-  }
-  else {
-    const Vector W = sqrtWeight(error);
-    vector_scale_inplace(W,A);
-    error = W.cwiseProduct(error);
-  }
-}
-
-// Reweight two block matrix with one error vector
-void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const {
-  if ( reweight_ == Block ) {
-    const double w = sqrtWeight(error.norm());
-    A1 *= w;
-    A2 *= w;
-    error *= w;
-  }
-  else {
-    const Vector W = sqrtWeight(error);
-    vector_scale_inplace(W,A1);
-    vector_scale_inplace(W,A2);
-    error = W.cwiseProduct(error);
-  }
-}
-
-// Reweight three block matrix with one error vector
-void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const {
-  if ( reweight_ == Block ) {
-    const double w = sqrtWeight(error.norm());
-    A1 *= w;
-    A2 *= w;
-    A3 *= w;
-    error *= w;
-  }
-  else {
-    const Vector W = sqrtWeight(error);
-    vector_scale_inplace(W,A1);
-    vector_scale_inplace(W,A2);
-    vector_scale_inplace(W,A3);
-    error = W.cwiseProduct(error);
-  }
-}
-
-/* ************************************************************************* */
-// Null model
-/* ************************************************************************* */
-
-void Null::print(const std::string &s="") const
-{ cout << s << "null ()" << endl; }
-
-Null::shared_ptr Null::Create()
-{ return shared_ptr(new Null()); }
-
-Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) {
-  if (c_ <= 0) {
-    throw runtime_error("mEstimator Fair takes only positive double in constructor.");
-  }
-}
-
-/* ************************************************************************* */
-// Fair
-/* ************************************************************************* */
-
-void Fair::print(const std::string &s="") const
-{ cout << s << "fair (" << c_ << ")" << endl; }
-
-bool Fair::equals(const Base &expected, double tol) const {
-  const Fair* p = dynamic_cast<const Fair*> (&expected);
-  if (p == NULL) return false;
-  return std::abs(c_ - p->c_ ) < tol;
-}
-
-Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight)
-{ return shared_ptr(new Fair(c, reweight)); }
-
-/* ************************************************************************* */
-// Huber
-/* ************************************************************************* */
-
-Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) {
-  if (k_ <= 0) {
-    throw runtime_error("mEstimator Huber takes only positive double in constructor.");
-  }
-}
-
-void Huber::print(const std::string &s="") const {
-  cout << s << "huber (" << k_ << ")" << endl;
-}
-
-bool Huber::equals(const Base &expected, double tol) const {
-  const Huber* p = dynamic_cast<const Huber*>(&expected);
-  if (p == NULL) return false;
-  return std::abs(k_ - p->k_) < tol;
-}
-
-Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) {
-  return shared_ptr(new Huber(c, reweight));
-}
-
-/* ************************************************************************* */
-// Cauchy
-/* ************************************************************************* */
-
-Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), ksquared_(k * k) {
-  if (k <= 0) {
-    throw runtime_error("mEstimator Cauchy takes only positive double in constructor.");
-  }
-}
-
-void Cauchy::print(const std::string &s="") const {
-  cout << s << "cauchy (" << k_ << ")" << endl;
-}
-
-bool Cauchy::equals(const Base &expected, double tol) const {
-  const Cauchy* p = dynamic_cast<const Cauchy*>(&expected);
-  if (p == NULL) return false;
-  return std::abs(ksquared_ - p->ksquared_) < tol;
-}
-
-Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) {
-  return shared_ptr(new Cauchy(c, reweight));
-}
-
-/* ************************************************************************* */
-// Tukey
-/* ************************************************************************* */
-Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
-
-void Tukey::print(const std::string &s="") const {
-  std::cout << s << ": Tukey (" << c_ << ")" << std::endl;
-}
-
-bool Tukey::equals(const Base &expected, double tol) const {
-  const Tukey* p = dynamic_cast<const Tukey*>(&expected);
-  if (p == NULL) return false;
-  return std::abs(c_ - p->c_) < tol;
-}
-
-Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) {
-  return shared_ptr(new Tukey(c, reweight));
-}
-
-/* ************************************************************************* */
-// Welsch
-/* ************************************************************************* */
-Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
-
-void Welsch::print(const std::string &s="") const {
-  std::cout << s << ": Welsch (" << c_ << ")" << std::endl;
-}
-
-bool Welsch::equals(const Base &expected, double tol) const {
-  const Welsch* p = dynamic_cast<const Welsch*>(&expected);
-  if (p == NULL) return false;
-  return std::abs(c_ - p->c_) < tol;
-}
-
-Welsch::shared_ptr Welsch::Create(double c, const ReweightScheme reweight) {
-  return shared_ptr(new Welsch(c, reweight));
-}
-
-#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
-Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
-
-void Welsh::print(const std::string &s="") const {
-  std::cout << s << ": Welsh (" << c_ << ")" << std::endl;
-}
-
-bool Welsh::equals(const Base &expected, double tol) const {
-  const Welsh* p = dynamic_cast<const Welsh*>(&expected);
-  if (p == NULL) return false;
-  return std::abs(c_ - p->c_) < tol;
-}
-
-Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) {
-  return shared_ptr(new Welsh(c, reweight));
-}
-#endif
-
-/* ************************************************************************* */
-// GemanMcClure
-/* ************************************************************************* */
-GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight)
-  : Base(reweight), c_(c) {
-}
-
-double GemanMcClure::weight(double error) const {
-  const double c2 = c_*c_;
-  const double c4 = c2*c2;
-  const double c2error = c2 + error*error;
-  return c4/(c2error*c2error);
-}
-
-void GemanMcClure::print(const std::string &s="") const {
-  std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl;
-}
-
-bool GemanMcClure::equals(const Base &expected, double tol) const {
-  const GemanMcClure* p = dynamic_cast<const GemanMcClure*>(&expected);
-  if (p == NULL) return false;
-  return std::abs(c_ - p->c_) < tol;
-}
-
-GemanMcClure::shared_ptr GemanMcClure::Create(double c, const ReweightScheme reweight) {
-  return shared_ptr(new GemanMcClure(c, reweight));
-}
-
-/* ************************************************************************* */
-// DCS
-/* ************************************************************************* */
-DCS::DCS(double c, const ReweightScheme reweight)
-  : Base(reweight), c_(c) {
-}
-
-double DCS::weight(double error) const {
-  const double e2 = error*error;
-  if (e2 > c_)
-  {
-    const double w = 2.0*c_/(c_ + e2);
-    return w*w;
-  }
-
-  return 1.0;
-}
-
-void DCS::print(const std::string &s="") const {
-  std::cout << s << ": DCS (" << c_ << ")" << std::endl;
-}
-
-bool DCS::equals(const Base &expected, double tol) const {
-  const DCS* p = dynamic_cast<const DCS*>(&expected);
-  if (p == NULL) return false;
-  return std::abs(c_ - p->c_) < tol;
-}
-
-DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) {
-  return shared_ptr(new DCS(c, reweight));
-}
-
-/* ************************************************************************* */
-// L2WithDeadZone
-/* ************************************************************************* */
-
-L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) : Base(reweight), k_(k) {
-  if (k_ <= 0) {
-    throw runtime_error("mEstimator L2WithDeadZone takes only positive double in constructor.");
-  }
-}
-
-void L2WithDeadZone::print(const std::string &s="") const {
-  std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl;
-}
-
-bool L2WithDeadZone::equals(const Base &expected, double tol) const {
-  const L2WithDeadZone* p = dynamic_cast<const L2WithDeadZone*>(&expected);
-  if (p == NULL) return false;
-  return std::abs(k_ - p->k_) < tol;
-}
-
-L2WithDeadZone::shared_ptr L2WithDeadZone::Create(double k, const ReweightScheme reweight) {
-  return shared_ptr(new L2WithDeadZone(k, reweight));
-}
-
-} // namespace mEstimator
-
 /* ************************************************************************* */
 // Robust
 /* ************************************************************************* */
@@ -941,7 +644,7 @@ void Robust::print(const std::string& name) const {
 
 bool Robust::equals(const Base& expected, double tol) const {
   const Robust* p = dynamic_cast<const Robust*> (&expected);
-  if (p == NULL) return false;
+  if (p == nullptr) return false;
   return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
 }
 
@@ -971,7 +674,7 @@ void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
 }
 
 Robust::shared_ptr Robust::Create(
-  const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
+const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
   return shared_ptr(new Robust(robust,noise));
 }
 
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index 5c540caa3..7494e0501 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -21,6 +21,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/dllexport.h>
+#include <gtsam/linear/LossFunctions.h>
 
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/extended_type_info.hpp>
@@ -39,6 +40,7 @@ namespace gtsam {
     class Constrained;
     class Isotropic;
     class Unit;
+    class RobustModel;
 
     //---------------------------------------------------------------------------------------
 
@@ -88,7 +90,27 @@ namespace gtsam {
       /// Unwhiten an error vector.
       virtual Vector unwhiten(const Vector& v) const = 0;
 
-      virtual double distance(const Vector& v) const = 0;
+      /// Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
+      virtual double squaredMahalanobisDistance(const Vector& v) const;
+
+      /// Mahalanobis distance
+      virtual double mahalanobisDistance(const Vector& v) const {
+        return std::sqrt(squaredMahalanobisDistance(v));
+      }
+
+      /// loss function, input is Mahalanobis distance
+      virtual double loss(const double squared_distance) const {
+        return 0.5 * squared_distance;
+      }
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+      /// calculate the error value given measurement error vector
+      virtual double error(const Vector& v) const = 0;
+
+      virtual double distance(const Vector& v) {
+        return error(v) * 2;
+      }      
+#endif
 
       virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
       virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
@@ -115,6 +137,14 @@ namespace gtsam {
         v = unwhiten(v);
       }
 
+      /** Useful function for robust noise models to get the unweighted but whitened error */
+      virtual Vector unweightedWhiten(const Vector& v) const {
+        return whiten(v);
+      }
+
+      /** get the weight from the effective loss function on residual vector v */
+      virtual double weight(const Vector& v) const { return 1.0; }
+
     private:
       /** Serialization function */
       friend class boost::serialization::access;
@@ -190,26 +220,30 @@ namespace gtsam {
        */
       static shared_ptr Covariance(const Matrix& covariance, bool smart = true);
 
-      virtual void print(const std::string& name) const;
-      virtual bool equals(const Base& expected, double tol=1e-9) const;
-      virtual Vector sigmas() const;
-      virtual Vector whiten(const Vector& v) const;
-      virtual Vector unwhiten(const Vector& v) const;
+      void print(const std::string& name) const override;
+      bool equals(const Base& expected, double tol=1e-9) const override;
+      Vector sigmas() const override;
+      Vector whiten(const Vector& v) const override;
+      Vector unwhiten(const Vector& v) const override;
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+      virtual double Mahalanobis(const Vector& v) const {
+        return squaredMahalanobisDistance(v);
+      }
 
       /**
-       * Mahalanobis distance v'*R'*R*v = <R*v,R*v>
+       * error value 0.5 * v'*R'*R*v
        */
-      virtual double Mahalanobis(const Vector& v) const;
-
-      inline virtual double distance(const Vector& v) const {
-        return Mahalanobis(v);
+      inline double error(const Vector& v) const override {
+        return 0.5 * squaredMahalanobisDistance(v);
       }
+#endif
 
       /**
        * Multiply a derivative with R (derivative of whiten)
        * Equivalent to whitening each column of the input matrix.
        */
-      virtual Matrix Whiten(const Matrix& H) const;
+      Matrix Whiten(const Matrix& H) const override;
 
       /**
        * In-place version
@@ -224,10 +258,10 @@ namespace gtsam {
       /**
        * Whiten a system, in place as well
        */
-      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const;
-      virtual void WhitenSystem(Matrix& A, Vector& b) const;
-      virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const;
-      virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
+      void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
+      void WhitenSystem(Matrix& A, Vector& b) const override;
+      void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
+      void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
 
       /**
        * Apply appropriately weighted QR factorization to the system [A b]
@@ -247,7 +281,7 @@ namespace gtsam {
       virtual Matrix information() const { return R().transpose() * R(); }
 
       /// Compute covariance matrix
-      virtual Matrix covariance() const { return information().inverse(); }
+      virtual Matrix covariance() const;
 
     private:
       /** Serialization function */
@@ -312,13 +346,13 @@ namespace gtsam {
         return Variances(precisions.array().inverse(), smart);
       }
 
-      virtual void print(const std::string& name) const;
-      virtual Vector sigmas() const { return sigmas_; }
-      virtual Vector whiten(const Vector& v) const;
-      virtual Vector unwhiten(const Vector& v) const;
-      virtual Matrix Whiten(const Matrix& H) const;
-      virtual void WhitenInPlace(Matrix& H) const;
-      virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
+      void print(const std::string& name) const override;
+      Vector sigmas() const override { return sigmas_; }
+      Vector whiten(const Vector& v) const override;
+      Vector unwhiten(const Vector& v) const override;
+      Matrix Whiten(const Matrix& H) const override;
+      void WhitenInPlace(Matrix& H) const override;
+      void WhitenInPlace(Eigen::Block<Matrix> H) const override;
 
       /**
        * Return standard deviations (sqrt of diagonal)
@@ -340,7 +374,7 @@ namespace gtsam {
       /**
        * Return R itself, but note that Whiten(H) is cheaper than R*H
        */
-      virtual Matrix R() const {
+      Matrix R() const override {
         return invsigmas().asDiagonal();
       }
 
@@ -394,10 +428,10 @@ namespace gtsam {
 
       typedef boost::shared_ptr<Constrained> shared_ptr;
 
-      virtual ~Constrained() {}
+      ~Constrained() {}
 
       /// true if a constrained noise mode, saves slow/clumsy dynamic casting
-      virtual bool isConstrained() const { return true; }
+      bool isConstrained() const override { return true; }
 
       /// Return true if a particular dimension is free or constrained
       bool constrained(size_t i) const;
@@ -449,12 +483,16 @@ namespace gtsam {
         return MixedVariances(precisions.array().inverse());
       }
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
       /**
-       * The distance function for a constrained noisemodel,
+       * The error function for a constrained noisemodel,
        * for non-constrained versions, uses sigmas, otherwise
        * uses the penalty function with mu
        */
-      virtual double distance(const Vector& v) const;
+      double error(const Vector& v) const override;
+#endif
+
+      double squaredMahalanobisDistance(const Vector& v) const override;
 
       /** Fully constrained variations */
       static shared_ptr All(size_t dim) {
@@ -471,16 +509,16 @@ namespace gtsam {
         return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0)));
       }
 
-      virtual void print(const std::string& name) const;
+      void print(const std::string& name) const override;
 
       /// Calculates error vector with weights applied
-      virtual Vector whiten(const Vector& v) const;
+      Vector whiten(const Vector& v) const override;
 
       /// Whitening functions will perform partial whitening on rows
       /// with a non-zero sigma.  Other rows remain untouched.
-      virtual Matrix Whiten(const Matrix& H) const;
-      virtual void WhitenInPlace(Matrix& H) const;
-      virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
+      Matrix Whiten(const Matrix& H) const override;
+      void WhitenInPlace(Matrix& H) const override;
+      void WhitenInPlace(Eigen::Block<Matrix> H) const override;
 
       /**
        * Apply QR factorization to the system [A b], taking into account constraints
@@ -491,7 +529,7 @@ namespace gtsam {
        * @param Ab is the m*(n+1) augmented system matrix [A b]
        * @return diagonal noise model can be all zeros, mixed, or not-constrained
        */
-      virtual Diagonal::shared_ptr QR(Matrix& Ab) const;
+      Diagonal::shared_ptr QR(Matrix& Ab) const override;
 
       /**
        * Returns a Unit version of a constrained noisemodel in which
@@ -553,14 +591,14 @@ namespace gtsam {
         return Variance(dim, 1.0/precision, smart);
       }
 
-      virtual void print(const std::string& name) const;
-      virtual double Mahalanobis(const Vector& v) const;
-      virtual Vector whiten(const Vector& v) const;
-      virtual Vector unwhiten(const Vector& v) const;
-      virtual Matrix Whiten(const Matrix& H) const;
-      virtual void WhitenInPlace(Matrix& H) const;
-      virtual void whitenInPlace(Vector& v) const;
-      virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
+      void print(const std::string& name) const override;
+      double squaredMahalanobisDistance(const Vector& v) const override;
+      Vector whiten(const Vector& v) const override;
+      Vector unwhiten(const Vector& v) const override;
+      Matrix Whiten(const Matrix& H) const override;
+      void WhitenInPlace(Matrix& H) const override;
+      void whitenInPlace(Vector& v) const override;
+      void WhitenInPlace(Eigen::Block<Matrix> H) const override;
 
       /**
        * Return standard deviation
@@ -593,7 +631,7 @@ namespace gtsam {
 
       typedef boost::shared_ptr<Unit> shared_ptr;
 
-      virtual ~Unit() {}
+      ~Unit() {}
 
       /**
        * Create a unit covariance noise model
@@ -603,19 +641,19 @@ namespace gtsam {
       }
 
       /// true if a unit noise model, saves slow/clumsy dynamic casting
-      virtual bool isUnit() const { return true; }
+      bool isUnit() const override { return true; }
 
-      virtual void print(const std::string& name) const;
-      virtual double Mahalanobis(const Vector& v) const {return v.dot(v); }
-      virtual Vector whiten(const Vector& v) const { return v; }
-      virtual Vector unwhiten(const Vector& v) const { return v; }
-      virtual Matrix Whiten(const Matrix& H) const { return H; }
-      virtual void WhitenInPlace(Matrix& /*H*/) const {}
-      virtual void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const {}
-      virtual void whitenInPlace(Vector& /*v*/) const {}
-      virtual void unwhitenInPlace(Vector& /*v*/) const {}
-      virtual void whitenInPlace(Eigen::Block<Vector>& /*v*/) const {}
-      virtual void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const {}
+      void print(const std::string& name) const override;
+      double squaredMahalanobisDistance(const Vector& v) const override {return v.dot(v); }
+      Vector whiten(const Vector& v) const override { return v; }
+      Vector unwhiten(const Vector& v) const override { return v; }
+      Matrix Whiten(const Matrix& H) const override { return H; }
+      void WhitenInPlace(Matrix& /*H*/) const override {}
+      void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const override {}
+      void whitenInPlace(Vector& /*v*/) const override {}
+      void unwhitenInPlace(Vector& /*v*/) const override {}
+      void whitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
+      void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
 
     private:
       /** Serialization function */
@@ -626,392 +664,6 @@ namespace gtsam {
       }
     };
 
-    /**
-     * The mEstimator name space contains all robust error functions.
-     * It mirrors the exposition at
-     *  https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
-     * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice.
-     *
-     * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples:
-     *
-     * Name        Symbol          Least-Squares   L1-norm    Huber
-     * Residual    \rho(x)         0.5*x^2         |x|        0.5*x^2 if x<k, 0.5*k^2 + k|x-k| otherwise
-     * Derivative  \phi(x)         x               sgn(x)     x       if x<k, k sgn(x)         otherwise
-     * Weight      w(x)=\phi(x)/x  1               1/|x|      1       if x<k, k/|x|            otherwise
-     *
-     * With these definitions, D(\rho(x), p) = \phi(x) D(x,p) = w(x) x D(x,p) = w(x) D(L2(x), p),
-     * and hence we can solve the equivalent weighted least squares problem \sum w(r_i) \rho(r_i)
-     *
-     * Each M-estimator in the mEstimator name space simply implements the above functions.
-     */
-    namespace mEstimator {
-
-      //---------------------------------------------------------------------------------------
-
-      class GTSAM_EXPORT Base {
-      public:
-        enum ReweightScheme { Scalar, Block };
-        typedef boost::shared_ptr<Base> shared_ptr;
-
-      protected:
-        /** the rows can be weighted independently according to the error
-        * or uniformly with the norm of the right hand side */
-        ReweightScheme reweight_;
-
-      public:
-        Base(const ReweightScheme reweight = Block):reweight_(reweight) {}
-        virtual ~Base() {}
-
-        /*
-         * This method is responsible for returning the total penalty for a given amount of error.
-         * For example, this method is responsible for implementing the quadratic function for an
-         * L2 penalty, the absolute value function for an L1 penalty, etc.
-         *
-         * TODO(mike): There is currently a bug in GTSAM, where none of the mEstimator classes
-         * implement a residual function, and GTSAM calls the weight function to evaluate the
-         * total penalty, rather than calling the residual function. The weight function should be
-         * used during iteratively reweighted least squares optimization, but should not be used to
-         * evaluate the total penalty. The long-term solution is for all mEstimators to implement
-         * both a weight and a residual function, and for GTSAM to call the residual function when
-         * evaluating the total penalty. But for now, I'm leaving this residual method as pure
-         * virtual, so the existing mEstimators can inherit this default fallback behavior.
-         */
-        virtual double residual(double error) const { return 0; };
-
-        /*
-         * This method is responsible for returning the weight function for a given amount of error.
-         * The weight function is related to the analytic derivative of the residual function. See
-         *  https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
-         * for details. This method is required when optimizing cost functions with robust penalties
-         * using iteratively re-weighted least squares.
-         */
-        virtual double weight(double error) const = 0;
-
-        virtual void print(const std::string &s) const = 0;
-        virtual bool equals(const Base& expected, double tol=1e-8) const = 0;
-
-        double sqrtWeight(double error) const {
-          return std::sqrt(weight(error));
-        }
-
-        /** produce a weight vector according to an error vector and the implemented
-        * robust function */
-        Vector weight(const Vector &error) const;
-
-        /** square root version of the weight function */
-        Vector sqrtWeight(const Vector &error) const {
-          return weight(error).cwiseSqrt();
-        }
-
-        /** reweight block matrices and a vector according to their weight implementation */
-        void reweight(Vector &error) const;
-        void reweight(std::vector<Matrix> &A, Vector &error) const;
-        void reweight(Matrix &A, Vector &error) const;
-        void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
-        void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
-
-      private:
-        /** Serialization function */
-        friend class boost::serialization::access;
-        template<class ARCHIVE>
-        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-          ar & BOOST_SERIALIZATION_NVP(reweight_);
-        }
-      };
-
-      /// Null class is not robust so is a Gaussian ?
-      class GTSAM_EXPORT Null : public Base {
-      public:
-        typedef boost::shared_ptr<Null> shared_ptr;
-
-        Null(const ReweightScheme reweight = Block) : Base(reweight) {}
-        virtual ~Null() {}
-        virtual double weight(double /*error*/) const { return 1.0; }
-        virtual void print(const std::string &s) const;
-        virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; }
-        static shared_ptr Create() ;
-
-      private:
-        /** Serialization function */
-        friend class boost::serialization::access;
-        template<class ARCHIVE>
-        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-        }
-      };
-
-      /// Fair implements the "Fair" robust error model (Zhang97ivc)
-      class GTSAM_EXPORT Fair : public Base {
-      protected:
-        double c_;
-
-      public:
-        typedef boost::shared_ptr<Fair> shared_ptr;
-
-        Fair(double c = 1.3998, const ReweightScheme reweight = Block);
-        double weight(double error) const {
-          return 1.0 / (1.0 + std::abs(error) / c_);
-        }
-        void print(const std::string &s) const;
-        bool equals(const Base& expected, double tol=1e-8) const;
-        static shared_ptr Create(double c, const ReweightScheme reweight = Block) ;
-
-      private:
-        /** Serialization function */
-        friend class boost::serialization::access;
-        template<class ARCHIVE>
-        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-          ar & BOOST_SERIALIZATION_NVP(c_);
-        }
-      };
-
-      /// Huber implements the "Huber" robust error model (Zhang97ivc)
-      class GTSAM_EXPORT Huber : public Base {
-      protected:
-        double k_;
-
-      public:
-        typedef boost::shared_ptr<Huber> shared_ptr;
-
-        Huber(double k = 1.345, const ReweightScheme reweight = Block);
-        double weight(double error) const {
-          double absError = std::abs(error);
-          return (absError < k_) ? (1.0) : (k_ / absError);
-        }
-        void print(const std::string &s) const;
-        bool equals(const Base& expected, double tol=1e-8) const;
-        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
-
-      private:
-        /** Serialization function */
-        friend class boost::serialization::access;
-        template<class ARCHIVE>
-        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-          ar & BOOST_SERIALIZATION_NVP(k_);
-        }
-      };
-
-      /// Cauchy implements the "Cauchy" robust error model (Lee2013IROS).  Contributed by:
-      ///   Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
-      ///   Information Technology, Karlsruhe, Germany.
-      ///   oberlaender@fzi.de
-      /// Thanks Jan!
-      class GTSAM_EXPORT Cauchy : public Base {
-      protected:
-        double k_, ksquared_;
-
-      public:
-        typedef boost::shared_ptr<Cauchy> shared_ptr;
-
-        Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
-        double weight(double error) const {
-          return ksquared_ / (ksquared_ + error*error);
-        }
-        void print(const std::string &s) const;
-        bool equals(const Base& expected, double tol=1e-8) const;
-        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
-
-      private:
-        /** Serialization function */
-        friend class boost::serialization::access;
-        template<class ARCHIVE>
-        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-          ar & BOOST_SERIALIZATION_NVP(k_);
-        }
-      };
-
-      /// Tukey implements the "Tukey" robust error model (Zhang97ivc)
-      class GTSAM_EXPORT Tukey : public Base {
-      protected:
-        double c_, csquared_;
-
-      public:
-        typedef boost::shared_ptr<Tukey> shared_ptr;
-
-        Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
-        double weight(double error) const {
-          if (std::abs(error) <= c_) {
-            double xc2 = error*error/csquared_;
-            return (1.0-xc2)*(1.0-xc2);
-          }
-          return 0.0;
-        }
-        void print(const std::string &s) const;
-        bool equals(const Base& expected, double tol=1e-8) const;
-        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
-
-      private:
-        /** Serialization function */
-        friend class boost::serialization::access;
-        template<class ARCHIVE>
-        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-          ar & BOOST_SERIALIZATION_NVP(c_);
-        }
-      };
-
-      /// Welsch implements the "Welsch" robust error model (Zhang97ivc)
-      class GTSAM_EXPORT Welsch : public Base {
-      protected:
-        double c_, csquared_;
-
-      public:
-        typedef boost::shared_ptr<Welsch> shared_ptr;
-
-        Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
-        double weight(double error) const {
-          double xc2 = (error*error)/csquared_;
-          return std::exp(-xc2);
-        }
-        void print(const std::string &s) const;
-        bool equals(const Base& expected, double tol=1e-8) const;
-        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
-
-      private:
-        /** Serialization function */
-        friend class boost::serialization::access;
-        template<class ARCHIVE>
-        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-          ar & BOOST_SERIALIZATION_NVP(c_);
-        }
-      };
-#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
-      /// @name Deprecated
-      /// @{
-      // Welsh implements the "Welsch" robust error model (Zhang97ivc)
-      // This was misspelled in previous versions of gtsam and should be
-      // removed in the future.
-      class GTSAM_EXPORT Welsh : public Base {
-      protected:
-        double c_, csquared_;
-
-      public:
-        typedef boost::shared_ptr<Welsh> shared_ptr;
-
-        Welsh(double c = 2.9846, const ReweightScheme reweight = Block);
-        double weight(double error) const {
-          double xc2 = (error*error)/csquared_;
-          return std::exp(-xc2);
-        }
-        void print(const std::string &s) const;
-        bool equals(const Base& expected, double tol=1e-8) const;
-        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
-
-      private:
-        /** Serialization function */
-        friend class boost::serialization::access;
-        template<class ARCHIVE>
-        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-          ar & BOOST_SERIALIZATION_NVP(c_);
-        }
-      };
-#endif
-
-      /// GemanMcClure implements the "Geman-McClure" robust error model
-      /// (Zhang97ivc).
-      ///
-      /// Note that Geman-McClure weight function uses the parameter c == 1.0,
-      /// but here it's allowed to use different values, so we actually have
-      /// the generalized Geman-McClure from (Agarwal15phd).
-      class GTSAM_EXPORT GemanMcClure : public Base {
-      public:
-        typedef boost::shared_ptr<GemanMcClure> shared_ptr;
-
-        GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
-        virtual ~GemanMcClure() {}
-        virtual double weight(double error) const;
-        virtual void print(const std::string &s) const;
-        virtual bool equals(const Base& expected, double tol=1e-8) const;
-        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
-
-      protected:
-        double c_;
-
-      private:
-        /** Serialization function */
-        friend class boost::serialization::access;
-        template<class ARCHIVE>
-        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-          ar & BOOST_SERIALIZATION_NVP(c_);
-        }
-      };
-
-      /// DCS implements the Dynamic Covariance Scaling robust error model
-      /// from the paper Robust Map Optimization (Agarwal13icra).
-      ///
-      /// Under the special condition of the parameter c == 1.0 and not
-      /// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure.
-      class GTSAM_EXPORT DCS : public Base {
-      public:
-        typedef boost::shared_ptr<DCS> shared_ptr;
-
-        DCS(double c = 1.0, const ReweightScheme reweight = Block);
-        virtual ~DCS() {}
-        virtual double weight(double error) const;
-        virtual void print(const std::string &s) const;
-        virtual bool equals(const Base& expected, double tol=1e-8) const;
-        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
-
-      protected:
-        double c_;
-
-      private:
-        /** Serialization function */
-        friend class boost::serialization::access;
-        template<class ARCHIVE>
-        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-          ar & BOOST_SERIALIZATION_NVP(c_);
-        }
-      };
-
-      /// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of width 2*k,
-      /// centered at the origin. The resulting penalty within the dead zone is always zero, and
-      /// grows quadratically outside the dead zone. In this sense, the L2WithDeadZone penalty is
-      /// "robust to inliers", rather than being robust to outliers. This penalty can be used to
-      /// create barrier functions in a general way.
-      class GTSAM_EXPORT L2WithDeadZone : public Base {
-      protected:
-          double k_;
-
-      public:
-          typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
-
-          L2WithDeadZone(double k, const ReweightScheme reweight = Block);
-          double residual(double error) const {
-            const double abs_error = std::abs(error);
-            return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error);
-          }
-          double weight(double error) const {
-            // note that this code is slightly uglier than above, because there are three distinct
-            // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two
-            // cases (deadzone, non-deadzone) above.
-            if (std::abs(error) <= k_) return 0.0;
-            else if (error > k_) return (-k_+error)/error;
-            else return (k_+error)/error;
-          }
-          void print(const std::string &s) const;
-          bool equals(const Base& expected, double tol=1e-8) const;
-          static shared_ptr Create(double k, const ReweightScheme reweight = Block);
-
-      private:
-          /** Serialization function */
-          friend class boost::serialization::access;
-          template<class ARCHIVE>
-          void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-            ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-            ar & BOOST_SERIALIZATION_NVP(k_);
-          }
-      };
-
-    } ///\namespace mEstimator
-
     /**
      *  Base class for robust error models
      *  The robust M-estimators above simply tell us how to re-weight the residual, and are
@@ -1050,10 +702,10 @@ namespace gtsam {
       : Base(noise->dim()), robust_(robust), noise_(noise) {}
 
       /// Destructor
-      virtual ~Robust() {}
+      ~Robust() {}
 
-      virtual void print(const std::string& name) const;
-      virtual bool equals(const Base& expected, double tol=1e-9) const;
+      void print(const std::string& name) const override;
+      bool equals(const Base& expected, double tol=1e-9) const override;
 
       /// Return the contained robust error function
       const RobustModel::shared_ptr& robust() const { return robust_; }
@@ -1062,23 +714,39 @@ namespace gtsam {
       const NoiseModel::shared_ptr& noise() const { return noise_; }
 
       // TODO: functions below are dummy but necessary for the noiseModel::Base
-      inline virtual Vector whiten(const Vector& v) const
+      inline Vector whiten(const Vector& v) const override
       { Vector r = v; this->WhitenSystem(r); return r; }
-      inline virtual Matrix Whiten(const Matrix& A) const
+      inline Matrix Whiten(const Matrix& A) const override
       { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
-      inline virtual Vector unwhiten(const Vector& /*v*/) const
+      inline Vector unwhiten(const Vector& /*v*/) const override
       { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
-      inline virtual double distance(const Vector& v) const
-      { return this->whiten(v).squaredNorm(); }
-      // TODO(mike): fold the use of the m-estimator residual(...) function into distance(...)
-      inline virtual double distance_non_whitened(const Vector& v) const
-      { return robust_->residual(v.norm()); }
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+      inline double distance(const Vector& v) override {
+        return robust_->loss(this->unweightedWhiten(v).norm());
+      }
+      // Fold the use of the m-estimator loss(...) function into error(...)
+      inline double error(const Vector& v) const override
+      { return robust_->loss(noise_->mahalanobisDistance(v)); }
+#endif
+
+      double loss(const double squared_distance) const override {
+        return robust_->loss(std::sqrt(squared_distance));
+      }
+
       // TODO: these are really robust iterated re-weighting support functions
       virtual void WhitenSystem(Vector& b) const;
-      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const;
-      virtual void WhitenSystem(Matrix& A, Vector& b) const;
-      virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const;
-      virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
+      void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
+      void WhitenSystem(Matrix& A, Vector& b) const override;
+      void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
+      void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
+
+      Vector unweightedWhiten(const Vector& v) const override {
+        return noise_->unweightedWhiten(v);
+      }
+      double weight(const Vector& v) const override {
+        // Todo(mikebosse): make the robust weight function input a vector.
+        return robust_->weight(v.norm());
+      }
 
       static shared_ptr Create(
         const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);
diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp
index 08307c5ab..a7af7d8d8 100644
--- a/gtsam/linear/PCGSolver.cpp
+++ b/gtsam/linear/PCGSolver.cpp
@@ -45,6 +45,17 @@ PCGSolver::PCGSolver(const PCGSolverParameters &p) {
   preconditioner_ = createPreconditioner(p.preconditioner_);
 }
 
+void PCGSolverParameters::setPreconditionerParams(const boost::shared_ptr<PreconditionerParameters> preconditioner) {
+  preconditioner_ = preconditioner;
+}
+
+void PCGSolverParameters::print(const std::string &s) const {
+  std::cout << s << std::endl;;
+  std::ostringstream os;
+  print(os);
+  std::cout << os.str() << std::endl;
+}
+
 /*****************************************************************************/
 VectorValues PCGSolver::optimize(const GaussianFactorGraph &gfg,
     const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h
index f5b278ae5..7752902ba 100644
--- a/gtsam/linear/PCGSolver.h
+++ b/gtsam/linear/PCGSolver.h
@@ -48,7 +48,12 @@ public:
     return *preconditioner_;
   }
 
+  // needed for python wrapper
+  void print(const std::string &s) const;
+
   boost::shared_ptr<PreconditionerParameters> preconditioner_;
+
+  void setPreconditionerParams(const boost::shared_ptr<PreconditionerParameters> preconditioner);
 };
 
 /**
diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp
index d83385d32..1c602a963 100644
--- a/gtsam/linear/Preconditioner.cpp
+++ b/gtsam/linear/Preconditioner.cpp
@@ -183,21 +183,21 @@ void BlockJacobiPreconditioner::clean() {
 }
 
 /***************************************************************************************/
-boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters) {
-
-  if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast<DummyPreconditionerParameters>(parameters) ) {
+boost::shared_ptr<Preconditioner> createPreconditioner(
+    const boost::shared_ptr<PreconditionerParameters> params) {
+  using boost::dynamic_pointer_cast;
+  if (dynamic_pointer_cast<DummyPreconditionerParameters>(params)) {
     return boost::make_shared<DummyPreconditioner>();
-  }
-  else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(parameters) ) {
+  } else if (dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(
+                 params)) {
     return boost::make_shared<BlockJacobiPreconditioner>();
-  }
-  else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast<SubgraphPreconditionerParameters>(parameters) ) {
+  } else if (auto subgraph =
+                 dynamic_pointer_cast<SubgraphPreconditionerParameters>(
+                     params)) {
     return boost::make_shared<SubgraphPreconditioner>(*subgraph);
   }
 
-  throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type");
+  throw invalid_argument(
+      "createPreconditioner: unexpected preconditioner parameter type");
 }
-
-}
-
-
+}  // namespace gtsam
diff --git a/gtsam/linear/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h
index 1fe7009bc..a24acfacd 100644
--- a/gtsam/linear/RegularHessianFactor.h
+++ b/gtsam/linear/RegularHessianFactor.h
@@ -77,11 +77,17 @@ public:
 
   /// Construct from a GaussianFactorGraph
   RegularHessianFactor(const GaussianFactorGraph& factors,
-                       boost::optional<const Scatter&> scatter = boost::none)
+                       const Scatter& scatter)
       : HessianFactor(factors, scatter) {
     checkInvariants();
   }
 
+  /// Construct from a GaussianFactorGraph
+  RegularHessianFactor(const GaussianFactorGraph& factors)
+      : HessianFactor(factors) {
+    checkInvariants();
+  }
+
 private:
 
   /// Check invariants after construction
diff --git a/gtsam/linear/RegularJacobianFactor.h b/gtsam/linear/RegularJacobianFactor.h
index 2b5cf85ff..0312efe78 100644
--- a/gtsam/linear/RegularJacobianFactor.h
+++ b/gtsam/linear/RegularJacobianFactor.h
@@ -102,10 +102,8 @@ public:
       DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
   }
 
-  /// Expose base class hessianDiagonal
-  virtual VectorValues hessianDiagonal() const {
-    return JacobianFactor::hessianDiagonal();
-  }
+  /// Using the base method
+  using GaussianFactor::hessianDiagonal;
 
   /// Raw memory access version of hessianDiagonal
   void hessianDiagonal(double* d) const {
diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp
index bfbc222ba..16c7e73e0 100644
--- a/gtsam/linear/Sampler.cpp
+++ b/gtsam/linear/Sampler.cpp
@@ -11,6 +11,8 @@
 
 /**
  * @file Sampler.cpp
+ * @brief sampling from a diagonal NoiseModel
+ * @author Frank Dellaert
  * @author Alex Cunningham
  */
 
@@ -18,25 +20,16 @@
 namespace gtsam {
 
 /* ************************************************************************* */
-Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed)
-  : model_(model), generator_(static_cast<unsigned>(seed))
-{
-}
+Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model,
+                 uint_fast64_t seed)
+    : model_(model), generator_(seed) {}
 
 /* ************************************************************************* */
-Sampler::Sampler(const Vector& sigmas, int32_t seed)
-: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(static_cast<unsigned>(seed))
-{
-}
+Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed)
+    : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {}
 
 /* ************************************************************************* */
-Sampler::Sampler(int32_t seed)
-: generator_(static_cast<unsigned>(seed))
-{
-}
-
-/* ************************************************************************* */
-Vector Sampler::sampleDiagonal(const Vector& sigmas) {
+Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
   size_t d = sigmas.size();
   Vector result(d);
   for (size_t i = 0; i < d; i++) {
@@ -46,28 +39,32 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) {
     if (sigma == 0.0) {
       result(i) = 0.0;
     } else {
-      typedef boost::normal_distribution<double> Normal;
+      typedef std::normal_distribution<double> Normal;
       Normal dist(0.0, sigma);
-      boost::variate_generator<boost::mt19937_64&, Normal> norm(generator_, dist);
-      result(i) = norm();
+      result(i) = dist(generator_);
     }
   }
   return result;
 }
 
 /* ************************************************************************* */
-Vector Sampler::sample() {
+Vector Sampler::sample() const {
   assert(model_.get());
   const Vector& sigmas = model_->sigmas();
   return sampleDiagonal(sigmas);
 }
 
 /* ************************************************************************* */
-Vector Sampler::sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) {
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+Sampler::Sampler(uint_fast64_t seed) : generator_(seed) {}
+
+Vector Sampler::sampleNewModel(
+    const noiseModel::Diagonal::shared_ptr& model) const {
   assert(model.get());
   const Vector& sigmas = model->sigmas();
   return sampleDiagonal(sigmas);
 }
+#endif
 /* ************************************************************************* */
 
-} // \namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h
index 6c84bfda2..54c240a2b 100644
--- a/gtsam/linear/Sampler.h
+++ b/gtsam/linear/Sampler.h
@@ -10,9 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @brief sampling that can be parameterized using a NoiseModel to generate samples from
  * @file Sampler.h
- * the given distribution
+ * @brief sampling from a NoiseModel
+ * @author Frank Dellaert
  * @author Alex Cunningham
  */
 
@@ -20,74 +20,81 @@
 
 #include <gtsam/linear/NoiseModel.h>
 
-#include <boost/random.hpp>
+#include <random>
 
 namespace gtsam {
 
 /**
  * Sampling structure that keeps internal random number generators for
  * diagonal distributions specified by NoiseModel
- *
- * This is primarily to allow for variable seeds, and does roughly the same
- * thing as sample() in NoiseModel.
  */
 class GTSAM_EXPORT Sampler {
-protected:
+ protected:
   /** noiseModel created at generation */
   noiseModel::Diagonal::shared_ptr model_;
 
   /** generator */
-  boost::mt19937_64 generator_;
+  mutable std::mt19937_64 generator_;
 
-public:
+ public:
   typedef boost::shared_ptr<Sampler> shared_ptr;
 
+  /// @name constructors
+  /// @{
+
   /**
    * Create a sampler for the distribution specified by a diagonal NoiseModel
    * with a manually specified seed
    *
    * NOTE: do not use zero as a seed, it will break the generator
    */
-  Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed = 42u);
+  explicit Sampler(const noiseModel::Diagonal::shared_ptr& model,
+                   uint_fast64_t seed = 42u);
 
   /**
-   * Create a sampler for a distribution specified by a vector of sigmas directly
+   * Create a sampler for a distribution specified by a vector of sigmas
+   * directly
    *
    * NOTE: do not use zero as a seed, it will break the generator
    */
-  Sampler(const Vector& sigmas, int32_t seed = 42u);
+  explicit Sampler(const Vector& sigmas, uint_fast64_t seed = 42u);
 
-  /**
-   * Create a sampler without a given noisemodel - pass in to sample
-   *
-   * NOTE: do not use zero as a seed, it will break the generator
-   */
-  Sampler(int32_t seed = 42u);
+  /// @}
+  /// @name access functions
+  /// @{
+
+  size_t dim() const {
+    assert(model_.get());
+    return model_->dim();
+  }
+
+  Vector sigmas() const {
+    assert(model_.get());
+    return model_->sigmas();
+  }
 
-  /** access functions */
-  size_t dim() const { assert(model_.get()); return model_->dim(); }
-  Vector sigmas() const { assert(model_.get()); return model_->sigmas(); }
   const noiseModel::Diagonal::shared_ptr& model() const { return model_; }
 
-  /**
-   * sample from distribution
-   * NOTE: not const due to need to update the underlying generator
-   */
-  Vector sample();
+  /// @}
+  /// @name basic functionality
+  /// @{
 
-  /**
-   * Sample from noisemodel passed in as an argument,
-   * can be used without having initialized a model for the system.
-   *
-   * NOTE: not const due to need to update the underlying generator
-   */
-  Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model);
+  /// sample from distribution
+  Vector sample() const;
 
-protected:
+  /// @}
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+  /// @name Deprecated
+  /// @{
+  explicit Sampler(uint_fast64_t seed = 42u);
+  Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) const;
+  /// @}
+#endif
+
+ protected:
   /** given sigmas for a diagonal model, returns a sample */
-  Vector sampleDiagonal(const Vector& sigmas);
-
+  Vector sampleDiagonal(const Vector& sigmas) const;
 };
 
-} // \namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp
index 5312da34b..07ecaf483 100644
--- a/gtsam/linear/Scatter.cpp
+++ b/gtsam/linear/Scatter.cpp
@@ -33,16 +33,16 @@ string SlotEntry::toString() const {
   return oss.str();
 }
 
+Scatter::Scatter(const GaussianFactorGraph& gfg) : Scatter(gfg, Ordering()) {}
+
 /* ************************************************************************* */
 Scatter::Scatter(const GaussianFactorGraph& gfg,
-    boost::optional<const Ordering&> ordering) {
+    const Ordering& ordering) {
   gttic(Scatter_Constructor);
 
   // If we have an ordering, pre-fill the ordered variables first
-  if (ordering) {
-    for (Key key : *ordering) {
-      add(key, 0);
-    }
+  for (Key key : ordering) {
+    add(key, 0);
   }
 
   // Now, find dimensions of variables and/or extend
@@ -68,7 +68,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg,
 
   // To keep the same behavior as before, sort the keys after the ordering
   iterator first = begin();
-  if (ordering) first += ordering->size();
+  first += ordering.size();
   if (first != end()) std::sort(first, end());
 }
 
diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h
index 793961c59..b05d191bc 100644
--- a/gtsam/linear/Scatter.h
+++ b/gtsam/linear/Scatter.h
@@ -23,8 +23,6 @@
 #include <gtsam/base/FastMap.h>
 #include <gtsam/dllexport.h>
 
-#include <boost/optional.hpp>
-
 namespace gtsam {
 
 class GaussianFactorGraph;
@@ -51,17 +49,18 @@ struct GTSAM_EXPORT SlotEntry {
 class Scatter : public FastVector<SlotEntry> {
  public:
   /// Default Constructor
-  Scatter() {}
+   GTSAM_EXPORT Scatter() {}
 
-  /// Construct from gaussian factor graph, with optional (partial or complete) ordering
-  Scatter(const GaussianFactorGraph& gfg,
-          boost::optional<const Ordering&> ordering = boost::none);
+  /// Construct from gaussian factor graph, without ordering
+   GTSAM_EXPORT explicit Scatter(const GaussianFactorGraph& gfg);
+
+  /// Construct from gaussian factor graph, with (partial or complete) ordering
+   GTSAM_EXPORT explicit Scatter(const GaussianFactorGraph& gfg, const Ordering& ordering);
 
   /// Add a key/dim pair
-  void add(Key key, size_t dim);
+   GTSAM_EXPORT void add(Key key, size_t dim);
 
  private:
-
   /// Find the SlotEntry with the right key (linear time worst case)
   iterator find(Key key);
 };
diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp
index a999b3a71..c6b3ca15f 100644
--- a/gtsam/linear/SubgraphBuilder.cpp
+++ b/gtsam/linear/SubgraphBuilder.cpp
@@ -17,6 +17,7 @@
 
 #include <gtsam/base/DSFVector.h>
 #include <gtsam/base/FastMap.h>
+#include <gtsam/base/WeightedSampler.h>
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/inference/VariableIndex.h>
 #include <gtsam/linear/Errors.h>
@@ -35,8 +36,11 @@
 #include <iostream>
 #include <numeric>  // accumulate
 #include <queue>
+#include <random>
 #include <set>
 #include <stdexcept>
+#include <string>
+#include <utility>
 #include <vector>
 
 using std::cout;
@@ -68,81 +72,11 @@ static vector<size_t> sort_idx(const Container &src) {
   return idx;
 }
 
-/*****************************************************************************/
-static vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
-  /* compute the sum of the weights */
-  const double sum = std::accumulate(weight.begin(), weight.end(), 0.0);
-  if (sum == 0.0) {
-    throw std::runtime_error("Weight vector has no non-zero weights");
-  }
-
-  /* make a normalized and accumulated version of the weight vector */
-  const size_t m = weight.size();
-  vector<double> cdf;
-  cdf.reserve(m);
-  for (size_t i = 0; i < m; ++i) {
-    cdf.push_back(weight[i] / sum);
-  }
-
-  vector<double> acc(m);
-  std::partial_sum(cdf.begin(), cdf.end(), acc.begin());
-
-  /* iid sample n times */
-  vector<size_t> samples;
-  samples.reserve(n);
-  const double denominator = (double)RAND_MAX;
-  for (size_t i = 0; i < n; ++i) {
-    const double value = rand() / denominator;
-    /* binary search the interval containing "value" */
-    const auto it = std::lower_bound(acc.begin(), acc.end(), value);
-    const size_t index = it - acc.begin();
-    samples.push_back(index);
-  }
-  return samples;
-}
-
-/*****************************************************************************/
-static vector<size_t> UniqueSampler(const vector<double> &weight,
-                                    const size_t n) {
-  const size_t m = weight.size();
-  if (n > m) throw std::invalid_argument("UniqueSampler: invalid input size");
-
-  vector<size_t> results;
-
-  size_t count = 0;
-  vector<bool> touched(m, false);
-  while (count < n) {
-    vector<size_t> localIndices;
-    localIndices.reserve(n - count);
-    vector<double> localWeights;
-    localWeights.reserve(n - count);
-
-    /* collect data */
-    for (size_t i = 0; i < m; ++i) {
-      if (!touched[i]) {
-        localIndices.push_back(i);
-        localWeights.push_back(weight[i]);
-      }
-    }
-
-    /* sampling and cache results */
-    vector<size_t> samples = iidSampler(localWeights, n - count);
-    for (const size_t &index : samples) {
-      if (touched[index] == false) {
-        touched[index] = true;
-        results.push_back(index);
-        if (++count >= n) break;
-      }
-    }
-  }
-  return results;
-}
-
 /****************************************************************************/
 Subgraph::Subgraph(const vector<size_t> &indices) {
   edges_.reserve(indices.size());
   for (const size_t &index : indices) {
-    const Edge e {index,1.0};
+    const Edge e{index, 1.0};
     edges_.push_back(e);
   }
 }
@@ -423,12 +357,13 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
 /****************************************************************/
 vector<size_t> SubgraphBuilder::sample(const vector<double> &weights,
                                        const size_t t) const {
-  return UniqueSampler(weights, t);
+  std::mt19937 rng(42);  // TODO(frank): allow us to use a different seed
+  WeightedSampler<std::mt19937> sampler(&rng);
+  return sampler.sampleWithoutReplacement(t, weights);
 }
 
 /****************************************************************/
-Subgraph SubgraphBuilder::operator()(
-    const GaussianFactorGraph &gfg) const {
+Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const {
   const auto &p = parameters_;
   const auto inverse_ordering = Ordering::Natural(gfg);
   const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
@@ -518,15 +453,15 @@ GaussianFactorGraph::shared_ptr buildFactorSubgraph(
   subgraphFactors->reserve(subgraph.size());
   for (const auto &e : subgraph) {
     const auto factor = gfg[e.index];
-    subgraphFactors->push_back(clone ? factor->clone(): factor);
+    subgraphFactors->push_back(clone ? factor->clone() : factor);
   }
   return subgraphFactors;
 }
 
 /**************************************************************************************************/
-std::pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
-splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph) {
-
+std::pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>  //
+splitFactorGraph(const GaussianFactorGraph &factorGraph,
+                 const Subgraph &subgraph) {
   // Get the subgraph by calling cheaper method
   auto subgraphFactors = buildFactorSubgraph(factorGraph, subgraph, false);
 
diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp
index cd3ae815b..0a25617e4 100644
--- a/gtsam/linear/VectorValues.cpp
+++ b/gtsam/linear/VectorValues.cpp
@@ -51,7 +51,11 @@ namespace gtsam {
       Key key;
       size_t n;
       boost::tie(key, n) = v;
+#ifdef TBB_GREATER_EQUAL_2020
       values_.emplace(key, x.segment(j, n));
+#else
+      values_.insert(std::make_pair(key, x.segment(j, n)));
+#endif
       j += n;
     }
   }
@@ -60,7 +64,11 @@ namespace gtsam {
   VectorValues::VectorValues(const Vector& x, const Scatter& scatter) {
     size_t j = 0;
     for (const SlotEntry& v : scatter) {
+#ifdef TBB_GREATER_EQUAL_2020
       values_.emplace(v.key, x.segment(j, v.dimension));
+#else
+      values_.insert(std::make_pair(v.key, x.segment(j, v.dimension)));
+#endif
       j += v.dimension;
     }
   }
@@ -70,7 +78,11 @@ namespace gtsam {
   {
     VectorValues result;
     for(const KeyValuePair& v: other)
+#ifdef TBB_GREATER_EQUAL_2020
       result.values_.emplace(v.first, Vector::Zero(v.second.size()));
+#else
+      result.values_.insert(std::make_pair(v.first, Vector::Zero(v.second.size())));
+#endif
     return result;
   }
 
@@ -84,16 +96,6 @@ namespace gtsam {
     return result.first;
   }
 
-  /* ************************************************************************* */
-  VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) {
-    std::pair<iterator, bool> result = values_.emplace(j, value);
-    if(!result.second)
-      throw std::invalid_argument(
-      "Requested to emplace variable '" + DefaultKeyFormatter(j)
-      + "' already in this VectorValues.");
-    return result.first;
-  }
-
   /* ************************************************************************* */
   void VectorValues::update(const VectorValues& values)
   {
@@ -129,7 +131,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  ostream& operator<<(ostream& os, const VectorValues& v) {
+  GTSAM_EXPORT ostream& operator<<(ostream& os, const VectorValues& v) {
     // Change print depending on whether we are using TBB
 #ifdef GTSAM_USE_TBB
     map<Key, Vector> sorted;
@@ -266,7 +268,11 @@ namespace gtsam {
     VectorValues result;
     // The result.end() hint here should result in constant-time inserts
     for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
+#ifdef TBB_GREATER_EQUAL_2020
       result.values_.emplace(j1->first, j1->second + j2->second);
+#else
+      result.values_.insert(std::make_pair(j1->first, j1->second + j2->second));
+#endif
 
     return result;
   }
@@ -324,7 +330,11 @@ namespace gtsam {
     VectorValues result;
     // The result.end() hint here should result in constant-time inserts
     for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
+#ifdef TBB_GREATER_EQUAL_2020
       result.values_.emplace(j1->first, j1->second - j2->second);
+#else
+      result.values_.insert(std::make_pair(j1->first, j1->second - j2->second));
+#endif
 
     return result;
   }
@@ -340,7 +350,11 @@ namespace gtsam {
   {
     VectorValues result;
     for(const VectorValues::KeyValuePair& key_v: v)
+#ifdef TBB_GREATER_EQUAL_2020
       result.values_.emplace(key_v.first, a * key_v.second);
+#else
+      result.values_.insert(std::make_pair(key_v.first, a * key_v.second));
+#endif
     return result;
   }
 
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index fe6b5fcb2..9e60ff2aa 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -179,7 +179,14 @@ namespace gtsam {
      *  j is already used.
      * @param value The vector to be inserted.
      * @param j The index with which the value will be associated. */
-    iterator emplace(Key j, const Vector& value);
+    template<class... Args>
+    inline std::pair<VectorValues::iterator, bool> emplace(Key j, Args&&... args) {
+#if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020)
+      return values_.emplace(std::piecewise_construct, std::forward_as_tuple(j), std::forward_as_tuple(args...));
+#else
+      return values_.insert(std::make_pair(j, Vector(std::forward<Args>(args)...)));
+#endif
+    }
 
     /** Insert a vector \c value with key \c j.  Throws an invalid_argument exception if the key \c
      *  j is already used.
@@ -197,8 +204,12 @@ namespace gtsam {
      *  and an iterator to the existing value is returned, along with 'false'.  If the value did not
      *  exist, it is inserted and an iterator pointing to the new element, along with 'true', is
      *  returned. */
-    std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
-      return values_.emplace(j, value); 
+    inline std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
+#ifdef TBB_GREATER_EQUAL_2020
+      return values_.emplace(j, value);
+#else
+      return values_.insert(std::make_pair(j, value));
+#endif
     }
 
     /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */
@@ -230,7 +241,7 @@ namespace gtsam {
     const_iterator find(Key j) const { return values_.find(j); }
 
     /// overload operator << to print to stringstream
-    friend std::ostream& operator<<(std::ostream&, const VectorValues&);
+    GTSAM_EXPORT friend std::ostream& operator<<(std::ostream&, const VectorValues&);
 
     /** print required by Testable for unit testing */
     void print(const std::string& str = "VectorValues",
diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h
index 1afaf79d1..811e07121 100644
--- a/gtsam/linear/linearAlgorithms-inst.h
+++ b/gtsam/linear/linearAlgorithms-inst.h
@@ -98,8 +98,13 @@ namespace gtsam
             // Insert solution into a VectorValues
             DenseIndex vectorPosition = 0;
             for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
-              VectorValues::const_iterator r =
-                collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
+              auto result = collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
+              if(!result.second)
+                  throw std::runtime_error(
+                      "Internal error while optimizing clique. Trying to insert key '" + DefaultKeyFormatter(*frontal)
+                      + "' that exists.");
+
+              VectorValues::const_iterator r = result.first;
               myData.cliqueResults.emplace(r->first, r);
               vectorPosition += c.getDim(frontal);
             }
diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp
index ca3ebf91c..17dc6a425 100644
--- a/gtsam/linear/tests/testGaussianBayesTree.cpp
+++ b/gtsam/linear/tests/testGaussianBayesTree.cpp
@@ -25,7 +25,6 @@ using namespace boost::assign;
 
 #include <gtsam/base/debug.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/geometry/Rot2.h>
 #include <gtsam/linear/GaussianJunctionTree.h>
 #include <gtsam/linear/GaussianBayesTree.h>
 #include <gtsam/linear/GaussianConditional.h>
diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp
index 110a1223a..21146066d 100644
--- a/gtsam/linear/tests/testJacobianFactor.cpp
+++ b/gtsam/linear/tests/testJacobianFactor.cpp
@@ -253,7 +253,7 @@ TEST(JacobianFactor, error)
 /* ************************************************************************* */
 TEST(JacobianFactor, matrices_NULL)
 {
-  // Make sure everything works with NULL noise model
+  // Make sure everything works with nullptr noise model
   JacobianFactor factor(simple::terms, simple::b);
 
   Matrix jacobianExpected(3, 9);
diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp
index 85dc4735d..42d68a603 100644
--- a/gtsam/linear/tests/testNoiseModel.cpp
+++ b/gtsam/linear/tests/testNoiseModel.cpp
@@ -68,10 +68,10 @@ TEST(NoiseModel, constructors)
   for(Gaussian::shared_ptr mi: m)
     EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened)));
 
-  // test Mahalanobis distance
+  // test squared Mahalanobis distance
   double distance = 5*5+10*10+15*15;
   for(Gaussian::shared_ptr mi: m)
-    DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9);
+    DOUBLES_EQUAL(distance,mi->squaredMahalanobisDistance(unwhitened),1e-9);
 
   // test R matrix
   for(Gaussian::shared_ptr mi: m)
@@ -182,8 +182,9 @@ TEST(NoiseModel, ConstrainedMixed )
   EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible)));
   EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible)));
 
-  DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9);
-  DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9);
+  DOUBLES_EQUAL(0.5 * (1000.0 + 0.25 + 0.25),d->loss(d->squaredMahalanobisDistance(infeasible)),1e-9);
+  DOUBLES_EQUAL(0.5, d->squaredMahalanobisDistance(feasible),1e-9);
+  DOUBLES_EQUAL(0.5 * 0.5, d->loss(0.5),1e-9);
 }
 
 /* ************************************************************************* */
@@ -197,8 +198,9 @@ TEST(NoiseModel, ConstrainedAll )
   EXPECT(assert_equal(Vector3(1.0, 1.0, 1.0),i->whiten(infeasible)));
   EXPECT(assert_equal(Vector3(0.0, 0.0, 0.0),i->whiten(feasible)));
 
-  DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);
-  DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9);
+  DOUBLES_EQUAL(0.5 * 1000.0 * 3.0,i->loss(i->squaredMahalanobisDistance(infeasible)),1e-9);
+  DOUBLES_EQUAL(0.0, i->squaredMahalanobisDistance(feasible), 1e-9);
+  DOUBLES_EQUAL(0.0, i->loss(0.0),1e-9);
 }
 
 /* ************************************************************************* */
@@ -451,12 +453,28 @@ TEST(NoiseModel, WhitenInPlace)
 
 /*
  * These tests are responsible for testing the weight functions for the m-estimators in GTSAM.
- * The weight function is related to the analytic derivative of the residual function. See
+ * The weight function is related to the analytic derivative of the loss function. See
  *  https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
  * for details. This weight function is required when optimizing cost functions with robust
  * penalties using iteratively re-weighted least squares.
  */
 
+TEST(NoiseModel, robustFunctionFair)
+{
+  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
+  const mEstimator::Fair::shared_ptr fair = mEstimator::Fair::Create(k);
+  DOUBLES_EQUAL(0.8333333333333333, fair->weight(error1), 1e-8);
+  DOUBLES_EQUAL(0.3333333333333333, fair->weight(error2), 1e-8);
+  // Test negative value to ensure we take absolute value of error.
+  DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8);
+  DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8);
+
+  DOUBLES_EQUAL(0.441961080151135, fair->loss(error1), 1e-8);
+  DOUBLES_EQUAL(22.534692783297260, fair->loss(error2), 1e-8);
+  DOUBLES_EQUAL(22.534692783297260, fair->loss(error3), 1e-8);
+  DOUBLES_EQUAL(0.441961080151135, fair->loss(error4), 1e-8);
+}
+
 TEST(NoiseModel, robustFunctionHuber)
 {
   const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
@@ -466,26 +484,86 @@ TEST(NoiseModel, robustFunctionHuber)
   // Test negative value to ensure we take absolute value of error.
   DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8);
   DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8);
+
+  DOUBLES_EQUAL(0.5000, huber->loss(error1), 1e-8);
+  DOUBLES_EQUAL(37.5000, huber->loss(error2), 1e-8);
+  DOUBLES_EQUAL(37.5000, huber->loss(error3), 1e-8);
+  DOUBLES_EQUAL(0.5000, huber->loss(error4), 1e-8);
+}
+
+TEST(NoiseModel, robustFunctionCauchy)
+{
+  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
+  const mEstimator::Cauchy::shared_ptr cauchy = mEstimator::Cauchy::Create(k);
+  DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error1), 1e-8);
+  DOUBLES_EQUAL(0.2000, cauchy->weight(error2), 1e-8);
+  // Test negative value to ensure we take absolute value of error.
+  DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8);
+  DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8);
+
+  DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error1), 1e-8);
+  DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error2), 1e-8);
+  DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error3), 1e-8);
+  DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error4), 1e-8);
 }
 
 TEST(NoiseModel, robustFunctionGemanMcClure)
 {
-  const double k = 1.0, error1 = 1.0, error2 = 10.0;
+  const double k = 1.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
   const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k);
-  const double weight1 = gmc->weight(error1),
-               weight2 = gmc->weight(error2);
-  DOUBLES_EQUAL(0.25      , weight1, 1e-8);
-  DOUBLES_EQUAL(9.80296e-5, weight2, 1e-8);
+  DOUBLES_EQUAL(0.25      , gmc->weight(error1), 1e-8);
+  DOUBLES_EQUAL(9.80296e-5, gmc->weight(error2), 1e-8);
+  DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8);
+  DOUBLES_EQUAL(0.25      , gmc->weight(error4), 1e-8);
+
+  DOUBLES_EQUAL(0.2500, gmc->loss(error1), 1e-8);
+  DOUBLES_EQUAL(0.495049504950495, gmc->loss(error2), 1e-8);
+  DOUBLES_EQUAL(0.495049504950495, gmc->loss(error3), 1e-8);
+  DOUBLES_EQUAL(0.2500, gmc->loss(error4), 1e-8);
+}
+
+TEST(NoiseModel, robustFunctionWelsch)
+{
+  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
+  const mEstimator::Welsch::shared_ptr welsch = mEstimator::Welsch::Create(k);
+  DOUBLES_EQUAL(0.960789439152323, welsch->weight(error1), 1e-8);
+  DOUBLES_EQUAL(0.018315638888734, welsch->weight(error2), 1e-8);
+  // Test negative value to ensure we take absolute value of error.
+  DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8);
+  DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8);
+
+  DOUBLES_EQUAL(0.490132010595960, welsch->loss(error1), 1e-8);
+  DOUBLES_EQUAL(12.271054513890823, welsch->loss(error2), 1e-8);
+  DOUBLES_EQUAL(12.271054513890823, welsch->loss(error3), 1e-8);
+  DOUBLES_EQUAL(0.490132010595960, welsch->loss(error4), 1e-8);
+}
+
+TEST(NoiseModel, robustFunctionTukey)
+{
+  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
+  const mEstimator::Tukey::shared_ptr tukey = mEstimator::Tukey::Create(k);
+  DOUBLES_EQUAL(0.9216, tukey->weight(error1), 1e-8);
+  DOUBLES_EQUAL(0.0, tukey->weight(error2), 1e-8);
+  // Test negative value to ensure we take absolute value of error.
+  DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8);
+  DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8);
+
+  DOUBLES_EQUAL(0.480266666666667, tukey->loss(error1), 1e-8);
+  DOUBLES_EQUAL(4.166666666666667, tukey->loss(error2), 1e-8);
+  DOUBLES_EQUAL(4.166666666666667, tukey->loss(error3), 1e-8);
+  DOUBLES_EQUAL(0.480266666666667, tukey->loss(error4), 1e-8);
 }
 
 TEST(NoiseModel, robustFunctionDCS)
 {
   const double k = 1.0, error1 = 1.0, error2 = 10.0;
   const mEstimator::DCS::shared_ptr dcs = mEstimator::DCS::Create(k);
-  const double weight1 = dcs->weight(error1),
-               weight2 = dcs->weight(error2);
-  DOUBLES_EQUAL(1.0       , weight1, 1e-8);
-  DOUBLES_EQUAL(0.00039211, weight2, 1e-8);
+
+  DOUBLES_EQUAL(1.0       , dcs->weight(error1), 1e-8);
+  DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8);
+
+  DOUBLES_EQUAL(0.5         , dcs->loss(error1), 1e-8);
+  DOUBLES_EQUAL(0.9900990099, dcs->loss(error2), 1e-8);
 }
 
 TEST(NoiseModel, robustFunctionL2WithDeadZone)
@@ -500,12 +578,12 @@ TEST(NoiseModel, robustFunctionL2WithDeadZone)
   DOUBLES_EQUAL(0.00990099009, lsdz->weight(e4), 1e-8);
   DOUBLES_EQUAL(0.9,           lsdz->weight(e5), 1e-8);
 
-  DOUBLES_EQUAL(40.5,    lsdz->residual(e0), 1e-8);
-  DOUBLES_EQUAL(0.00005, lsdz->residual(e1), 1e-8);
-  DOUBLES_EQUAL(0.0,     lsdz->residual(e2), 1e-8);
-  DOUBLES_EQUAL(0.0,     lsdz->residual(e3), 1e-8);
-  DOUBLES_EQUAL(0.00005, lsdz->residual(e4), 1e-8);
-  DOUBLES_EQUAL(40.5,    lsdz->residual(e5), 1e-8);
+  DOUBLES_EQUAL(40.5,    lsdz->loss(e0), 1e-8);
+  DOUBLES_EQUAL(0.00005, lsdz->loss(e1), 1e-8);
+  DOUBLES_EQUAL(0.0,     lsdz->loss(e2), 1e-8);
+  DOUBLES_EQUAL(0.0,     lsdz->loss(e3), 1e-8);
+  DOUBLES_EQUAL(0.00005, lsdz->loss(e4), 1e-8);
+  DOUBLES_EQUAL(40.5,    lsdz->loss(e5), 1e-8);
 }
 
 /* ************************************************************************* */
@@ -589,11 +667,11 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone)
 
 /*
  * TODO(mike): There is currently a bug in GTSAM, where none of the mEstimator classes
- * implement a residual function, and GTSAM calls the weight function to evaluate the
- * total penalty, rather than calling the residual function. The weight function should be
+ * implement a loss function, and GTSAM calls the weight function to evaluate the
+ * total penalty, rather than calling the loss function. The weight function should be
  * used during iteratively reweighted least squares optimization, but should not be used to
  * evaluate the total penalty. The long-term solution is for all mEstimators to implement
- * both a weight and a residual function, and for GTSAM to call the residual function when
+ * both a weight and a loss function, and for GTSAM to call the loss function when
  * evaluating the total penalty. This bug causes the test below to fail, so I'm leaving it
  * commented out until the underlying bug in GTSAM is fixed.
  *
@@ -605,13 +683,44 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone)
 
 }
 
+TEST(NoiseModel, lossFunctionAtZero)
+{
+  const double k = 5.0;
+  auto fair = mEstimator::Fair::Create(k);
+  DOUBLES_EQUAL(fair->loss(0), 0, 1e-8);
+  DOUBLES_EQUAL(fair->weight(0), 1, 1e-8);
+  auto huber = mEstimator::Huber::Create(k);
+  DOUBLES_EQUAL(huber->loss(0), 0, 1e-8);
+  DOUBLES_EQUAL(huber->weight(0), 1, 1e-8);
+  auto cauchy = mEstimator::Cauchy::Create(k);
+  DOUBLES_EQUAL(cauchy->loss(0), 0, 1e-8);
+  DOUBLES_EQUAL(cauchy->weight(0), 1, 1e-8);
+  auto gmc = mEstimator::GemanMcClure::Create(k);
+  DOUBLES_EQUAL(gmc->loss(0), 0, 1e-8);
+  DOUBLES_EQUAL(gmc->weight(0), 1, 1e-8);
+  auto welsch = mEstimator::Welsch::Create(k);
+  DOUBLES_EQUAL(welsch->loss(0), 0, 1e-8);
+  DOUBLES_EQUAL(welsch->weight(0), 1, 1e-8);
+  auto tukey = mEstimator::Tukey::Create(k);
+  DOUBLES_EQUAL(tukey->loss(0), 0, 1e-8);
+  DOUBLES_EQUAL(tukey->weight(0), 1, 1e-8);
+  auto dcs = mEstimator::DCS::Create(k);
+  DOUBLES_EQUAL(dcs->loss(0), 0, 1e-8);
+  DOUBLES_EQUAL(dcs->weight(0), 1, 1e-8);
+  // auto lsdz = mEstimator::L2WithDeadZone::Create(k);
+  // DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8);
+  // DOUBLES_EQUAL(lsdz->weight(0), 1, 1e-8);
+}
+
+
 /* ************************************************************************* */
 #define TEST_GAUSSIAN(gaussian)\
   EQUALITY(info, gaussian->information());\
   EQUALITY(cov, gaussian->covariance());\
   EXPECT(assert_equal(white, gaussian->whiten(e)));\
   EXPECT(assert_equal(e, gaussian->unwhiten(white)));\
-  EXPECT_DOUBLES_EQUAL(251, gaussian->distance(e), 1e-9);\
+  EXPECT_DOUBLES_EQUAL(251.0, gaussian->squaredMahalanobisDistance(e), 1e-9);\
+  EXPECT_DOUBLES_EQUAL(0.5 * 251.0, gaussian->loss(251.0), 1e-9);\
   Matrix A = R.inverse(); Vector b = e;\
   gaussian->WhitenSystem(A, b);\
   EXPECT(assert_equal(I, A));\
diff --git a/gtsam/linear/tests/testSampler.cpp b/gtsam/linear/tests/testSampler.cpp
index bd718500e..5831d9048 100644
--- a/gtsam/linear/tests/testSampler.cpp
+++ b/gtsam/linear/tests/testSampler.cpp
@@ -10,8 +10,10 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file testSampler
+ * @file testSampler.cpp
+ * @brief unit tests for Sampler class
  * @author Alex Cunningham
+ * @author Frank Dellaert
  */
 
 #include <CppUnitLite/TestHarness.h>
@@ -22,14 +24,15 @@ using namespace gtsam;
 
 const double tol = 1e-5;
 
+static const Vector3 kSigmas(1.0, 0.1, 0.0);
+
 /* ************************************************************************* */
 TEST(testSampler, basic) {
-  Vector sigmas = Vector3(1.0, 0.1, 0.0);
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas);
+  auto model = noiseModel::Diagonal::Sigmas(kSigmas);
   char seed = 'A';
   Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1);
-  EXPECT(assert_equal(sigmas, sampler1.sigmas()));
-  EXPECT(assert_equal(sigmas, sampler2.sigmas()));
+  EXPECT(assert_equal(kSigmas, sampler1.sigmas()));
+  EXPECT(assert_equal(kSigmas, sampler2.sigmas()));
   EXPECT_LONGS_EQUAL(3, sampler1.dim());
   EXPECT_LONGS_EQUAL(3, sampler2.dim());
   Vector actual1 = sampler1.sample();
@@ -38,5 +41,8 @@ TEST(testSampler, basic) {
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */
diff --git a/gtsam/mainpage.dox b/gtsam/mainpage.dox
index db42b1277..ee7bd9924 100644
--- a/gtsam/mainpage.dox
+++ b/gtsam/mainpage.dox
@@ -1,7 +1,6 @@
 // This causes Doxygen to find classes inside the gtsam namespace without
 // explicitly specifying it when writing class names.
 namespace gtsam {
-
 /**
 
 \mainpage GTSAM
@@ -17,11 +16,10 @@ To use GTSAM to solve your own problems, you will often have to create new facto
 
 -# The number of variables your factor involves is <b>unknown</b> at compile time - derive from NoiseModelFactor and implement NoiseModelFactor::unwhitenedError()
    - This is a factor expressing the sum-of-squares error between a measurement \f$ z \f$ and a measurement prediction function \f$ h(x) \f$, on which the errors are expected to follow some distribution specified by a noise model (see noiseModel).
--# The number of variables your factor involves is <b>known</b> at compile time and is between 1 and 6 - derive from NonlinearFactor1, NonlinearFactor2, NonlinearFactor3, NonlinearFactor4, NonlinearFactor5, or NonlinearFactor6, and implement <b>\c evaluateError()</b>
+-# The number of variables your factor involves is <b>known</b> at compile time and is between 1 and 6 - derive from NoiseModelFactor1, NoiseModelFactor2, NoiseModelFactor3, NoiseModelFactor4, NoiseModelFactor5, or NoiseModelFactor6, and implement <b>\c evaluateError()</b>
    - This factor expresses the same sum-of-squares error with a noise model, but makes the implementation task slightly easier than with %NoiseModelFactor.
 -# Derive from NonlinearFactor
    - This is more advanced and allows creating factors without an explicit noise model, or that linearize to HessianFactor instead of JacobianFactor.
 
 */
-
-}
\ No newline at end of file
+}
diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp
index 0d7e05515..4604a55dd 100644
--- a/gtsam/navigation/AHRSFactor.cpp
+++ b/gtsam/navigation/AHRSFactor.cpp
@@ -114,7 +114,7 @@ void AHRSFactor::print(const string& s,
 //------------------------------------------------------------------------------
 bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
   const This *e = dynamic_cast<const This*>(&other);
-  return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
+  return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
 }
 
 //------------------------------------------------------------------------------
diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp
index 3c6af9332..7f335152e 100644
--- a/gtsam/navigation/AttitudeFactor.cpp
+++ b/gtsam/navigation/AttitudeFactor.cpp
@@ -52,7 +52,7 @@ void Rot3AttitudeFactor::print(const string& s,
 bool Rot3AttitudeFactor::equals(const NonlinearFactor& expected,
     double tol) const {
   const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
+  return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
       && this->bRef_.equals(e->bRef_, tol);
 }
 
@@ -69,7 +69,7 @@ void Pose3AttitudeFactor::print(const string& s,
 bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected,
     double tol) const {
   const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
+  return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
       && this->bRef_.equals(e->bRef_, tol);
 }
 
diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h
index 19c1f8139..5a0031caf 100644
--- a/gtsam/navigation/AttitudeFactor.h
+++ b/gtsam/navigation/AttitudeFactor.h
@@ -35,7 +35,7 @@ class AttitudeFactor {
 
 protected:
 
-  const Unit3 nZ_, bRef_; ///< Position measurement in
+  Unit3 nZ_, bRef_; ///< Position measurement in
 
 public:
 
@@ -56,12 +56,19 @@ public:
   Vector attitudeError(const Rot3& p,
       OptionalJacobian<2,3> H = boost::none) const;
 
+  const Unit3& nZ() const {
+    return nZ_;
+  }
+  const Unit3& bRef() const {
+    return bRef_;
+  }
+
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
-    ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));
+    ar & boost::serialization::make_nvp("nZ_",  nZ_);
+    ar & boost::serialization::make_nvp("bRef_", bRef_);
   }
 };
 
@@ -118,12 +125,6 @@ public:
       boost::optional<Matrix&> H = boost::none) const {
     return attitudeError(nRb, H);
   }
-  Unit3 nZ() const {
-    return nZ_;
-  }
-  Unit3 bRef() const {
-    return bRef_;
-  }
 
 private:
 
@@ -138,7 +139,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /// traits
@@ -204,12 +205,6 @@ public:
     }
     return e;
   }
-  Unit3 nZ() const {
-    return nZ_;
-  }
-  Unit3 bRef() const {
-    return bRef_;
-  }
 
 private:
 
@@ -224,7 +219,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /// traits
diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp
index 1967e4a56..d7b4b7bf1 100644
--- a/gtsam/navigation/CombinedImuFactor.cpp
+++ b/gtsam/navigation/CombinedImuFactor.cpp
@@ -17,9 +17,11 @@
  *  @author Vadim Indelman
  *  @author David Jensen
  *  @author Frank Dellaert
+ *  @author Varun Agrawal
  **/
 
 #include <gtsam/navigation/CombinedImuFactor.h>
+#include <boost/serialization/export.hpp>
 
 /* External or standard includes */
 #include <ostream>
@@ -28,6 +30,31 @@ namespace gtsam {
 
 using namespace std;
 
+//------------------------------------------------------------------------------
+// Inner class PreintegrationCombinedParams
+//------------------------------------------------------------------------------
+void PreintegrationCombinedParams::print(const string& s) const {
+  PreintegrationParams::print(s);
+  cout << "biasAccCovariance:\n[\n" << biasAccCovariance << "\n]"
+       << endl;
+  cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]"
+       << endl;
+  cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]"
+       << endl;
+}
+
+//------------------------------------------------------------------------------
+bool PreintegrationCombinedParams::equals(const PreintegratedRotationParams& other,
+                                  double tol) const {
+  auto e = dynamic_cast<const PreintegrationCombinedParams*>(&other);
+  return e != nullptr && PreintegrationParams::equals(other, tol) &&
+         equal_with_abs_tol(biasAccCovariance, e->biasAccCovariance,
+                            tol) &&
+         equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance,
+                            tol) &&
+         equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, tol);
+}
+
 //------------------------------------------------------------------------------
 // Inner class PreintegratedCombinedMeasurements
 //------------------------------------------------------------------------------
@@ -174,7 +201,7 @@ void CombinedImuFactor::print(const string& s,
 //------------------------------------------------------------------------------
 bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const {
   const This* e = dynamic_cast<const This*>(&other);
-  return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
+  return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
 }
 
 //------------------------------------------------------------------------------
@@ -242,6 +269,13 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
   return r;
 }
 
+//------------------------------------------------------------------------------
+std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
+  f._PIM_.print("combined preintegrated measurements:\n");
+  os << "  noise model sigmas: " << f.noiseModel_->sigmas().transpose();
+  return os;
+}
+
 //------------------------------------------------------------------------------
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 CombinedImuFactor::CombinedImuFactor(
@@ -279,3 +313,6 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
 }
  /// namespace gtsam
 
+/// Boost serialization export definition for derived class
+BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams);
+
diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h
index 2ad71cb3c..a89568433 100644
--- a/gtsam/navigation/CombinedImuFactor.h
+++ b/gtsam/navigation/CombinedImuFactor.h
@@ -17,6 +17,7 @@
  *  @author Vadim Indelman
  *  @author David Jensen
  *  @author Frank Dellaert
+ *  @author Varun Agrawal
  **/
 
 #pragma once
@@ -26,6 +27,7 @@
 #include <gtsam/navigation/TangentPreintegration.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/serialization.h>
 
 namespace gtsam {
 
@@ -54,6 +56,65 @@ typedef ManifoldPreintegration PreintegrationType;
  *     Robotics: Science and Systems (RSS), 2015.
  */
 
+/// Parameters for pre-integration using PreintegratedCombinedMeasurements:
+/// Usage: Create just a single Params and pass a shared pointer to the constructor
+struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
+  Matrix3 biasAccCovariance;    ///< continuous-time "Covariance" describing accelerometer bias random walk
+  Matrix3 biasOmegaCovariance;  ///< continuous-time "Covariance" describing gyroscope bias random walk
+  Matrix6 biasAccOmegaInt;     ///< covariance of bias used for pre-integration
+
+  /// Default constructor makes uninitialized params struct.
+  /// Used for serialization.
+  PreintegrationCombinedParams()
+      : biasAccCovariance(I_3x3),
+        biasOmegaCovariance(I_3x3),
+        biasAccOmegaInt(I_6x6) {}
+
+  /// See two named constructors below for good values of n_gravity in body frame
+  PreintegrationCombinedParams(const Vector3& n_gravity) :
+    PreintegrationParams(n_gravity), biasAccCovariance(I_3x3),
+    biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
+
+  }
+
+  // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
+  static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedD(double g = 9.81) {
+    return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, g)));
+  }
+
+  // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
+  static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedU(double g = 9.81) {
+    return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
+  }
+
+  void print(const std::string& s="") const;
+  bool equals(const PreintegratedRotationParams& other, double tol) const;
+
+  void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
+  void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
+  void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
+  
+  const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
+  const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
+  const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
+  
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    namespace bs = ::boost::serialization;
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
+    ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
+    ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
+    ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
+  }
+
+public:
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
+};
+
 /**
  * PreintegratedCombinedMeasurements integrates the IMU measurements
  * (rotation rates and accelerations) and the corresponding covariance matrix.
@@ -67,47 +128,7 @@ typedef ManifoldPreintegration PreintegrationType;
 class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType {
 
 public:
-
-  /// Parameters for pre-integration:
-  /// Usage: Create just a single Params and pass a shared pointer to the constructor
-  struct Params : PreintegrationParams {
-    Matrix3 biasAccCovariance;    ///< continuous-time "Covariance" describing accelerometer bias random walk
-    Matrix3 biasOmegaCovariance;  ///< continuous-time "Covariance" describing gyroscope bias random walk
-    Matrix6 biasAccOmegaInt;     ///< covariance of bias used for pre-integration
-
-    /// See two named constructors below for good values of n_gravity in body frame
-    Params(const Vector3& n_gravity) :
-        PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
-            I_3x3), biasAccOmegaInt(I_6x6) {
-    }
-
-    // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
-    static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
-      return boost::shared_ptr<Params>(new Params(Vector3(0, 0, g)));
-    }
-
-    // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
-    static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
-      return boost::shared_ptr<Params>(new Params(Vector3(0, 0, -g)));
-    }
-
-   private:
-    /// Default constructor makes unitialized params struct
-    Params() {}
-
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template <class ARCHIVE>
-    void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
-      ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
-      ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
-      ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
-      ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
-    }
-
-   public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  };
+  typedef PreintegrationCombinedParams Params;
 
  protected:
   /* Covariance matrix of the preintegrated measurements
@@ -118,7 +139,6 @@ public:
    */
   Eigen::Matrix<double, 15, 15> preintMeasCov_;
 
-
   friend class CombinedImuFactor;
 
  public:
@@ -126,11 +146,14 @@ public:
   /// @{
 
   /// Default constructor only for serialization and Cython wrapper
-  PreintegratedCombinedMeasurements() {}
+  PreintegratedCombinedMeasurements() {
+    preintMeasCov_.setZero();
+  }
 
   /**
    *  Default constructor, initializes the class with no measurements
-   *  @param bias Current estimate of acceleration and rotation rate biases
+   *  @param p       Parameters, typically fixed in a single application
+   *  @param biasHat Current estimate of acceleration and rotation rate biases
    */
   PreintegratedCombinedMeasurements(
       const boost::shared_ptr<Params>& p,
@@ -139,6 +162,19 @@ public:
     preintMeasCov_.setZero();
   }
 
+  /**
+  *  Construct preintegrated directly from members: base class and preintMeasCov
+  *  @param base               PreintegrationType instance
+  *  @param preintMeasCov      Covariance matrix used in noise model.
+  */
+  PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix<double, 15, 15>& preintMeasCov)
+     : PreintegrationType(base),
+       preintMeasCov_(preintMeasCov) {
+  }
+
+  /// Virtual destructor
+  virtual ~PreintegratedCombinedMeasurements() {}
+
   /// @}
 
   /// @name Basic utilities
@@ -148,20 +184,25 @@ public:
   void resetIntegration() override;
 
   /// const reference to params, shadows definition in base class
-  Params& p() const { return *boost::static_pointer_cast<Params>(this->p_);}
+  Params& p() const { return *boost::static_pointer_cast<Params>(this->p_); }
   /// @}
 
   /// @name Access instance variables
   /// @{
+  /// Return pre-integrated measurement covariance
   Matrix preintMeasCov() const { return preintMeasCov_; }
   /// @}
 
   /// @name Testable
   /// @{
+  /// print
   void print(const std::string& s = "Preintegrated Measurements:") const override;
-  bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const;
+  /// equals
+  bool equals(const PreintegratedCombinedMeasurements& expected,
+              double tol = 1e-9) const;
   /// @}
 
+
   /// @name Main functionality
   /// @{
 
@@ -195,12 +236,13 @@ public:
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    namespace bs = ::boost::serialization;
     ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
     ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /**
@@ -234,9 +276,6 @@ private:
 
   PreintegratedCombinedMeasurements _PIM_;
 
-  /** Default constructor - only use for serialization */
-  CombinedImuFactor() {}
-
 public:
 
   /** Shorthand for a smart pointer to a factor */
@@ -246,6 +285,9 @@ public:
   typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
 #endif
 
+  /** Default constructor - only use for serialization */
+  CombinedImuFactor() {}
+
   /**
    * Constructor
    * @param pose_i Previous pose key
@@ -267,12 +309,17 @@ public:
 
   /** implement functions needed for Testable */
 
+  /// @name Testable
+  /// @{
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const CombinedImuFactor&);
   /// print
   virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const;
 
   /// equals
   virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  /// @}
 
   /** Access the preintegrated measurements. */
 
@@ -311,19 +358,31 @@ public:
 #endif
 
 private:
-
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar & boost::serialization::make_nvp("NoiseModelFactor6",
-         boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(_PIM_);
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor6);
+    ar& BOOST_SERIALIZATION_NVP(_PIM_);
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // class CombinedImuFactor
 
-} /// namespace gtsam
+template <>
+struct traits<PreintegrationCombinedParams>
+    : public Testable<PreintegrationCombinedParams> {};
+
+template <>
+struct traits<PreintegratedCombinedMeasurements>
+    : public Testable<PreintegratedCombinedMeasurements> {};
+
+template <>
+struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
+
+}  // namespace gtsam
+
+/// Add Boost serialization export key (declaration) for derived class
+BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams);
diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp
index 05b7259bf..f2448c488 100644
--- a/gtsam/navigation/GPSFactor.cpp
+++ b/gtsam/navigation/GPSFactor.cpp
@@ -32,7 +32,7 @@ void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
 //***************************************************************************
 bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
   const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol) && traits<Point3>::Equals(nT_, e->nT_, tol);
+  return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(nT_, e->nT_, tol);
 }
 
 //***************************************************************************
@@ -73,7 +73,7 @@ void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const
 //***************************************************************************
 bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const {
   const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol) &&
+  return e != nullptr && Base::equals(*e, tol) &&
          traits<Point3>::Equals(nT_, e->nT_, tol);
 }
 
diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h
index ff1a53025..d52b4eb29 100644
--- a/gtsam/navigation/ImuBias.h
+++ b/gtsam/navigation/ImuBias.h
@@ -171,7 +171,7 @@ private:
 
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
   /// @}
 
 }; // ConstantBias class
diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h
index 8e3f8f0a4..7e080ffd5 100644
--- a/gtsam/navigation/ImuFactor.h
+++ b/gtsam/navigation/ImuFactor.h
@@ -59,7 +59,7 @@ typedef ManifoldPreintegration PreintegrationType;
  */
 
 /**
- * PreintegratedIMUMeasurements accumulates (integrates) the IMU measurements
+ * PreintegratedImuMeasurements accumulates (integrates) the IMU measurements
  * (rotation rates and accelerations) and the corresponding covariance matrix.
  * The measurements are then used to build the Preintegrated IMU factor.
  * Integration is done incrementally (ideally, one integrates the measurement
@@ -87,8 +87,8 @@ public:
 
  /**
    *  Constructor, initializes the class with no measurements
-   *  @param bias Current estimate of acceleration and rotation rate biases
-   *  @param p    Parameters, typically fixed in a single application
+   *  @param p       Parameters, typically fixed in a single application
+   *  @param biasHat Current estimate of acceleration and rotation rate biases
    */
   PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
       const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
@@ -164,7 +164,7 @@ private:
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     namespace bs = ::boost::serialization;
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
-    ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size()));
+    ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
   }
 };
 
diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h
index 22897b9d4..a290972e4 100644
--- a/gtsam/navigation/ManifoldPreintegration.h
+++ b/gtsam/navigation/ManifoldPreintegration.h
@@ -118,15 +118,13 @@ private:
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     namespace bs = ::boost::serialization;
-    ar & BOOST_SERIALIZATION_NVP(p_);
-    ar & BOOST_SERIALIZATION_NVP(deltaTij_);
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
     ar & BOOST_SERIALIZATION_NVP(deltaXij_);
-    ar & BOOST_SERIALIZATION_NVP(biasHat_);
-    ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
-    ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
-    ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
-    ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
-    ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
+    ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
+    ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
+    ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
+    ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
+    ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
   }
 };
 
diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h
index e9afcd3ac..f66e9d7a9 100644
--- a/gtsam/navigation/NavState.h
+++ b/gtsam/navigation/NavState.h
@@ -64,7 +64,7 @@ public:
       R_(pose.rotation()), t_(pose.translation()), v_(v) {
   }
   /// Construct from SO(3) and R^6
-  NavState(const Matrix3& R, const Vector9 tv) :
+  NavState(const Matrix3& R, const Vector6& tv) :
       R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
   }
   /// Named constructor with derivatives
diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp
index 8c29d85dd..c5d48b734 100644
--- a/gtsam/navigation/PreintegratedRotation.cpp
+++ b/gtsam/navigation/PreintegratedRotation.cpp
@@ -25,7 +25,7 @@ using namespace std;
 
 namespace gtsam {
 
-void PreintegratedRotation::Params::print(const string& s) const {
+void PreintegratedRotationParams::print(const string& s) const {
   cout << s << endl;
   cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
   if (omegaCoriolis)
@@ -34,8 +34,8 @@ void PreintegratedRotation::Params::print(const string& s) const {
     body_P_sensor->print("body_P_sensor");
 }
 
-bool PreintegratedRotation::Params::equals(
-    const PreintegratedRotation::Params& other, double tol) const {
+bool PreintegratedRotationParams::equals(
+    const PreintegratedRotationParams& other, double tol) const {
   if (body_P_sensor) {
     if (!other.body_P_sensor
         || !assert_equal(*body_P_sensor, *other.body_P_sensor, tol))
diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h
index 71ef5d08f..0e0559a32 100644
--- a/gtsam/navigation/PreintegratedRotation.h
+++ b/gtsam/navigation/PreintegratedRotation.h
@@ -61,15 +61,21 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     namespace bs = ::boost::serialization;
-    ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
-    ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
+    ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
     ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
+
+    // Provide support for Eigen::Matrix in boost::optional
+    bool omegaCoriolisFlag = omegaCoriolis.is_initialized();
+    ar & boost::serialization::make_nvp("omegaCoriolisFlag", omegaCoriolisFlag);
+    if (omegaCoriolisFlag) {
+      ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis);
+    }
   }
 
 #ifdef GTSAM_USE_QUATERNIONS
   // Align if we are using Quaternions
 public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
 };
 
@@ -182,7 +188,7 @@ class GTSAM_EXPORT PreintegratedRotation {
 #ifdef GTSAM_USE_QUATERNIONS
   // Align if we are using Quaternions
   public:
-	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
 };
 
diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp
index 0e8cf67c9..4af752ac0 100644
--- a/gtsam/navigation/PreintegrationBase.cpp
+++ b/gtsam/navigation/PreintegrationBase.cpp
@@ -115,20 +115,21 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
 NavState PreintegrationBase::predict(const NavState& state_i,
     const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
     OptionalJacobian<9, 6> H2) const {
-  // TODO(frank): make sure this stuff is still correct
   Matrix96 D_biasCorrected_bias;
   Vector9 biasCorrected = biasCorrectedDelta(bias_i,
-      H2 ? &D_biasCorrected_bias : 0);
+                                             H2 ? &D_biasCorrected_bias : nullptr);
 
   // Correct for initial velocity and gravity
   Matrix9 D_delta_state, D_delta_biasCorrected;
   Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
-      p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
-      H2 ? &D_delta_biasCorrected : 0);
+                                  p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : nullptr,
+                                  H2 ? &D_delta_biasCorrected : nullptr);
 
   // Use retract to get back to NavState manifold
   Matrix9 D_predict_state, D_predict_delta;
-  NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
+  NavState state_j = state_i.retract(xi,
+                                     H1 ? &D_predict_state : nullptr,
+                                     H2 || H2 ? &D_predict_delta : nullptr);
   if (H1)
     *H1 = D_predict_state + D_predict_delta * D_delta_state;
   if (H2)
diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index 9926d207a..29d7814b5 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@@ -213,8 +213,18 @@ class GTSAM_EXPORT PreintegrationBase {
   /// @}
 #endif
 
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    ar & BOOST_SERIALIZATION_NVP(p_);
+    ar & BOOST_SERIALIZATION_NVP(biasHat_);
+    ar & BOOST_SERIALIZATION_NVP(deltaTij_);
+  }
+
  public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 }  /// namespace gtsam
diff --git a/gtsam/navigation/PreintegrationParams.cpp b/gtsam/navigation/PreintegrationParams.cpp
index 61cd1617c..2298bb696 100644
--- a/gtsam/navigation/PreintegrationParams.cpp
+++ b/gtsam/navigation/PreintegrationParams.cpp
@@ -27,7 +27,7 @@ namespace gtsam {
 
 //------------------------------------------------------------------------------
 void PreintegrationParams::print(const string& s) const {
-  PreintegratedRotation::Params::print(s);
+  PreintegratedRotationParams::print(s);
   cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
        << endl;
   cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]"
@@ -39,10 +39,10 @@ void PreintegrationParams::print(const string& s) const {
 }
 
 //------------------------------------------------------------------------------
-bool PreintegrationParams::equals(const PreintegratedRotation::Params& other,
+bool PreintegrationParams::equals(const PreintegratedRotationParams& other,
                                   double tol) const {
   auto e = dynamic_cast<const PreintegrationParams*>(&other);
-  return e != nullptr && PreintegratedRotation::Params::equals(other, tol) &&
+  return e != nullptr && PreintegratedRotationParams::equals(other, tol) &&
          use2ndOrderCoriolis == e->use2ndOrderCoriolis &&
          equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance,
                             tol) &&
diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h
index 0fb54a358..9ae66e678 100644
--- a/gtsam/navigation/PreintegrationParams.h
+++ b/gtsam/navigation/PreintegrationParams.h
@@ -29,10 +29,19 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
   bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
   Vector3 n_gravity; ///< Gravity vector in nav frame
 
+  /// Default constructor for serialization only
+  PreintegrationParams()
+      : PreintegratedRotationParams(),
+        accelerometerCovariance(I_3x3),
+        integrationCovariance(I_3x3),
+        use2ndOrderCoriolis(false),
+        n_gravity(0, 0, -1) {}
+
   /// The Params constructor insists on getting the navigation frame gravity vector
   /// For convenience, two commonly used conventions are provided by named constructors below
   PreintegrationParams(const Vector3& n_gravity)
-      : accelerometerCovariance(I_3x3),
+      : PreintegratedRotationParams(),
+        accelerometerCovariance(I_3x3),
         integrationCovariance(I_3x3),
         use2ndOrderCoriolis(false),
         n_gravity(n_gravity) {}
@@ -47,8 +56,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
     return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g)));
   }
 
-  void print(const std::string& s) const;
-  bool equals(const PreintegratedRotation::Params& other, double tol) const;
+  void print(const std::string& s="") const;
+  bool equals(const PreintegratedRotationParams& other, double tol) const;
 
   void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
   void setIntegrationCovariance(const Matrix3& cov)   { integrationCovariance = cov; }
@@ -60,18 +69,15 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
   bool           getUse2ndOrderCoriolis()     const { return use2ndOrderCoriolis; }
 
 protected:
-  /// Default constructor for serialization only: uninitialized!
-  PreintegrationParams() {}
 
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     namespace bs = ::boost::serialization;
-    ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
-         boost::serialization::base_object<PreintegratedRotationParams>(*this));
-    ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
-    ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotationParams);
+    ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
+    ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
     ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
     ar & BOOST_SERIALIZATION_NVP(n_gravity);
   }
@@ -79,7 +85,7 @@ protected:
 #ifdef GTSAM_USE_QUATERNIONS
   // Align if we are using Quaternions
 public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
 };
 
diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h
index 5fb9f78d7..649d0fe12 100644
--- a/gtsam/navigation/ScenarioRunner.h
+++ b/gtsam/navigation/ScenarioRunner.h
@@ -48,7 +48,7 @@ class GTSAM_EXPORT ScenarioRunner {
   const Bias estimatedBias_;
 
   // Create two samplers for acceleration and omega noise
-  mutable Sampler gyroSampler_, accSampler_;
+  Sampler gyroSampler_, accSampler_;
 
  public:
   ScenarioRunner(const Scenario& scenario, const SharedParams& p,
diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp
index 4229d4f0c..56d7aa6d3 100644
--- a/gtsam/navigation/TangentPreintegration.cpp
+++ b/gtsam/navigation/TangentPreintegration.cpp
@@ -68,30 +68,30 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
   Matrix3 w_tangent_H_theta, invH;
   const Vector3 w_tangent = // angular velocity mapped back to tangent space
       local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
-  const SO3 R = local.expmap();
+  const Rot3 R(local.expmap());  // nRb: rotation of body in nav frame
   const Vector3 a_nav = R * a_body;
   const double dt22 = 0.5 * dt * dt;
 
   Vector9 preintegratedPlus;
-  preintegratedPlus << // new preintegrated vector:
-      theta + w_tangent * dt, // theta
-  position + velocity * dt + a_nav * dt22, // position
-  velocity + a_nav * dt; // velocity
+  preintegratedPlus <<                          // new preintegrated vector:
+      theta + w_tangent * dt,                   // theta
+      position + velocity * dt + a_nav * dt22,  // position
+      velocity + a_nav * dt;                    // velocity
 
   if (A) {
     // Exact derivative of R*a with respect to theta:
-    const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();
+    const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.dexp();
 
     A->setIdentity();
-    A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
-    A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
-    A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
-    A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
+    A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt;  // theta
+    A->block<3, 3>(3, 0) = a_nav_H_theta * dt22;  // position wrpt theta...
+    A->block<3, 3>(3, 6) = I_3x3 * dt;            // .. and velocity
+    A->block<3, 3>(6, 0) = a_nav_H_theta * dt;    // velocity wrpt theta
   }
   if (B) {
     B->block<3, 3>(0, 0) = Z_3x3;
-    B->block<3, 3>(3, 0) = R * dt22;
-    B->block<3, 3>(6, 0) = R * dt;
+    B->block<3, 3>(3, 0) = R.matrix() * dt22;
+    B->block<3, 3>(6, 0) = R.matrix() * dt;
   }
   if (C) {
     C->block<3, 3>(0, 0) = invH * dt;
@@ -110,7 +110,7 @@ void TangentPreintegration::update(const Vector3& measuredAcc,
   Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
   Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
 
-  // Possibly correct for sensor pose
+  // Possibly correct for sensor pose by converting to body frame
   Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
   if (p().body_P_sensor)
     boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h
index 11945e53a..1b51b4e1e 100644
--- a/gtsam/navigation/TangentPreintegration.h
+++ b/gtsam/navigation/TangentPreintegration.h
@@ -132,16 +132,14 @@ private:
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     namespace bs = ::boost::serialization;
-    ar & BOOST_SERIALIZATION_NVP(p_);
-    ar & BOOST_SERIALIZATION_NVP(biasHat_);
-    ar & BOOST_SERIALIZATION_NVP(deltaTij_);
-    ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size()));
-    ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size()));
-    ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size()));
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
+    ar & BOOST_SERIALIZATION_NVP(preintegrated_);
+    ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasAcc_);
+    ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasOmega_);
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 } /// namespace gtsam
diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp
index d32553b94..74acf69da 100644
--- a/gtsam/navigation/tests/testAHRSFactor.cpp
+++ b/gtsam/navigation/tests/testAHRSFactor.cpp
@@ -15,6 +15,7 @@
  * @author Krunal Chande
  * @author Luca Carlone
  * @author Frank Dellaert
+ * @author Varun Agrawal
  */
 
 #include <gtsam/navigation/AHRSFactor.h>
@@ -200,7 +201,7 @@ TEST(AHRSFactor, Error) {
   // 1e-5 needs to be added only when using quaternions for rotations
 
   EXPECT(assert_equal(H3e, H3a, 1e-5));
-  // FIXME!! DOes not work. Different matrix dimensions.
+  // 1e-5 needs to be added only when using quaternions for rotations
 }
 
 /* ************************************************************************* */
@@ -341,7 +342,7 @@ TEST( AHRSFactor, fistOrderExponential ) {
 //******************************************************************************
 TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
   // Linearization point
-  Vector3 bias; ///< Current estimate of rotation rate bias
+  Vector3 bias = Vector3::Zero(); ///< Current estimate of rotation rate bias
 
   Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
 
diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp
index 70b78c916..38f16f55f 100644
--- a/gtsam/navigation/tests/testAttitudeFactor.cpp
+++ b/gtsam/navigation/tests/testAttitudeFactor.cpp
@@ -75,6 +75,23 @@ TEST(Rot3AttitudeFactor, Serialization) {
   EXPECT(serializationTestHelpers::equalsBinary(factor));
 }
 
+/* ************************************************************************* */
+TEST(Rot3AttitudeFactor, CopyAndMove) {
+  Unit3 nDown(0, 0, -1);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
+  Rot3AttitudeFactor factor(0, nDown, model);
+
+  // Copy assignable.
+  EXPECT(std::is_copy_assignable<Rot3AttitudeFactor>::value);
+  Rot3AttitudeFactor factor_copied = factor;
+  EXPECT(assert_equal(factor, factor_copied));
+
+  // Move assignable.
+  EXPECT(std::is_move_assignable<Rot3AttitudeFactor>::value);
+  Rot3AttitudeFactor factor_moved = std::move(factor_copied);
+  EXPECT(assert_equal(factor, factor_moved));
+}
+
 // *************************************************************************
 TEST( Pose3AttitudeFactor, Constructor ) {
 
@@ -119,6 +136,23 @@ TEST(Pose3AttitudeFactor, Serialization) {
   EXPECT(serializationTestHelpers::equalsBinary(factor));
 }
 
+/* ************************************************************************* */
+TEST(Pose3AttitudeFactor, CopyAndMove) {
+  Unit3 nDown(0, 0, -1);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
+  Pose3AttitudeFactor factor(0, nDown, model);
+
+  // Copy assignable.
+  EXPECT(std::is_copy_assignable<Pose3AttitudeFactor>::value);
+  Pose3AttitudeFactor factor_copied = factor;
+  EXPECT(assert_equal(factor, factor_copied));
+
+  // Move assignable.
+  EXPECT(std::is_move_assignable<Pose3AttitudeFactor>::value);
+  Pose3AttitudeFactor factor_moved = std::move(factor_copied);
+  EXPECT(assert_equal(factor, factor_moved));
+}
+
 // *************************************************************************
 int main() {
   TestResult tr;
diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp
index 460695ec6..474b00add 100644
--- a/gtsam/navigation/tests/testImuFactor.cpp
+++ b/gtsam/navigation/tests/testImuFactor.cpp
@@ -717,7 +717,6 @@ TEST(ImuFactor, bodyPSensorNoBias) {
 /* ************************************************************************* */
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>
 
@@ -763,20 +762,17 @@ TEST(ImuFactor, bodyPSensorWithBias) {
   NonlinearFactorGraph graph;
   Values values;
 
-  PriorFactor<Pose3> priorPose(X(0), Pose3(), priorNoisePose);
-  graph.add(priorPose);
+  graph.addPrior(X(0), Pose3(), priorNoisePose);
   values.insert(X(0), Pose3());
 
-  PriorFactor<Vector3> priorVel(V(0), zeroVel, priorNoiseVel);
-  graph.add(priorVel);
+  graph.addPrior(V(0), zeroVel, priorNoiseVel);
   values.insert(V(0), zeroVel);
 
   // The key to this test is that we specify the bias, in the sensor frame, as known a priori
   // We also create factors below that encode our assumption that this bias is constant over time
   // In theory, after optimization, we should recover that same bias estimate
   Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
-  PriorFactor<Bias> priorBiasFactor(B(0), priorBias, priorNoiseBias);
-  graph.add(priorBiasFactor);
+  graph.addPrior(B(0), priorBias, priorNoiseBias);
   values.insert(B(0), priorBias);
 
   // Now add IMU factors and bias noise models
diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp
index 9f9781d2c..ed72e18e9 100644
--- a/gtsam/navigation/tests/testImuFactorSerialization.cpp
+++ b/gtsam/navigation/tests/testImuFactorSerialization.cpp
@@ -16,15 +16,19 @@
  * @author  Frank Dellaert
  * @author  Richard Roberts
  * @author  Stephen Williams
+ * @author  Varun Agrawal
  */
 
-#include <gtsam/navigation/ImuFactor.h>
-#include <gtsam/base/serializationTestHelpers.h>
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/serializationTestHelpers.h>
+#include <gtsam/navigation/CombinedImuFactor.h>
+#include <gtsam/navigation/ImuFactor.h>
+
 #include <fstream>
 
 using namespace std;
 using namespace gtsam;
+using namespace gtsam::serializationTestHelpers;
 
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
                         "gtsam_noiseModel_Constrained");
@@ -38,23 +42,23 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,
 BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
 BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
 
-TEST(ImuFactor, serialization) {
-  using namespace gtsam::serializationTestHelpers;
-
+template <typename P>
+P getPreintegratedMeasurements() {
   // Create default parameters with Z-down and above noise paramaters
-  auto p = PreintegrationParams::MakeSharedD(9.81);
-  p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0));
+  auto p = P::Params::MakeSharedD(9.81);
+  p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0, 0, 0));
   p->accelerometerCovariance = 1e-7 * I_3x3;
   p->gyroscopeCovariance = 1e-8 * I_3x3;
   p->integrationCovariance = 1e-9 * I_3x3;
 
   const double deltaT = 0.005;
-  const imuBias::ConstantBias priorBias(
-      Vector3(0, 0, 0), Vector3(0, 0.01, 0));  // Biases (acc, rot)
 
-  PreintegratedImuMeasurements pim(p, priorBias);
+  // Biases (acc, rot)
+  const imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0));
 
-  // measurements are needed for non-inf noise model, otherwise will throw err
+  P pim(p, priorBias);
+
+  // measurements are needed for non-inf noise model, otherwise will throw error
   // when deserialize
   const Vector3 measuredOmega(0, 0.01, 0);
   const Vector3 measuredAcc(0, 0, -9.81);
@@ -62,11 +66,45 @@ TEST(ImuFactor, serialization) {
   for (int j = 0; j < 200; ++j)
     pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
+  return pim;
+}
+
+TEST(ImuFactor, serialization) {
+  auto pim = getPreintegratedMeasurements<PreintegratedImuMeasurements>();
+
+  EXPECT(equalsObj<PreintegratedImuMeasurements>(pim));
+  EXPECT(equalsXML<PreintegratedImuMeasurements>(pim));
+  EXPECT(equalsBinary<PreintegratedImuMeasurements>(pim));
+
   ImuFactor factor(1, 2, 3, 4, 5, pim);
 
-  EXPECT(equalsObj(factor));
-  EXPECT(equalsXML(factor));
-  EXPECT(equalsBinary(factor));
+  EXPECT(equalsObj<ImuFactor>(factor));
+  EXPECT(equalsXML<ImuFactor>(factor));
+  EXPECT(equalsBinary<ImuFactor>(factor));
+}
+
+TEST(ImuFactor2, serialization) {
+  auto pim = getPreintegratedMeasurements<PreintegratedImuMeasurements>();
+
+  ImuFactor2 factor(1, 2, 3, pim);
+
+  EXPECT(equalsObj<ImuFactor2>(factor));
+  EXPECT(equalsXML<ImuFactor2>(factor));
+  EXPECT(equalsBinary<ImuFactor2>(factor));
+}
+
+TEST(CombinedImuFactor, Serialization) {
+  auto pim = getPreintegratedMeasurements<PreintegratedCombinedMeasurements>();
+
+  EXPECT(equalsObj<PreintegratedCombinedMeasurements>(pim));
+  EXPECT(equalsXML<PreintegratedCombinedMeasurements>(pim));
+  EXPECT(equalsBinary<PreintegratedCombinedMeasurements>(pim));
+
+  const CombinedImuFactor factor(1, 2, 3, 4, 5, 6, pim);
+
+  EXPECT(equalsObj<CombinedImuFactor>(factor));
+  EXPECT(equalsXML<CombinedImuFactor>(factor));
+  EXPECT(equalsBinary<CombinedImuFactor>(factor));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp
similarity index 100%
rename from gtsam/navigation/tests/testScenarios.cpp
rename to gtsam/navigation/tests/testScenarioRunner.cpp
diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h
index ff059ef78..682cca29a 100644
--- a/gtsam/nonlinear/AdaptAutoDiff.h
+++ b/gtsam/nonlinear/AdaptAutoDiff.h
@@ -57,7 +57,7 @@ class AdaptAutoDiff {
     if (H1 || H2) {
       // Get derivatives with AutoDiff
       const double* parameters[] = {v1.data(), v2.data()};
-      double rowMajor1[M * N1], rowMajor2[M * N2];  // on the stack
+      double rowMajor1[M * N1] = {}, rowMajor2[M * N2] = {};  // on the stack
       double* jacobians[] = {rowMajor1, rowMajor2};
       success = AutoDiff<FUNCTOR, double, N1, N2>::Differentiate(
           f, parameters, M, result.data(), jacobians);
diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h
index 7d02d479c..9e8aa3f29 100644
--- a/gtsam/nonlinear/Expression.h
+++ b/gtsam/nonlinear/Expression.h
@@ -66,7 +66,7 @@ public:
   // Expressions wrap trees of functions that can evaluate their own derivatives.
   // The meta-functions below are useful to specify the type of those functions.
   // Example, a function taking a camera and a 3D point and yielding a 2D point:
-  //   Expression<Point2>::BinaryFunction<SimpleCamera,Point3>::type
+  //   Expression<Point2>::BinaryFunction<PinholeCamera<Cal3_S2>,Point3>::type
   template<class A1>
   struct UnaryFunction {
     typedef boost::function<
diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h
index 04d82fe9a..c42b2bdfc 100644
--- a/gtsam/nonlinear/ExpressionFactor.h
+++ b/gtsam/nonlinear/ExpressionFactor.h
@@ -209,7 +209,7 @@ private:
  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
  enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
   public:
-	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+	  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 // ExpressionFactor
 
diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h
new file mode 100644
index 000000000..a83198967
--- /dev/null
+++ b/gtsam/nonlinear/FunctorizedFactor.h
@@ -0,0 +1,148 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 FunctorizedFactor.h
+ * @date May 31, 2020
+ * @author Varun Agrawal
+ **/
+
+#pragma once
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+#include <cmath>
+
+namespace gtsam {
+
+/**
+ * Factor which evaluates provided unary functor and uses the result to compute
+ * error with respect to the provided measurement.
+ *
+ * Template parameters are
+ * @param R: The return type of the functor after evaluation.
+ * @param T: The argument type for the functor.
+ *
+ * Example:
+ *   Key key = Symbol('X', 0);
+ *   auto model = noiseModel::Isotropic::Sigma(9, 1);
+ *
+ *   /// Functor that takes a matrix and multiplies every element by m
+ *   class MultiplyFunctor {
+ *     double m_; ///< simple multiplier
+ *    public:
+ *     MultiplyFunctor(double m) : m_(m) {}
+ *     Matrix operator()(const Matrix &X,
+ *              OptionalJacobian<-1, -1> H = boost::none) const {
+ *       if (H)
+ *         *H = m_ * Matrix::Identity(X.rows()*X.cols(), X.rows()*X.cols());
+ *       return m_ * X;
+ *     }
+ *   };
+ *
+ *   Matrix measurement = Matrix::Identity(3, 3);
+ *   double multiplier = 2.0;
+ *
+ *   FunctorizedFactor<Matrix, Matrix> factor(keyX, measurement, model,
+ *     MultiplyFunctor(multiplier));
+ */
+template <typename R, typename T>
+class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
+ private:
+  using Base = NoiseModelFactor1<T>;
+
+  R measured_;  ///< value that is compared with functor return value
+  SharedNoiseModel noiseModel_;                          ///< noise model
+  std::function<R(T, boost::optional<Matrix &>)> func_;  ///< functor instance
+
+ public:
+  /** default constructor - only use for serialization */
+  FunctorizedFactor() {}
+
+  /** Construct with given x and the parameters of the basis
+   *
+   * @param key: Factor key
+   * @param z: Measurement object of same type as that returned by functor
+   * @param model: Noise model
+   * @param func: The instance of the functor object
+   */
+  FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model,
+                    const std::function<R(T, boost::optional<Matrix &>)> func)
+      : Base(model, key), measured_(z), noiseModel_(model), func_(func) {}
+
+  virtual ~FunctorizedFactor() {}
+
+  /// @return a deep copy of this factor
+  virtual NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<NonlinearFactor>(
+        NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
+  }
+
+  Vector evaluateError(const T &params,
+                       boost::optional<Matrix &> H = boost::none) const {
+    R x = func_(params, H);
+    Vector error = traits<R>::Local(measured_, x);
+    return error;
+  }
+
+  /// @name Testable
+  /// @{
+  void print(const std::string &s = "",
+             const KeyFormatter &keyFormatter = DefaultKeyFormatter) const {
+    Base::print(s, keyFormatter);
+    std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
+              << keyFormatter(this->key()) << ")" << std::endl;
+    traits<R>::Print(measured_, "  measurement: ");
+    std::cout << "  noise model sigmas: " << noiseModel_->sigmas().transpose()
+              << std::endl;
+  }
+
+  virtual bool equals(const NonlinearFactor &other, double tol = 1e-9) const {
+    const FunctorizedFactor<R, T> *e =
+        dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
+    const bool base = Base::equals(*e, tol);
+    return e && Base::equals(other, tol) &&
+           traits<R>::Equals(this->measured_, e->measured_, tol);
+  }
+  /// @}
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &boost::serialization::make_nvp(
+        "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
+    ar &BOOST_SERIALIZATION_NVP(measured_);
+    ar &BOOST_SERIALIZATION_NVP(func_);
+  }
+};
+
+/// traits
+template <typename R, typename T>
+struct traits<FunctorizedFactor<R, T>>
+    : public Testable<FunctorizedFactor<R, T>> {};
+
+/**
+ * Helper function to create a functorized factor.
+ *
+ * Uses function template deduction to identify return type and functor type, so
+ * template list only needs the functor argument type.
+ */
+template <typename T, typename R, typename FUNC>
+FunctorizedFactor<R, T> MakeFunctorizedFactor(Key key, const R &z,
+                                              const SharedNoiseModel &model,
+                                              const FUNC func) {
+  return FunctorizedFactor<R, T>(key, z, model, func);
+}
+
+}  // namespace gtsam
diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h
index 5cf962e43..c6e1001c4 100644
--- a/gtsam/nonlinear/ISAM2Params.h
+++ b/gtsam/nonlinear/ISAM2Params.h
@@ -219,7 +219,7 @@ struct GTSAM_EXPORT ISAM2Params {
 
   /// When you will be removing many factors, e.g. when using ISAM2 as a
   /// fixed-lag smoother, enable this option to add factors in the first
-  /// available factor slots, to avoid accumulating NULL factor slots, at the
+  /// available factor slots, to avoid accumulating nullptr factor slots, at the
   /// cost of having to search for slots every time a factor is added.
   bool findUnusedFactorSlots;
 
diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index c85891af2..9b894db25 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -158,10 +158,13 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
     if (params_.verbosityLM >= LevenbergMarquardtParams::TRYDELTA)
       delta.print("delta");
 
-    // cost change in the linearized system (old - new)
+    // Compute the old linearized error as it is not the same
+    // as the nonlinear error when robust noise models are used.
+    double oldLinearizedError = linear.error(VectorValues::Zero(delta));
     double newlinearizedError = linear.error(delta);
 
-    double linearizedCostChange = currentState->error - newlinearizedError;
+    // cost change in the linearized system (old - new)
+    double linearizedCostChange = oldLinearizedError - newlinearizedError;
     if (verbose)
       cout << "newlinearizedError = " << newlinearizedError
            << "  linearizedCostChange = " << linearizedCostChange << endl;
@@ -188,8 +191,8 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
       // cost change in the original, nonlinear system (old - new)
       costChange = currentState->error - newError;
 
-      if (linearizedCostChange >
-          1e-20) {  // the (linear) error has to decrease to satisfy this condition
+      if (linearizedCostChange > std::numeric_limits<double>::epsilon() * oldLinearizedError) {
+        // the (linear) error has to decrease to satisfy this condition
         // fidelity of linearized model VS original system between
         modelFidelity = costChange / linearizedCostChange;
         // if we decrease the error in the nonlinear system and modelFidelity is above threshold
diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp
index fcbbf2f44..c29a79623 100644
--- a/gtsam/nonlinear/Marginals.cpp
+++ b/gtsam/nonlinear/Marginals.cpp
@@ -26,19 +26,70 @@ using namespace std;
 namespace gtsam {
 
 /* ************************************************************************* */
-Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization,
-                     EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering)
-{
+Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization)
+                     : values_(solution), factorization_(factorization) {
   gttic(MarginalsConstructor);
-
-  // Linearize graph
   graph_ = *graph.linearize(solution);
+  computeBayesTree();
+}
 
-  // Store values
-  values_ = solution;
+/* ************************************************************************* */
+Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering,
+                     Factorization factorization)
+                     : values_(solution), factorization_(factorization) {
+  gttic(MarginalsConstructor);
+  graph_ = *graph.linearize(solution);
+  computeBayesTree(ordering);
+}
 
+/* ************************************************************************* */
+Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization)
+                     : graph_(graph), values_(solution), factorization_(factorization) {
+  gttic(MarginalsConstructor);
+  computeBayesTree();
+}
+
+/* ************************************************************************* */
+Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering,
+                     Factorization factorization)
+                     : graph_(graph), values_(solution), factorization_(factorization) {
+  gttic(MarginalsConstructor);
+  computeBayesTree(ordering);
+}
+
+/* ************************************************************************* */
+Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization)
+                     : graph_(graph), factorization_(factorization) {
+  gttic(MarginalsConstructor);
+  for (const auto& keyValue: solution) {
+    values_.insert(keyValue.first, keyValue.second);
+  }
+  computeBayesTree();
+}
+
+/* ************************************************************************* */
+Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering,
+                     Factorization factorization)
+                     : graph_(graph), factorization_(factorization) {
+  gttic(MarginalsConstructor);
+  for (const auto& keyValue: solution) {
+    values_.insert(keyValue.first, keyValue.second);
+  }
+  computeBayesTree(ordering);
+}
+
+/* ************************************************************************* */
+void Marginals::computeBayesTree() {
+  // Compute BayesTree
+  if(factorization_ == CHOLESKY)
+    bayesTree_ = *graph_.eliminateMultifrontal(EliminatePreferCholesky);
+  else if(factorization_ == QR)
+    bayesTree_ = *graph_.eliminateMultifrontal(EliminateQR);
+}
+
+/* ************************************************************************* */
+void Marginals::computeBayesTree(const Ordering& ordering) {
   // Compute BayesTree
-  factorization_ = factorization;
   if(factorization_ == CHOLESKY)
     bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminatePreferCholesky);
   else if(factorization_ == QR)
@@ -109,9 +160,9 @@ JointMarginal Marginals::jointMarginalInformation(const KeyVector& variables) co
         jointFG = *bayesTree_.joint(variables[0], variables[1], EliminateQR);
     } else {
       if(factorization_ == CHOLESKY)
-        jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, boost::none, EliminatePreferCholesky));
+        jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, EliminatePreferCholesky));
       else if(factorization_ == QR)
-        jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, boost::none, EliminateQR));
+        jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, EliminateQR));
     }
 
     // Get information matrix
@@ -125,7 +176,7 @@ JointMarginal Marginals::jointMarginalInformation(const KeyVector& variables) co
     // Get dimensions from factor graph
     std::vector<size_t> dims;
     dims.reserve(variablesSorted.size());
-    for(Key key: variablesSorted) {
+    for(const auto& key: variablesSorted) {
       dims.push_back(values_.at(key).dim());
     }
 
@@ -142,7 +193,7 @@ VectorValues Marginals::optimize() const {
 void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const {
   cout << s << "Joint marginal on keys ";
   bool first = true;
-  for(Key key: keys_) {
+  for(const auto& key: keys_) {
     if(!first)
       cout << ", ";
     else
diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h
index e0a78042d..4e201cc38 100644
--- a/gtsam/nonlinear/Marginals.h
+++ b/gtsam/nonlinear/Marginals.h
@@ -51,14 +51,54 @@ public:
   /// Default constructor only for Cython wrapper
   Marginals(){}
 
-  /** Construct a marginals class.
+  /** Construct a marginals class from a nonlinear factor graph.
    * @param graph The factor graph defining the full joint density on all variables.
    * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer).
    * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems).
-   * @param ordering An optional variable ordering for elimination.
    */
-  Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY,
-            EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering = boost::none);
+  Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
+
+  /** Construct a marginals class from a nonlinear factor graph.
+   * @param graph The factor graph defining the full joint density on all variables.
+   * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer).
+   * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems).
+   * @param ordering The ordering for elimination.
+   */
+  Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering,
+              Factorization factorization = CHOLESKY);
+
+  /** Construct a marginals class from a linear factor graph.
+   * @param graph The factor graph defining the full joint density on all variables.
+   * @param solution The solution point to compute Gaussian marginals.
+   * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems).
+   */          
+  Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
+
+  /** Construct a marginals class from a linear factor graph.
+   * @param graph The factor graph defining the full joint density on all variables.
+   * @param solution The solution point to compute Gaussian marginals.
+   * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems).
+   * @param ordering The ordering for elimination.
+   */          
+  Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering,
+              Factorization factorization = CHOLESKY);
+
+  /** Construct a marginals class from a linear factor graph.
+   * @param graph The factor graph defining the full joint density on all variables.
+   * @param solution The solution point to compute Gaussian marginals.
+   * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems).
+   * @param ordering An optional variable ordering for elimination.
+   */          
+  Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY);
+
+  /** Construct a marginals class from a linear factor graph.
+   * @param graph The factor graph defining the full joint density on all variables.
+   * @param solution The solution point to compute Gaussian marginals.
+   * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems).
+   * @param ordering An optional variable ordering for elimination.
+   */          
+  Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering,
+              Factorization factorization = CHOLESKY);
 
   /** print */
   void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
@@ -81,6 +121,28 @@ public:
 
   /** Optimize the bayes tree */
   VectorValues optimize() const;
+            
+protected:
+  
+  /** Compute the Bayes Tree as a helper function to the constructor */
+  void computeBayesTree();
+
+  /** Compute the Bayes Tree as a helper function to the constructor */
+  void computeBayesTree(const Ordering& ordering);
+
+public:
+  /** \deprecated argument order changed due to removing boost::optional<Ordering> */
+  Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization,
+            const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
+
+  /** \deprecated argument order changed due to removing boost::optional<Ordering> */
+  Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization,
+            const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
+
+  /** \deprecated argument order changed due to removing boost::optional<Ordering> */
+  Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization,
+            const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
+
 };
 
 /**
diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
index 32aead750..54db42b79 100644
--- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
+++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
@@ -43,7 +43,8 @@ static VectorValues gradientInPlace(const NonlinearFactorGraph &nfg,
 
 NonlinearConjugateGradientOptimizer::NonlinearConjugateGradientOptimizer(
     const NonlinearFactorGraph& graph, const Values& initialValues, const Parameters& params)
-    : Base(graph, std::unique_ptr<State>(new State(initialValues, graph.error(initialValues)))) {}
+    : Base(graph, std::unique_ptr<State>(new State(initialValues, graph.error(initialValues)))),
+    params_(params) {}
 
 double NonlinearConjugateGradientOptimizer::System::error(const State& state) const {
   return graph_.error(state);
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index 4d928482e..1bba57051 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -175,6 +175,8 @@ public:
 
   /// @}
 
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
+
 private:
 
   /** Serialization function */
@@ -263,6 +265,8 @@ public:
     traits<X>::Print(value_, "Value");
   }
 
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
+
 private:
 
   /** Serialization function */
@@ -327,6 +331,8 @@ public:
     return traits<X>::Local(x1,x2);
   }
 
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
+
 private:
 
   /** Serialization function */
diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp
index 5c89425c8..1cfcba274 100644
--- a/gtsam/nonlinear/NonlinearFactor.cpp
+++ b/gtsam/nonlinear/NonlinearFactor.cpp
@@ -93,13 +93,35 @@ Vector NoiseModelFactor::whitenedError(const Values& c) const {
   return noiseModel_ ? noiseModel_->whiten(b) : b;
 }
 
+/* ************************************************************************* */
+Vector NoiseModelFactor::unweightedWhitenedError(const Values& c) const {
+  const Vector b = unwhitenedError(c);
+  check(noiseModel_, b.size());
+  return noiseModel_ ? noiseModel_->unweightedWhiten(b) : b;
+}
+
+/* ************************************************************************* */
+double NoiseModelFactor::weight(const Values& c) const {
+  if (active(c)) {
+    if (noiseModel_) {
+      const Vector b = unwhitenedError(c);
+      check(noiseModel_, b.size());
+      return 0.5 * noiseModel_->weight(b);
+    }
+    else
+      return 1.0;
+  } else {
+    return 0.0;
+  }
+}
+
 /* ************************************************************************* */
 double NoiseModelFactor::error(const Values& c) const {
   if (active(c)) {
     const Vector b = unwhitenedError(c);
     check(noiseModel_, b.size());
     if (noiseModel_)
-      return 0.5 * noiseModel_->distance(b);
+      return noiseModel_->loss(noiseModel_->squaredMahalanobisDistance(b));
     else
       return 0.5 * b.squaredNorm();
   } else {
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 1942734c5..63547a248 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -226,6 +226,16 @@ public:
    */
   Vector whitenedError(const Values& c) const;
 
+  /**
+   * Vector of errors, whitened, but unweighted by any loss function
+   */
+  Vector unweightedWhitenedError(const Values& c) const;
+
+  /**
+   * Compute the effective weight of the factor from the noise model.
+   */
+  double weight(const Values& c) const;
+
   /**
    * Calculate the error of the factor.
    * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index a4bdd83e3..5f6cdcc98 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -54,26 +54,33 @@ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& key
   for (size_t i = 0; i < factors_.size(); i++) {
     stringstream ss;
     ss << "Factor " << i << ": ";
-    if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter);
+    if (factors_[i] != nullptr) factors_[i]->print(ss.str(), keyFormatter);
     cout << endl;
   }
 }
 
 /* ************************************************************************* */
 void NonlinearFactorGraph::printErrors(const Values& values, const std::string& str,
-                                       const KeyFormatter& keyFormatter) const {
+    const KeyFormatter& keyFormatter,
+    const std::function<bool(const Factor* /*factor*/, double /*whitenedError*/, size_t /*index*/)>& printCondition) const
+{
   cout << str << "size: " << size() << endl
        << endl;
   for (size_t i = 0; i < factors_.size(); i++) {
+    const sharedFactor& factor = factors_[i];
+    const double errorValue = (factor != nullptr ? factors_[i]->error(values) : .0);
+    if (!printCondition(factor.get(),errorValue,i))
+      continue; // User-provided filter did not pass
+
     stringstream ss;
     ss << "Factor " << i << ": ";
-    if (factors_[i] == NULL) {
-      cout << "NULL" << endl;
+    if (factor == nullptr) {
+      cout << "nullptr" << "\n";
     } else {
-      factors_[i]->print(ss.str(), keyFormatter);
-      cout << "error = " << factors_[i]->error(values) << endl;
+      factor->print(ss.str(), keyFormatter);
+      cout << "error = " << errorValue << "\n";
     }
-    cout << endl;
+    cout << endl; // only one "endl" at end might be faster, \n for each factor
   }
 }
 
@@ -344,23 +351,31 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
 }
 
 /* ************************************************************************* */
-static Scatter scatterFromValues(const Values& values, boost::optional<Ordering&> ordering) {
+static Scatter scatterFromValues(const Values& values) {
   gttic(scatterFromValues);
 
   Scatter scatter;
   scatter.reserve(values.size());
 
-  if (!ordering) {
-    // use "natural" ordering with keys taken from the initial values
-    for (const auto& key_value : values) {
-      scatter.add(key_value.key, key_value.value.dim());
-    }
-  } else {
-    // copy ordering into keys and lookup dimension in values, is O(n*log n)
-    for (Key key : *ordering) {
-      const Value& value = values.at(key);
-      scatter.add(key, value.dim());
-    }
+  // use "natural" ordering with keys taken from the initial values
+  for (const auto& key_value : values) {
+    scatter.add(key_value.key, key_value.value.dim());
+  }
+
+  return scatter;
+}
+
+/* ************************************************************************* */
+static Scatter scatterFromValues(const Values& values, const Ordering& ordering) {
+  gttic(scatterFromValues);
+
+  Scatter scatter;
+  scatter.reserve(values.size());
+
+  // copy ordering into keys and lookup dimension in values, is O(n*log n)
+  for (Key key : ordering) {
+    const Value& value = values.at(key);
+    scatter.add(key, value.dim());
   }
 
   return scatter;
@@ -368,11 +383,7 @@ static Scatter scatterFromValues(const Values& values, boost::optional<Ordering&
 
 /* ************************************************************************* */
 HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor(
-    const Values& values, boost::optional<Ordering&> ordering, const Dampen& dampen) const {
-  gttic(NonlinearFactorGraph_linearizeToHessianFactor);
-
-  Scatter scatter = scatterFromValues(values, ordering);
-
+    const Values& values, const Scatter& scatter, const Dampen& dampen) const {
   // NOTE(frank): we are heavily leaning on friendship below
   HessianFactor::shared_ptr hessianFactor(new HessianFactor(scatter));
 
@@ -393,9 +404,36 @@ HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor(
   return hessianFactor;
 }
 
+/* ************************************************************************* */
+HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor(
+    const Values& values, const Ordering& order, const Dampen& dampen) const {
+  gttic(NonlinearFactorGraph_linearizeToHessianFactor);
+
+  Scatter scatter = scatterFromValues(values, order);
+  return linearizeToHessianFactor(values, scatter, dampen);
+}
+
+/* ************************************************************************* */
+HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor(
+    const Values& values, const Dampen& dampen) const {
+  gttic(NonlinearFactorGraph_linearizeToHessianFactor);
+
+  Scatter scatter = scatterFromValues(values);
+  return linearizeToHessianFactor(values, scatter, dampen);
+}
+
 /* ************************************************************************* */
 Values NonlinearFactorGraph::updateCholesky(const Values& values,
-                                            boost::optional<Ordering&> ordering,
+                                            const Dampen& dampen) const {
+  gttic(NonlinearFactorGraph_updateCholesky);
+  auto hessianFactor = linearizeToHessianFactor(values, dampen);
+  VectorValues delta = hessianFactor->solve();
+  return values.retract(delta);
+}
+
+/* ************************************************************************* */
+Values NonlinearFactorGraph::updateCholesky(const Values& values,
+                                            const Ordering& ordering,
                                             const Dampen& dampen) const {
   gttic(NonlinearFactorGraph_updateCholesky);
   auto hessianFactor = linearizeToHessianFactor(values, ordering, dampen);
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index 323461459..0e17700d0 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -24,6 +24,7 @@
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/inference/FactorGraph.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 
 #include <boost/shared_ptr.hpp>
 #include <functional>
@@ -103,7 +104,9 @@ namespace gtsam {
 
     /** print errors along with factors*/
     void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ",
-                     const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter,
+      const std::function<bool(const Factor* /*factor*/, double /*whitenedError*/, size_t /*index*/)>&
+        printCondition = [](const Factor *,double, size_t) {return true;}) const;
 
     /** Test equality */
     bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
@@ -149,17 +152,31 @@ namespace gtsam {
      * Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly
      * into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing
      * a new graph, and hence useful in case a dense solve is appropriate for your problem.
-     * An optional ordering can be given that still decides how the Hessian is laid out.
      * An optional lambda function can be used to apply damping on the filled Hessian.
      * No parallelism is exploited, because all the factors write in the same memory.
      */
     boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
-        const Values& values, boost::optional<Ordering&> ordering = boost::none,
-        const Dampen& dampen = nullptr) const;
+        const Values& values, const Dampen& dampen = nullptr) const;
+
+    /**
+     * Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly
+     * into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing
+     * a new graph, and hence useful in case a dense solve is appropriate for your problem.
+     * An ordering is given that still decides how the Hessian is laid out.
+     * An optional lambda function can be used to apply damping on the filled Hessian.
+     * No parallelism is exploited, because all the factors write in the same memory.
+     */
+    boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
+        const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const;
 
     /// Linearize and solve in one pass.
     /// Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values.
-    Values updateCholesky(const Values& values, boost::optional<Ordering&> ordering = boost::none,
+    Values updateCholesky(const Values& values,
+                          const Dampen& dampen = nullptr) const;
+
+    /// Linearize and solve in one pass.
+    /// Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values.
+    Values updateCholesky(const Values& values, const Ordering& ordering,
                           const Dampen& dampen = nullptr) const;
 
     /// Clone() performs a deep-copy of the graph, including all of the factors
@@ -188,8 +205,42 @@ namespace gtsam {
       push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
     }
 
+    /**
+     * Convenience method which adds a PriorFactor to the factor graph.
+     * @param key    Variable key
+     * @param prior  The variable's prior value
+     * @param model  Noise model for prior factor
+     */
+    template<typename T>
+    void addPrior(Key key, const T& prior,
+                  const SharedNoiseModel& model = nullptr) {
+      emplace_shared<PriorFactor<T>>(key, prior, model);
+    }
+
+    /**
+     * Convenience method which adds a PriorFactor to the factor graph.
+     * @param key         Variable key
+     * @param prior       The variable's prior value
+     * @param covariance  Covariance matrix.
+     * 
+     * Note that the smart noise model associated with the prior factor
+     * automatically picks the right noise model (e.g. a diagonal noise model
+     * if the provided covariance matrix is diagonal).
+     */
+    template<typename T>
+    void addPrior(Key key, const T& prior, const Matrix& covariance) {
+      emplace_shared<PriorFactor<T>>(key, prior, covariance);
+    }
+
   private:
 
+    /**
+     * Linearize from Scatter rather than from Ordering.  Made private because
+     *  it doesn't include gttic.
+     */
+    boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
+        const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const;
+
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
@@ -197,6 +248,19 @@ namespace gtsam {
       ar & boost::serialization::make_nvp("NonlinearFactorGraph",
                 boost::serialization::base_object<Base>(*this));
     }
+
+  public:
+
+    /** \deprecated */
+    boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
+        const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
+      {return linearizeToHessianFactor(values, dampen);}
+
+    /** \deprecated */
+    Values updateCholesky(const Values& values, boost::none_t,
+                          const Dampen& dampen = nullptr) const
+      {return updateCholesky(values, dampen);}
+
   };
 
 /// traits
diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp
index e1efa2061..328a3facf 100644
--- a/gtsam/nonlinear/NonlinearOptimizer.cpp
+++ b/gtsam/nonlinear/NonlinearOptimizer.cpp
@@ -101,7 +101,7 @@ void NonlinearOptimizer::defaultOptimize() {
       cout << "newError: " << error() << endl;
   } while (iterations() < params.maxIterations &&
            !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol,
-                             currentError, error(), params.verbosity));
+                             currentError, error(), params.verbosity) && std::isfinite(currentError));
 
   // Printing if verbose
   if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) {
@@ -128,28 +128,34 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg,
                                        const NonlinearOptimizerParams& params) const {
   // solution of linear solver is an update to the linearization point
   VectorValues delta;
-  boost::optional<const Ordering&> optionalOrdering;
-  if (params.ordering)
-    optionalOrdering.reset(*params.ordering);
 
   // Check which solver we are using
   if (params.isMultifrontal()) {
     // Multifrontal QR or Cholesky (decided by params.getEliminationFunction())
-    delta = gfg.optimize(optionalOrdering, params.getEliminationFunction());
+    if (params.ordering)
+      delta = gfg.optimize(*params.ordering, params.getEliminationFunction());
+    else
+      delta = gfg.optimize(params.getEliminationFunction());
   } else if (params.isSequential()) {
     // Sequential QR or Cholesky (decided by params.getEliminationFunction())
-    delta = gfg.eliminateSequential(optionalOrdering, params.getEliminationFunction(), boost::none,
-                                    params.orderingType)->optimize();
+    if (params.ordering)
+      delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(),
+                                      boost::none, params.orderingType)->optimize();
+    else
+      delta = gfg.eliminateSequential(params.getEliminationFunction(), boost::none,
+                                      params.orderingType)->optimize();
   } else if (params.isIterative()) {
     // Conjugate Gradient -> needs params.iterativeParams
     if (!params.iterativeParams)
-      throw std::runtime_error("NonlinearOptimizer::solve: cg parameter has to be assigned ...");
+      throw std::runtime_error(
+          "NonlinearOptimizer::solve: cg parameter has to be assigned ...");
 
-    if (boost::shared_ptr<PCGSolverParameters> pcg =
-            boost::dynamic_pointer_cast<PCGSolverParameters>(params.iterativeParams)) {
+    if (auto pcg = boost::dynamic_pointer_cast<PCGSolverParameters>(
+            params.iterativeParams)) {
       delta = PCGSolver(*pcg).optimize(gfg);
-    } else if (boost::shared_ptr<SubgraphSolverParameters> spcg =
-                   boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams)) {
+    } else if (auto spcg =
+                   boost::dynamic_pointer_cast<SubgraphSolverParameters>(
+                       params.iterativeParams)) {
       if (!params.ordering)
         throw std::runtime_error("SubgraphSolver needs an ordering");
       delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize();
diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h
index 180f4fb84..65fdd1c92 100644
--- a/gtsam/nonlinear/NonlinearOptimizerParams.h
+++ b/gtsam/nonlinear/NonlinearOptimizerParams.h
@@ -31,7 +31,7 @@ namespace gtsam {
 /** The common parameters for Nonlinear optimizers.  Most optimizers
  * deriving from NonlinearOptimizer also subclass the parameters.
  */
-class NonlinearOptimizerParams {
+class GTSAM_EXPORT NonlinearOptimizerParams {
 public:
   /** See NonlinearOptimizerParams::verbosity */
   enum Verbosity {
@@ -52,7 +52,7 @@ public:
 
   virtual ~NonlinearOptimizerParams() {
   }
-  GTSAM_EXPORT virtual void print(const std::string& str = "") const;
+  virtual void print(const std::string& str = "") const;
 
   size_t getMaxIterations() const { return maxIterations; }
   double getRelativeErrorTol() const { return relativeErrorTol; }
@@ -68,8 +68,8 @@ public:
     verbosity = verbosityTranslator(src);
   }
 
-  GTSAM_EXPORT static Verbosity verbosityTranslator(const std::string &s) ;
-  GTSAM_EXPORT static std::string verbosityTranslator(Verbosity value) ;
+  static Verbosity verbosityTranslator(const std::string &s) ;
+  static std::string verbosityTranslator(Verbosity value) ;
 
   /** See NonlinearOptimizerParams::linearSolverType */
   enum LinearSolverType {
@@ -82,7 +82,7 @@ public:
   };
 
   LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer
-  boost::optional<Ordering> ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty)
+  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.
 
   inline bool isMultifrontal() const {
@@ -144,10 +144,10 @@ public:
   }
 
 private:
-  GTSAM_EXPORT std::string linearSolverTranslator(LinearSolverType linearSolverType) const;
-  GTSAM_EXPORT LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const;
-  GTSAM_EXPORT std::string orderingTypeTranslator(Ordering::OrderingType type) const;
-  GTSAM_EXPORT Ordering::OrderingType orderingTypeTranslator(const std::string& type) const;
+  std::string linearSolverTranslator(LinearSolverType linearSolverType) const;
+  LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const;
+  std::string orderingTypeTranslator(Ordering::OrderingType type) const;
+  Ordering::OrderingType orderingTypeTranslator(const std::string& type) const;
 };
 
 // For backward compatibility:
diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h
new file mode 100644
index 000000000..0afbaa588
--- /dev/null
+++ b/gtsam/nonlinear/PriorFactor.h
@@ -0,0 +1,120 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  PriorFactor.h
+ *  @author Frank Dellaert
+ **/
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/Testable.h>
+
+#include <string>
+
+namespace gtsam {
+
+  /**
+   * A class for a soft prior on any Value type
+   * @addtogroup SLAM
+   */
+  template<class VALUE>
+  class PriorFactor: public NoiseModelFactor1<VALUE> {
+
+  public:
+    typedef VALUE T;
+
+  private:
+
+    typedef NoiseModelFactor1<VALUE> Base;
+
+    VALUE prior_; /** The measurement */
+
+    /** concept check by type */
+    GTSAM_CONCEPT_TESTABLE_TYPE(T)
+
+  public:
+
+    /// shorthand for a smart pointer to a factor
+    typedef typename boost::shared_ptr<PriorFactor<VALUE> > shared_ptr;
+
+    /// Typedef to this class
+    typedef PriorFactor<VALUE> This;
+
+    /** default constructor - only use for serialization */
+    PriorFactor() {}
+
+    virtual ~PriorFactor() {}
+
+    /** Constructor */
+    PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) :
+      Base(model, key), prior_(prior) {
+    }
+
+    /** Convenience constructor that takes a full covariance argument */
+    PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) :
+      Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) {
+    }
+
+    /// @return a deep copy of this factor
+    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+
+    /** implement functions needed for Testable */
+
+    /** print */
+    virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+                                                 DefaultKeyFormatter) const {
+      std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
+      traits<T>::Print(prior_, "  prior mean: ");
+      if (this->noiseModel_)
+        this->noiseModel_->print("  noise model: ");
+      else
+        std::cout << "no noise model" << std::endl;
+    }
+
+    /** equals */
+    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+      const This* e = dynamic_cast<const This*> (&expected);
+      return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
+    }
+
+    /** implement functions needed to derive from Factor */
+
+    /** vector of errors */
+    Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const {
+      if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
+      // manifold equivalent of z-x -> Local(x,z)
+      // TODO(ASL) Add Jacobians.
+      return -traits<T>::Local(x, prior_);
+    }
+
+    const VALUE & prior() const { return prior_; }
+
+  private:
+
+    /** Serialization function */
+    friend class boost::serialization::access;
+    template<class ARCHIVE>
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+      ar & boost::serialization::make_nvp("NoiseModelFactor1",
+          boost::serialization::base_object<Base>(*this));
+      ar & BOOST_SERIALIZATION_NVP(prior_);
+    }
+
+	// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+	enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
+  public:
+	GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+  };
+
+} /// namespace gtsam
diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index ff220044a..d0f8a4790 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -348,7 +348,9 @@ namespace gtsam {
        throw ValuesKeyDoesNotExist("at", j);
 
     // Check the type and throw exception if incorrect
-    return internal::handle<ValueType>()(j,item->second);
+    // 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.cpp b/gtsam/nonlinear/Values.cpp
index 1bd8b3a73..98790ccd9 100644
--- a/gtsam/nonlinear/Values.cpp
+++ b/gtsam/nonlinear/Values.cpp
@@ -232,10 +232,21 @@ namespace gtsam {
 
   /* ************************************************************************* */
   const char* ValuesIncorrectType::what() const throw() {
-    if(message_.empty())
-      message_ =
-          "Attempting to retrieve value with key \"" + DefaultKeyFormatter(key_) + "\", type stored in Values is " +
-          std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name());
+    if(message_.empty()) {
+      std::string storedTypeName = demangle(storedTypeId_.name());
+      std::string requestedTypeName = demangle(requestedTypeId_.name());
+
+      if (storedTypeName == requestedTypeName) {
+        message_ = "WARNING: Detected types with same name but different `typeid`. \
+          This is usually caused by incorrect linking/inlining settings when compiling libraries using GTSAM. \
+          If you are a user, please report to the author of the library using GTSAM. \
+          If you are a package maintainer, please consult `cmake/GtsamPybindWrap.cmake`, line 74 for details.";
+      } else {
+        message_ =
+        "Attempting to retrieve value with key \"" + DefaultKeyFormatter(key_) + "\", type stored in Values is " +
+         storedTypeName + " but requested type was " + requestedTypeName;
+      }
+    }
     return message_.c_str();
   }
 
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 4b0fceaf9..041aa3441 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -39,6 +39,7 @@
 #pragma GCC diagnostic pop
 #endif
 #include <boost/ptr_container/serialize_ptr_map.hpp>
+#include <boost/shared_ptr.hpp>
 
 #include <string>
 #include <utility>
@@ -275,8 +276,8 @@ namespace gtsam {
     void update(Key j, const Value& val);
 
     /** Templated version to update a variable with the given j,
-      * throws KeyAlreadyExists<J> if j is already present
-      * if no chart is specified, the DefaultChart<ValueType> is used
+      * throws KeyDoesNotExist<J> if j is not present.
+      * If no chart is specified, the DefaultChart<ValueType> is used.
       */
     template <typename T>
     void update(Key j, const T& val);
diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h
index ace0aaea8..2943b5e68 100644
--- a/gtsam/nonlinear/internal/ExecutionTrace.h
+++ b/gtsam/nonlinear/internal/ExecutionTrace.h
@@ -169,6 +169,12 @@ class ExecutionTrace {
       content.ptr->reverseAD2(dTdA, jacobians);
   }
 
+  ~ExecutionTrace() {
+    if (kind == Function) {
+      content.ptr->~CallRecord<Dim>();
+    }
+  }
+
   /// Define type so we can apply it as a meta-function
   typedef ExecutionTrace<T> type;
 };
diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h
index d11908b54..0ae37f130 100644
--- a/gtsam/nonlinear/internal/ExpressionNode.h
+++ b/gtsam/nonlinear/internal/ExpressionNode.h
@@ -84,7 +84,7 @@ public:
   /// Streaming
   GTSAM_EXPORT
   friend std::ostream& operator<<(std::ostream& os, const ExpressionNode& node) {
-    os << "Expression of type " << typeid(T).name();
+    os << "Expression of type " << demangle(typeid(T).name());
     if (node.traceSize_ > 0) os << ", trace size = " << node.traceSize_;
     os << "\n";
     return os;
@@ -149,6 +149,8 @@ public:
       ExecutionTraceStorage* traceStorage) const {
     return constant_;
   }
+
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 //-----------------------------------------------------------------------------
@@ -217,7 +219,7 @@ static void PrintJacobianAndTrace(const std::string& indent,
                                   const typename Jacobian<T, A>::type& dTdA,
                                   const ExecutionTrace<A> trace) {
   static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]");
-  std::cout << indent << "D(" << typeid(T).name() << ")/D(" << typeid(A).name()
+  std::cout << indent << "D(" << demangle(typeid(T).name()) << ")/D(" << demangle(typeid(A).name())
             << ") = " << dTdA.format(kMatlabFormat) << std::endl;
   trace.print(indent);
 }
@@ -603,7 +605,7 @@ class ScalarMultiplyNode : public ExpressionNode<T> {
     /// Print to std::cout
     void print(const std::string& indent) const {
       std::cout << indent << "ScalarMultiplyNode::Record {" << std::endl;
-      std::cout << indent << "D(" << typeid(T).name() << ")/D(" << typeid(T).name()
+      std::cout << indent << "D(" << demangle(typeid(T).name()) << ")/D(" << demangle(typeid(T).name())
                 << ") = " << scalar_dTdA << std::endl;
       trace.print();
       std::cout << indent << "}" << std::endl;
diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
index a85118c00..2f4f21286 100644
--- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
+++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
@@ -168,7 +168,11 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)),
 Point3 point(10, 0, -5);  // negative Z-axis convention of Snavely!
 Vector9 P = Camera().localCoordinates(camera);
 Vector3 X = point;
+#ifdef GTSAM_POSE3_EXPMAP
+Vector2 expectedMeasurement(1.3124675, 1.2057287);
+#else
 Vector2 expectedMeasurement(1.2431567, 1.2525694);
+#endif
 Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
 Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
 }
@@ -177,7 +181,11 @@ Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
 // Check that Local worked as expected
 TEST(AdaptAutoDiff, Local) {
   using namespace example;
+#ifdef GTSAM_POSE3_EXPMAP
+  Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0.7583528428, 4.9582357859, -0.224941471539, 1, 0, 0).finished();
+#else
   Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0, 5, 0, 1, 0, 0).finished();
+#endif
   EXPECT(equal_with_abs_tol(expectedP, P));
   Vector3 expectedX(10, 0, -5);  // negative Z-axis convention of Snavely!
   EXPECT(equal_with_abs_tol(expectedX, X));
diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp
index 494a59fff..c5ccc0f52 100644
--- a/gtsam/nonlinear/tests/testCallRecord.cpp
+++ b/gtsam/nonlinear/tests/testCallRecord.cpp
@@ -98,7 +98,7 @@ struct Record: public internal::CallRecordImplementor<Record, Cols> {
   friend struct internal::CallRecordImplementor;
 };
 
-internal::JacobianMap & NJM= *static_cast<internal::JacobianMap *>(NULL);
+internal::JacobianMap & NJM= *static_cast<internal::JacobianMap *>(nullptr);
 
 /* ************************************************************************* */
 typedef Eigen::Matrix<double, Eigen::Dynamic, Cols> DynRowMat;
diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp
index c2b245780..58f76089a 100644
--- a/gtsam/nonlinear/tests/testExecutionTrace.cpp
+++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp
@@ -16,6 +16,7 @@
  * @brief unit tests for Expression internals
  */
 
+#include <gtsam/nonlinear/internal/CallRecord.h>
 #include <gtsam/nonlinear/internal/ExecutionTrace.h>
 #include <gtsam/geometry/Point2.h>
 
diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp
new file mode 100644
index 000000000..12dd6b91c
--- /dev/null
+++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp
@@ -0,0 +1,185 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------1-------------------------------------------
+ */
+
+/**
+ * @file testFunctorizedFactor.cpp
+ * @date May 31, 2020
+ * @author Varun Agrawal
+ * @brief unit tests for FunctorizedFactor class
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/FunctorizedFactor.h>
+#include <gtsam/nonlinear/factorTesting.h>
+
+using namespace std;
+using namespace gtsam;
+
+Key key = Symbol('X', 0);
+auto model = noiseModel::Isotropic::Sigma(9, 1);
+
+/// Functor that takes a matrix and multiplies every element by m
+class MultiplyFunctor {
+  double m_;  ///< simple multiplier
+
+ public:
+  MultiplyFunctor(double m) : m_(m) {}
+
+  Matrix operator()(const Matrix &X,
+                    OptionalJacobian<-1, -1> H = boost::none) const {
+    if (H) *H = m_ * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
+    return m_ * X;
+  }
+};
+
+/* ************************************************************************* */
+// Test identity operation for FunctorizedFactor.
+TEST(FunctorizedFactor, Identity) {
+  Matrix X = Matrix::Identity(3, 3), measurement = Matrix::Identity(3, 3);
+
+  double multiplier = 1.0;
+  auto functor = MultiplyFunctor(multiplier);
+  auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model, functor);
+
+  Vector error = factor.evaluateError(X);
+
+  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
+}
+
+/* ************************************************************************* */
+// Test FunctorizedFactor with multiplier value of 2.
+TEST(FunctorizedFactor, Multiply2) {
+  double multiplier = 2.0;
+  Matrix X = Matrix::Identity(3, 3);
+  Matrix measurement = multiplier * Matrix::Identity(3, 3);
+
+  auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model,
+                                              MultiplyFunctor(multiplier));
+
+  Vector error = factor.evaluateError(X);
+
+  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
+}
+
+/* ************************************************************************* */
+// Test equality function for FunctorizedFactor.
+TEST(FunctorizedFactor, Equality) {
+  Matrix measurement = Matrix::Identity(2, 2);
+
+  double multiplier = 2.0;
+
+  auto factor1 = MakeFunctorizedFactor<Matrix>(key, measurement, model,
+                                               MultiplyFunctor(multiplier));
+  auto factor2 = MakeFunctorizedFactor<Matrix>(key, measurement, model,
+                                               MultiplyFunctor(multiplier));
+
+  EXPECT(factor1.equals(factor2));
+}
+
+/* *************************************************************************** */
+// Test Jacobians of FunctorizedFactor.
+TEST(FunctorizedFactor, Jacobians) {
+  Matrix X = Matrix::Identity(3, 3);
+  Matrix actualH;
+
+  double multiplier = 2.0;
+
+  auto factor =
+      MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
+
+  Values values;
+  values.insert<Matrix>(key, X);
+
+  // Check Jacobians
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+// Test print result of FunctorizedFactor.
+TEST(FunctorizedFactor, Print) {
+  Matrix X = Matrix::Identity(2, 2);
+
+  double multiplier = 2.0;
+
+  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"
+      "FunctorizedFactor(X0)\n"
+      "  measurement: [\n"
+      "	1, 0;\n"
+      " 	0, 1\n"
+      "]\n"
+      "  noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
+
+  CHECK_EQUAL(expected, actual);
+}
+
+/* ************************************************************************* */
+// Test FunctorizedFactor using a std::function type.
+TEST(FunctorizedFactor, Functional) {
+  double multiplier = 2.0;
+  Matrix X = Matrix::Identity(3, 3);
+  Matrix measurement = multiplier * Matrix::Identity(3, 3);
+
+  std::function<Matrix(Matrix, boost::optional<Matrix &>)> functional =
+      MultiplyFunctor(multiplier);
+  auto factor =
+      MakeFunctorizedFactor<Matrix>(key, measurement, model, functional);
+
+  Vector error = factor.evaluateError(X);
+
+  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
+}
+
+/* ************************************************************************* */
+// Test FunctorizedFactor with a lambda function.
+TEST(FunctorizedFactor, Lambda) {
+  double multiplier = 2.0;
+  Matrix X = Matrix::Identity(3, 3);
+  Matrix measurement = multiplier * Matrix::Identity(3, 3);
+
+  auto lambda = [multiplier](const Matrix &X,
+                             OptionalJacobian<-1, -1> H = boost::none) {
+    if (H)
+      *H = multiplier *
+           Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
+    return multiplier * X;
+  };
+  // FunctorizedFactor<Matrix> factor(key, measurement, model, lambda);
+  auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model, lambda);
+
+  Vector error = factor.evaluateError(X);
+
+  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index b8eee540d..2f624f527 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -589,6 +589,23 @@ TEST(Values, MatrixDynamicInsertFixedRead) {
   CHECK_EXCEPTION(values.at<Matrix23>(key1), exception);
 }
 
+TEST(Values, Demangle) {
+  Values values;
+  Matrix13 v; v << 5.0, 6.0, 7.0;
+  values.insert(key1, v);
+  string expected = "Values with 1 values:\nValue v1: (Eigen::Matrix<double, 1, 3, 1, 1, 3>) [\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));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h
index 867db70e0..a78f7a2d0 100644
--- a/gtsam/nonlinear/utilities.h
+++ b/gtsam/nonlinear/utilities.h
@@ -28,7 +28,7 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 
 #include <exception>
 
@@ -169,7 +169,7 @@ void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
 }
 
 /// Insert a number of initial point values by backprojecting
-void insertBackprojections(Values& values, const SimpleCamera& camera,
+void insertBackprojections(Values& values, const PinholeCamera<Cal3_S2>& camera,
     const Vector& J, const Matrix& Z, double depth) {
   if (Z.rows() != 2)
     throw std::invalid_argument("insertBackProjections: Z must be 2*K");
@@ -237,13 +237,16 @@ Values localToWorld(const Values& local, const Pose2& base,
       // if value is a Pose2, compose it with base pose
       Pose2 pose = local.at<Pose2>(key);
       world.insert(key, base.compose(pose));
-    } catch (std::exception e1) {
+    } catch (const std::exception& e1) {
       try {
         // if value is a Point2, transform it from base pose
         Point2 point = local.at<Point2>(key);
         world.insert(key, base.transformFrom(point));
-      } catch (std::exception e2) {
+      } catch (const std::exception& e2) {
         // if not Pose2 or Point2, do nothing
+        #ifndef NDEBUG
+          std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl;
+        #endif
       }
     }
   }
diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp
index 5d57886f5..54d4a43c0 100644
--- a/gtsam/sam/tests/testRangeFactor.cpp
+++ b/gtsam/sam/tests/testRangeFactor.cpp
@@ -19,7 +19,8 @@
 #include <gtsam/sam/RangeFactor.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/serializationTestHelpers.h>
 #include <gtsam/base/TestableAssertions.h>
@@ -353,11 +354,13 @@ TEST(RangeFactor, Point3) {
 }
 
 /* ************************************************************************* */
-// Do tests with SimpleCamera
+// Do tests with PinholeCamera<Cal3_S2>
 TEST( RangeFactor, Camera) {
-  RangeFactor<SimpleCamera,Point3> factor1(poseKey, pointKey, measurement, model);
-  RangeFactor<SimpleCamera,Pose3> factor2(poseKey, pointKey, measurement, model);
-  RangeFactor<SimpleCamera,SimpleCamera> factor3(poseKey, pointKey, measurement, model);
+  using Camera = PinholeCamera<Cal3_S2>;
+
+  RangeFactor<Camera, Point3> factor1(poseKey, pointKey, measurement, model);
+  RangeFactor<Camera, Pose3>  factor2(poseKey, pointKey, measurement, model);
+  RangeFactor<Camera, Camera> factor3(poseKey, pointKey, measurement, model);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h
new file mode 100644
index 000000000..d63633d7e
--- /dev/null
+++ b/gtsam/sfm/TranslationFactor.h
@@ -0,0 +1,84 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, 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
+
+ * -------------------------------------------------------------------------- */
+
+#pragma once
+
+/**
+ * @file TranslationFactor.h
+ * @author Frank Dellaert
+ * @date March 2020
+ * @brief Binary factor for a relative translation direction measurement.
+ */
+
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * Binary factor for a relative translation direction measurement
+ * w_aZb. The measurement equation is
+ *    w_aZb = Unit3(Tb - Ta)
+ * i.e., w_aZb is the translation direction from frame A to B, in world
+ * coordinates. Although Unit3 instances live on a manifold, following
+ * Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
+ * ambient world coordinate frame:
+ *    normalized(Tb - Ta) - w_aZb.point3()
+ *
+ * @addtogroup SFM
+ */
+class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
+ private:
+  typedef NoiseModelFactor2<Point3, Point3> Base;
+  Point3 measured_w_aZb_;
+
+ public:
+  /// default constructor
+  TranslationFactor() {}
+
+  TranslationFactor(Key a, Key b, const Unit3& w_aZb,
+                    const SharedNoiseModel& noiseModel)
+      : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {}
+
+  /**
+   * @brief Caclulate error: (norm(Tb - Ta) - measurement)
+   * where Tb and Ta are Point3 translations and measurement is
+   * the Unit3 translation direction from a to b.
+   * 
+   * @param Ta translation for key a
+   * @param Tb translation for key b
+   * @param H1 optional jacobian in Ta
+   * @param H2 optional jacobian in Tb
+   * @return * Vector
+   */
+  Vector evaluateError(
+      const Point3& Ta, const Point3& Tb,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none) const override {
+    const Point3 dir = Tb - Ta;
+    Matrix33 H_predicted_dir;
+    const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr);
+    if (H1) *H1 = -H_predicted_dir;
+    if (H2) *H2 = H_predicted_dir;
+    return predicted - measured_w_aZb_;
+  }
+
+ private:
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Base", boost::serialization::base_object<Base>(*this));
+  }
+};  // \ TranslationFactor
+}  // namespace gtsam
diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp
new file mode 100644
index 000000000..aeeae688f
--- /dev/null
+++ b/gtsam/sfm/TranslationRecovery.cpp
@@ -0,0 +1,103 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, 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 TranslationRecovery.h
+ * @author Frank Dellaert
+ * @date March 2020
+ * @brief test recovering translations when rotations are given.
+ */
+
+#include <gtsam/sfm/TranslationRecovery.h>
+
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/sfm/TranslationFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+
+using namespace gtsam;
+using namespace std;
+
+NonlinearFactorGraph TranslationRecovery::buildGraph() const {
+  auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01);
+  NonlinearFactorGraph graph;
+
+  // Add all relative translation edges
+  for (auto edge : relativeTranslations_) {
+    Key a, b;
+    tie(a, b) = edge.first;
+    const Unit3 w_aZb = edge.second;
+    graph.emplace_shared<TranslationFactor>(a, b, w_aZb, noiseModel);
+  }
+
+  return graph;
+}
+
+void TranslationRecovery::addPrior(const double scale,
+                                   NonlinearFactorGraph* graph) const {
+  auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01);
+  auto edge = relativeTranslations_.begin();
+  Key a, b;
+  tie(a, b) = edge->first;
+  const Unit3 w_aZb = edge->second;
+  graph->emplace_shared<PriorFactor<Point3> >(a, Point3(0, 0, 0), noiseModel);
+  graph->emplace_shared<PriorFactor<Point3> >(b, scale * w_aZb.point3(),
+                                              noiseModel);
+}
+
+Values TranslationRecovery::initalizeRandomly() const {
+  // Create a lambda expression that checks whether value exists and randomly
+  // initializes if not.
+  Values initial;
+  auto insert = [&initial](Key j) {
+    if (!initial.exists(j)) {
+      initial.insert<Point3>(j, Vector3::Random());
+    }
+  };
+
+  // Loop over measurements and add a random translation
+  for (auto edge : relativeTranslations_) {
+    Key a, b;
+    tie(a, b) = edge.first;
+    insert(a);
+    insert(b);
+  }
+  return initial;
+}
+
+Values TranslationRecovery::run(const double scale) const {
+  auto graph = buildGraph();
+  addPrior(scale, &graph);
+  const Values initial = initalizeRandomly();
+  LevenbergMarquardtOptimizer lm(graph, initial, params_);
+  Values result = lm.optimize();
+  return result;
+}
+
+TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(
+    const Values& poses, const vector<KeyPair>& edges) {
+  TranslationEdges relativeTranslations;
+  for (auto edge : edges) {
+    Key a, b;
+    tie(a, b) = edge;
+    const Pose3 wTa = poses.at<Pose3>(a), wTb = poses.at<Pose3>(b);
+    const Point3 Ta = wTa.translation(), Tb = wTb.translation();
+    const Unit3 w_aZb(Tb - Ta);
+    relativeTranslations[edge] = w_aZb;
+  }
+  return relativeTranslations;
+}
diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h
new file mode 100644
index 000000000..bb3c3cdb1
--- /dev/null
+++ b/gtsam/sfm/TranslationRecovery.h
@@ -0,0 +1,114 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, 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 TranslationRecovery.h
+ * @author Frank Dellaert
+ * @date March 2020
+ * @brief test recovering translations when rotations are given.
+ */
+
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Values.h>
+
+#include <map>
+#include <utility>
+
+namespace gtsam {
+
+// Set up an optimization problem for the unknown translations Ti in the world
+// coordinate frame, given the known camera attitudes wRi with respect to the
+// world frame, and a set of (noisy) translation directions of type Unit3,
+// w_aZb. The measurement equation is
+//    w_aZb = Unit3(Tb - Ta)   (1)
+// i.e., w_aZb is the translation direction from frame A to B, in world
+// coordinates. Although Unit3 instances live on a manifold, following
+// Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
+// ambient world coordinate frame.
+//
+// It is clear that we cannot recover the scale, nor the absolute position,
+// so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing
+// the translations Ta and Tb associated with the first measurement w_aZb,
+// clamping them to their initial values as given to this method. If no initial
+// values are given, we use the origin for Tb and set Tb to make (1) come
+// through, i.e.,
+//    Tb = s * wRa * Point3(w_aZb)     (2)
+// where s is an arbitrary scale that can be supplied, default 1.0. Hence, two
+// versions are supplied below corresponding to whether we have initial values
+// or not.
+class TranslationRecovery {
+ public:
+  using KeyPair = std::pair<Key, Key>;
+  using TranslationEdges = std::map<KeyPair, Unit3>;
+
+ private:
+  TranslationEdges relativeTranslations_;
+  LevenbergMarquardtParams params_;
+
+ public:
+  /**
+   * @brief Construct a new Translation Recovery object
+   *
+   * @param relativeTranslations the relative translations, in world coordinate
+   * frames, indexed in a map by a pair of Pose keys.
+   * @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be
+   * used to modify the parameters for the LM optimizer. By default, uses the
+   * default LM parameters. 
+   */
+  TranslationRecovery(const TranslationEdges& relativeTranslations,
+                      const LevenbergMarquardtParams& lmParams = LevenbergMarquardtParams())
+      : relativeTranslations_(relativeTranslations), params_(lmParams) {
+    params_.setVerbosityLM("Summary");
+  }
+
+  /**
+   * @brief Build the factor graph to do the optimization.
+   *
+   * @return NonlinearFactorGraph
+   */
+  NonlinearFactorGraph buildGraph() const;
+
+  /**
+   * @brief Add priors on ednpoints of first measurement edge.
+   *
+   * @param scale scale for first relative translation which fixes gauge.
+   * @param graph factor graph to which prior is added.
+   */
+  void addPrior(const double scale, NonlinearFactorGraph* graph) const;
+
+  /**
+   * @brief Create random initial translations.
+   *
+   * @return Values
+   */
+  Values initalizeRandomly() const;
+
+  /**
+   * @brief Build and optimize factor graph.
+   *
+   * @param scale scale for first relative translation which fixes gauge.
+   * @return Values
+   */
+  Values run(const double scale = 1.0) const;
+
+  /**
+   * @brief Simulate translation direction measurements
+   *
+   * @param poses SE(3) ground truth poses stored as Values
+   * @param edges pairs (a,b) for which a measurement w_aZb will be generated.
+   * @return TranslationEdges map from a KeyPair to the simulated Unit3
+   * translation direction measurement between the cameras in KeyPair.
+   */
+  static TranslationEdges SimulateMeasurements(
+      const Values& poses, const std::vector<KeyPair>& edges);
+};
+}  // namespace gtsam
diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp
new file mode 100644
index 000000000..37e8b6c0f
--- /dev/null
+++ b/gtsam/sfm/tests/testTranslationFactor.cpp
@@ -0,0 +1,108 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, 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  testTranslationFactor.cpp
+ *  @brief Unit tests for TranslationFactor Class
+ *  @author Frank dellaert
+ *  @date March 2020
+ */
+
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/sfm/TranslationFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+// Create a noise model for the chordal error
+static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.05));
+
+// Keys are deliberately *not* in sorted order to test that case.
+static const Key kKey1(2), kKey2(1);
+static const Unit3 kMeasured(1, 0, 0);
+
+/* ************************************************************************* */
+TEST(TranslationFactor, Constructor) {
+  TranslationFactor factor(kKey1, kKey2, kMeasured, model);
+}
+
+/* ************************************************************************* */
+TEST(TranslationFactor, ZeroError) {
+  // Create a factor
+  TranslationFactor factor(kKey1, kKey2, kMeasured, model);
+
+  // Set the linearization
+  Point3 T1(1, 0, 0), T2(2, 0, 0);
+
+  // Use the factor to calculate the error
+  Vector actualError(factor.evaluateError(T1, T2));
+
+  // Verify we get the expected error
+  Vector expected = (Vector3() << 0, 0, 0).finished();
+  EXPECT(assert_equal(expected, actualError, 1e-9));
+
+
+}
+
+/* ************************************************************************* */
+TEST(TranslationFactor, NonZeroError) {
+  // create a factor
+  TranslationFactor factor(kKey1, kKey2, kMeasured, model);
+
+  // set the linearization
+  Point3 T1(0, 1, 1), T2(0, 2, 2);
+
+  // use the factor to calculate the error
+  Vector actualError(factor.evaluateError(T1, T2));
+
+  // verify we get the expected error
+  Vector expected = (Vector3() << -1, 1/sqrt(2), 1/sqrt(2)).finished();
+  EXPECT(assert_equal(expected, actualError, 1e-9));
+}
+
+/* ************************************************************************* */
+Vector factorError(const Point3& T1, const Point3& T2,
+                   const TranslationFactor& factor) {
+  return factor.evaluateError(T1, T2);
+}
+
+TEST(TranslationFactor, Jacobian) {
+  // Create a factor
+  TranslationFactor factor(kKey1, kKey2, kMeasured, model);
+
+  // Set the linearization
+  Point3 T1(1, 0, 0), T2(2, 0, 0);
+
+  // Use the factor to calculate the Jacobians
+  Matrix H1Actual, H2Actual;
+  factor.evaluateError(T1, T2, H1Actual, H2Actual);
+
+  // Use numerical derivatives to calculate the Jacobians
+  Matrix H1Expected, H2Expected;
+  H1Expected = numericalDerivative11<Vector, Point3>(
+      boost::bind(&factorError, _1, T2, factor), T1);
+  H2Expected = numericalDerivative11<Vector, Point3>(
+      boost::bind(&factorError, T1, _1, factor), T2);
+
+  // Verify the Jacobians are correct
+  EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
+  EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h
index b52e115fd..9e4f7db5a 100644
--- a/gtsam/slam/AntiFactor.h
+++ b/gtsam/slam/AntiFactor.h
@@ -67,7 +67,7 @@ namespace gtsam {
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e =  dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol);
+      return e != nullptr && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol);
     }
 
     /** implement functions needed to derive from Factor */
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index f949514e3..b1d4904aa 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -81,7 +81,7 @@ namespace gtsam {
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e =  dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
+      return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
     }
 
     /** implement functions needed to derive from Factor */
@@ -126,7 +126,7 @@ namespace gtsam {
 	  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
 	  enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
     public:
-      EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+      GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   }; // \class BetweenFactor
 
   /// traits
diff --git a/gtsam/slam/EssentialMatrixConstraint.cpp b/gtsam/slam/EssentialMatrixConstraint.cpp
index e27bbc8c5..5397c2e57 100644
--- a/gtsam/slam/EssentialMatrixConstraint.cpp
+++ b/gtsam/slam/EssentialMatrixConstraint.cpp
@@ -37,7 +37,7 @@ void EssentialMatrixConstraint::print(const std::string& s,
 bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected,
     double tol) const {
   const This *e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol)
+  return e != nullptr && Base::equals(*e, tol)
       && this->measuredE_.equals(e->measuredE_, tol);
 }
 
diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h
index 179200fe1..e474ce5b3 100644
--- a/gtsam/slam/EssentialMatrixConstraint.h
+++ b/gtsam/slam/EssentialMatrixConstraint.h
@@ -105,7 +105,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // \class EssentialMatrixConstraint
 
diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index 2e921bfef..c214a2f48 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -9,7 +9,7 @@
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/EssentialMatrix.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <iostream>
 
 namespace gtsam {
@@ -81,7 +81,7 @@ public:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /**
@@ -201,7 +201,7 @@ public:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // EssentialMatrixFactor2
 
@@ -286,7 +286,7 @@ public:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // EssentialMatrixFactor3
 
diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp
new file mode 100644
index 000000000..904addb03
--- /dev/null
+++ b/gtsam/slam/FrobeniusFactor.cpp
@@ -0,0 +1,117 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, 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   FrobeniusFactor.cpp
+ * @date   March 2019
+ * @author Frank Dellaert
+ * @brief  Various factors that minimize some Frobenius norm
+ */
+
+#include <gtsam/slam/FrobeniusFactor.h>
+
+#include <gtsam/base/timing.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+
+#include <cmath>
+#include <iostream>
+#include <vector>
+
+using namespace std;
+
+namespace gtsam {
+
+//******************************************************************************
+boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
+    const SharedNoiseModel& model, size_t d, bool defaultToUnit) {
+  double sigma = 1.0;
+  if (model != nullptr) {
+    if (model->dim() != 6) {
+      if (!defaultToUnit)
+        throw std::runtime_error("Can only convert Pose3 noise models");
+    } else {
+      auto sigmas = model->sigmas().head(3).eval();
+      if (sigmas(1) != sigmas(0) || sigmas(2) != sigmas(0)) {
+        if (!defaultToUnit)
+          throw std::runtime_error("Can only convert isotropic rotation noise");
+      } else {
+        sigma = sigmas(0);
+      }
+    }
+  }
+  return noiseModel::Isotropic::Sigma(d, sigma);
+}
+
+//******************************************************************************
+FrobeniusWormholeFactor::FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12,
+                                                 size_t p,
+                                                 const SharedNoiseModel& model)
+    : NoiseModelFactor2<SOn, SOn>(ConvertPose3NoiseModel(model, p * 3), j1, j2),
+      M_(R12.matrix()),               // 3*3 in all cases
+      p_(p),                          // 4 for SO(4)
+      pp_(p * p),                     // 16 for SO(4)
+      dimension_(SOn::Dimension(p)),  // 6 for SO(4)
+      G_(pp_, dimension_)             // 16*6 for SO(4)
+{
+  // Calculate G matrix of vectorized generators
+  Matrix Z = Matrix::Zero(p, p);
+  for (size_t j = 0; j < dimension_; j++) {
+    const auto X = SOn::Hat(Eigen::VectorXd::Unit(dimension_, j));
+    G_.col(j) = Eigen::Map<const Matrix>(X.data(), pp_, 1);
+  }
+  assert(noiseModel()->dim() == 3 * p_);
+}
+
+//******************************************************************************
+Vector FrobeniusWormholeFactor::evaluateError(
+    const SOn& Q1, const SOn& Q2, boost::optional<Matrix&> H1,
+    boost::optional<Matrix&> H2) const {
+  gttic(FrobeniusWormholeFactorP_evaluateError);
+
+  const Matrix& M1 = Q1.matrix();
+  const Matrix& M2 = Q2.matrix();
+  assert(M1.rows() == p_ && M2.rows() == p_);
+
+  const size_t dim = 3 * p_;  // Stiefel manifold dimension
+  Vector fQ2(dim), hQ1(dim);
+
+  // Vectorize and extract only d leftmost columns, i.e. vec(M2*P)
+  fQ2 << Eigen::Map<const Matrix>(M2.data(), dim, 1);
+
+  // Vectorize M1*P*R12
+  const Matrix Q1PR12 = M1.leftCols<3>() * M_;
+  hQ1 << Eigen::Map<const Matrix>(Q1PR12.data(), dim, 1);
+
+  // If asked, calculate Jacobian as (M \otimes M1) * G
+  if (H1) {
+    const size_t p2 = 2 * p_;
+    Matrix RPxQ = Matrix::Zero(dim, pp_);
+    RPxQ.block(0, 0, p_, dim) << M1 * M_(0, 0), M1 * M_(1, 0), M1 * M_(2, 0);
+    RPxQ.block(p_, 0, p_, dim) << M1 * M_(0, 1), M1 * M_(1, 1), M1 * M_(2, 1);
+    RPxQ.block(p2, 0, p_, dim) << M1 * M_(0, 2), M1 * M_(1, 2), M1 * M_(2, 2);
+    *H1 = -RPxQ * G_;
+  }
+  if (H2) {
+    const size_t p2 = 2 * p_;
+    Matrix PxQ = Matrix::Zero(dim, pp_);
+    PxQ.block(0, 0, p_, p_) = M2;
+    PxQ.block(p_, p_, p_, p_) = M2;
+    PxQ.block(p2, p2, p_, p_) = M2;
+    *H2 = PxQ * G_;
+  }
+
+  return fQ2 - hQ1;
+}
+
+//******************************************************************************
+
+}  // namespace gtsam
diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h
new file mode 100644
index 000000000..a73445ae0
--- /dev/null
+++ b/gtsam/slam/FrobeniusFactor.h
@@ -0,0 +1,145 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, 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   FrobeniusFactor.h
+ * @date   March 2019
+ * @author Frank Dellaert
+ * @brief  Various factors that minimize some Frobenius norm
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/SOn.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * When creating (any) FrobeniusFactor we convert a 6-dimensional Pose3
+ * BetweenFactor noise model into an 9 or 16-dimensional isotropic noise
+ * model used to weight the Frobenius norm.  If the noise model passed is
+ * null we return a Dim-dimensional isotropic noise model with sigma=1.0. If
+ * not, we we check if the 3-dimensional noise model on rotations is
+ * isotropic. If it is, we extend to 'Dim' dimensions, otherwise we throw an
+ * error. If defaultToUnit == false throws an exception on unexepcted input.
+ */
+  GTSAM_EXPORT boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
+    const SharedNoiseModel& model, size_t d, bool defaultToUnit = true);
+
+/**
+ * FrobeniusPrior calculates the Frobenius norm between a given matrix and an
+ * element of SO(3) or SO(4).
+ */
+template <class Rot>
+class FrobeniusPrior : public NoiseModelFactor1<Rot> {
+  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
+  using MatrixNN = typename Rot::MatrixNN;
+  Eigen::Matrix<double, Dim, 1> vecM_;  ///< vectorized matrix to approximate
+
+ public:
+  /// Constructor
+  FrobeniusPrior(Key j, const MatrixNN& M,
+                 const SharedNoiseModel& model = nullptr)
+      : NoiseModelFactor1<Rot>(ConvertPose3NoiseModel(model, Dim), j) {
+    vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
+  }
+
+  /// Error is just Frobenius norm between Rot element and vectorized matrix M.
+  Vector evaluateError(const Rot& R,
+                       boost::optional<Matrix&> H = boost::none) const {
+    return R.vec(H) - vecM_;  // Jacobian is computed only when needed.
+  }
+};
+
+/**
+ * FrobeniusFactor calculates the Frobenius norm between rotation matrices.
+ * The template argument can be any fixed-size SO<N>.
+ */
+template <class Rot>
+class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
+  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
+
+ public:
+  /// Constructor
+  FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
+      : NoiseModelFactor2<Rot, Rot>(ConvertPose3NoiseModel(model, Dim), j1,
+                                    j2) {}
+
+  /// Error is just Frobenius norm between rotation matrices.
+  Vector evaluateError(const Rot& R1, const Rot& R2,
+                       boost::optional<Matrix&> H1 = boost::none,
+                       boost::optional<Matrix&> H2 = boost::none) const {
+    Vector error = R2.vec(H2) - R1.vec(H1);
+    if (H1) *H1 = -*H1;
+    return error;
+  }
+};
+
+/**
+ * FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm
+ * of the rotation error between measured and predicted (rather than the
+ * Logmap of the error). This factor is only defined for fixed-dimension types,
+ * and in fact only SO3 and SO4 really work, as we need SO<N>::AdjointMap.
+ */
+template <class Rot>
+class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
+  Rot R12_;  ///< measured rotation between R1 and R2
+  Eigen::Matrix<double, Rot::dimension, Rot::dimension>
+      R2hat_H_R1_;  ///< fixed derivative of R2hat wrpt R1
+  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
+
+ public:
+  /// Constructor
+  FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
+                         const SharedNoiseModel& model = nullptr)
+      : NoiseModelFactor2<Rot, Rot>(
+            ConvertPose3NoiseModel(model, Dim), j1, j2),
+        R12_(R12),
+        R2hat_H_R1_(R12.inverse().AdjointMap()) {}
+
+  /// Error is Frobenius norm between R1*R12 and R2.
+  Vector evaluateError(const Rot& R1, const Rot& R2,
+                       boost::optional<Matrix&> H1 = boost::none,
+                       boost::optional<Matrix&> H2 = boost::none) const {
+    const Rot R2hat = R1.compose(R12_);
+    Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
+    Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);
+    if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_;
+    return error;
+  }
+};
+
+/**
+ * FrobeniusWormholeFactor is a BetweenFactor that moves in SO(p), but will
+ * land on the SO(3) sub-manifold of SO(p) at the global minimum. It projects
+ * the SO(p) matrices down to a Stiefel manifold of p*d matrices.
+ * TODO(frank): template on D=2 or 3
+ */
+class GTSAM_EXPORT FrobeniusWormholeFactor : public NoiseModelFactor2<SOn, SOn> {
+  Matrix M_;                   ///< measured rotation between R1 and R2
+  size_t p_, pp_, dimension_;  ///< dimensionality constants
+  Matrix G_;                   ///< matrix of vectorized generators
+
+ public:
+  /// Constructor. Note we convert to 3*p-dimensional noise model.
+  FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12, size_t p = 4,
+                          const SharedNoiseModel& model = nullptr);
+
+  /// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]
+  /// projects down from SO(p) to the Stiefel manifold of px3 matrices.
+  Vector evaluateError(const SOn& Q1, const SOn& Q2,
+                       boost::optional<Matrix&> H1 = boost::none,
+                       boost::optional<Matrix&> H2 = boost::none) const;
+};
+
+}  // namespace gtsam
diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp
index a1baab5fa..3d4a4d40d 100644
--- a/gtsam/slam/InitializePose3.cpp
+++ b/gtsam/slam/InitializePose3.cpp
@@ -17,7 +17,7 @@
  */
 
 #include <gtsam/slam/InitializePose3.h> 
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/inference/Symbol.h>
@@ -75,27 +75,15 @@ Values InitializePose3::normalizeRelaxedRotations(
     const VectorValues& relaxedRot3) {
   gttic(InitializePose3_computeOrientationsChordal);
 
-  Matrix ppm = Z_3x3; // plus plus minus
-  ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
-
   Values validRot3;
   for(const auto& it: relaxedRot3) {
     Key key = it.first;
     if (key != kAnchorKey) {
-      Matrix3 R;
-      R << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized
+      Matrix3 M;
+      M << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized
 
       // ClosestTo finds rotation matrix closest to H in Frobenius sense
-      // Rot3 initRot = Rot3::ClosestTo(M.transpose());
-
-      Matrix U, V; Vector s;
-      svd(R.transpose(), U, s, V);
-      Matrix3 normalizedRotMat = U * V.transpose();
-
-      if(normalizedRotMat.determinant() < 0)
-        normalizedRotMat = U * ppm * V.transpose();
-
-      Rot3 initRot = Rot3(normalizedRotMat);
+      Rot3 initRot = Rot3::ClosestTo(M.transpose());
       validRot3.insert(key, initRot);
     }
   }
diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h
index b94714dd6..bc906d24e 100644
--- a/gtsam/slam/JacobianFactorSVD.h
+++ b/gtsam/slam/JacobianFactorSVD.h
@@ -65,7 +65,7 @@ public:
       Base() {
     size_t numKeys = Enull.rows() / ZDim;
     size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
-    // PLAIN NULL SPACE TRICK
+    // PLAIN nullptr SPACE TRICK
     // Matrix Q = Enull * Enull.transpose();
     // for(const KeyMatrixZD& it: Fblocks)
     //   QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h
new file mode 100644
index 000000000..f10cc7e42
--- /dev/null
+++ b/gtsam/slam/KarcherMeanFactor-inl.h
@@ -0,0 +1,77 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 KarcherMeanFactor.cpp
+ * @author Frank Dellaert
+ * @date March 2019
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/KarcherMeanFactor.h>
+
+using namespace std;
+
+namespace gtsam {
+
+template <class T, class ALLOC>
+T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
+  // Cost function C(R) = \sum PriorFactor(R_i)::error(R)
+  // No closed form solution.
+  NonlinearFactorGraph graph;
+  static const Key kKey(0);
+  for (const auto& R : rotations) {
+    graph.addPrior<T>(kKey, R);
+  }
+  Values initial;
+  initial.insert<T>(kKey, T());
+  auto result = GaussNewtonOptimizer(graph, initial).optimize();
+  return result.at<T>(kKey);
+}
+
+template <class T,
+        typename = typename std::enable_if< std::is_same<gtsam::Rot3, T>::value >::type >
+T FindKarcherMean(const std::vector<T>& rotations) {
+  return FindKarcherMeanImpl(rotations);
+}
+
+template <class T>
+T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations) {
+  return FindKarcherMeanImpl(rotations);
+}
+
+template <class T>
+T FindKarcherMean(std::initializer_list<T>&& rotations) {
+  return FindKarcherMeanImpl(std::vector<T, Eigen::aligned_allocator<T> >(rotations));
+}
+
+template <class T>
+template <typename CONTAINER>
+KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER& keys, int d)
+    : NonlinearFactor(keys) {
+  if (d <= 0) {
+    throw std::invalid_argument(
+        "KarcherMeanFactor needs dimension for dynamic types.");
+  }
+  // Create the constant Jacobian made of D*D identity matrices,
+  // where D is the dimensionality of the manifold.
+  const auto I = Eigen::MatrixXd::Identity(d, d);
+  std::map<Key, Matrix> terms;
+  for (Key j : keys) {
+    terms[j] = I;
+  }
+  jacobian_ =
+      boost::make_shared<JacobianFactor>(terms, Eigen::VectorXd::Zero(d));
+}
+}  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h
new file mode 100644
index 000000000..54b3930d4
--- /dev/null
+++ b/gtsam/slam/KarcherMeanFactor.h
@@ -0,0 +1,73 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 KarcherMeanFactor.h
+ * @author Frank Dellaert
+ * @date March 2019
+ */
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/linear/JacobianFactor.h>
+
+#include <map>
+#include <vector>
+
+namespace gtsam {
+/**
+ * Optimize for the Karcher mean, minimizing the geodesic distance to each of
+ * the given rotations, by constructing a factor graph out of simple
+ * PriorFactors.
+ */
+template <class T>
+T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations);
+
+template <class T>
+T FindKarcherMean(std::initializer_list<T>&& rotations);
+
+/**
+ * The KarcherMeanFactor creates a constraint on all SO(n) variables with
+ * given keys that the Karcher mean (see above) will stay the same. Note the
+ * mean itself is irrelevant to the constraint and is not a parameter: the
+ * constraint is implemented as enforcing that the sum of local updates is
+ * equal to zero, hence creating a rank-dim constraint. Note it is implemented as
+ * a soft constraint, as typically it is used to fix a gauge freedom.
+ * */
+template <class T>
+class KarcherMeanFactor : public NonlinearFactor {
+  /// Constant Jacobian made of d*d identity matrices
+  boost::shared_ptr<JacobianFactor> jacobian_;
+
+  enum {D = traits<T>::dimension};
+
+ public:
+  /// Construct from given keys.
+  template <typename CONTAINER>
+  KarcherMeanFactor(const CONTAINER& keys, int d=D);
+
+  /// Destructor
+  virtual ~KarcherMeanFactor() {}
+
+  /// Calculate the error of the factor: always zero
+  double error(const Values& c) const override { return 0; }
+
+  /// get the dimension of the factor (number of rows on linearization)
+  size_t dim() const override { return D; }
+
+  /// linearize to a GaussianFactor
+  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
+    return jacobian_;
+  }
+};
+// \KarcherMeanFactor
+}  // namespace gtsam
diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp
index 5b0c6a6b9..56968301a 100644
--- a/gtsam/slam/OrientedPlane3Factor.cpp
+++ b/gtsam/slam/OrientedPlane3Factor.cpp
@@ -31,7 +31,7 @@ void OrientedPlane3DirectionPrior::print(const string& s,
 bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
     double tol) const {
   const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol)
+  return e != nullptr && Base::equals(*e, tol)
       && this->measured_p_.equals(e->measured_p_, tol);
 }
 
diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h
index 78030ff34..f4ce1520a 100644
--- a/gtsam/slam/PoseRotationPrior.h
+++ b/gtsam/slam/PoseRotationPrior.h
@@ -62,7 +62,7 @@ public:
   /** equals specialized to this factor */
   virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
     const This *e = dynamic_cast<const This*> (&expected);
-    return e != NULL && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
+    return e != nullptr && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
   }
 
   /** print contents */
diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h
index c38d13b58..516582d83 100644
--- a/gtsam/slam/PoseTranslationPrior.h
+++ b/gtsam/slam/PoseTranslationPrior.h
@@ -76,7 +76,7 @@ public:
   /** equals specialized to this factor */
   virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
     const This *e = dynamic_cast<const This*> (&expected);
-    return e != NULL && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
+    return e != nullptr && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
   }
 
   /** print contents */
diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h
index 5b085652f..335eb2b5a 100644
--- a/gtsam/slam/PriorFactor.h
+++ b/gtsam/slam/PriorFactor.h
@@ -15,106 +15,6 @@
  **/
 #pragma once
 
-#include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/base/Testable.h>
-
-#include <string>
-
-namespace gtsam {
-
-  /**
-   * A class for a soft prior on any Value type
-   * @addtogroup SLAM
-   */
-  template<class VALUE>
-  class PriorFactor: public NoiseModelFactor1<VALUE> {
-
-  public:
-    typedef VALUE T;
-
-  private:
-
-    typedef NoiseModelFactor1<VALUE> Base;
-
-    VALUE prior_; /** The measurement */
-
-    /** concept check by type */
-    GTSAM_CONCEPT_TESTABLE_TYPE(T)
-
-  public:
-
-    /// shorthand for a smart pointer to a factor
-    typedef typename boost::shared_ptr<PriorFactor<VALUE> > shared_ptr;
-
-    /// Typedef to this class
-    typedef PriorFactor<VALUE> This;
-
-    /** default constructor - only use for serialization */
-    PriorFactor() {}
-
-    virtual ~PriorFactor() {}
-
-    /** Constructor */
-    PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) :
-      Base(model, key), prior_(prior) {
-    }
-
-    /** Convenience constructor that takes a full covariance argument */
-    PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) :
-      Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) {
-    }
-
-    /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
-
-    /** implement functions needed for Testable */
-
-    /** print */
-    virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-                                                 DefaultKeyFormatter) const {
-      std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
-      traits<T>::Print(prior_, "  prior mean: ");
-      if (this->noiseModel_)
-        this->noiseModel_->print("  noise model: ");
-      else
-        std::cout << "no noise model" << std::endl;
-    }
-
-    /** equals */
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
-      const This* e = dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
-    }
-
-    /** implement functions needed to derive from Factor */
-
-    /** vector of errors */
-    Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const {
-      if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
-      // manifold equivalent of z-x -> Local(x,z)
-      // TODO(ASL) Add Jacobians.
-      return -traits<T>::Local(x, prior_);
-    }
-
-    const VALUE & prior() const { return prior_; }
-
-  private:
-
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & boost::serialization::make_nvp("NoiseModelFactor1",
-          boost::serialization::base_object<Base>(*this));
-      ar & BOOST_SERIALIZATION_NVP(prior_);
-    }
-
-	// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
-	enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
-  public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
-  };
-
-} /// namespace gtsam
+// Note: PriorFactor has been moved to gtsam/nonlinear/PriorFactor.h. This file
+// has been left here for backwards compatibility.
+#include <gtsam/nonlinear/PriorFactor.h>
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index 4a8a6b138..0bed15fdc 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -21,7 +21,8 @@
 #pragma once
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <boost/optional.hpp>
 
 namespace gtsam {
@@ -188,7 +189,7 @@ namespace gtsam {
     }
 
   public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
   /// traits
diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h
index 07b749811..71474a8ab 100644
--- a/gtsam/slam/RegularImplicitSchurFactor.h
+++ b/gtsam/slam/RegularImplicitSchurFactor.h
@@ -10,7 +10,11 @@
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/VectorValues.h>
+
 #include <iosfwd>
+#include <map>
+#include <string>
+#include <vector>
 
 namespace gtsam {
 
@@ -76,7 +80,7 @@ public:
 
   /// print
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const {
+      DefaultKeyFormatter) const override {
     std::cout << " RegularImplicitSchurFactor " << std::endl;
     Factor::print(s);
     for (size_t pos = 0; pos < size(); ++pos) {
@@ -88,7 +92,7 @@ public:
   }
 
   /// equals
-  bool equals(const GaussianFactor& lf, double tol) const {
+  bool equals(const GaussianFactor& lf, double tol) const override {
     const This* f = dynamic_cast<const This*>(&lf);
     if (!f)
       return false;
@@ -104,48 +108,48 @@ public:
   }
 
   /// Degrees of freedom of camera
-  virtual DenseIndex getDim(const_iterator variable) const {
+  DenseIndex getDim(const_iterator variable) const override {
     return D;
   }
 
-  virtual void updateHessian(const KeyVector& keys,
-                         SymmetricBlockMatrix* info) const {
+  void updateHessian(const KeyVector& keys,
+                         SymmetricBlockMatrix* info) const override {
     throw std::runtime_error(
         "RegularImplicitSchurFactor::updateHessian non implemented");
   }
-  virtual Matrix augmentedJacobian() const {
+  Matrix augmentedJacobian() const override {
     throw std::runtime_error(
         "RegularImplicitSchurFactor::augmentedJacobian non implemented");
     return Matrix();
   }
-  virtual std::pair<Matrix, Vector> jacobian() const {
+  std::pair<Matrix, Vector> jacobian() const override {
     throw std::runtime_error(
         "RegularImplicitSchurFactor::jacobian non implemented");
     return std::make_pair(Matrix(), Vector());
   }
 
   /// *Compute* full augmented information matrix
-  virtual Matrix augmentedInformation() const {
-
+  Matrix augmentedInformation() const override {
     // Do the Schur complement
-    SymmetricBlockMatrix augmentedHessian = //
+    SymmetricBlockMatrix augmentedHessian =
         Set::SchurComplement(FBlocks_, E_, b_);
     return augmentedHessian.selfadjointView();
   }
 
   /// *Compute* full information matrix
-  virtual Matrix information() const {
+  Matrix information() const override {
     Matrix augmented = augmentedInformation();
     int m = this->keys_.size();
     size_t M = D * m;
     return augmented.block(0, 0, M, M);
   }
 
-  /// Return the diagonal of the Hessian for this factor
-  virtual VectorValues hessianDiagonal() const {
-    // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
-    VectorValues d;
+  /// Using the base method
+  using GaussianFactor::hessianDiagonal;
 
+  /// Add the diagonal of the Hessian for this factor to existing VectorValues
+  void hessianDiagonalAdd(VectorValues &d) const override {
+    // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
     for (size_t k = 0; k < size(); ++k) { // for each camera
       Key j = keys_[k];
 
@@ -153,7 +157,7 @@ public:
       // D x 3 = (D x ZDim) * (ZDim x 3)
       const MatrixZD& Fj = FBlocks_[k];
       Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
-          * E_.block<ZDim, 3>(ZDim * k, 0);
+                                        * E_.block<ZDim, 3>(ZDim * k, 0);
 
       Eigen::Matrix<double, D, 1> dj;
       for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
@@ -163,16 +167,19 @@ public:
         // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
         dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
       }
-      d.insert(j, dj);
+
+      auto result = d.emplace(j, dj);
+      if(!result.second) {
+        result.first->second += dj;
+      }
     }
-    return d;
   }
 
   /**
    * @brief add the contribution of this factor to the diagonal of the hessian
    * d(output) = d(input) + deltaHessianFactor
    */
-  virtual void hessianDiagonal(double* d) const {
+  void hessianDiagonal(double* d) const override {
     // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
     // Use eigen magic to access raw memory
     typedef Eigen::Matrix<double, D, 1> DVector;
@@ -198,7 +205,7 @@ public:
   }
 
   /// Return the block diagonal of the Hessian for this factor
-  virtual std::map<Key, Matrix> hessianBlockDiagonal() const {
+  std::map<Key, Matrix> hessianBlockDiagonal() const override {
     std::map<Key, Matrix> blocks;
     // F'*(I - E*P*E')*F
     for (size_t pos = 0; pos < size(); ++pos) {
@@ -223,17 +230,18 @@ public:
     return blocks;
   }
 
-  virtual GaussianFactor::shared_ptr clone() const {
+  GaussianFactor::shared_ptr clone() const override {
     return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
         FBlocks_, PointCovariance_, E_, b_);
     throw std::runtime_error(
         "RegularImplicitSchurFactor::clone non implemented");
   }
-  virtual bool empty() const {
+
+  bool empty() const override {
     return false;
   }
 
-  virtual GaussianFactor::shared_ptr negate() const {
+  GaussianFactor::shared_ptr negate() const override {
     return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
         FBlocks_, PointCovariance_, E_, b_);
     throw std::runtime_error(
@@ -284,7 +292,7 @@ public:
    * f = nonlinear error
    * (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - 2*b) + f
    */
-  virtual double error(const VectorValues& x) const {
+  double error(const VectorValues& x) const override {
 
     // resize does not do malloc if correct size
     e1.resize(size());
@@ -379,13 +387,12 @@ public:
   void multiplyHessianAdd(double alpha, const double* x, double* y,
       std::vector<size_t> keys) const {
   }
-  ;
 
   /**
    * @brief Hessian-vector multiply, i.e. y += F'*alpha*(I - E*P*E')*F*x
    */
   void multiplyHessianAdd(double alpha, const VectorValues& x,
-      VectorValues& y) const {
+      VectorValues& y) const override {
 
     // resize does not do malloc if correct size
     e1.resize(size());
@@ -428,7 +435,7 @@ public:
   /**
    * Calculate gradient, which is -F'Q*b, see paper
    */
-  VectorValues gradientAtZero() const {
+  VectorValues gradientAtZero() const override {
     // calculate Q*b
     e1.resize(size());
     e2.resize(size());
@@ -450,7 +457,7 @@ public:
   /**
    * Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS
    */
-  virtual void gradientAtZero(double* d) const {
+  void gradientAtZero(double* d) const override {
 
     // Use eigen magic to access raw memory
     typedef Eigen::Matrix<double, D, 1> DVector;
@@ -470,7 +477,7 @@ public:
   }
 
   /// Gradient wrt a key at any values
-  Vector gradient(Key key, const VectorValues& x) const {
+  Vector gradient(Key key, const VectorValues& x) const override {
     throw std::runtime_error(
         "gradient for RegularImplicitSchurFactor is not implemented yet");
   }
diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h
index 9d0975df3..948fffe13 100644
--- a/gtsam/slam/RotateFactor.h
+++ b/gtsam/slam/RotateFactor.h
@@ -112,6 +112,8 @@ public:
     }
     return error;
   }
+
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 }  // namespace gtsam
 
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 34f9b9e9f..1c80b8744 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -81,7 +81,7 @@ protected:
   mutable FBlocks Fs;
 
  public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
@@ -207,10 +207,18 @@ protected:
     Vector ue = cameras.reprojectionError(point, measured_, Fs, E);
     if (body_P_sensor_ && Fs) {
       const Pose3 sensor_P_body = body_P_sensor_->inverse();
+      constexpr int camera_dim = traits<CAMERA>::dimension;
+      constexpr int pose_dim = traits<Pose3>::dimension;
+
       for (size_t i = 0; i < Fs->size(); i++) {
-        const Pose3 w_Pose_body = cameras[i].pose() * sensor_P_body;
-        Matrix J(6, 6);
-        const Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
+        const Pose3 world_P_body = cameras[i].pose() * sensor_P_body;
+        Eigen::Matrix<double, camera_dim, camera_dim> J;
+        J.setZero();
+        Eigen::Matrix<double, pose_dim, pose_dim> H;
+        // Call compose to compute Jacobian for camera extrinsics
+        world_P_body.compose(*body_P_sensor_, H);
+        // Assign extrinsics part of the Jacobian
+        J.template block<pose_dim, pose_dim>(0, 0) = H;
         Fs->at(i) = Fs->at(i) * J;
       }
     }
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 8a5c87ecf..669935994 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -37,6 +37,7 @@
 #include <boost/assign/list_inserter.hpp>
 #include <boost/filesystem/operations.hpp>
 #include <boost/filesystem/path.hpp>
+#include <boost/optional.hpp>
 
 #include <cmath>
 #include <fstream>
@@ -252,7 +253,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
   is.seekg(0, ios::beg);
 
   // If asked, create a sampler with random number generator
-  Sampler sampler;
+  std::unique_ptr<Sampler> sampler;
   if (addNoise) {
     noiseModel::Diagonal::shared_ptr noise;
     if (model)
@@ -261,7 +262,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
       throw invalid_argument(
           "gtsam::load2D: invalid noise model for adding noise"
               "(current version assumes diagonal noise model)!");
-    sampler = Sampler(noise);
+    sampler.reset(new Sampler(noise));
   }
 
   // Parse the pose constraints
@@ -289,7 +290,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
         model = modelInFile;
 
       if (addNoise)
-        l1Xl2 = l1Xl2.retract(sampler.sample());
+        l1Xl2 = l1Xl2.retract(sampler->sample());
 
       // Insert vertices if pure odometry file
       if (!initial->exists(id1))
@@ -441,11 +442,11 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
       auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
       if (!p) continue;
       const Pose3& pose = p->value();
-      Point3 t = pose.translation();
-      Rot3 R = pose.rotation();
-      stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " "  << t.y() << " " << t.z()
-        << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()
-        << " " << R.toQuaternion().w() << endl;
+      const Point3 t = pose.translation();
+      const auto q = pose.rotation().toQuaternion();
+      stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " "
+             << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " "
+             << q.z() << " " << q.w() << endl;
   }
 
   // save edges (2D or 3D)
@@ -485,13 +486,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
         throw invalid_argument("writeG2o: invalid noise model!");
       }
       Matrix Info = gaussianModel->R().transpose() * gaussianModel->R();
-      Pose3 pose3D = factor3D->measured();
-      Point3 p = pose3D.translation();
-      Rot3 R = pose3D.rotation();
-
-      stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2() << " "
-          << p.x() << " "  << p.y() << " " << p.z()  << " " << R.toQuaternion().x()
-          << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()  << " " << R.toQuaternion().w();
+      const Pose3 pose3D = factor3D->measured();
+      const Point3 p = pose3D.translation();
+      const auto q = pose3D.rotation().toQuaternion();
+      stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2()
+             << " " << p.x() << " " << p.y() << " " << p.z() << " " << q.x()
+             << " " << q.y() << " " << q.z() << " " << q.w();
 
       Matrix InfoG2o = I_6x6;
       InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation
@@ -510,6 +510,11 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
   stream.close();
 }
 
+/* ************************************************************************* */
+static Rot3 NormalizedRot3(double w, double x, double y, double z) {
+  const double norm = sqrt(w * w + x * x + y * y + z * z), f = 1.0 / norm;
+  return Rot3::Quaternion(f * w, f * x, f * y, f * z);
+}
 /* ************************************************************************* */
 std::map<Key, Pose3> parse3DPoses(const string& filename) {
   ifstream is(filename.c_str());
@@ -534,17 +539,24 @@ std::map<Key, Pose3> parse3DPoses(const string& filename) {
       Key id;
       double x, y, z, qx, qy, qz, qw;
       ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
-      poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}));
+      poses.emplace(id, Pose3(NormalizedRot3(qw, qx, qy, qz), {x, y, z}));
     }
   }
   return poses;
 }
 
 /* ************************************************************************* */
-BetweenFactorPose3s parse3DFactors(const string& filename) {
+BetweenFactorPose3s parse3DFactors(
+    const string& filename,
+    const noiseModel::Diagonal::shared_ptr& corruptingNoise) {
   ifstream is(filename.c_str());
   if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
 
+  boost::optional<Sampler> sampler;
+  if (corruptingNoise) {
+    sampler = Sampler(corruptingNoise);
+  }
+
   std::vector<BetweenFactor<Pose3>::shared_ptr> factors;
   while (!is.eof()) {
     char buf[LINESIZE];
@@ -585,8 +597,13 @@ BetweenFactorPose3s parse3DFactors(const string& filename) {
       mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonal
 
       SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
+      auto R12 = NormalizedRot3(qw, qx, qy, qz);
+      if (sampler) {
+        R12 = R12.retract(sampler->sample());
+      }
+
       factors.emplace_back(new BetweenFactor<Pose3>(
-          id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model));
+          id1, id2, Pose3(R12, {x, y, z}), model));
     }
   }
   return factors;
@@ -646,7 +663,7 @@ Pose3 gtsam2openGL(const Pose3& PoseGTSAM) {
 }
 
 /* ************************************************************************* */
-bool readBundler(const string& filename, SfM_data &data) {
+bool readBundler(const string& filename, SfmData &data) {
   // Load the data file
   ifstream is(filename.c_str(), ifstream::in);
   if (!is) {
@@ -697,7 +714,7 @@ bool readBundler(const string& filename, SfM_data &data) {
   // Get the information for the 3D points
   data.tracks.reserve(nrPoints);
   for (size_t j = 0; j < nrPoints; j++) {
-    SfM_Track track;
+    SfmTrack track;
 
     // Get the 3D position
     float x, y, z;
@@ -733,7 +750,7 @@ bool readBundler(const string& filename, SfM_data &data) {
 }
 
 /* ************************************************************************* */
-bool readBAL(const string& filename, SfM_data &data) {
+bool readBAL(const string& filename, SfmData &data) {
   // Load the data file
   ifstream is(filename.c_str(), ifstream::in);
   if (!is) {
@@ -781,7 +798,7 @@ bool readBAL(const string& filename, SfM_data &data) {
     // Get the 3D position
     float x, y, z;
     is >> x >> y >> z;
-    SfM_Track& track = data.tracks[j];
+    SfmTrack& track = data.tracks[j];
     track.p = Point3(x, y, z);
     track.r = 0.4f;
     track.g = 0.4f;
@@ -793,7 +810,7 @@ bool readBAL(const string& filename, SfM_data &data) {
 }
 
 /* ************************************************************************* */
-bool writeBAL(const string& filename, SfM_data &data) {
+bool writeBAL(const string& filename, SfmData &data) {
   // Open the output file
   ofstream os;
   os.open(filename.c_str());
@@ -815,7 +832,7 @@ bool writeBAL(const string& filename, SfM_data &data) {
   os << endl;
 
   for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
-    const SfM_Track& track = data.tracks[j];
+    const SfmTrack& track = data.tracks[j];
 
     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
@@ -866,12 +883,12 @@ bool writeBAL(const string& filename, SfM_data &data) {
   return true;
 }
 
-bool writeBALfromValues(const string& filename, const SfM_data &data,
+bool writeBALfromValues(const string& filename, const SfmData &data,
     Values& values) {
   using Camera = PinholeCamera<Cal3Bundler>;
-  SfM_data dataValues = data;
+  SfmData dataValues = data;
 
-  // Store poses or cameras in SfM_data
+  // Store poses or cameras in SfmData
   size_t nrPoses = values.count<Pose3>();
   if (nrPoses == dataValues.number_cameras()) { // we only estimated camera poses
     for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
@@ -899,7 +916,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data,
     }
   }
 
-  // Store 3D points in SfM_data
+  // Store 3D points in SfmData
   size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.number_tracks();
   if (nrPoints != nrTracks) {
     cout
@@ -921,24 +938,24 @@ bool writeBALfromValues(const string& filename, const SfM_data &data,
     }
   }
 
-  // Write SfM_data to file
+  // Write SfmData to file
   return writeBAL(filename, dataValues);
 }
 
-Values initialCamerasEstimate(const SfM_data& db) {
+Values initialCamerasEstimate(const SfmData& db) {
   Values initial;
   size_t i = 0; // NO POINTS:  j = 0;
-  for(const SfM_Camera& camera: db.cameras)
+  for(const SfmCamera& camera: db.cameras)
     initial.insert(i++, camera);
   return initial;
 }
 
-Values initialCamerasAndPointsEstimate(const SfM_data& db) {
+Values initialCamerasAndPointsEstimate(const SfmData& db) {
   Values initial;
   size_t i = 0, j = 0;
-  for(const SfM_Camera& camera: db.cameras)
+  for(const SfmCamera& camera: db.cameras)
     initial.insert((i++), camera);
-  for(const SfM_Track& track: db.tracks)
+  for(const SfmTrack& track: db.tracks)
     initial.insert(P(j++), track.p);
   return initial;
 }
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index 2106df48d..032799429 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -12,7 +12,9 @@
 /**
  * @file dataset.h
  * @date Jan 22, 2010
- * @author nikai, Luca Carlone
+ * @author Ni Kai
+ * @author Luca Carlone
+ * @author Varun Agrawal
  * @brief utility functions for loading datasets
  */
 
@@ -157,7 +159,8 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
 
 /// Parse edges in 3D TORO graph file into a set of BetweenFactors.
 using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>;
-GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename);
+GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename, 
+    const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr);
 
 /// Parse vertices in 3D TORO graph file into a map of Pose3s.
 GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
@@ -166,68 +169,86 @@ GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
 GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
 
 /// A measurement with its camera index
-typedef std::pair<size_t, Point2> SfM_Measurement;
+typedef std::pair<size_t, Point2> SfmMeasurement;
 
-/// SfM_Track
-typedef std::pair<size_t, size_t> SIFT_Index;
+/// SfmTrack
+typedef std::pair<size_t, size_t> SiftIndex;
 
 /// Define the structure for the 3D points
-struct SfM_Track {
-  SfM_Track():p(0,0,0) {}
+struct SfmTrack {
+  SfmTrack(): p(0,0,0) {}
   Point3 p; ///< 3D position of the point
   float r, g, b; ///< RGB color of the 3D point
-  std::vector<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v))
-  std::vector<SIFT_Index> siftIndices;
+  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();
   }
+  /// Get the measurement (camera index, Point2) at pose index `idx`
+  SfmMeasurement measurement(size_t idx) const {
+    return measurements[idx];
+  }
+  /// Get the SIFT feature index corresponding to the measurement at `idx`
+  SiftIndex siftIndex(size_t idx) const {
+    return siftIndices[idx];
+  }
 };
 
 /// Define the structure for the camera poses
-typedef PinholeCamera<Cal3Bundler> SfM_Camera;
+typedef PinholeCamera<Cal3Bundler> SfmCamera;
 
 /// Define the structure for SfM data
-struct SfM_data {
-  std::vector<SfM_Camera> cameras; ///< Set of cameras
-  std::vector<SfM_Track> tracks; ///< Sparse set of points
+struct SfmData {
+  std::vector<SfmCamera> cameras; ///< Set of cameras
+  std::vector<SfmTrack> tracks; ///< Sparse set of points
   size_t number_cameras() const {
     return cameras.size();
-  } ///< The number of camera poses
+  }
+  /// The number of reconstructed 3D points
   size_t number_tracks() const {
     return tracks.size();
-  } ///< The number of reconstructed 3D points
+  }
+  /// The camera pose at frame index `idx`
+  SfmCamera camera(size_t idx) const {
+    return cameras[idx];
+  }
+  /// The track formed by series of landmark measurements
+  SfmTrack track(size_t idx) const {
+    return tracks[idx];
+  }
 };
 
 /**
  * @brief This function parses a bundler output file and stores the data into a
- * SfM_data structure
+ * SfmData structure
  * @param filename The name of the bundler file
  * @param data SfM structure where the data is stored
  * @return true if the parsing was successful, false otherwise
  */
-GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
+GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData &data);
 
 /**
  * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
- * SfM_data structure
+ * SfmData structure
  * @param filename The name of the BAL file
  * @param data SfM structure where the data is stored
  * @return true if the parsing was successful, false otherwise
  */
-GTSAM_EXPORT bool readBAL(const std::string& filename, SfM_data &data);
+GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData &data);
 
 /**
  * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
- * SfM_data structure
+ * SfmData structure
  * @param filename The name of the BAL file to write
  * @param data SfM structure where the data is stored
  * @return true if the parsing was successful, false otherwise
  */
-GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
+GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData &data);
 
 /**
  * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
- * SfM_data structure and a value structure (measurements are the same as the SfM input data,
+ * SfmData structure and a value structure (measurements are the same as the SfM input data,
  * while camera poses and values are read from Values)
  * @param filename The name of the BAL file to write
  * @param data SfM structure where the data is stored
@@ -237,7 +258,7 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
  * @return true if the parsing was successful, false otherwise
  */
 GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
-    const SfM_data &data, Values& values);
+    const SfmData &data, Values& values);
 
 /**
  * @brief This function converts an openGL camera pose to an GTSAM camera pose
@@ -268,16 +289,25 @@ GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
 
 /**
  * @brief This function creates initial values for cameras from db
- * @param SfM_data
+ * @param SfmData
  * @return Values
  */
-GTSAM_EXPORT Values initialCamerasEstimate(const SfM_data& db);
+GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
 
 /**
  * @brief This function creates initial values for cameras and points from db
- * @param SfM_data
+ * @param SfmData
  * @return Values
  */
-GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db);
+GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
+
+/// Aliases for backwards compatibility
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+typedef SfmMeasurement SfM_Measurement;
+typedef SiftIndex SIFT_Index;
+typedef SfmTrack SfM_Track;
+typedef SfmCamera SfM_Camera;
+typedef SfmData SfM_data;
+#endif
 
 } // namespace gtsam
diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h
index 4294d17d1..d60923d8e 100644
--- a/gtsam/slam/expressions.h
+++ b/gtsam/slam/expressions.h
@@ -11,6 +11,7 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Line3.h>
 #include <gtsam/geometry/PinholeCamera.h>
 
 namespace gtsam {
@@ -31,6 +32,7 @@ typedef Expression<Point3> Point3_;
 typedef Expression<Unit3> Unit3_;
 typedef Expression<Rot3> Rot3_;
 typedef Expression<Pose3> Pose3_;
+typedef Expression<Line3> Line3_;
 
 inline Point3_ transformTo(const Pose3_& x, const Point3_& p) {
   return Point3_(x, &Pose3::transformTo, p);
@@ -40,6 +42,12 @@ inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) {
   return Point3_(x, &Pose3::transformFrom, p);
 }
 
+inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) {
+  Line3 (*f)(const Pose3 &, const Line3 &,
+             OptionalJacobian<4, 6>, OptionalJacobian<4, 4>) = &transformTo;
+  return Line3_(f, wTc, wL);
+}
+
 namespace internal {
 // define getter that returns value rather than reference
 inline Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) {
diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp
index 34c8385e8..76edc8b9d 100644
--- a/gtsam/slam/lago.cpp
+++ b/gtsam/slam/lago.cpp
@@ -17,7 +17,7 @@
  */
 
 #include <gtsam/slam/lago.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/geometry/Pose2.h>
diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp
index e34e13279..4188f5639 100644
--- a/gtsam/slam/tests/testAntiFactor.cpp
+++ b/gtsam/slam/tests/testAntiFactor.cpp
@@ -18,7 +18,6 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <gtsam/slam/AntiFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -90,7 +89,7 @@ TEST( AntiFactor, EquivalentBayesNet)
   SharedNoiseModel sigma(noiseModel::Unit::Create(6));
 
   NonlinearFactorGraph graph;
-  graph.emplace_shared<PriorFactor<Pose3> >(1, pose1, sigma);
+  graph.addPrior(1, pose1, sigma);
   graph.emplace_shared<BetweenFactor<Pose3> >(1, 2, pose1.between(pose2), sigma);
 
   // Create a configuration corresponding to the ground truth
diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp
index 08de83eb1..8088ab18a 100644
--- a/gtsam/slam/tests/testDataset.cpp
+++ b/gtsam/slam/tests/testDataset.cpp
@@ -106,61 +106,22 @@ TEST( dataSet, Balbianello)
 {
   ///< The structure where we will save the SfM data
   const string filename = findExampleDataFile("Balbianello");
-  SfM_data mydata;
+  SfmData mydata;
   CHECK(readBundler(filename, mydata));
 
   // Check number of things
   EXPECT_LONGS_EQUAL(5,mydata.number_cameras());
   EXPECT_LONGS_EQUAL(544,mydata.number_tracks());
-  const SfM_Track& track0 = mydata.tracks[0];
+  const SfmTrack& track0 = mydata.tracks[0];
   EXPECT_LONGS_EQUAL(3,track0.number_measurements());
 
   // Check projection of a given point
   EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
-  const SfM_Camera& camera0 = mydata.cameras[0];
+  const SfmCamera& camera0 = mydata.cameras[0];
   Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
   EXPECT(assert_equal(expected,actual,1));
 }
 
-/* ************************************************************************* */
-TEST( dataSet, readG2o)
-{
-  const string g2oFile = findExampleDataFile("pose2example");
-  NonlinearFactorGraph::shared_ptr actualGraph;
-  Values::shared_ptr actualValues;
-  boost::tie(actualGraph, actualValues) = readG2o(g2oFile);
-
-  Values expectedValues;
-  expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000));
-  expectedValues.insert(1, Pose2(1.030390, 0.011350, -0.081596));
-  expectedValues.insert(2, Pose2(2.036137, -0.129733, -0.301887));
-  expectedValues.insert(3, Pose2(3.015097, -0.442395, -0.345514));
-  expectedValues.insert(4, Pose2(3.343949, 0.506678, 1.214715));
-  expectedValues.insert(5, Pose2(3.684491, 1.464049, 1.183785));
-  expectedValues.insert(6, Pose2(4.064626, 2.414783, 1.176333));
-  expectedValues.insert(7, Pose2(4.429778, 3.300180, 1.259169));
-  expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391));
-  expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016));
-  expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934));
-  EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
-
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699));
-  NonlinearFactorGraph expectedGraph;
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(9,10, Pose2(1.003350, 0.022250, -0.195918), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3,10, Pose2(0.044020, 0.988477, -1.553511), model);
-  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
-}
-
 /* ************************************************************************* */
 TEST(dataSet, readG2o3D) {
   const string g2oFile = findExampleDataFile("pose3example");
@@ -273,59 +234,103 @@ TEST( dataSet, readG2o3DNonDiagonalNoise)
 }
 
 /* ************************************************************************* */
-TEST( dataSet, readG2oHuber)
-{
-  const string g2oFile = findExampleDataFile("pose2example");
-  NonlinearFactorGraph::shared_ptr actualGraph;
-  Values::shared_ptr actualValues;
-  bool is3D = false;
-  boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
+TEST(dataSet, readG2oCheckDeterminants) {
+  const string g2oFile = findExampleDataFile("toyExample.g2o");
 
-  noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699));
-  SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
+  // Check determinants in factors
+  auto factors = parse3DFactors(g2oFile);
+  EXPECT_LONGS_EQUAL(6, factors.size());
+  for (const auto& factor : factors) {
+    const Rot3 R = factor->measured().rotation();
+    EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9);
+  }
 
-  NonlinearFactorGraph expectedGraph;
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(9,10, Pose2(1.003350, 0.022250, -0.195918), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3,10, Pose2(0.044020, 0.988477, -1.553511), model);
-  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
+  // Check determinants in initial values
+  const map<Key, Pose3> poses = parse3DPoses(g2oFile);
+  EXPECT_LONGS_EQUAL(5, poses.size());
+  for (const auto& key_value : poses) {
+    const Rot3 R = key_value.second.rotation();
+    EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9);
+  }
 }
 
 /* ************************************************************************* */
-TEST( dataSet, readG2oTukey)
-{
+static NonlinearFactorGraph expectedGraph(const SharedNoiseModel& model) {
+  NonlinearFactorGraph g;
+  using Factor = BetweenFactor<Pose2>;
+  g.emplace_shared<Factor>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model);
+  g.emplace_shared<Factor>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
+  g.emplace_shared<Factor>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
+  g.emplace_shared<Factor>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
+  g.emplace_shared<Factor>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
+  g.emplace_shared<Factor>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
+  g.emplace_shared<Factor>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
+  g.emplace_shared<Factor>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
+  g.emplace_shared<Factor>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
+  g.emplace_shared<Factor>(9, 10, Pose2(1.003350, 0.022250, -0.195918), model);
+  g.emplace_shared<Factor>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
+  g.emplace_shared<Factor>(3, 10, Pose2(0.044020, 0.988477, -1.553511), model);
+  return g;
+}
+
+/* ************************************************************************* */
+TEST(dataSet, readG2o) {
+  const string g2oFile = findExampleDataFile("pose2example");
+  NonlinearFactorGraph::shared_ptr actualGraph;
+  Values::shared_ptr actualValues;
+  boost::tie(actualGraph, actualValues) = readG2o(g2oFile);
+
+  auto model = noiseModel::Diagonal::Precisions(
+      Vector3(44.721360, 44.721360, 30.901699));
+  EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5));
+
+  Values expectedValues;
+  expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000));
+  expectedValues.insert(1, Pose2(1.030390, 0.011350, -0.081596));
+  expectedValues.insert(2, Pose2(2.036137, -0.129733, -0.301887));
+  expectedValues.insert(3, Pose2(3.015097, -0.442395, -0.345514));
+  expectedValues.insert(4, Pose2(3.343949, 0.506678, 1.214715));
+  expectedValues.insert(5, Pose2(3.684491, 1.464049, 1.183785));
+  expectedValues.insert(6, Pose2(4.064626, 2.414783, 1.176333));
+  expectedValues.insert(7, Pose2(4.429778, 3.300180, 1.259169));
+  expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391));
+  expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016));
+  expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934));
+  EXPECT(assert_equal(expectedValues, *actualValues, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(dataSet, readG2oHuber) {
   const string g2oFile = findExampleDataFile("pose2example");
   NonlinearFactorGraph::shared_ptr actualGraph;
   Values::shared_ptr actualValues;
   bool is3D = false;
-  boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
+  boost::tie(actualGraph, actualValues) =
+      readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
 
-  noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699));
-  SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
+  auto baseModel = noiseModel::Diagonal::Precisions(
+      Vector3(44.721360, 44.721360, 30.901699));
+  auto model = noiseModel::Robust::Create(
+      noiseModel::mEstimator::Huber::Create(1.345), baseModel);
 
-  NonlinearFactorGraph expectedGraph;
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(9,10, Pose2(1.003350, 0.022250, -0.195918), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
-  expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3,10, Pose2(0.044020, 0.988477, -1.553511), model);
-  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
+  EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(dataSet, readG2oTukey) {
+  const string g2oFile = findExampleDataFile("pose2example");
+  NonlinearFactorGraph::shared_ptr actualGraph;
+  Values::shared_ptr actualValues;
+  bool is3D = false;
+  boost::tie(actualGraph, actualValues) =
+      readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
+
+  auto baseModel = noiseModel::Diagonal::Precisions(
+      Vector3(44.721360, 44.721360, 30.901699));
+  auto model = noiseModel::Robust::Create(
+      noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
+
+  EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5));
 }
 
 /* ************************************************************************* */
@@ -389,18 +394,18 @@ TEST( dataSet, readBAL_Dubrovnik)
 {
   ///< The structure where we will save the SfM data
   const string filename = findExampleDataFile("dubrovnik-3-7-pre");
-  SfM_data mydata;
+  SfmData mydata;
   CHECK(readBAL(filename, mydata));
 
   // Check number of things
   EXPECT_LONGS_EQUAL(3,mydata.number_cameras());
   EXPECT_LONGS_EQUAL(7,mydata.number_tracks());
-  const SfM_Track& track0 = mydata.tracks[0];
+  const SfmTrack& track0 = mydata.tracks[0];
   EXPECT_LONGS_EQUAL(3,track0.number_measurements());
 
   // Check projection of a given point
   EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
-  const SfM_Camera& camera0 = mydata.cameras[0];
+  const SfmCamera& camera0 = mydata.cameras[0];
   Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
   EXPECT(assert_equal(expected,actual,12));
 }
@@ -444,7 +449,7 @@ TEST( dataSet, writeBAL_Dubrovnik)
 {
   ///< Read a file using the unit tested readBAL
   const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
-  SfM_data readData;
+  SfmData readData;
   readBAL(filenameToRead, readData);
 
   // Write readData to file filenameToWrite
@@ -452,7 +457,7 @@ TEST( dataSet, writeBAL_Dubrovnik)
   CHECK(writeBAL(filenameToWrite, readData));
 
   // Read what we wrote
-  SfM_data writtenData;
+  SfmData writtenData;
   CHECK(readBAL(filenameToWrite, writtenData));
 
   // Check that what we read is the same as what we wrote
@@ -467,8 +472,8 @@ TEST( dataSet, writeBAL_Dubrovnik)
 
   for (size_t j = 0; j < readData.number_tracks(); j++){
     // check point
-    SfM_Track expectedTrack  = writtenData.tracks[j];
-    SfM_Track actualTrack = readData.tracks[j];
+    SfmTrack expectedTrack  = writtenData.tracks[j];
+    SfmTrack actualTrack = readData.tracks[j];
     Point3 expectedPoint = expectedTrack.p;
     Point3 actualPoint = actualTrack.p;
     EXPECT(assert_equal(expectedPoint,actualPoint));
@@ -492,10 +497,10 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
 
   ///< Read a file using the unit tested readBAL
   const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
-  SfM_data readData;
+  SfmData readData;
   readBAL(filenameToRead, readData);
 
-  Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3));
+  Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.3,0.1,0.3));
 
   Values value;
   for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera
@@ -514,19 +519,19 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
   writeBALfromValues(filenameToWrite, readData, value);
 
   // Read the file we wrote
-  SfM_data writtenData;
+  SfmData writtenData;
   readBAL(filenameToWrite, writtenData);
 
   // Check that the reprojection errors are the same and the poses are correct
   // Check number of things
   EXPECT_LONGS_EQUAL(3,writtenData.number_cameras());
   EXPECT_LONGS_EQUAL(7,writtenData.number_tracks());
-  const SfM_Track& track0 = writtenData.tracks[0];
+  const SfmTrack& track0 = writtenData.tracks[0];
   EXPECT_LONGS_EQUAL(3,track0.number_measurements());
 
   // Check projection of a given point
   EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
-  const SfM_Camera& camera0 = writtenData.cameras[0];
+  const SfmCamera& camera0 = writtenData.cameras[0];
   Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
   EXPECT(assert_equal(expected,actual,12));
 
diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
index b2d368b67..95db611d7 100644
--- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp
+++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
@@ -13,6 +13,7 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
@@ -34,7 +35,7 @@ gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse();
 namespace example1 {
 
 const string filename = findExampleDataFile("5pointExample1.txt");
-SfM_data data;
+SfmData data;
 bool readOK = readBAL(filename, data);
 Rot3 c1Rc2 = data.cameras[1].pose().rotation();
 Point3 c1Tc2 = data.cameras[1].pose().translation();
@@ -357,7 +358,7 @@ TEST (EssentialMatrixFactor3, minimization) {
 namespace example2 {
 
 const string filename = findExampleDataFile("5pointExample2.txt");
-SfM_data data;
+SfmData data;
 bool readOK = readBAL(filename, data);
 Rot3 aRb = data.cameras[1].pose().rotation();
 Point3 aTb = data.cameras[1].pose().translation();
diff --git a/gtsam/slam/tests/testFrobeniusFactor.cpp b/gtsam/slam/tests/testFrobeniusFactor.cpp
new file mode 100644
index 000000000..9cb0c19fa
--- /dev/null
+++ b/gtsam/slam/tests/testFrobeniusFactor.cpp
@@ -0,0 +1,244 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, 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
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * testFrobeniusFactor.cpp
+ *
+ * @file   testFrobeniusFactor.cpp
+ * @date   March 2019
+ * @author Frank Dellaert
+ * @brief  Check evaluateError for various Frobenius norm
+ */
+
+#include <gtsam/base/lieProxies.h>
+#include <gtsam/base/testLie.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/SO3.h>
+#include <gtsam/geometry/SO4.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/slam/FrobeniusFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+//******************************************************************************
+namespace so3 {
+SO3 id;
+Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished();
+SO3 R1 = SO3::Expmap(v1);
+Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished();
+SO3 R2 = SO3::Expmap(v2);
+SO3 R12 = R1.between(R2);
+}  // namespace so3
+
+/* ************************************************************************* */
+TEST(FrobeniusPriorSO3, evaluateError) {
+  using namespace ::so3;
+  auto factor = FrobeniusPrior<SO3>(1, R2.matrix());
+  Vector actual = factor.evaluateError(R1);
+  Vector expected = R1.vec() - R2.vec();
+  EXPECT(assert_equal(expected, actual, 1e-9));
+
+  Values values;
+  values.insert(1, R1);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(FrobeniusPriorSO3, ClosestTo) {
+  // Example top-left of SO(4) matrix not quite on SO(3) manifold
+  Matrix3 M;
+  M << 0.79067393, 0.6051136, -0.0930814,   //
+      0.4155925, -0.64214347, -0.64324489,  //
+      -0.44948549, 0.47046326, -0.75917576;
+
+  SO3 expected = SO3::ClosestTo(M);
+
+  // manifold optimization gets same result as SVD solution in ClosestTo
+  NonlinearFactorGraph graph;
+  graph.emplace_shared<FrobeniusPrior<SO3> >(1, M);
+
+  Values initial;
+  initial.insert(1, SO3(I_3x3));
+  auto result = GaussNewtonOptimizer(graph, initial).optimize();
+  EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-6);
+  EXPECT(assert_equal(expected, result.at<SO3>(1), 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(FrobeniusPriorSO3, ChordalL2mean) {
+  // See Hartley13ijcv:
+  // Cost function C(R) = \sum FrobeniusPrior(R,R_i)
+  // Closed form solution = ClosestTo(C_e), where
+  // C_e = \sum R_i !!!!
+
+  // We will test by computing mean of R1=exp(v1) R1^T=exp(-v1):
+  using namespace ::so3;
+  SO3 expected;  // identity
+  Matrix3 M = R1.matrix() + R1.matrix().transpose();
+  EXPECT(assert_equal(expected, SO3::ClosestTo(M), 1e-6));
+  EXPECT(assert_equal(expected, SO3::ChordalMean({R1, R1.inverse()}), 1e-6));
+
+  // manifold optimization gets same result as ChordalMean
+  NonlinearFactorGraph graph;
+  graph.emplace_shared<FrobeniusPrior<SO3> >(1, R1.matrix());
+  graph.emplace_shared<FrobeniusPrior<SO3> >(1, R1.matrix().transpose());
+
+  Values initial;
+  initial.insert<SO3>(1, R1.inverse());
+  auto result = GaussNewtonOptimizer(graph, initial).optimize();
+  EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 0.1);  // Why so loose?
+  EXPECT(assert_equal(expected, result.at<SO3>(1), 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(FrobeniusFactorSO3, evaluateError) {
+  using namespace ::so3;
+  auto factor = FrobeniusFactor<SO3>(1, 2);
+  Vector actual = factor.evaluateError(R1, R2);
+  Vector expected = R2.vec() - R1.vec();
+  EXPECT(assert_equal(expected, actual, 1e-9));
+
+  Values values;
+  values.insert(1, R1);
+  values.insert(2, R2);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+// Commented out as SO(n) not yet supported (and might never be)
+// TEST(FrobeniusBetweenFactorSOn, evaluateError) {
+//   using namespace ::so3;
+//   auto factor =
+//       FrobeniusBetweenFactor<SOn>(1, 2, SOn::FromMatrix(R12.matrix()));
+//   Vector actual = factor.evaluateError(SOn::FromMatrix(R1.matrix()),
+//                                        SOn::FromMatrix(R2.matrix()));
+//   Vector expected = Vector9::Zero();
+//   EXPECT(assert_equal(expected, actual, 1e-9));
+
+//   Values values;
+//   values.insert(1, R1);
+//   values.insert(2, R2);
+//   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+// }
+
+/* ************************************************************************* */
+TEST(FrobeniusBetweenFactorSO3, evaluateError) {
+  using namespace ::so3;
+  auto factor = FrobeniusBetweenFactor<SO3>(1, 2, R12);
+  Vector actual = factor.evaluateError(R1, R2);
+  Vector expected = Vector9::Zero();
+  EXPECT(assert_equal(expected, actual, 1e-9));
+
+  Values values;
+  values.insert(1, R1);
+  values.insert(2, R2);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+//******************************************************************************
+namespace so4 {
+SO4 id;
+Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished();
+SO4 Q1 = SO4::Expmap(v1);
+Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished();
+SO4 Q2 = SO4::Expmap(v2);
+}  // namespace so4
+
+/* ************************************************************************* */
+TEST(FrobeniusFactorSO4, evaluateError) {
+  using namespace ::so4;
+  auto factor = FrobeniusFactor<SO4>(1, 2, noiseModel::Unit::Create(6));
+  Vector actual = factor.evaluateError(Q1, Q2);
+  Vector expected = Q2.vec() - Q1.vec();
+  EXPECT(assert_equal(expected, actual, 1e-9));
+
+  Values values;
+  values.insert(1, Q1);
+  values.insert(2, Q2);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(FrobeniusBetweenFactorSO4, evaluateError) {
+  using namespace ::so4;
+  Matrix4 M{I_4x4};
+  M.topLeftCorner<3, 3>() = ::so3::R12.matrix();
+  auto factor = FrobeniusBetweenFactor<SO4>(1, 2, Q1.between(Q2));
+  Matrix H1, H2;
+  Vector actual = factor.evaluateError(Q1, Q2, H1, H2);
+  Vector expected = SO4::VectorN2::Zero();
+  EXPECT(assert_equal(expected, actual, 1e-9));
+
+  Values values;
+  values.insert(1, Q1);
+  values.insert(2, Q2);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+//******************************************************************************
+namespace submanifold {
+SO4 id;
+Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
+SO3 R1 = SO3::Expmap(v1.tail<3>());
+SO4 Q1 = SO4::Expmap(v1);
+Vector6 v2 = (Vector(6) << 0, 0, 0, 0.01, 0.02, 0.03).finished();
+SO3 R2 = SO3::Expmap(v2.tail<3>());
+SO4 Q2 = SO4::Expmap(v2);
+SO3 R12 = R1.between(R2);
+}  // namespace submanifold
+
+/* ************************************************************************* */
+TEST(FrobeniusWormholeFactor, evaluateError) {
+  auto model = noiseModel::Isotropic::Sigma(6, 1.2);  // dimension = 6 not 16
+  for (const size_t p : {5, 4, 3}) {
+    Matrix M = Matrix::Identity(p, p);
+    M.topLeftCorner(3, 3) = submanifold::R1.matrix();
+    SOn Q1(M);
+    M.topLeftCorner(3, 3) = submanifold::R2.matrix();
+    SOn Q2(M);
+    auto factor =
+        FrobeniusWormholeFactor(1, 2, Rot3(::so3::R12.matrix()), p, model);
+    Matrix H1, H2;
+    factor.evaluateError(Q1, Q2, H1, H2);
+
+    // Test derivatives
+    Values values;
+    values.insert(1, Q1);
+    values.insert(2, Q2);
+    EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+  }
+}
+
+/* ************************************************************************* */
+TEST(FrobeniusWormholeFactor, equivalenceToSO3) {
+  using namespace ::submanifold;
+  auto R12 = ::so3::R12.retract(Vector3(0.1, 0.2, -0.1));
+  auto model = noiseModel::Isotropic::Sigma(6, 1.2);  // wrong dimension
+  auto factor3 = FrobeniusBetweenFactor<SO3>(1, 2, R12, model);
+  auto factor4 = FrobeniusWormholeFactor(1, 2, Rot3(R12.matrix()), 4, model);
+  const Matrix3 E3(factor3.evaluateError(R1, R2).data());
+  const Matrix43 E4(
+      factor4.evaluateError(SOn(Q1.matrix()), SOn(Q2.matrix())).data());
+  EXPECT(assert_equal((Matrix)E4.topLeftCorner<3, 3>(), E3, 1e-9));
+  EXPECT(assert_equal((Matrix)E4.row(3), Matrix13::Zero(), 1e-9));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp
index 8e0b5ad8f..e19a41b8d 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp
@@ -18,7 +18,6 @@
 
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/sam/RangeFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/geometry/PinholeCamera.h>
@@ -388,8 +387,7 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
   graph.emplace_shared<
       RangeFactor<GeneralCamera, Pose3> >(X(0), X(1), 2.,
           noiseModel::Isotropic::Sigma(1, 1.));
-  graph.emplace_shared<
-      PriorFactor<Pose3> >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
+  graph.addPrior(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
           noiseModel::Isotropic::Sigma(6, 1.));
 
   Values init;
@@ -413,14 +411,12 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
 TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
   // Tests range factor between a CalibratedCamera and a Pose3
   NonlinearFactorGraph graph;
-  graph.emplace_shared<
-      PriorFactor<CalibratedCamera> >(X(0), CalibratedCamera(),
+  graph.addPrior(X(0), CalibratedCamera(),
           noiseModel::Isotropic::Sigma(6, 1.));
   graph.emplace_shared<
       RangeFactor<CalibratedCamera, Pose3> >(X(0), X(1), 2.,
           noiseModel::Isotropic::Sigma(1, 1.));
-  graph.emplace_shared<
-      PriorFactor<Pose3> >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
+  graph.addPrior(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
           noiseModel::Isotropic::Sigma(6, 1.));
 
   Values init;
diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp
index 6fe8b3d7c..ba41cdc9b 100644
--- a/gtsam/slam/tests/testInitializePose3.cpp
+++ b/gtsam/slam/tests/testInitializePose3.cpp
@@ -21,7 +21,6 @@
 #include <gtsam/slam/InitializePose3.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/geometry/Pose3.h>
 #include <CppUnitLite/TestHarness.h>
@@ -67,7 +66,7 @@ NonlinearFactorGraph graph() {
   g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), model));
   g.add(BetweenFactor<Pose3>(x2, x0, pose2.between(pose0), model));
   g.add(BetweenFactor<Pose3>(x0, x3, pose0.between(pose3), model));
-  g.add(PriorFactor<Pose3>(x0, pose0, model));
+  g.addPrior(x0, pose0, model);
   return g;
 }
 
@@ -78,7 +77,7 @@ NonlinearFactorGraph graph2() {
   g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0)));
   g.add(BetweenFactor<Pose3>(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information
   g.add(BetweenFactor<Pose3>(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin
-  g.add(PriorFactor<Pose3>(x0, pose0, model));
+  g.addPrior(x0, pose0, model);
   return g;
 }
 }
@@ -267,7 +266,7 @@ TEST( InitializePose3, initializePoses )
   bool is3D = true;
   boost::tie(inputGraph, expectedValues) = readG2o(g2oFile, is3D);
   noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
-  inputGraph->add(PriorFactor<Pose3>(0, Pose3(), priorModel));
+  inputGraph->addPrior(0, Pose3(), priorModel);
 
   Values initial = InitializePose3::initialize(*inputGraph);
   EXPECT(assert_equal(*expectedValues,initial,0.1));  // TODO(frank): very loose !!
diff --git a/gtsam/slam/tests/testKarcherMeanFactor.cpp b/gtsam/slam/tests/testKarcherMeanFactor.cpp
new file mode 100644
index 000000000..c129b8fa3
--- /dev/null
+++ b/gtsam/slam/tests/testKarcherMeanFactor.cpp
@@ -0,0 +1,111 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testKarcherMeanFactor.cpp
+ * @author Frank Dellaert
+ */
+
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/SO4.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/KarcherMeanFactor-inl.h>
+#include <gtsam/slam/KarcherMeanFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+// Rot3 version
+/* ************************************************************************* */
+static const Rot3 R = Rot3::Expmap(Vector3(0.1, 0, 0));
+
+/* ************************************************************************* */
+// Check that optimizing for Karcher mean (which minimizes Between distance)
+// gets correct result.
+TEST(KarcherMean, FindRot3) {
+  std::vector<Rot3> rotations = {R, R.inverse()};
+  Rot3 expected;
+  auto actual = FindKarcherMean(rotations);
+  EXPECT(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+// Check that the InnerConstraint factor leaves the mean unchanged.
+TEST(KarcherMean, FactorRot3) {
+  // Make a graph with two variables, one between, and one InnerConstraint
+  // The optimal result should satisfy the between, while moving the other
+  // variable to make the mean the same as before.
+  // Mean of R and R' is identity. Let's make a BetweenFactor making R21 =
+  // R*R*R, i.e. geodesic length is 3 rather than 2.
+  NonlinearFactorGraph graph;
+  graph.emplace_shared<BetweenFactor<Rot3>>(1, 2, R * R * R);
+  std::vector<Key> keys{1, 2};
+  graph.emplace_shared<KarcherMeanFactor<Rot3>>(keys);
+
+  Values initial;
+  initial.insert<Rot3>(1, R.inverse());
+  initial.insert<Rot3>(2, R);
+  const auto expected = FindKarcherMean<Rot3>({R, R.inverse()});
+
+  auto result = GaussNewtonOptimizer(graph, initial).optimize();
+  const auto actual =
+      FindKarcherMean<Rot3>({result.at<Rot3>(1), result.at<Rot3>(2)});
+  EXPECT(assert_equal(expected, actual));
+  EXPECT(
+      assert_equal(R * R * R, result.at<Rot3>(1).between(result.at<Rot3>(2))));
+}
+
+/* ************************************************************************* */
+// SO(4) version
+/* ************************************************************************* */
+static const SO4 Q = SO4::Expmap((Vector6() << 1, 2, 3, 4, 5, 6).finished());
+
+/* ************************************************************************* */
+TEST(KarcherMean, FindSO4) {
+  std::vector<SO4, Eigen::aligned_allocator<SO4>> rotations = {Q, Q.inverse()};
+  auto expected = SO4();  //::ChordalMean(rotations);
+  auto actual = FindKarcherMean(rotations);
+  EXPECT(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST(KarcherMean, FactorSO4) {
+  NonlinearFactorGraph graph;
+  graph.emplace_shared<BetweenFactor<SO4>>(1, 2, Q * Q * Q);
+  std::vector<Key> keys{1, 2};
+  graph.emplace_shared<KarcherMeanFactor<SO4>>(keys);
+
+  Values initial;
+  initial.insert<SO4>(1, Q.inverse());
+  initial.insert<SO4>(2, Q);
+
+  std::vector<SO4, Eigen::aligned_allocator<SO4> > rotations = {Q, Q.inverse()};
+  const auto expected = FindKarcherMean<SO4>(rotations);
+
+  auto result = GaussNewtonOptimizer(graph, initial).optimize();
+  const auto actual =
+      FindKarcherMean<SO4>({result.at<SO4>(1), result.at<SO4>(2)});
+  EXPECT(assert_equal(expected, actual));
+  EXPECT(assert_equal((Matrix)(Q * Q * Q).matrix(),
+                      result.at<SO4>(1).between(result.at<SO4>(2)).matrix()));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp
index f8157c116..55449d86e 100644
--- a/gtsam/slam/tests/testLago.cpp
+++ b/gtsam/slam/tests/testLago.cpp
@@ -22,7 +22,6 @@
 #include <gtsam/slam/lago.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/inference/Symbol.h>
 
@@ -61,7 +60,7 @@ NonlinearFactorGraph graph() {
   g.add(BetweenFactor<Pose2>(x2, x3, pose2.between(pose3), model));
   g.add(BetweenFactor<Pose2>(x2, x0, pose2.between(pose0), model));
   g.add(BetweenFactor<Pose2>(x0, x3, pose0.between(pose3), model));
-  g.add(PriorFactor<Pose2>(x0, pose0, model));
+  g.addPrior(x0, pose0, model);
   return g;
 }
 }
@@ -167,7 +166,7 @@ TEST( Lago, smallGraphVectorValuesSP ) {
 TEST( Lago, multiplePosePriors ) {
   bool useOdometricPath = false;
   NonlinearFactorGraph g = simpleLago::graph();
-  g.add(PriorFactor<Pose2>(x1, simpleLago::pose1, model));
+  g.addPrior(x1, simpleLago::pose1, model);
   VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@@ -180,7 +179,7 @@ TEST( Lago, multiplePosePriors ) {
 /* *************************************************************************** */
 TEST( Lago, multiplePosePriorsSP ) {
   NonlinearFactorGraph g = simpleLago::graph();
-  g.add(PriorFactor<Pose2>(x1, simpleLago::pose1, model));
+  g.addPrior(x1, simpleLago::pose1, model);
   VectorValues initial = lago::initializeOrientations(g);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@@ -194,7 +193,7 @@ TEST( Lago, multiplePosePriorsSP ) {
 TEST( Lago, multiplePoseAndRotPriors ) {
   bool useOdometricPath = false;
   NonlinearFactorGraph g = simpleLago::graph();
-  g.add(PriorFactor<Rot2>(x1, simpleLago::pose1.theta(), model));
+  g.addPrior(x1, simpleLago::pose1.theta(), model);
   VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@@ -207,7 +206,7 @@ TEST( Lago, multiplePoseAndRotPriors ) {
 /* *************************************************************************** */
 TEST( Lago, multiplePoseAndRotPriorsSP ) {
   NonlinearFactorGraph g = simpleLago::graph();
-  g.add(PriorFactor<Rot2>(x1, simpleLago::pose1.theta(), model));
+  g.addPrior(x1, simpleLago::pose1.theta(), model);
   VectorValues initial = lago::initializeOrientations(g);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@@ -267,7 +266,7 @@ TEST( Lago, largeGraphNoisy_orientations ) {
   // Add prior on the pose having index (key) = 0
   NonlinearFactorGraph graphWithPrior = *g;
   noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(Vector3(1e-2, 1e-2, 1e-4));
-  graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+  graphWithPrior.addPrior(0, Pose2(), priorModel);
 
   VectorValues actualVV = lago::initializeOrientations(graphWithPrior);
   Values actual;
@@ -302,7 +301,7 @@ TEST( Lago, largeGraphNoisy ) {
   // Add prior on the pose having index (key) = 0
   NonlinearFactorGraph graphWithPrior = *g;
   noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(Vector3(1e-2, 1e-2, 1e-4));
-  graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+  graphWithPrior.addPrior(0, Pose2(), priorModel);
 
   Values actual = lago::initialize(graphWithPrior);
 
diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp
index ea42a1ecd..81f67f1ee 100644
--- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp
+++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp
@@ -17,7 +17,6 @@
  */
 
 #include <gtsam/slam/OrientedPlane3Factor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/base/numericalDerivative.h>
@@ -49,10 +48,9 @@ TEST (OrientedPlane3Factor, lm_translation_error) {
   Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
   Vector sigmas(6);
   sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001;
-  PriorFactor<Pose3> pose_prior(init_sym, init_pose,
-      noiseModel::Diagonal::Sigmas(sigmas));
+  new_graph.addPrior(
+      init_sym, init_pose, noiseModel::Diagonal::Sigmas(sigmas));
   new_values.insert(init_sym, init_pose);
-  new_graph.add(pose_prior);
 
   // Add two landmark measurements, differing in range
   new_values.insert(lm_sym, test_lm0);
@@ -94,11 +92,10 @@ TEST (OrientedPlane3Factor, lm_rotation_error) {
   // Init pose and prior.  Pose Prior is needed since a single plane measurement does not fully constrain the pose
   Symbol init_sym('x', 0);
   Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
-  PriorFactor<Pose3> pose_prior(init_sym, init_pose,
+  new_graph.addPrior(init_sym, init_pose,
       noiseModel::Diagonal::Sigmas(
           (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished()));
   new_values.insert(init_sym, init_pose);
-  new_graph.add(pose_prior);
 
 //  // Add two landmark measurements, differing in angle
   new_values.insert(lm_sym, test_lm0);
@@ -175,7 +172,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
 
 /* ************************************************************************* */
 int main() {
-  srand(time(NULL));
+  srand(time(nullptr));
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }
diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp
index b26d633f5..2dc083cb2 100644
--- a/gtsam/slam/tests/testPriorFactor.cpp
+++ b/gtsam/slam/tests/testPriorFactor.cpp
@@ -6,7 +6,7 @@
  */
 
 #include <gtsam/base/Vector.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
index b85dd891a..09ee61c97 100644
--- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
+++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
@@ -41,7 +41,7 @@ using namespace gtsam;
 const Matrix26 F0 = Matrix26::Ones();
 const Matrix26 F1 = 2 * Matrix26::Ones();
 const Matrix26 F3 = 3 * Matrix26::Ones();
-const vector<Matrix26, Eigen::aligned_allocator<Matrix26> > FBlocks = list_of<Matrix26>(F0)(F1)(F3);
+const vector<Matrix26, Eigen::aligned_allocator<Matrix26> > FBlocks {F0, F1, F3};
 const KeyVector keys {0, 1, 3};
 // RHS and sigmas
 const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();
diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp
index f69f4c113..fd771f102 100644
--- a/gtsam/slam/tests/testSmartFactorBase.cpp
+++ b/gtsam/slam/tests/testSmartFactorBase.cpp
@@ -37,11 +37,11 @@ class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
 public:
   typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
   PinholeFactor() {}
-  PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
-  }
-  virtual double error(const Values& values) const {
-    return 0.0;
-  }
+  PinholeFactor(const SharedNoiseModel& sharedNoiseModel,
+                boost::optional<Pose3> body_P_sensor = boost::none,
+                size_t expectedNumberCameras = 10)
+      : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {}
+  virtual double error(const Values& values) const { return 0.0; }
   virtual boost::shared_ptr<GaussianFactor> linearize(
       const Values& values) const {
     return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
@@ -60,6 +60,40 @@ TEST(SmartFactorBase, Pinhole) {
   EXPECT_LONGS_EQUAL(2 * 2, f.dim());
 }
 
+TEST(SmartFactorBase, PinholeWithSensor) {
+  Pose3 body_P_sensor(Rot3(), Point3(1, 0, 0));
+  PinholeFactor f = PinholeFactor(unit2, body_P_sensor);
+  EXPECT(assert_equal<Pose3>(f.body_P_sensor(), body_P_sensor));
+
+  PinholeFactor::Cameras cameras;
+  // Assume body at origin.
+  Pose3 world_P_body = Pose3();
+  // Camera coordinates in world frame.
+  Pose3 wTc = world_P_body * body_P_sensor;
+  cameras.push_back(PinholeCamera<Cal3Bundler>(wTc));
+  
+  // Simple point to project slightly off image center
+  Point3 p(0, 0, 10);
+  Point2 measurement = cameras[0].project(p);
+  f.add(measurement, 1);
+
+  PinholeFactor::Cameras::FBlocks Fs;
+  Matrix E;
+  Vector error = f.unwhitenedError<Point3>(cameras, p, Fs, E);
+
+  Vector expectedError = Vector::Zero(2);  
+  Matrix29 expectedFs;
+  expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1, 0, 0, 0, 0;
+  Matrix23 expectedE;
+  expectedE << 0.1, 0, 0.01, 0, 0.1, 0;
+
+  EXPECT(assert_equal(error, expectedError));
+  // We only have the jacobian for the 1 camera
+  // Use of a lower tolerance value due to compiler precision mismatch.
+  EXPECT(assert_equal(expectedFs, Fs[0], 1e-3));
+  EXPECT(assert_equal(expectedE, E));
+}
+
 /* ************************************************************************* */
 #include <gtsam/geometry/StereoCamera.h>
 
diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp
index c6d1a5b2c..1fd06cc9f 100644
--- a/gtsam/slam/tests/testSmartProjectionFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp
@@ -195,8 +195,8 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimize ) {
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
-  graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
-  graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
+  graph.addPrior(c1, cam1, noisePrior);
+  graph.addPrior(c2, cam2, noisePrior);
 
   // Create initial estimate
   Values initial;
@@ -281,14 +281,14 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
 
   KeyVector views {c1, c2, c3};
 
-  SfM_Track track1;
+  SfmTrack track1;
   for (size_t i = 0; i < 3; ++i) {
     track1.measurements.emplace_back(i + 1, measurements_cam1.at(i));
   }
   SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
   smartFactor1->add(track1);
 
-  SfM_Track track2;
+  SfmTrack track2;
   for (size_t i = 0; i < 3; ++i) {
     track2.measurements.emplace_back(i + 1, measurements_cam2.at(i));
   }
@@ -304,8 +304,8 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
-  graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
+  graph.addPrior(c1, cam1, noisePrior);
+  graph.addPrior(c2, cam2, noisePrior);
 
   Values values;
   values.insert(c1, cam1);
@@ -378,8 +378,8 @@ TEST(SmartProjectionFactor, perturbCamerasAndOptimize ) {
   graph.push_back(smartFactor3);
   graph.push_back(smartFactor4);
   graph.push_back(smartFactor5);
-  graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
-  graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
+  graph.addPrior(c1, cam1, noisePrior);
+  graph.addPrior(c2, cam2, noisePrior);
 
   Values values;
   values.insert(c1, cam1);
@@ -453,8 +453,8 @@ TEST(SmartProjectionFactor, Cal3Bundler ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
-  graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
+  graph.addPrior(c1, cam1, noisePrior);
+  graph.addPrior(c2, cam2, noisePrior);
 
   Values values;
   values.insert(c1, cam1);
@@ -526,8 +526,8 @@ TEST(SmartProjectionFactor, Cal3Bundler2 ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
-  graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
+  graph.addPrior(c1, cam1, noisePrior);
+  graph.addPrior(c2, cam2, noisePrior);
 
   Values values;
   values.insert(c1, cam1);
diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
index f833941bc..0cc5e8f55 100644
--- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
@@ -159,7 +159,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
   factor.computeJacobians(Fs, E, b, cameras, *point);
   double actualError3 = b.squaredNorm();
   EXPECT(assert_equal(expectedE, E, 1e-7));
-  EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8);
+  EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6);
 }
 
 /* *************************************************************************/
@@ -243,8 +243,8 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, wTb1, noisePrior);
-  graph.emplace_shared<PriorFactor<Pose3> >(x2, wTb2, noisePrior);
+  graph.addPrior(x1, wTb1, noisePrior);
+  graph.addPrior(x2, wTb2, noisePrior);
 
   // Check errors at ground truth poses
   Values gtValues;
@@ -302,8 +302,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
-  graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
+  graph.addPrior(x1, cam1.pose(), noisePrior);
+  graph.addPrior(x2, cam2.pose(), noisePrior);
 
   Values groundTruth;
   groundTruth.insert(x1, cam1.pose());
@@ -329,7 +329,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
   result = optimizer.optimize();
-  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-8));
+  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
 }
 
 /* *************************************************************************/
@@ -407,10 +407,10 @@ TEST( SmartProjectionPoseFactor, Factors ) {
 
     boost::shared_ptr<RegularHessianFactor<6> > actual =
         smartFactor1->createHessianFactor(cameras, 0.0);
-    EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8));
-    EXPECT(assert_equal(expected, *actual, 1e-8));
-    EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8);
-    EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8);
+    EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
+    EXPECT(assert_equal(expected, *actual, 1e-6));
+    EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
+    EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
   }
 
   {
@@ -448,15 +448,15 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
     Matrix3 P = (E.transpose() * E).inverse();
     JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n);
-    EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8));
+    EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6));
 
     boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
         smartFactor1->createJacobianQFactor(cameras, 0.0);
     CHECK(actualQ);
-    EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-8));
+    EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6));
     EXPECT(assert_equal(expectedQ, *actualQ));
-    EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-8);
-    EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-8);
+    EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-6);
+    EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-6);
 
     // Whiten for RegularImplicitSchurFactor (does not have noise model)
     model->WhitenSystem(E, b);
@@ -470,11 +470,11 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     boost::shared_ptr<RegularImplicitSchurFactor<Camera> > actual =
         smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
     CHECK(actual);
-    EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8));
-    EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8));
+    EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6));
+    EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
     EXPECT(assert_equal(expected, *actual));
-    EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8);
-    EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8);
+    EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
+    EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
   }
 
   {
@@ -484,15 +484,15 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     double s = sigma * sin(M_PI_4);
     SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma);
     JacobianFactor expected(x1, s * A1, x2, s * A2, b, n);
-    EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8));
+    EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6));
 
     boost::shared_ptr<JacobianFactor> actual =
         smartFactor1->createJacobianSVDFactor(cameras, 0.0);
     CHECK(actual);
-    EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8));
+    EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
     EXPECT(assert_equal(expected, *actual));
-    EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8);
-    EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8);
+    EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
+    EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
   }
 }
 
@@ -525,8 +525,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
-  graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
+  graph.addPrior(x1, cam1.pose(), noisePrior);
+  graph.addPrior(x2, cam2.pose(), noisePrior);
 
   //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@@ -589,8 +589,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
-  graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
+  graph.addPrior(x1, cam1.pose(), noisePrior);
+  graph.addPrior(x2, cam2.pose(), noisePrior);
 
   //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@@ -603,7 +603,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
   result = optimizer.optimize();
-  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-8));
+  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
 }
 
 /* *************************************************************************/
@@ -647,8 +647,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
-  graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
+  graph.addPrior(x1, cam1.pose(), noisePrior);
+  graph.addPrior(x2, cam2.pose(), noisePrior);
 
   //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@@ -716,8 +716,8 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
   graph.push_back(smartFactor4);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
-  graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
+  graph.addPrior(x1, cam1.pose(), noisePrior);
+  graph.addPrior(x2, cam2.pose(), noisePrior);
 
   Values values;
   values.insert(x1, cam1.pose());
@@ -766,8 +766,8 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
-  graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
+  graph.addPrior(x1, cam1.pose(), noisePrior);
+  graph.addPrior(x2, cam2.pose(), noisePrior);
 
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
@@ -779,7 +779,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
   result = optimizer.optimize();
-  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-8));
+  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
 }
 
 /* *************************************************************************/
@@ -806,8 +806,8 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
   graph.emplace_shared<ProjectionFactor>(cam3.project(landmark3), model, x3, L(3), sharedK2);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, level_pose, noisePrior);
-  graph.emplace_shared<PriorFactor<Pose3> >(x2, pose_right, noisePrior);
+  graph.addPrior(x1, level_pose, noisePrior);
+  graph.addPrior(x2, pose_right, noisePrior);
 
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
       Point3(0.5, 0.1, 0.3));
@@ -899,7 +899,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
   Matrix GraphInformation = GaussianGraph->hessian().first;
 
   // Check Hessian
-  EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8));
+  EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6));
 
   Matrix AugInformationMatrix = factor1->augmentedInformation()
       + factor2->augmentedInformation() + factor3->augmentedInformation();
@@ -908,7 +908,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
   Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector
 
   // Check Hessian
-  EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8));
+  EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6));
 }
 
 /* *************************************************************************/
@@ -948,7 +948,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
   NonlinearFactorGraph graph;
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
+  graph.addPrior(x1, cam1.pose(), noisePrior);
   graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
   graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
 
@@ -1012,7 +1012,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
+  graph.addPrior(x1, cam1.pose(), noisePrior);
   graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
   graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
 
@@ -1223,8 +1223,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
-  graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
+  graph.addPrior(x1, cam1.pose(), noisePrior);
+  graph.addPrior(x2, cam2.pose(), noisePrior);
 
   //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@@ -1296,7 +1296,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
+  graph.addPrior(x1, cam1.pose(), noisePrior);
   graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
   graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
 
diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp
index 1d2baefee..4bbef6530 100644
--- a/gtsam/slam/tests/testTriangulationFactor.cpp
+++ b/gtsam/slam/tests/testTriangulationFactor.cpp
@@ -17,7 +17,7 @@
  */
 
 #include <gtsam/geometry/triangulation.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/StereoCamera.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/nonlinear/Expression.h>
@@ -39,7 +39,7 @@ static const boost::shared_ptr<Cal3_S2> sharedCal = //
 // Looking along X-axis, 1 meter above ground plane (x-y)
 static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
 static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
-SimpleCamera camera1(pose1, *sharedCal);
+PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
 
 // landmark ~5 meters infront of camera
 static const Point3 landmark(5, 0.5, 1.2);
@@ -52,7 +52,7 @@ TEST( triangulation, TriangulationFactor ) {
 
   Key pointKey(1);
   SharedNoiseModel model;
-  typedef TriangulationFactor<SimpleCamera> Factor;
+  typedef TriangulationFactor<PinholeCamera<Cal3_S2> > Factor;
   Factor factor(camera1, z1, model, pointKey);
 
   // Use the factor to calculate the Jacobians
diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h
index 5f7bdde7e..e28f28764 100644
--- a/gtsam/symbolic/SymbolicBayesTree.h
+++ b/gtsam/symbolic/SymbolicBayesTree.h
@@ -30,7 +30,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   /// A clique in a SymbolicBayesTree
-  class SymbolicBayesTreeClique :
+  class GTSAM_EXPORT SymbolicBayesTreeClique :
     public BayesTreeCliqueBase<SymbolicBayesTreeClique, SymbolicFactorGraph>
   {
   public:
@@ -45,7 +45,7 @@ namespace gtsam {
   /* ************************************************************************* */
   /// A Bayes tree that represents the connectivity between variables but is not associated with any
   /// probability functions.
-  class SymbolicBayesTree :
+  class GTSAM_EXPORT SymbolicBayesTree :
     public BayesTree<SymbolicBayesTreeClique>
   {
   private:
@@ -59,7 +59,7 @@ namespace gtsam {
     SymbolicBayesTree() {}
 
     /** check equality */
-    GTSAM_EXPORT bool equals(const This& other, double tol = 1e-9) const;
+    bool equals(const This& other, double tol = 1e-9) const;
 
   private:
     /** Serialization function */
diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in
index 8a9a13648..01ac00b37 100644
--- a/gtsam_extra.cmake.in
+++ b/gtsam_extra.cmake.in
@@ -9,5 +9,6 @@ set (GTSAM_USE_TBB @GTSAM_USE_TBB@)
 set (GTSAM_DEFAULT_ALLOCATOR @GTSAM_DEFAULT_ALLOCATOR@)
 
 if("@GTSAM_INSTALL_CYTHON_TOOLBOX@")
+  list(APPEND GTSAM_CYTHON_INSTALL_PATH "@GTSAM_CYTHON_INSTALL_PATH@")
   list(APPEND GTSAM_EIGENCY_INSTALL_PATH "@GTSAM_EIGENCY_INSTALL_PATH@")
 endif()
diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt
index 53ba83fad..010b32710 100644
--- a/gtsam_unstable/CMakeLists.txt
+++ b/gtsam_unstable/CMakeLists.txt
@@ -48,7 +48,7 @@ endforeach(subdir)
 set(library_name GTSAM_UNSTABLE) # For substitution in dllexport.h.in
 configure_file("${GTSAM_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h")
 list(APPEND gtsam_unstable_srcs "${PROJECT_BINARY_DIR}/dllexport.h")
-install(FILES "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION include/gtsam_unstable)
+install(FILES "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam_unstable)
 
 # assemble gtsam_unstable components
 set(gtsam_unstable_srcs
@@ -101,9 +101,9 @@ endif()
 install(
 	TARGETS gtsam_unstable
 	EXPORT GTSAM-exports
-	LIBRARY DESTINATION lib
-	ARCHIVE DESTINATION lib
-	RUNTIME DESTINATION bin)
+	LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+	ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+	RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})
 list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
 
diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h
index fd675d0d5..96424324b 100644
--- a/gtsam_unstable/base/BTree.h
+++ b/gtsam_unstable/base/BTree.h
@@ -72,7 +72,7 @@ namespace gtsam {
     }; // Node
 
     // We store a shared pointer to the root of the functional tree
-    // composed of Node classes. If root_==NULL, the tree is empty.
+    // composed of Node classes. If root_==nullptr, the tree is empty.
     typedef boost::shared_ptr<const Node> sharedNode;
     sharedNode root_;
 
@@ -223,7 +223,7 @@ namespace gtsam {
 
     /** Return height of the tree, 0 if empty */
     size_t height() const {
-      return (root_ != NULL) ? root_->height_ : 0;
+      return (root_ != nullptr) ? root_->height_ : 0;
     }
 
     /** return size of the tree */
diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp
index 0223250b5..525abd098 100644
--- a/gtsam_unstable/discrete/CSP.cpp
+++ b/gtsam_unstable/discrete/CSP.cpp
@@ -14,7 +14,14 @@ using namespace std;
 namespace gtsam {
 
   /// Find the best total assignment - can be expensive
-  CSP::sharedValues CSP::optimalAssignment(OptionalOrdering ordering) const {
+  CSP::sharedValues CSP::optimalAssignment() const {
+    DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential();
+    sharedValues mpe = chordal->optimize();
+    return mpe;
+  }
+
+  /// Find the best total assignment - can be expensive
+  CSP::sharedValues CSP::optimalAssignment(const Ordering& ordering) const {
     DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(ordering);
     sharedValues mpe = chordal->optimize();
     return mpe;
diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h
index bbdadd3dc..9e843f667 100644
--- a/gtsam_unstable/discrete/CSP.h
+++ b/gtsam_unstable/discrete/CSP.h
@@ -60,7 +60,10 @@ namespace gtsam {
 //    }
 
     /// Find the best total assignment - can be expensive
-    sharedValues optimalAssignment(OptionalOrdering ordering = boost::none) const;
+    sharedValues optimalAssignment() const;
+
+    /// Find the best total assignment - can be expensive
+    sharedValues optimalAssignment(const Ordering& ordering) const;
 
 //    /*
 //     * Perform loopy belief propagation
diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp
index 494f2731f..c7e46e400 100644
--- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp
+++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp
@@ -8,7 +8,7 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/sam/RangeFactor.h>
 #include <gtsam_unstable/slam/PartialPriorFactor.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp
index 41e75432f..95629fb43 100644
--- a/gtsam_unstable/examples/ConcurrentCalibration.cpp
+++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp
@@ -26,7 +26,6 @@
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/dataset.h>
 
 #include <string>
@@ -61,7 +60,7 @@ int main(int argc, char** argv){
   initial_estimate.insert(Symbol('K', 0), *noisy_K);
 
   noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished());
-  graph.emplace_shared<PriorFactor<Cal3_S2> >(Symbol('K', 0), *noisy_K, calNoise);
+  graph.addPrior(Symbol('K', 0), *noisy_K, calNoise);
 
 
   ifstream pose_file(pose_loc.c_str());
@@ -77,7 +76,7 @@ int main(int argc, char** argv){
   }
 
   noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01);
-  graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', pose_id), Pose3(m), poseNoise);
+  graph.addPrior(Symbol('x', pose_id), Pose3(m), poseNoise);
 
   // camera and landmark keys
   size_t x, l;
diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp
index 429a2c2b2..939d3a5c8 100644
--- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp
+++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp
@@ -34,7 +34,6 @@
 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
 // Here we will use Between factors for the relative motion described by odometry measurements.
 // Also, we will initialize the robot at the origin using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 
 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
@@ -86,7 +85,7 @@ int main(int argc, char** argv) {
   Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
   auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   Key priorKey = 0;
-  newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
+  newFactors.addPrior(priorKey, priorMean, priorNoise);
   newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
   newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds;
 
diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp
index 71153ee31..16ede58e0 100644
--- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp
+++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp
@@ -30,7 +30,6 @@
 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
 // Here we will use Between factors for the relative motion described by odometry measurements.
 // Also, we will initialize the robot at the origin using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 
 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
@@ -80,7 +79,7 @@ int main(int argc, char** argv) {
   Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
   noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   Key priorKey = 0;
-  newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
+  newFactors.addPrior(priorKey, priorMean, priorNoise);
   newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
   newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds;
 
@@ -145,5 +144,19 @@ int main(int argc, char** argv) {
     cout << setprecision(5) << "    Key: " << key_timestamp.first << "  Time: " << key_timestamp.second << endl;
   }
 
+  // Here is an example of how to get the full Jacobian of the problem.
+  // First, get the linearization point.
+  Values result = smootherISAM2.calculateEstimate();
+
+  // Get the factor graph
+  auto &factorGraph = smootherISAM2.getFactors();
+
+  // Linearize to a Gaussian factor graph
+  boost::shared_ptr<GaussianFactorGraph> linearGraph = factorGraph.linearize(result);
+
+  // Converts the linear graph into a Jacobian factor and extracts the Jacobian matrix
+  Matrix jacobian = linearGraph->jacobian().first;
+  cout << " Jacobian: " << jacobian << endl;
+
   return 0;
 }
diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
index 9dbeeac89..5fdc7a743 100644
--- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
+++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
@@ -37,7 +37,6 @@
 
 // In GTSAM, measurement functions are represented as 'factors'. Several common factors
 // have been provided with the library for solving robotics SLAM problems.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/RangeFactor.h>
 #include <gtsam_unstable/slam/SmartRangeFactor.h>
@@ -127,7 +126,7 @@ int main(int argc, char** argv) {
   Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
       M_PI - 2.02108900000000);
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
+  newFactors.addPrior(0, pose0, priorNoise);
 
   ofstream os2("rangeResultLM.txt");
   ofstream os3("rangeResultSR.txt");
diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp
index a63a0ba20..90b2a30ff 100644
--- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp
+++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp
@@ -37,7 +37,6 @@
 
 // In GTSAM, measurement functions are represented as 'factors'. Several common factors
 // have been provided with the library for solving robotics SLAM problems.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/RangeFactor.h>
 
@@ -124,7 +123,7 @@ int main(int argc, char** argv) {
   // Add prior on first pose
   Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, -2.02108900000000);
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
+  newFactors.addPrior(0, pose0, priorNoise);
 
   //  initialize points (Gaussian)
   Values initial;
diff --git a/gtsam_unstable/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp
new file mode 100644
index 000000000..f72b2cf95
--- /dev/null
+++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp
@@ -0,0 +1,160 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, 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  TimeOfArrivalExample.cpp
+ *  @brief Track a moving object "Time of Arrival" measurements at 4
+ * microphones.
+ *  @author Frank Dellaert
+ *  @author Jay Chakravarty
+ *  @date March 2020
+ */
+
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam_unstable/geometry/Event.h>
+#include <gtsam_unstable/slam/TOAFactor.h>
+
+#include <boost/bind.hpp>
+#include <boost/format.hpp>
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+// units
+static const double ms = 1e-3;
+static const double cm = 1e-2;
+
+// Instantiate functor with speed of sound value
+static const TimeOfArrival kTimeOfArrival(330);
+
+/* ************************************************************************* */
+// Create microphones
+vector<Point3> defineMicrophones() {
+  const double height = 0.5;
+  vector<Point3> microphones;
+  microphones.push_back(Point3(0, 0, height));
+  microphones.push_back(Point3(403 * cm, 0, height));
+  microphones.push_back(Point3(403 * cm, 403 * cm, height));
+  microphones.push_back(Point3(0, 403 * cm, 2 * height));
+  return microphones;
+}
+
+/* ************************************************************************* */
+// Create ground truth trajectory
+vector<Event> createTrajectory(size_t n) {
+  vector<Event> trajectory;
+  double timeOfEvent = 10;
+  // simulate emitting a sound every second while moving on straight line
+  for (size_t key = 0; key < n; key++) {
+    trajectory.push_back(
+        Event(timeOfEvent, 245 * cm + key * 1.0, 201.5 * cm, (212 - 45) * cm));
+    timeOfEvent += 1;
+  }
+  return trajectory;
+}
+
+/* ************************************************************************* */
+// Simulate time-of-arrival measurements for a single event
+vector<double> simulateTOA(const vector<Point3>& microphones,
+                           const Event& event) {
+  size_t K = microphones.size();
+  vector<double> simulatedTOA(K);
+  for (size_t i = 0; i < K; i++) {
+    simulatedTOA[i] = kTimeOfArrival(event, microphones[i]);
+  }
+  return simulatedTOA;
+}
+
+/* ************************************************************************* */
+// Simulate time-of-arrival measurements for an entire trajectory
+vector<vector<double>> simulateTOA(const vector<Point3>& microphones,
+                                   const vector<Event>& trajectory) {
+  vector<vector<double>> simulatedTOA;
+  for (auto event : trajectory) {
+    simulatedTOA.push_back(simulateTOA(microphones, event));
+  }
+  return simulatedTOA;
+}
+
+/* ************************************************************************* */
+// create factor graph
+NonlinearFactorGraph createGraph(const vector<Point3>& microphones,
+                                 const vector<vector<double>>& simulatedTOA) {
+  NonlinearFactorGraph graph;
+
+  // Create a noise model for the TOA error
+  auto model = noiseModel::Isotropic::Sigma(1, 0.5 * ms);
+
+  size_t K = microphones.size();
+  size_t key = 0;
+  for (auto toa : simulatedTOA) {
+    for (size_t i = 0; i < K; i++) {
+      graph.emplace_shared<TOAFactor>(key, microphones[i], toa[i], model);
+    }
+    key += 1;
+  }
+  return graph;
+}
+
+/* ************************************************************************* */
+// create initial estimate for n events
+Values createInitialEstimate(size_t n) {
+  Values initial;
+
+  Event zero;
+  for (size_t key = 0; key < n; key++) {
+    initial.insert(key, zero);
+  }
+  return initial;
+}
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+  // Create microphones
+  auto microphones = defineMicrophones();
+  size_t K = microphones.size();
+  for (size_t i = 0; i < K; i++) {
+    cout << "mic" << i << " = " << microphones[i] << endl;
+  }
+
+  // Create a ground truth trajectory
+  const size_t n = 5;
+  auto groundTruth = createTrajectory(n);
+
+  // Simulate time-of-arrival measurements
+  auto simulatedTOA = simulateTOA(microphones, groundTruth);
+  for (size_t key = 0; key < n; key++) {
+    for (size_t i = 0; i < K; i++) {
+      cout << "z_" << key << i << " = " << simulatedTOA[key][i] / ms << " ms"
+           << endl;
+    }
+  }
+
+  // Create factor graph
+  auto graph = createGraph(microphones, simulatedTOA);
+
+  // Create initial estimate
+  auto initialEstimate = createInitialEstimate(n);
+  initialEstimate.print("Initial Estimate:\n");
+
+  // Optimize using Levenberg-Marquardt optimization.
+  LevenbergMarquardtParams params;
+  params.setAbsoluteErrorTol(1e-10);
+  params.setVerbosityLM("SUMMARY");
+  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
+  Values result = optimizer.optimize();
+  result.print("Final Result:\n");
+}
+/* ************************************************************************* */
diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp
index c503514a6..45a24c101 100644
--- a/gtsam_unstable/geometry/Event.cpp
+++ b/gtsam_unstable/geometry/Event.cpp
@@ -24,15 +24,16 @@ namespace gtsam {
 
 /* ************************************************************************* */
 void Event::print(const std::string& s) const {
-  std::cout << s << "time = " << time_ << "location = " << location_.transpose();
+  std::cout << s << "{'time':" << time_
+            << ", 'location': " << location_.transpose() << "}";
 }
 
 /* ************************************************************************* */
 bool Event::equals(const Event& other, double tol) const {
-  return std::abs(time_ - other.time_) < tol
-      && traits<Point3>::Equals(location_, other.location_, tol);
+  return std::abs(time_ - other.time_) < tol &&
+         traits<Point3>::Equals(location_, other.location_, tol);
 }
 
 /* ************************************************************************* */
 
-} //\ namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h
index fc186857f..383c34915 100644
--- a/gtsam_unstable/geometry/Event.h
+++ b/gtsam_unstable/geometry/Event.h
@@ -20,42 +20,43 @@
 #pragma once
 
 #include <gtsam/geometry/Point3.h>
+#include <gtsam_unstable/dllexport.h>
+
 #include <cmath>
 #include <iosfwd>
-#include <gtsam_unstable/dllexport.h>
+#include <string>
 
 namespace gtsam {
 
-/// A space-time event
+/**
+ * A space-time event models an event that happens at a certain 3D location, at
+ * a certain time. One use for it is in sound-based or UWB-ranging tracking or
+ * SLAM, where we have "time of arrival" measurements at a set of sensors. The
+ * TOA functor below provides a measurement function for those applications.
+ */
 class Event {
+  double time_;      ///< Time event was generated
+  Point3 location_;  ///< Location at time event was generated
 
-  double time_; ///< Time event was generated
-  Point3 location_; ///< Location at time event was generated
-
-public:
+ public:
   enum { dimension = 4 };
 
   /// Default Constructor
-  Event() :
-      time_(0), location_(0,0,0) {
-  }
+  Event() : time_(0), location_(0, 0, 0) {}
 
   /// Constructor from time and location
-  Event(double t, const Point3& p) :
-      time_(t), location_(p) {
-  }
+  Event(double t, const Point3& p) : time_(t), location_(p) {}
 
   /// Constructor with doubles
-  Event(double t, double x, double y, double z) :
-      time_(t), location_(x, y, z) {
-  }
+  Event(double t, double x, double y, double z)
+      : time_(t), location_(x, y, z) {}
 
-  double time() const { return time_;}
-  Point3 location() const { return location_;}
+  double time() const { return time_; }
+  Point3 location() const { return location_; }
 
-  // TODO we really have to think of a better way to do linear arguments
-  double height(OptionalJacobian<1,4> H = boost::none) const {
-    static const Matrix14 JacobianZ = (Matrix14() << 0,0,0,1).finished();
+  // TODO(frank) we really have to think of a better way to do linear arguments
+  double height(OptionalJacobian<1, 4> H = boost::none) const {
+    static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished();
     if (H) *H = JacobianZ;
     return location_.z();
   }
@@ -64,7 +65,8 @@ public:
   GTSAM_UNSTABLE_EXPORT void print(const std::string& s = "") const;
 
   /** equals with an tolerance */
-  GTSAM_UNSTABLE_EXPORT bool equals(const Event& other, double tol = 1e-9) const;
+  GTSAM_UNSTABLE_EXPORT bool equals(const Event& other,
+                                    double tol = 1e-9) const;
 
   /// Updates a with tangent space delta
   inline Event retract(const Vector4& v) const {
@@ -73,28 +75,44 @@ public:
 
   /// Returns inverse retraction
   inline Vector4 localCoordinates(const Event& q) const {
-    return Vector4::Zero(); // TODO
-  }
-
-  /// Time of arrival to given microphone
-  double toa(const Point3& microphone, //
-      OptionalJacobian<1, 4> H1 = boost::none, //
-      OptionalJacobian<1, 3> H2 = boost::none) const {
-    static const double Speed = 330;
-    Matrix13 D1, D2;
-    double distance = gtsam::distance3(location_, microphone, D1, D2);
-    if (H1)
-      // derivative of toa with respect to event
-      *H1 << 1.0, D1 / Speed;
-    if (H2)
-      // derivative of toa with respect to microphone location
-      *H2 << D2 / Speed;
-    return time_ + distance / Speed;
+    return Vector4::Zero();  // TODO(frank) implement!
   }
 };
 
 // Define GTSAM traits
-template<>
+template <>
 struct traits<Event> : internal::Manifold<Event> {};
 
-} //\ namespace gtsam
+/// Time of arrival to given sensor
+class TimeOfArrival {
+  const double speed_;  ///< signal speed
+
+ public:
+  typedef double result_type;
+
+  /// Constructor with optional speed of signal, in m/sec
+  explicit TimeOfArrival(double speed = 330) : speed_(speed) {}
+
+  /// Calculate time of arrival
+  double measure(const Event& event, const Point3& sensor) const {
+    double distance = gtsam::distance3(event.location(), sensor);
+    return event.time() + distance / speed_;
+  }
+
+  /// Calculate time of arrival, with derivatives
+  double operator()(const Event& event, const Point3& sensor,  //
+                    OptionalJacobian<1, 4> H1 = boost::none,   //
+                    OptionalJacobian<1, 3> H2 = boost::none) const {
+    Matrix13 D1, D2;
+    double distance = gtsam::distance3(event.location(), sensor, D1, D2);
+    if (H1)
+      // derivative of toa with respect to event
+      *H1 << 1.0, D1 / speed_;
+    if (H2)
+      // derivative of toa with respect to sensor location
+      *H2 << D2 / speed_;
+    return event.time() + distance / speed_;
+  }
+};
+
+}  // namespace gtsam
diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp
index eb732f2c5..2d4bd7a2d 100644
--- a/gtsam_unstable/geometry/SimPolygon2D.cpp
+++ b/gtsam_unstable/geometry/SimPolygon2D.cpp
@@ -4,10 +4,7 @@
  */
 
 #include <iostream>
-#include <boost/random/linear_congruential.hpp>
-#include <boost/random/uniform_real.hpp>
-#include <boost/random/normal_distribution.hpp>
-#include <boost/random/variate_generator.hpp>
+#include <random>
 
 #include <gtsam_unstable/geometry/SimPolygon2D.h>
 
@@ -16,11 +13,11 @@ namespace gtsam {
 using namespace std;
 
 const size_t max_it = 100000;
-boost::minstd_rand SimPolygon2D::rng(42u);
+std::minstd_rand SimPolygon2D::rng(42u);
 
 /* ************************************************************************* */
 void SimPolygon2D::seedGenerator(unsigned long seed) {
-  rng = boost::minstd_rand(seed);
+  rng = std::minstd_rand(seed);
 }
 
 /* ************************************************************************* */
@@ -225,23 +222,23 @@ SimPolygon2D SimPolygon2D::randomRectangle(
 
 /* ***************************************************************** */
 Point2 SimPolygon2D::randomPoint2(double s) {
-  boost::uniform_real<>  gen_t(-s/2.0, s/2.0);
+  std::uniform_real_distribution<> gen_t(-s/2.0, s/2.0);
   return Point2(gen_t(rng), gen_t(rng));
 }
 
 /* ***************************************************************** */
 Rot2 SimPolygon2D::randomAngle() {
-  boost::uniform_real<>  gen_r(-M_PI, M_PI); // modified range to avoid degenerate cases in triangles
+   // modified range to avoid degenerate cases in triangles:
+  std::uniform_real_distribution<> gen_r(-M_PI, M_PI);
   return Rot2::fromAngle(gen_r(rng));
 }
 
 /* ***************************************************************** */
 double SimPolygon2D::randomDistance(double mu, double sigma, double min_dist) {
-  boost::normal_distribution<double> norm_dist(mu, sigma);
-  boost::variate_generator<boost::minstd_rand&, boost::normal_distribution<double> > gen_d(rng, norm_dist);
+  std::normal_distribution<> norm_dist(mu, sigma);
   double d = -10.0;
   for (size_t i=0; i<max_it; ++i) {
-    d = std::abs(gen_d());
+    d = std::abs(norm_dist(rng));
     if (d > min_dist)
       return d;
   }
@@ -294,8 +291,8 @@ Point2 SimPolygon2D::randomBoundedPoint2(
     const Point2Vector& landmarks,
     const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist) {
 
-  boost::uniform_real<>  gen_x(0.0, UR_corner.x() - LL_corner.x());
-  boost::uniform_real<>  gen_y(0.0, UR_corner.y() - LL_corner.y());
+  std::uniform_real_distribution<> gen_x(0.0, UR_corner.x() - LL_corner.x());
+  std::uniform_real_distribution<> gen_y(0.0, UR_corner.y() - LL_corner.y());
 
   for (size_t i=0; i<max_it; ++i) {
     Point2 p = Point2(gen_x(rng), gen_y(rng)) + LL_corner;
diff --git a/gtsam_unstable/geometry/SimPolygon2D.h b/gtsam_unstable/geometry/SimPolygon2D.h
index 0a0e0da7f..1711abcf1 100644
--- a/gtsam_unstable/geometry/SimPolygon2D.h
+++ b/gtsam_unstable/geometry/SimPolygon2D.h
@@ -6,10 +6,11 @@
 
 #pragma once
 
-#include <map>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam_unstable/geometry/SimWall2D.h>
-#include <boost/random/linear_congruential.hpp>
+
+#include <map>
+#include <random>
 
 namespace gtsam {
 
@@ -19,7 +20,7 @@ namespace gtsam {
 class GTSAM_UNSTABLE_EXPORT SimPolygon2D {
 protected:
   Point2Vector landmarks_;
-  static boost::minstd_rand rng;
+  static std::minstd_rand rng;
 
 public:
 
diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp
index ec8ca1f4b..0349f3293 100644
--- a/gtsam_unstable/geometry/tests/testEvent.cpp
+++ b/gtsam_unstable/geometry/tests/testEvent.cpp
@@ -17,10 +17,12 @@
  *  @date December 2014
  */
 
-#include <gtsam_unstable/geometry/Event.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/nonlinear/Expression.h>
+#include <gtsam_unstable/geometry/Event.h>
+
 #include <CppUnitLite/TestHarness.h>
+
 #include <boost/bind.hpp>
 
 using namespace std;
@@ -30,56 +32,59 @@ using namespace gtsam;
 static const double ms = 1e-3;
 static const double cm = 1e-2;
 typedef Eigen::Matrix<double, 1, 1> Vector1;
-static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms));
+static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
 
 static const double timeOfEvent = 25;
 static const Event exampleEvent(timeOfEvent, 1, 0, 0);
-static const Point3 microphoneAt0(0,0,0);
+static const Point3 microphoneAt0(0, 0, 0);
+
+static const double kSpeedOfSound = 340;
+static const TimeOfArrival kToa(kSpeedOfSound);
 
 //*****************************************************************************
-TEST( Event, Constructor ) {
+TEST(Event, Constructor) {
   const double t = 0;
   Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm);
 }
 
 //*****************************************************************************
-TEST( Event, Toa1 ) {
+TEST(Event, Toa1) {
   Event event(0, 1, 0, 0);
-  double expected = 1. / 330;
-  EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9);
+  double expected = 1. / kSpeedOfSound;
+  EXPECT_DOUBLES_EQUAL(expected, kToa(event, microphoneAt0), 1e-9);
 }
 
 //*****************************************************************************
-TEST( Event, Toa2 ) {
-  double expectedTOA = timeOfEvent + 1. / 330;
-  EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9);
+TEST(Event, Toa2) {
+  double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
+  EXPECT_DOUBLES_EQUAL(expectedTOA, kToa(exampleEvent, microphoneAt0), 1e-9);
 }
 
 //*************************************************************************
-TEST (Event, Derivatives) {
+TEST(Event, Derivatives) {
   Matrix14 actualH1;
   Matrix13 actualH2;
-  exampleEvent.toa(microphoneAt0, actualH1, actualH2);
+  kToa(exampleEvent, microphoneAt0, actualH1, actualH2);
   Matrix expectedH1 = numericalDerivative11<double, Event>(
-      boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none),
+      boost::bind(kToa, _1, microphoneAt0, boost::none, boost::none),
       exampleEvent);
   EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
   Matrix expectedH2 = numericalDerivative11<double, Point3>(
-      boost::bind(&Event::toa, exampleEvent, _1, boost::none, boost::none),
+      boost::bind(kToa, exampleEvent, _1, boost::none, boost::none),
       microphoneAt0);
   EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
 }
 
 //*****************************************************************************
-TEST( Event, Expression ) {
+TEST(Event, Expression) {
   Key key = 12;
   Expression<Event> event_(key);
-  Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
-  Expression<double> expression(&Event::toa, event_, knownMicrophone_);
+  Expression<Point3> knownMicrophone_(microphoneAt0);  // constant expression
+  Expression<double> expression(kToa, event_, knownMicrophone_);
 
   Values values;
   values.insert(key, exampleEvent);
-  double expectedTOA = timeOfEvent + 1. / 330;
+  double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
   EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9);
 }
 
@@ -97,4 +102,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 //*****************************************************************************
-
diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
index 6ac3389ed..4c6251831 100644
--- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
+++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
@@ -9,7 +9,8 @@
 
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/Testable.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 
 #include <gtsam_unstable/geometry/InvDepthCamera3.h>
 
@@ -18,7 +19,7 @@ using namespace gtsam;
 
 static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
 Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
-SimpleCamera level_camera(level_pose, *K);
+PinholeCamera<Cal3_S2> level_camera(level_pose, *K);
 
 /* ************************************************************************* */
 TEST( InvDepthFactor, Project1) {
diff --git a/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp b/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp
index 6528f3f91..5cdd6c100 100644
--- a/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp
+++ b/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp
@@ -1,12 +1,13 @@
 /**
- * @file testSimPolygon
+ * @file testSimPolygon2D.cpp
  * @author Alex Cunningham
  */
 
-#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam_unstable/geometry/SimPolygon2D.h>
 
+#include <CppUnitLite/TestHarness.h>
+
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp
index aaeb0854d..b07b5acd6 100644
--- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp
+++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp
@@ -17,7 +17,6 @@
  */
 
 #include <gtsam_unstable/geometry/Similarity3.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/ExpressionFactorGraph.h>
@@ -263,11 +262,10 @@ TEST(Similarity3, Optimization) {
   Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
   noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1);
   Symbol key('x', 1);
-  PriorFactor<Similarity3> factor(key, prior, model);
 
   // Create graph
   NonlinearFactorGraph graph;
-  graph.push_back(factor);
+  graph.addPrior(key, prior, model);
 
   // Create initial estimate with identity transform
   Values initial;
@@ -304,7 +302,6 @@ TEST(Similarity3, Optimization2) {
       (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished());
   SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas(
       (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished());
-  PriorFactor<Similarity3> factor(X(1), prior, model); // Prior !
   BetweenFactor<Similarity3> b1(X(1), X(2), m1, betweenNoise);
   BetweenFactor<Similarity3> b2(X(2), X(3), m2, betweenNoise);
   BetweenFactor<Similarity3> b3(X(3), X(4), m3, betweenNoise);
@@ -313,7 +310,7 @@ TEST(Similarity3, Optimization2) {
 
   // Create graph
   NonlinearFactorGraph graph;
-  graph.push_back(factor);
+  graph.addPrior(X(1), prior, model); // Prior !
   graph.push_back(b1);
   graph.push_back(b2);
   graph.push_back(b3);
diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h
index d77895d86..ef2d16bf0 100644
--- a/gtsam_unstable/gtsam_unstable.h
+++ b/gtsam_unstable/gtsam_unstable.h
@@ -274,7 +274,7 @@ class SimPolygon2D {
  };
 
 // Nonlinear factors from gtsam, for our Value types
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 template<T = {gtsam::PoseRTV}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
@@ -377,6 +377,30 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
 
 typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> RangeFactorRTV;
 
+#include <gtsam_unstable/geometry/Event.h>
+class Event {
+  Event();
+  Event(double t, const gtsam::Point3& p);
+  Event(double t, double x, double y, double z);
+  double time() const;
+  gtsam::Point3 location() const;
+  double height() const;
+  void print(string s) const;
+};
+
+class TimeOfArrival {
+  TimeOfArrival();
+  TimeOfArrival(double speed);
+  double measure(const gtsam::Event& event, const gtsam::Point3& sensor) const;
+};
+
+#include <gtsam_unstable/slam/TOAFactor.h>
+virtual class TOAFactor : gtsam::NonlinearFactor {
+  // For now, because of overload issues, we only expose constructor with known sensor coordinates:
+  TOAFactor(size_t key1, gtsam::Point3 sensor, double measured,
+            const gtsam::noiseModel::Base* noiseModel);
+  static void InsertEvent(size_t key, const gtsam::Event& event, gtsam::Values* values);
+};
 
 #include <gtsam/nonlinear/NonlinearEquality.h>
 template<T = {gtsam::PoseRTV}>
diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp
index fa94dd255..36a2cacd8 100644
--- a/gtsam_unstable/linear/tests/testLinearEquality.cpp
+++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp
@@ -136,7 +136,7 @@ TEST(LinearEquality, error)
 /* ************************************************************************* */
 TEST(LinearEquality, matrices_NULL)
 {
-  // Make sure everything works with NULL noise model
+  // Make sure everything works with nullptr noise model
   LinearEquality factor(simple::terms, simple::b, 0);
 
   Matrix AExpected(3, 9);
diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
index b0a19c6a4..777e4b2d3 100644
--- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
@@ -39,7 +39,7 @@ bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs,
     double tol) const {
   const BatchFixedLagSmoother* e =
       dynamic_cast<const BatchFixedLagSmoother*>(&rhs);
-  return e != NULL && FixedLagSmoother::equals(*e, tol)
+  return e != nullptr && FixedLagSmoother::equals(*e, tol)
       && factors_.equals(e->factors_, tol) && theta_.equals(e->theta_, tol);
 }
 
@@ -145,7 +145,7 @@ void BatchFixedLagSmoother::removeFactors(
     } else {
       // TODO: Throw an error??
       cout << "Attempting to remove a factor from slot " << slot
-          << ", but it is already NULL." << endl;
+          << ", but it is already nullptr." << endl;
     }
   }
 }
@@ -370,7 +370,7 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(
       cout << " " << DefaultKeyFormatter(key);
     }
   } else {
-    cout << " NULL";
+    cout << " nullptr";
   }
   cout << " )" << endl;
 }
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
index 97df375d5..758bcfe48 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
@@ -38,7 +38,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p
     }
     std::cout << ")" << std::endl;
   } else {
-    std::cout << "{ NULL }" << std::endl;
+    std::cout << "{ nullptr }" << std::endl;
   }
 }
 
@@ -79,7 +79,7 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr&
     }
     std::cout << ")" << std::endl;
   } else {
-    std::cout << "{ NULL }" << std::endl;
+    std::cout << "{ nullptr }" << std::endl;
   }
 }
 
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
index 775dccbb0..600baa9f0 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
@@ -394,7 +394,7 @@ void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared
     }
     std::cout << ")" << std::endl;
   } else {
-    std::cout << "{ NULL }" << std::endl;
+    std::cout << "{ nullptr }" << std::endl;
   }
 }
 
@@ -408,7 +408,7 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr
     }
     std::cout << ")" << std::endl;
   } else {
-    std::cout << "{ NULL }" << std::endl;
+    std::cout << "{ nullptr }" << std::endl;
   }
 }
 
diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
index a99360ffb..d29dfe8ca 100644
--- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
@@ -58,7 +58,7 @@ bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs,
     double tol) const {
   const IncrementalFixedLagSmoother* e =
       dynamic_cast<const IncrementalFixedLagSmoother*>(&rhs);
-  return e != NULL && FixedLagSmoother::equals(*e, tol)
+  return e != nullptr && FixedLagSmoother::equals(*e, tol)
       && isam_.equals(e->isam_, tol);
 }
 
diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
index fe89c5b26..3cf6c16d3 100644
--- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
+++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
@@ -39,7 +39,7 @@ public:
 
   /** default constructor */
   IncrementalFixedLagSmoother(double smootherLag = 0.0,
-      const ISAM2Params& parameters = ISAM2Params()) :
+      const ISAM2Params& parameters = DefaultISAM2Params()) :
       FixedLagSmoother(smootherLag), isam_(parameters) {
   }
 
@@ -114,6 +114,14 @@ public:
   const ISAM2Result& getISAM2Result() const{ return isamResult_; }
 
 protected:
+
+  /** Create default parameters */
+  static ISAM2Params DefaultISAM2Params() {
+    ISAM2Params params;
+    params.findUnusedFactorSlots = true;
+    return params;
+  }
+
   /** An iSAM2 object used to perform inference. The smoother lag is controlled
    * by what factors are removed each iteration */
   ISAM2 isam_;
diff --git a/gtsam_unstable/nonlinear/NonlinearClusterTree.h b/gtsam_unstable/nonlinear/NonlinearClusterTree.h
index a483c9521..5d089f123 100644
--- a/gtsam_unstable/nonlinear/NonlinearClusterTree.h
+++ b/gtsam_unstable/nonlinear/NonlinearClusterTree.h
@@ -46,21 +46,26 @@ class NonlinearClusterTree : public ClusterTree<NonlinearFactorGraph> {
     // linearize local custer factors straight into hessianFactor, which is returned
     // If no ordering given, uses colamd
     HessianFactor::shared_ptr linearizeToHessianFactor(
-        const Values& values, boost::optional<Ordering> ordering = boost::none,
+        const Values& values,
         const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
-      if (!ordering)
-        ordering.reset(Ordering::ColamdConstrainedFirst(factors, orderedFrontalKeys, true));
-      return factors.linearizeToHessianFactor(values, *ordering, dampen);
+      Ordering ordering;
+      ordering = Ordering::ColamdConstrainedFirst(factors, orderedFrontalKeys, true);
+      return factors.linearizeToHessianFactor(values, ordering, dampen);
     }
 
-    // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent
+    // linearize local custer factors straight into hessianFactor, which is returned
+    // If no ordering given, uses colamd
+    HessianFactor::shared_ptr linearizeToHessianFactor(
+        const Values& values, const Ordering& ordering,
+        const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
+      return factors.linearizeToHessianFactor(values, ordering, dampen);
+    }
+
+    // Helper function: recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent
     // TODO(frank): Use TBB to support deep trees and parallelism
     std::pair<GaussianBayesNet, HessianFactor::shared_ptr> linearizeAndEliminate(
-        const Values& values, boost::optional<Ordering> ordering = boost::none,
-        const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
-      // Linearize and create HessianFactor f(front,separator)
-      HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, ordering, dampen);
-
+        const Values& values,
+        const HessianFactor::shared_ptr& localFactor) const {
       // Get contributions f(front) from children, as well as p(children|front)
       GaussianBayesNet bayesNet;
       for (const auto& child : children) {
@@ -72,12 +77,45 @@ class NonlinearClusterTree : public ClusterTree<NonlinearFactorGraph> {
       return {bayesNet, localFactor};
     }
 
+    // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent
+    // TODO(frank): Use TBB to support deep trees and parallelism
+    std::pair<GaussianBayesNet, HessianFactor::shared_ptr> linearizeAndEliminate(
+        const Values& values,
+        const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
+      // Linearize and create HessianFactor f(front,separator)
+      HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, dampen);
+      return linearizeAndEliminate(values, localFactor);
+    }
+
+    // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent
+    // TODO(frank): Use TBB to support deep trees and parallelism
+    std::pair<GaussianBayesNet, HessianFactor::shared_ptr> linearizeAndEliminate(
+        const Values& values, const Ordering& ordering,
+        const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
+      // Linearize and create HessianFactor f(front,separator)
+      HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, ordering, dampen);
+      return linearizeAndEliminate(values, localFactor);
+    }
+
     // Recursively eliminate subtree rooted at this Cluster
     // Version that updates existing Bayes net and returns a new Hessian factor on parent clique
     // It is possible to pass in a nullptr for the bayesNet if only interested in the new factor
     HessianFactor::shared_ptr linearizeAndEliminate(
         const Values& values, GaussianBayesNet* bayesNet,
-        boost::optional<Ordering> ordering = boost::none,
+        const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
+      auto bayesNet_newFactor_pair = linearizeAndEliminate(values, dampen);
+      if (bayesNet) {
+        bayesNet->push_back(bayesNet_newFactor_pair.first);
+      }
+      return bayesNet_newFactor_pair.second;
+    }
+
+    // Recursively eliminate subtree rooted at this Cluster
+    // Version that updates existing Bayes net and returns a new Hessian factor on parent clique
+    // It is possible to pass in a nullptr for the bayesNet if only interested in the new factor
+    HessianFactor::shared_ptr linearizeAndEliminate(
+        const Values& values, GaussianBayesNet* bayesNet,
+        const Ordering& ordering,
         const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
       auto bayesNet_newFactor_pair = linearizeAndEliminate(values, ordering, dampen);
       if (bayesNet) {
diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp
index a4811abd8..a708c57cc 100644
--- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp
@@ -26,7 +26,6 @@
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 
 using namespace std;
@@ -84,7 +83,7 @@ TEST( BatchFixedLagSmoother, Example )
     Values newValues;
     Timestamps newTimestamps;
 
-    newFactors.push_back(PriorFactor<Point2>(key0, Point2(0.0, 0.0), odometerNoise));
+    newFactors.addPrior(key0, Point2(0.0, 0.0), odometerNoise);
     newValues.insert(key0, Point2(0.01, 0.01));
     newTimestamps[key0] = 0.0;
 
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
index ff5a096b0..1f19c0e8a 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
@@ -17,7 +17,7 @@
  */
 
 #include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -134,7 +134,7 @@ TEST( ConcurrentBatchFilter, getFactors )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors1;
-  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.addPrior(1, poseInitial, noisePrior);
   newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
@@ -184,7 +184,7 @@ TEST( ConcurrentBatchFilter, getLinearizationPoint )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors1;
-  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.addPrior(1, poseInitial, noisePrior);
   newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
@@ -246,7 +246,7 @@ TEST( ConcurrentBatchFilter, calculateEstimate )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors2;
-  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.addPrior(1, poseInitial, noisePrior);
   newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
@@ -330,7 +330,7 @@ TEST( ConcurrentBatchFilter, update_multiple )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors2;
-  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.addPrior(1, poseInitial, noisePrior);
   newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
@@ -380,7 +380,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@@ -403,7 +403,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
 
 
   NonlinearFactorGraph partialGraph;
-  partialGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  partialGraph.addPrior(1, poseInitial, noisePrior);
   partialGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   partialGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
 
@@ -504,7 +504,7 @@ TEST( ConcurrentBatchFilter, synchronize_1 )
   // Insert factors into the filter, but do not marginalize out any variables.
   // The synchronization should still be empty
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues;
   newValues.insert(1, Pose3().compose(poseError));
@@ -1090,7 +1090,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@@ -1121,7 +1121,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 )
   NonlinearFactorGraph actualGraph = filter.getFactors();
 
   NonlinearFactorGraph expectedGraph;
-  expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  expectedGraph.addPrior(1, poseInitial, noisePrior);
   // we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   // we should add an empty one, so that the ordering and labeling of the factors is preserved
   expectedGraph.push_back(NonlinearFactor::shared_ptr());
@@ -1145,7 +1145,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@@ -1176,7 +1176,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 )
   NonlinearFactorGraph actualGraph = filter.getFactors();
 
   NonlinearFactorGraph expectedGraph;
-  expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  expectedGraph.addPrior(1, poseInitial, noisePrior);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@@ -1200,8 +1200,8 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_3 )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@@ -1233,7 +1233,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_3 )
   NonlinearFactorGraph expectedGraph;
   // we should add an empty one, so that the ordering and labeling of the factors is preserved
   expectedGraph.push_back(NonlinearFactor::shared_ptr());
-  expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  expectedGraph.addPrior(1, poseInitial, noisePrior);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@@ -1253,7 +1253,7 @@ TEST( ConcurrentBatchFilter, removeFactors_values )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors;
-  newFactors.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   newFactors.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
   newFactors.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@@ -1286,7 +1286,7 @@ TEST( ConcurrentBatchFilter, removeFactors_values )
 
   // note: factors are removed before the optimization
   NonlinearFactorGraph expectedGraph;
-  expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  expectedGraph.addPrior(1, poseInitial, noisePrior);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
index 47002acb6..ae9a3f827 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
@@ -17,7 +17,7 @@
  */
 
 #include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -104,7 +104,7 @@ TEST( ConcurrentBatchSmoother, getFactors )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors1;
-  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.addPrior(1, poseInitial, noisePrior);
   newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
@@ -154,7 +154,7 @@ TEST( ConcurrentBatchSmoother, getLinearizationPoint )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors1;
-  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.addPrior(1, poseInitial, noisePrior);
   newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
@@ -216,7 +216,7 @@ TEST( ConcurrentBatchSmoother, calculateEstimate )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors2;
-  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.addPrior(1, poseInitial, noisePrior);
   newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
@@ -302,7 +302,7 @@ TEST( ConcurrentBatchSmoother, update_multiple )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors2;
-  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.addPrior(1, poseInitial, noisePrior);
   newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
@@ -527,7 +527,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 )
   filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
   smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
-  smootherFactors.push_back(PriorFactor<Pose3>(4, poseInitial, noisePrior));
+  smootherFactors.addPrior(4, poseInitial, noisePrior);
   smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
   smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
 
@@ -588,7 +588,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@@ -617,7 +617,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 )
   NonlinearFactorGraph actualGraph = smoother.getFactors();
 
   NonlinearFactorGraph expectedGraph;
-  expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  expectedGraph.addPrior(1, poseInitial, noisePrior);
   // we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   // we should add an empty one, so that the ordering and labeling of the factors is preserved
   expectedGraph.push_back(NonlinearFactor::shared_ptr());
@@ -642,7 +642,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@@ -670,7 +670,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 )
   NonlinearFactorGraph actualGraph = smoother.getFactors();
 
   NonlinearFactorGraph expectedGraph;
-  expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  expectedGraph.addPrior(1, poseInitial, noisePrior);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@@ -694,8 +694,8 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_3 )
 
   // Add some factors to the Smoother
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@@ -724,7 +724,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_3 )
   NonlinearFactorGraph expectedGraph;
   // we should add an empty one, so that the ordering and labeling of the factors is preserved
   expectedGraph.push_back(NonlinearFactor::shared_ptr());
-  expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  expectedGraph.addPrior(1, poseInitial, noisePrior);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@@ -744,7 +744,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_values )
 
   // Add some factors to the Smoother
   NonlinearFactorGraph newFactors;
-  newFactors.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   newFactors.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
   newFactors.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@@ -774,7 +774,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_values )
 
   // note: factors are removed before the optimization
   NonlinearFactorGraph expectedGraph;
-  expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  expectedGraph.addPrior(1, poseInitial, noisePrior);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
index 342e2e79f..c5dba1a69 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
@@ -17,7 +17,7 @@
  */
 
 #include <gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -153,7 +153,7 @@ TEST( ConcurrentIncrementalFilter, getFactors )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors1;
-  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.addPrior(1, poseInitial, noisePrior);
   newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
@@ -203,7 +203,7 @@ TEST( ConcurrentIncrementalFilter, getLinearizationPoint )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors1;
-  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.addPrior(1, poseInitial, noisePrior);
   newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
@@ -259,7 +259,7 @@ TEST( ConcurrentIncrementalFilter, calculateEstimate )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors2;
-  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.addPrior(1, poseInitial, noisePrior);
   newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
@@ -343,7 +343,7 @@ TEST( ConcurrentIncrementalFilter, update_multiple )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors2;
-  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.addPrior(1, poseInitial, noisePrior);
   newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
@@ -393,7 +393,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@@ -423,7 +423,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
 
   // ----------------------------------------------------------------------------------------------
   NonlinearFactorGraph partialGraph;
-  partialGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  partialGraph.addPrior(1, poseInitial, noisePrior);
   partialGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   partialGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
 
@@ -476,7 +476,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@@ -507,7 +507,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
 
   // ----------------------------------------------------------------------------------------------
   NonlinearFactorGraph partialGraph;
-  partialGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  partialGraph.addPrior(1, poseInitial, noisePrior);
   partialGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   partialGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
 
@@ -605,7 +605,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_1 )
   // Insert factors into the filter, but do not marginalize out any variables.
   // The synchronization should still be empty
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues;
   newValues.insert(1, Pose3().compose(poseError));
@@ -1192,7 +1192,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@@ -1224,7 +1224,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 )
   NonlinearFactorGraph actualGraph = filter.getFactors();
 
   NonlinearFactorGraph expectedGraph;
-  expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  expectedGraph.addPrior(1, poseInitial, noisePrior);
   // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery))
   // we should add an empty one, so that the ordering and labeling of the factors is preserved
   expectedGraph.push_back(NonlinearFactor::shared_ptr());
@@ -1251,7 +1251,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@@ -1282,7 +1282,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 )
   NonlinearFactorGraph actualGraph = filter.getFactors();
 
   NonlinearFactorGraph expectedGraph;
-  expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  expectedGraph.addPrior(1, poseInitial, noisePrior);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@@ -1310,8 +1310,8 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@@ -1343,7 +1343,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 )
   NonlinearFactorGraph expectedGraph;
   // we should add an empty one, so that the ordering and labeling of the factors is preserved
   expectedGraph.push_back(NonlinearFactor::shared_ptr());
-  expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  expectedGraph.addPrior(1, poseInitial, noisePrior);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@@ -1367,7 +1367,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@@ -1399,7 +1399,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values )
   Values actualValues = filter.getLinearizationPoint();
 
   NonlinearFactorGraph expectedGraph;
-  expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  expectedGraph.addPrior(1, poseInitial, noisePrior);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
   expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
index 98b4bf8cc..5de115013 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
@@ -17,7 +17,7 @@
  */
 
 #include <gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -115,7 +115,7 @@ TEST( ConcurrentIncrementalSmootherDL, getFactors )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors1;
-  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.addPrior(1, poseInitial, noisePrior);
   newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
@@ -166,7 +166,7 @@ TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors1;
-  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.addPrior(1, poseInitial, noisePrior);
   newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
@@ -223,7 +223,7 @@ TEST( ConcurrentIncrementalSmootherDL, calculateEstimate )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors2;
-  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.addPrior(1, poseInitial, noisePrior);
   newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
@@ -310,7 +310,7 @@ TEST( ConcurrentIncrementalSmootherDL, update_multiple )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors2;
-  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.addPrior(1, poseInitial, noisePrior);
   newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
@@ -545,7 +545,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
   filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
   smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
-  smootherFactors.push_back(PriorFactor<Pose3>(4, poseInitial, noisePrior));
+  smootherFactors.addPrior(4, poseInitial, noisePrior);
   smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
   smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
 
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
index d4ebc7c0a..ec78ee6e2 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
@@ -17,7 +17,7 @@
  */
 
 #include <gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -114,7 +114,7 @@ TEST( ConcurrentIncrementalSmootherGN, getFactors )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors1;
-  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.addPrior(1, poseInitial, noisePrior);
   newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
@@ -165,7 +165,7 @@ TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors1;
-  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.addPrior(1, poseInitial, noisePrior);
   newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
@@ -222,7 +222,7 @@ TEST( ConcurrentIncrementalSmootherGN, calculateEstimate )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors2;
-  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.addPrior(1, poseInitial, noisePrior);
   newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
@@ -309,7 +309,7 @@ TEST( ConcurrentIncrementalSmootherGN, update_multiple )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors2;
-  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.addPrior(1, poseInitial, noisePrior);
   newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
@@ -546,7 +546,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
   filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
   smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
-  smootherFactors.push_back(PriorFactor<Pose3>(4, poseInitial, noisePrior));
+  smootherFactors.addPrior(4, poseInitial, noisePrior);
   smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
   smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
 
@@ -609,7 +609,7 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.addPrior(1, poseInitial, noisePrior);
   newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
   newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@@ -639,7 +639,7 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 )
   actualGraph.print("actual graph:  \n");
 
   NonlinearFactorGraph expectedGraph;
-  expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
+  expectedGraph.addPrior(1, poseInitial, noisePrior);
   // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   // we should add an empty one, so that the ordering and labeling of the factors is preserved
   expectedGraph.push_back(NonlinearFactor::shared_ptr());
diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
index 8d99ed482..3454c352a 100644
--- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
@@ -18,7 +18,6 @@
 
 
 #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -82,7 +81,7 @@ TEST( IncrementalFixedLagSmoother, Example )
     Values newValues;
     Timestamps newTimestamps;
 
-    newFactors.push_back(PriorFactor<Point2>(key0, Point2(0.0, 0.0), odometerNoise));
+    newFactors.addPrior(key0, Point2(0.0, 0.0), odometerNoise);
     newValues.insert(key0, Point2(0.01, 0.01));
     newTimestamps[key0] = 0.0;
 
diff --git a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp
index 2240034af..e0c234b7b 100644
--- a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp
+++ b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp
@@ -6,7 +6,6 @@
 
 #include <gtsam_unstable/nonlinear/NonlinearClusterTree.h>
 #include <gtsam/sam/BearingRangeFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Point2.h>
@@ -27,7 +26,7 @@ NonlinearFactorGraph planarSLAMGraph() {
   // Prior on pose x1 at the origin.
   Pose2 prior(0.0, 0.0, 0.0);
   auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.emplace_shared<PriorFactor<Pose2> >(x1, prior, priorNoise);
+  graph.addPrior(x1, prior, priorNoise);
 
   // Two odometry factors
   Pose2 odometry(2.0, 0.0, 0.0);
diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h
index 297057e3f..ce657e7a0 100644
--- a/gtsam_unstable/partition/FindSeparator-inl.h
+++ b/gtsam_unstable/partition/FindSeparator-inl.h
@@ -12,9 +12,11 @@
 
 #include <stdexcept>
 #include <iostream>
+#include <vector>
 #include <boost/tuple/tuple.hpp>
 #include <boost/shared_array.hpp>
-#include <boost/timer.hpp>
+
+#include <gtsam/base/timing.h>
 
 #include "FindSeparator.h"
 
@@ -40,14 +42,14 @@ namespace gtsam { namespace partition {
     const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) {
 
     // control parameters
-    idx_t vwgt[n];                  // the weights of the vertices
+    std::vector<idx_t> vwgt;        // the weights of the vertices
     idx_t options[METIS_NOPTIONS];
     METIS_SetDefaultOptions(options);  // use defaults
     idx_t sepsize;                      // the size of the separator, output
     sharedInts part_(new idx_t[n]);      // the partition of each vertex, output
 
     // set uniform weights on the vertices
-    std::fill(vwgt, vwgt+n, 1);
+    vwgt.assign(n, 1);
 
     // TODO: Fix at later time
     //boost::timer::cpu_timer TOTALTmr;
@@ -61,7 +63,7 @@ namespace gtsam { namespace partition {
 
     // call metis parition routine
     METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(),
-           vwgt, options, &sepsize, part_.get());
+           &vwgt[0], options, &sepsize, part_.get());
 
     if (verbose) {
       //boost::cpu_times const elapsed_times(timer.elapsed());
@@ -82,14 +84,14 @@ namespace gtsam { namespace partition {
     graph_t *graph;
     real_t *tpwgts2;
     ctrl_t *ctrl;
-    ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL);
+    ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, nullptr, nullptr);
     ctrl->iptype = METIS_IPTYPE_GROW;
-    //if () == NULL)
+    //if () == nullptr)
     //  return METIS_ERROR_INPUT;
 
     InitRandom(ctrl->seed);
 
-    graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL);
+    graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, nullptr, nullptr);
 
     AllocateWorkSpace(ctrl, graph);
 
@@ -127,14 +129,14 @@ namespace gtsam { namespace partition {
     const sharedInts& adjwgt, bool verbose) {
 
     // control parameters
-    idx_t vwgt[n];                  // the weights of the vertices
+    std::vector<idx_t> vwgt;                  // the weights of the vertices
     idx_t options[METIS_NOPTIONS];
     METIS_SetDefaultOptions(options);  // use defaults
     idx_t edgecut;                      // the number of edge cuts, output
     sharedInts part_(new idx_t[n]);      // the partition of each vertex, output
 
     // set uniform weights on the vertices
-    std::fill(vwgt, vwgt+n, 1);
+    vwgt.assign(n, 1);
 
     //TODO: Fix later
     //boost::timer TOTALTmr;
@@ -150,7 +152,7 @@ namespace gtsam { namespace partition {
     //int wgtflag = 1; // only edge weights
     //int numflag = 0; // c style numbering starting from 0
     //int nparts = 2; // partition the graph to 2 submaps
-    modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(),
+    modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), &vwgt[0], adjwgt.get(),
         options, &edgecut, part_.get());
 
 
diff --git a/gtsam_unstable/partition/GenericGraph.h b/gtsam_unstable/partition/GenericGraph.h
index ec0027635..3dc640e97 100644
--- a/gtsam_unstable/partition/GenericGraph.h
+++ b/gtsam_unstable/partition/GenericGraph.h
@@ -12,6 +12,7 @@
 #include <list>
 #include <vector>
 #include <stdexcept>
+#include <string>
 #include <boost/shared_ptr.hpp>
 
 #include "PartitionWorkSpace.h"
diff --git a/gtsam_unstable/partition/tests/CMakeLists.txt b/gtsam_unstable/partition/tests/CMakeLists.txt
index 2bb95e243..9e74d9996 100644
--- a/gtsam_unstable/partition/tests/CMakeLists.txt
+++ b/gtsam_unstable/partition/tests/CMakeLists.txt
@@ -1,2 +1,2 @@
 set(ignore_test "testNestedDissection.cpp")
-gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis")
+gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis-gtsam")
diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h
index f22de48cf..35b4677d5 100644
--- a/gtsam_unstable/slam/AHRS.h
+++ b/gtsam_unstable/slam/AHRS.h
@@ -77,6 +77,8 @@ public:
   void print(const std::string& s = "") const;
 
   virtual ~AHRS();
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 } /* namespace gtsam */
diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h
index f2bcb7cd7..cf56afab2 100644
--- a/gtsam_unstable/slam/BiasedGPSFactor.h
+++ b/gtsam_unstable/slam/BiasedGPSFactor.h
@@ -66,7 +66,7 @@ namespace gtsam {
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e =  dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) && traits<Point3>::Equals(this->measured_, e->measured_, tol);
+      return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(this->measured_, e->measured_, tol);
     }
 
     /** implement functions needed to derive from Factor */
diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
index 58284c3a6..f499a0f4b 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
@@ -155,7 +155,7 @@ public:
   /** equals */
   virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
     const This *e =  dynamic_cast<const This*> (&expected);
-    return e != NULL && Base::equals(*e, tol)
+    return e != nullptr && Base::equals(*e, tol)
     && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
     && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
     && (delta_angles_ - e->delta_angles_).norm() < tol
diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
index acca233d9..86efd10c1 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
@@ -153,7 +153,7 @@ public:
   /** equals */
   virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
     const This *e =  dynamic_cast<const This*> (&expected);
-    return e != NULL && Base::equals(*e, tol)
+    return e != nullptr && Base::equals(*e, tol)
     && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
     && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
     && (delta_angles_ - e->delta_angles_).norm() < tol
diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
index 68c972a86..762199753 100644
--- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
+++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
@@ -81,7 +81,7 @@ public:
   /** equals */
   virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
     const This *e =  dynamic_cast<const This*> (&expected);
-    return e != NULL && Base::equals(*e, tol);
+    return e != nullptr && Base::equals(*e, tol);
   }
 
   /** implement functions needed to derive from Factor */
diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
index c3a67232a..4763c4263 100644
--- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
+++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
@@ -135,7 +135,7 @@ public:
   /** equals */
   virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
     const This *e =  dynamic_cast<const This*> (&expected);
-    return e != NULL && Base::equals(*e, tol)
+    return e != nullptr && Base::equals(*e, tol)
       && (measurement_acc_ - e->measurement_acc_).norm() < tol
       && (measurement_gyro_ - e->measurement_gyro_).norm() < tol
       && (dt_ - e->dt_) < tol
diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h
index 3e1263bb9..5bef97bcf 100644
--- a/gtsam_unstable/slam/MultiProjectionFactor.h
+++ b/gtsam_unstable/slam/MultiProjectionFactor.h
@@ -21,7 +21,7 @@
 #pragma once
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <boost/optional.hpp>
 
 namespace gtsam {
diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h
index c71ee7abd..c6d892e93 100644
--- a/gtsam_unstable/slam/PartialPriorFactor.h
+++ b/gtsam_unstable/slam/PartialPriorFactor.h
@@ -100,7 +100,7 @@ namespace gtsam {
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e = dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) &&
+      return e != nullptr && Base::equals(*e, tol) &&
           gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) &&
           this->mask_ == e->mask_;
     }
diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h
index 03803b5f4..bb5835f80 100644
--- a/gtsam_unstable/slam/PoseBetweenFactor.h
+++ b/gtsam_unstable/slam/PoseBetweenFactor.h
@@ -79,7 +79,7 @@ namespace gtsam {
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e =  dynamic_cast<const This*> (&expected);
-      return e != NULL
+      return e != nullptr
           && Base::equals(*e, tol)
           && this->measured_.equals(e->measured_, tol)
           && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h
index d8649a0d5..ce9861160 100644
--- a/gtsam_unstable/slam/PosePriorFactor.h
+++ b/gtsam_unstable/slam/PosePriorFactor.h
@@ -74,7 +74,7 @@ namespace gtsam {
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This* e = dynamic_cast<const This*> (&expected);
-      return e != NULL
+      return e != nullptr
           && Base::equals(*e, tol)
           && this->prior_.equals(e->prior_, tol)
           && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h
new file mode 100644
index 000000000..ec7da22ef
--- /dev/null
+++ b/gtsam_unstable/slam/PoseToPointFactor.h
@@ -0,0 +1,90 @@
+/**
+ *  @file   PoseToPointFactor.hpp
+ *  @brief  This factor can be used to track a 3D landmark over time by
+ *providing local measurements of its location.
+ *  @author David Wisth
+ **/
+#pragma once
+
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <ostream>
+
+namespace gtsam {
+
+/**
+ * A class for a measurement between a pose and a point.
+ * @addtogroup SLAM
+ */
+class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
+ private:
+  typedef PoseToPointFactor This;
+  typedef NoiseModelFactor2<Pose3, Point3> Base;
+
+  Point3 measured_; /** the point measurement in local coordinates */
+
+ public:
+  // shorthand for a smart pointer to a factor
+  typedef boost::shared_ptr<PoseToPointFactor> shared_ptr;
+
+  /** default constructor - only use for serialization */
+  PoseToPointFactor() {}
+
+  /** Constructor */
+  PoseToPointFactor(Key key1, Key key2, const Point3& measured,
+                    const SharedNoiseModel& model)
+      : Base(model, key1, key2), measured_(measured) {}
+
+  virtual ~PoseToPointFactor() {}
+
+  /** implement functions needed for Testable */
+
+  /** print */
+  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+                                               DefaultKeyFormatter) const {
+    std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << ","
+              << keyFormatter(this->key2()) << ")\n"
+              << "  measured: " << measured_.transpose() << std::endl;
+    this->noiseModel_->print("  noise model: ");
+  }
+
+  /** equals */
+  virtual bool equals(const NonlinearFactor& expected,
+                      double tol = 1e-9) const {
+    const This* e = dynamic_cast<const This*>(&expected);
+    return e != nullptr && Base::equals(*e, tol) &&
+           traits<Point3>::Equals(this->measured_, e->measured_, tol);
+  }
+
+  /** implement functions needed to derive from Factor */
+
+  /** vector of errors
+   * @brief Error = wTwi.inverse()*wPwp - measured_
+   * @param wTwi The pose of the sensor in world coordinates
+   * @param wPwp The estimated point location in world coordinates
+   *
+   * Note: measured_ and the error are in local coordiantes.
+   */
+  Vector evaluateError(const Pose3& wTwi, const Point3& wPwp,
+                       boost::optional<Matrix&> H1 = boost::none,
+                       boost::optional<Matrix&> H2 = boost::none) const {
+    return wTwi.transformTo(wPwp, H1, H2) - measured_;
+  }
+
+  /** return the measured */
+  const Point3& measured() const { return measured_; }
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
+    ar& BOOST_SERIALIZATION_NVP(measured_);
+  }
+
+};  // \class PoseToPointFactor
+
+}  // namespace gtsam
diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp
index 32e8731cd..b35171041 100644
--- a/gtsam_unstable/slam/RelativeElevationFactor.cpp
+++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp
@@ -38,7 +38,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p
 /* ************************************************************************* */
 bool RelativeElevationFactor::equals(const NonlinearFactor& expected, double tol) const {
   const This *e = dynamic_cast<const This*> (&expected);
-  return e != NULL && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol;
+  return e != nullptr && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol;
 }
 
 /* ************************************************************************* */
diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h
index 1df14a8de..8ad4a14a6 100644
--- a/gtsam_unstable/slam/TOAFactor.h
+++ b/gtsam_unstable/slam/TOAFactor.h
@@ -17,33 +17,51 @@
  *  @date December 2014
  */
 
+#pragma once
+
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam_unstable/geometry/Event.h>
 
 namespace gtsam {
 
 /// A "Time of Arrival" factor - so little code seems hardly worth it :-)
-class TOAFactor: public ExpressionFactor<double> {
-
+class TOAFactor : public ExpressionFactor<double> {
   typedef Expression<double> Double_;
 
-public:
-
+ public:
   /**
-   * Constructor
-   * @param some expression yielding an event
-   * @param microphone_ expression yielding a microphone location
-   * @param toaMeasurement time of arrival at microphone
+   * Most general constructor with two expressions
+   * @param eventExpression expression yielding an event
+   * @param sensorExpression expression yielding a sensor location
+   * @param toaMeasurement time of arrival at sensor
    * @param model noise model
+   * @param speed optional speed of signal, in m/sec
    */
   TOAFactor(const Expression<Event>& eventExpression,
-      const Expression<Point3>& microphone_, double toaMeasurement,
-      const SharedNoiseModel& model) :
-      ExpressionFactor<double>(model, toaMeasurement,
-          Double_(&Event::toa, eventExpression, microphone_)) {
-  }
+            const Expression<Point3>& sensorExpression, double toaMeasurement,
+            const SharedNoiseModel& model, double speed = 330)
+      : ExpressionFactor<double>(
+            model, toaMeasurement,
+            Double_(TimeOfArrival(speed), eventExpression, sensorExpression)) {}
 
+  /**
+   * Constructor with fixed sensor
+   * @param eventExpression expression yielding an event
+   * @param sensor a known sensor location
+   * @param toaMeasurement time of arrival at sensor
+   * @param model noise model
+   * @param toa optional time of arrival functor
+   */
+  TOAFactor(const Expression<Event>& eventExpression, const Point3& sensor,
+            double toaMeasurement, const SharedNoiseModel& model,
+            double speed = 330)
+      : TOAFactor(eventExpression, Expression<Point3>(sensor), toaMeasurement,
+                  model, speed) {}
+
+  static void InsertEvent(Key key, const Event& event,
+                          boost::shared_ptr<Values> values) {
+    values->insert(key, event);
+  }
 };
 
-} //\ namespace gtsam
-
+}  // namespace gtsam
diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp
index 17c95e614..8a661f2ef 100644
--- a/gtsam_unstable/slam/serialization.cpp
+++ b/gtsam_unstable/slam/serialization.cpp
@@ -15,7 +15,7 @@
 #include <gtsam/slam/BetweenFactor.h>
 //#include <gtsam/slam/BoundingConstraint.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/sam/RangeFactor.h>
 #include <gtsam/slam/StereoFactor.h>
@@ -31,20 +31,21 @@
 using namespace gtsam;
 
 // Creating as many permutations of factors as possible
-typedef PriorFactor<LieVector>         PriorFactorLieVector;
-typedef PriorFactor<LieMatrix>         PriorFactorLieMatrix;
-typedef PriorFactor<Point2>            PriorFactorPoint2;
-typedef PriorFactor<StereoPoint2>      PriorFactorStereoPoint2;
-typedef PriorFactor<Point3>            PriorFactorPoint3;
-typedef PriorFactor<Rot2>              PriorFactorRot2;
-typedef PriorFactor<Rot3>              PriorFactorRot3;
-typedef PriorFactor<Pose2>             PriorFactorPose2;
-typedef PriorFactor<Pose3>             PriorFactorPose3;
-typedef PriorFactor<Cal3_S2>           PriorFactorCal3_S2;
-typedef PriorFactor<Cal3DS2>           PriorFactorCal3DS2;
-typedef PriorFactor<CalibratedCamera>  PriorFactorCalibratedCamera;
-typedef PriorFactor<SimpleCamera>      PriorFactorSimpleCamera;
-typedef PriorFactor<StereoCamera>      PriorFactorStereoCamera;
+typedef PriorFactor<LieVector>            PriorFactorLieVector;
+typedef PriorFactor<LieMatrix>            PriorFactorLieMatrix;
+typedef PriorFactor<Point2>               PriorFactorPoint2;
+typedef PriorFactor<StereoPoint2>         PriorFactorStereoPoint2;
+typedef PriorFactor<Point3>               PriorFactorPoint3;
+typedef PriorFactor<Rot2>                 PriorFactorRot2;
+typedef PriorFactor<Rot3>                 PriorFactorRot3;
+typedef PriorFactor<Pose2>                PriorFactorPose2;
+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;
 
 typedef BetweenFactor<LieVector>       BetweenFactorLieVector;
 typedef BetweenFactor<LieMatrix>       BetweenFactorLieMatrix;
@@ -55,29 +56,32 @@ typedef BetweenFactor<Rot3>            BetweenFactorRot3;
 typedef BetweenFactor<Pose2>           BetweenFactorPose2;
 typedef BetweenFactor<Pose3>           BetweenFactorPose3;
 
-typedef NonlinearEquality<LieVector>         NonlinearEqualityLieVector;
-typedef NonlinearEquality<LieMatrix>         NonlinearEqualityLieMatrix;
-typedef NonlinearEquality<Point2>            NonlinearEqualityPoint2;
-typedef NonlinearEquality<StereoPoint2>      NonlinearEqualityStereoPoint2;
-typedef NonlinearEquality<Point3>            NonlinearEqualityPoint3;
-typedef NonlinearEquality<Rot2>              NonlinearEqualityRot2;
-typedef NonlinearEquality<Rot3>              NonlinearEqualityRot3;
-typedef NonlinearEquality<Pose2>             NonlinearEqualityPose2;
-typedef NonlinearEquality<Pose3>             NonlinearEqualityPose3;
-typedef NonlinearEquality<Cal3_S2>           NonlinearEqualityCal3_S2;
-typedef NonlinearEquality<Cal3DS2>           NonlinearEqualityCal3DS2;
-typedef NonlinearEquality<CalibratedCamera>  NonlinearEqualityCalibratedCamera;
-typedef NonlinearEquality<SimpleCamera>      NonlinearEqualitySimpleCamera;
-typedef NonlinearEquality<StereoCamera>      NonlinearEqualityStereoCamera;
+typedef NonlinearEquality<LieVector>              NonlinearEqualityLieVector;
+typedef NonlinearEquality<LieMatrix>              NonlinearEqualityLieMatrix;
+typedef NonlinearEquality<Point2>                 NonlinearEqualityPoint2;
+typedef NonlinearEquality<StereoPoint2>           NonlinearEqualityStereoPoint2;
+typedef NonlinearEquality<Point3>                 NonlinearEqualityPoint3;
+typedef NonlinearEquality<Rot2>                   NonlinearEqualityRot2;
+typedef NonlinearEquality<Rot3>                   NonlinearEqualityRot3;
+typedef NonlinearEquality<Pose2>                  NonlinearEqualityPose2;
+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;
 
-typedef RangeFactor<Pose2, Point2>                      RangeFactor2D;
-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<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
-typedef RangeFactor<SimpleCamera, SimpleCamera>         RangeFactorSimpleCamera;
+typedef RangeFactor<Pose2, Point2>                              RangeFactor2D;
+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;
 typedef BearingRangeFactor<Pose3, Point3>  BearingRangeFactor3D;
@@ -85,7 +89,7 @@ typedef BearingRangeFactor<Pose3, Point3>  BearingRangeFactor3D;
 typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorCal3_S2;
 typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
 
-typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
 //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 
 typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
@@ -126,6 +130,7 @@ 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);
 
 /* Create GUIDs for factors */
diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
index 5bb4dfd2e..70368cc0e 100644
--- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
+++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
@@ -13,7 +13,6 @@
 #include <gtsam/base/numericalDerivative.h>
 
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 
 
 using namespace std;
@@ -280,8 +279,8 @@ TEST (BetweenFactorEM, updateNoiseModel ) {
   SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2));
 
   NonlinearFactorGraph graph;
-  graph.push_back(gtsam::PriorFactor<Pose2>(key1, p1, model));
-  graph.push_back(gtsam::PriorFactor<Pose2>(key2, p2, model));
+  graph.addPrior(key1, p1, model);
+  graph.addPrior(key2, p2, model);
 
   f.updateNoiseModels(values, graph);
 
diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp
index a74bfc3c6..14ad43ae2 100644
--- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp
+++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp
@@ -7,7 +7,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -23,7 +23,7 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
 
 // camera pose at (0,0,1) looking straight along the x-axis.
 Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
-SimpleCamera level_camera(level_pose, *K);
+PinholeCamera<Cal3_S2> level_camera(level_pose, *K);
 
 typedef InvDepthFactor3<Pose3, Vector5, double> InverseDepthFactor;
 typedef NonlinearEquality<Pose3> PoseConstraint;
@@ -64,7 +64,7 @@ TEST( InvDepthFactor, optimize) {
 
   // add a camera 2 meters to the right
   Pose3 right_pose = level_pose * Pose3(Rot3(), Point3(2,0,0));
-  SimpleCamera right_camera(right_pose, *K);
+  PinholeCamera<Cal3_S2> right_camera(right_pose, *K);
 
   // projection measurement of landmark into right camera
   // this measurement disagrees with the depth initialization
diff --git a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp
index ca91d4cb5..141a50465 100644
--- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp
+++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp
@@ -16,7 +16,6 @@
  *  @date Nov 2009
  */
 
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam_unstable/slam/MultiProjectionFactor.h>
diff --git a/gtsam_unstable/slam/tests/testOccupancyGrid.cpp b/gtsam_unstable/slam/tests/testOccupancyGrid.cpp
index d92e5442e..1d28a7523 100644
--- a/gtsam_unstable/slam/tests/testOccupancyGrid.cpp
+++ b/gtsam_unstable/slam/tests/testOccupancyGrid.cpp
@@ -11,13 +11,12 @@
 
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/geometry/Pose2.h>
-#include <boost/random/mersenne_twister.hpp>
-//#include <boost/random/uniform_int_distribution.hpp>  // FIXME: does not exist in boost 1.46
-#include <boost/random/uniform_int.hpp> // Old header - should still exist
 
 #include <vector>
+#include <cmath.>
+#include <random>
+
 #include <stdlib.h>
-#include <math.h>
 
 using namespace std;
 using namespace gtsam;
@@ -229,8 +228,8 @@ public:
     Marginals marginals(size);
 
     // NOTE: using older interface for boost.random due to interface changes after boost 1.46
-    boost::mt19937 rng;
-    boost::uniform_int<Index> random_cell(0,size-1);
+    std::mt19937 rng;
+    std::uniform_int_distribution<> random_cell(0, size - 1);
 
     // run Metropolis for the requested number of operations
     // compute initial probability of occupancy grid, P(x_t)
diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.h b/gtsam_unstable/slam/tests/testPoseToPointFactor.h
new file mode 100644
index 000000000..8f8563e9d
--- /dev/null
+++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.h
@@ -0,0 +1,86 @@
+/**
+ * @file   testPoseToPointFactor.cpp
+ * @brief
+ * @author David Wisth
+ * @date   June 20, 2020
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam_unstable/slam/PoseToPointFactor.h>
+
+using namespace gtsam;
+using namespace gtsam::noiseModel;
+
+/// Verify zero error when there is no noise
+TEST(PoseToPointFactor, errorNoiseless) {
+  Pose3 pose = Pose3::identity();
+  Point3 point(1.0, 2.0, 3.0);
+  Point3 noise(0.0, 0.0, 0.0);
+  Point3 measured = t + noise;
+
+  Key pose_key(1);
+  Key point_key(2);
+  PoseToPointFactor factor(pose_key, point_key, measured,
+                           Isotropic::Sigma(3, 0.05));
+  Vector expectedError = Vector3(0.0, 0.0, 0.0);
+  Vector actualError = factor.evaluateError(pose, point);
+  EXPECT(assert_equal(expectedError, actualError, 1E-5));
+}
+
+/// Verify expected error in test scenario
+TEST(PoseToPointFactor, errorNoise) {
+  Pose3 pose = Pose3::identity();
+  Point3 point(1.0, 2.0, 3.0);
+  Point3 noise(-1.0, 0.5, 0.3);
+  Point3 measured = t + noise;
+
+  Key pose_key(1);
+  Key point_key(2);
+  PoseToPointFactor factor(pose_key, point_key, measured,
+                           Isotropic::Sigma(3, 0.05));
+  Vector expectedError = noise;
+  Vector actualError = factor.evaluateError(pose, point);
+  EXPECT(assert_equal(expectedError, actualError, 1E-5));
+}
+
+/// Check Jacobians are correct
+TEST(PoseToPointFactor, jacobian) {
+  // Measurement
+  gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3);
+
+  // Linearisation point
+  gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2);
+  gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5 * M_PI, -0.3 * M_PI, 0.4 * M_PI);
+  Pose3 p(p_R, p_t);
+
+  gtsam::Point3 l = gtsam::Point3(3, 0, 5);
+
+  // Factor
+  Key pose_key(1);
+  Key point_key(2);
+  SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
+  PoseToPointFactor factor(pose_key, point_key, l_meas, noise);
+
+  // Calculate numerical derivatives
+  auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2,
+                       boost::none, boost::none);
+  Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l);
+  Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l);
+
+  // Use the factor to calculate the derivative
+  Matrix actual_H1;
+  Matrix actual_H2;
+  factor.evaluateError(p, l, actual_H1, actual_H2);
+
+  // Verify we get the expected error
+  EXPECT_TRUE(assert_equal(numerical_H1, actual_H1, 1e-8));
+  EXPECT_TRUE(assert_equal(numerical_H2, actual_H2, 1e-8));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
diff --git a/gtsam_unstable/slam/tests/testSerialization.cpp b/gtsam_unstable/slam/tests/testSerialization.cpp
index dc05711e3..792fd1133 100644
--- a/gtsam_unstable/slam/tests/testSerialization.cpp
+++ b/gtsam_unstable/slam/tests/testSerialization.cpp
@@ -15,7 +15,6 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
 
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
 
@@ -48,7 +47,7 @@ Values exampleValues() {
 
 NonlinearFactorGraph exampleGraph() {
   NonlinearFactorGraph graph;
-  graph.add(PriorFactor<Pose2>(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3))));
+  graph.addPrior(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)));
   graph.add(BetweenFactor<Pose2>(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3))));
   graph.add(BearingRangeFactor<Pose2,Point2>(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(Vector::Ones(2))));
   return graph;
diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp
index 8a6aab6b7..f900b2ffa 100644
--- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp
+++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp
@@ -19,7 +19,6 @@
 #include <gtsam_unstable/slam/SmartRangeFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
@@ -116,8 +115,8 @@ TEST( SmartRangeFactor, optimization ) {
   graph.push_back(f);
   const noiseModel::Base::shared_ptr //
   priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI));
-  graph.emplace_shared<PriorFactor<Pose2> >(1, pose1, priorNoise);
-  graph.emplace_shared<PriorFactor<Pose2> >(2, pose2, priorNoise);
+  graph.addPrior(1, pose1, priorNoise);
+  graph.addPrior(2, pose2, priorNoise);
 
   // Try optimizing
   LevenbergMarquardtParams params;
diff --git a/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp b/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp
index 432bcb7d4..107defb4c 100644
--- a/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp
+++ b/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp
@@ -198,9 +198,7 @@ TEST(testISAM2SmartFactor, Stereo_Batch) {
 
     // prior, for the first keyframe:
     if (kf_id == 0) {
-      const auto prior = boost::make_shared<PriorFactor<Pose3>>(
-          X(kf_id), Pose3::identity(), priorPoseNoise);
-      batch_graph.push_back(prior);
+      batch_graph.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise);
     }
 
     batch_values.insert(X(kf_id), Pose3::identity());
@@ -309,9 +307,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) {
 
     // prior, for the first keyframe:
     if (kf_id == 0) {
-      const auto prior = boost::make_shared<PriorFactor<Pose3>>(
-          X(kf_id), Pose3::identity(), priorPoseNoise);
-      newFactors.push_back(prior);
+      newFactors.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise);
     }
 
     // 2) Run iSAM2:
diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
index 0ac2c24ee..a0bfc3649 100644
--- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
+++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
@@ -335,8 +335,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
-  graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
+  graph.addPrior(x1, pose1, noisePrior);
+  graph.addPrior(x2, pose2, noisePrior);
 
   //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@@ -396,8 +396,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
   // add factors
   NonlinearFactorGraph graph2;
 
-  graph2.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
-  graph2.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
+  graph2.addPrior(x1, pose1, noisePrior);
+  graph2.addPrior(x2, pose2, noisePrior);
 
   typedef GenericStereoFactor<Pose3, Point3> ProjectionFactor;
 
@@ -477,8 +477,8 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
-  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
+  graph.addPrior(x1, pose1, noisePrior);
+  graph.addPrior(x2, pose2, noisePrior);
 
   //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@@ -523,9 +523,9 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
   Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
   Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
 
-  SimpleCamera cam1(cameraPose1, *K); // with camera poses
-  SimpleCamera cam2(cameraPose2, *K);
-  SimpleCamera cam3(cameraPose3, *K);
+  PinholeCamera<Cal3_S2> cam1(cameraPose1, *K); // with camera poses
+  PinholeCamera<Cal3_S2> cam2(cameraPose2, *K);
+  PinholeCamera<Cal3_S2> cam3(cameraPose3, *K);
 
   // create arbitrary body_Pose_sensor (transforms from sensor to body)
   Pose3 sensor_to_body =  Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
@@ -586,8 +586,8 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior));
-  graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior));
+  graph.addPrior(x1, bodyPose1, noisePrior);
+  graph.addPrior(x2, bodyPose2, noisePrior);
 
   // Check errors at ground truth poses
   Values gtValues;
@@ -660,8 +660,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
-  graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
+  graph.addPrior(x1, pose1, noisePrior);
+  graph.addPrior(x2, pose2, noisePrior);
 
   //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@@ -732,8 +732,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
-  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
+  graph.addPrior(x1, pose1, noisePrior);
+  graph.addPrior(x2, pose2, noisePrior);
 
   //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@@ -801,8 +801,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
-  graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
+  graph.addPrior(x1, pose1, noisePrior);
+  graph.addPrior(x2, pose2, noisePrior);
 
   //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@@ -883,8 +883,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
   graph.push_back(smartFactor4);
-  graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
-  graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
+  graph.addPrior(x1, pose1, noisePrior);
+  graph.addPrior(x2, pose2, noisePrior);
 
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp
index 4b10d5c0b..042130a24 100644
--- a/gtsam_unstable/slam/tests/testTOAFactor.cpp
+++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp
@@ -17,15 +17,16 @@
  *  @date December 2014
  */
 
-#include <gtsam_unstable/geometry/Event.h>
-#include <gtsam/nonlinear/ExpressionFactorGraph.h>
-#include <gtsam/nonlinear/expressions.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam_unstable/geometry/Event.h>
+#include <gtsam_unstable/slam/TOAFactor.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/format.hpp>
 #include <boost/bind.hpp>
+#include <boost/format.hpp>
 
 using namespace std;
 using namespace gtsam;
@@ -43,83 +44,59 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
 
 static const double timeOfEvent = 25;
 static const Event exampleEvent(timeOfEvent, 1, 0, 0);
-static const Point3 microphoneAt0(0,0,0);
+static const Point3 sensorAt0(0, 0, 0);
 
 //*****************************************************************************
-TEST( TOAFactor, NewWay ) {
+TEST(TOAFactor, NewWay) {
   Key key = 12;
-  Event_ eventExpression(key);
-  Point3_ microphoneConstant(microphoneAt0); // constant expression
   double measurement = 7;
-  Double_ expression(&Event::toa, eventExpression, microphoneConstant);
-  ExpressionFactor<double> factor(model, measurement, expression);
+  TOAFactor factor(key, sensorAt0, measurement, model);
 }
 
 //*****************************************************************************
-TEST( TOAFactor, WholeEnchilada ) {
-
-  static const bool verbose = false;
-
-  // Create microphones
+TEST(TOAFactor, WholeEnchilada) {
+  // Create sensors
   const double height = 0.5;
-  vector<Point3> microphones;
-  microphones.push_back(Point3(0, 0, height));
-  microphones.push_back(Point3(403 * cm, 0, height));
-  microphones.push_back(Point3(403 * cm, 403 * cm, height));
-  microphones.push_back(Point3(0, 403 * cm, 2 * height));
-  EXPECT_LONGS_EQUAL(4, microphones.size());
-//  microphones.push_back(Point3(200 * cm, 200 * cm, height));
+  vector<Point3> sensors;
+  sensors.push_back(Point3(0, 0, height));
+  sensors.push_back(Point3(403 * cm, 0, height));
+  sensors.push_back(Point3(403 * cm, 403 * cm, height));
+  sensors.push_back(Point3(0, 403 * cm, 2 * height));
+  EXPECT_LONGS_EQUAL(4, sensors.size());
+  //  sensors.push_back(Point3(200 * cm, 200 * cm, height));
 
   // Create a ground truth point
   const double timeOfEvent = 0;
   Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm);
 
   // Simulate simulatedTOA
-  size_t K = microphones.size();
+  size_t K = sensors.size();
   vector<double> simulatedTOA(K);
+  TimeOfArrival toa;
   for (size_t i = 0; i < K; i++) {
-    simulatedTOA[i] = groundTruthEvent.toa(microphones[i]);
-    if (verbose) {
-      cout << "mic" << i << " = " << microphones[i] << endl;
-      cout << "z" << i << " = " << simulatedTOA[i] / ms << endl;
-    }
+    simulatedTOA[i] = toa(groundTruthEvent, sensors[i]);
   }
 
   // Now, estimate using non-linear optimization
-  ExpressionFactorGraph graph;
+  NonlinearFactorGraph graph;
   Key key = 12;
-  Event_ eventExpression(key);
   for (size_t i = 0; i < K; i++) {
-    Point3_ microphone_i(microphones[i]); // constant expression
-    Double_ predictTOA(&Event::toa, eventExpression, microphone_i);
-    graph.addExpressionFactor(predictTOA, simulatedTOA[i], model);
+    graph.emplace_shared<TOAFactor>(key, sensors[i], simulatedTOA[i], model);
   }
 
-  /// Print the graph
-  if (verbose)
-    GTSAM_PRINT(graph);
-
   // Create initial estimate
   Values initialEstimate;
-  //Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
+  // Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
   Vector4 delta;
   delta << 0.1, 0.1, -0.1, 0.1;
   Event estimatedEvent = groundTruthEvent.retract(delta);
   initialEstimate.insert(key, estimatedEvent);
 
-  // Print
-  if (verbose)
-    initialEstimate.print("Initial Estimate:\n");
-
   // Optimize using Levenberg-Marquardt optimization.
   LevenbergMarquardtParams params;
   params.setAbsoluteErrorTol(1e-10);
-  if (verbose)
-    params.setVerbosity("ERROR");
   LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
   Values result = optimizer.optimize();
-  if (verbose)
-    result.print("Final Result:\n");
 
   EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6));
 }
@@ -129,4 +106,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 //*****************************************************************************
-
diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp
index 26ed76d02..a7b9136f8 100644
--- a/gtsam_unstable/timing/timeDSFvariants.cpp
+++ b/gtsam_unstable/timing/timeDSFvariants.cpp
@@ -16,24 +16,23 @@
  * @date    Oct 26, 2013
  */
 
+#include <gtsam/base/timing.h>
 #include <gtsam/base/DSFVector.h>
 #include <gtsam_unstable/base/DSF.h>
 #include <gtsam/base/DSFMap.h>
 
-#include <boost/random.hpp>
-#include <boost/timer.hpp>
 #include <boost/format.hpp>
 #include <boost/assign/std/vector.hpp>
 
-#include <iostream>
 #include <fstream>
+#include <iostream>
+#include <random>
 #include <vector>
 #include <utility>
 
 using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
-using boost::timer;
 using boost::format;
 
 int main(int argc, char* argv[]) {
@@ -59,58 +58,79 @@ int main(int argc, char* argv[]) {
     cout << format("\nTesting with %1% images, %2% points, %3% matches\n")
             % (int)m % (int)N % (int)nm;
     cout << "Generating " << nm << " matches" << endl;
-    boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rn(
-        boost::mt19937(), boost::uniform_int<size_t>(0, N - 1));
+    std::mt19937 rng;
+    std::uniform_int_distribution<> rn(0, N - 1);
+
     typedef pair<size_t, size_t> Match;
     vector<Match> matches;
     matches.reserve(nm);
     for (size_t k = 0; k < nm; k++)
-      matches.push_back(Match(rn(), rn()));
+      matches.push_back(Match(rn(rng), rn(rng)));
 
     os << format("%1%,%2%,%3%,") % (int)m % (int)N % (int)nm;
 
     {
       // DSFBase version
-      timer tim;
+      double dsftime = 0;
+      gttic_(dsftime);
       DSFBase dsf(N); // Allow for N keys
       for(const Match& m: matches)
         dsf.merge(m.first, m.second);
-      os << tim.elapsed() << ",";
-      cout << format("DSFBase: %1% s") % tim.elapsed() << endl;
+      gttoc_(dsftime);
+      tictoc_getNode(dsftimeNode, dsftime);
+      dsftime = dsftimeNode->secs();
+      os << dsftime << ",";
+      cout << format("DSFBase: %1% s") % dsftime << endl;
+      tictoc_reset_();
     }
 
     {
       // DSFMap version
-      timer tim;
+      double dsftime = 0;
+      gttic_(dsftime);
       DSFMap<size_t> dsf;
       for(const Match& m: matches)
         dsf.merge(m.first, m.second);
-      os << tim.elapsed() << endl;
-      cout << format("DSFMap: %1% s") % tim.elapsed() << endl;
+      gttoc_(dsftime);
+      tictoc_getNode(dsftimeNode, dsftime);
+      dsftime = dsftimeNode->secs();
+      os << dsftime << endl;
+      cout << format("DSFMap: %1% s") % dsftime << endl;
+      tictoc_reset_();
     }
 
     if (false) {
       // DSF version, functional
-      timer tim;
+      double dsftime = 0;
+      gttic_(dsftime);
       DSF<size_t> dsf;
       for (size_t j = 0; j < N; j++)
         dsf = dsf.makeSet(j);
       for(const Match& m: matches)
         dsf = dsf.makeUnion(m.first, m.second);
-      os << tim.elapsed() << endl;
-      cout << format("DSF functional: %1% s") % tim.elapsed() << endl;
+      gttoc_(dsftime);
+      tictoc_getNode(dsftimeNode, dsftime);
+      dsftime = dsftimeNode->secs();
+      os << dsftime << endl;
+      cout << format("DSF functional: %1% s") % dsftime << endl;
+      tictoc_reset_();
     }
 
     if (false) {
       // DSF version, in place - always slower - use functional !
-      timer tim;
+      double dsftime = 0;
+      gttic_(dsftime);
       DSF<size_t> dsf;
       for (size_t j = 0; j < N; j++)
         dsf.makeSetInPlace(j);
       for(const Match& m: matches)
         dsf.makeUnionInPlace(m.first, m.second);
-      os << tim.elapsed() << ",";
-      cout << format("DSF in-place: %1% s") % tim.elapsed() << endl;
+      gttoc_(dsftime);
+      tictoc_getNode(dsftimeNode, dsftime);
+      dsftime = dsftimeNode->secs();
+      os << dsftime << ",";
+      cout << format("DSF in-place: %1% s") % dsftime << endl;
+      tictoc_reset_();
     }
 
   }
diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m
index 10fd5e142..035e7e509 100644
--- a/matlab/+gtsam/Contents.m
+++ b/matlab/+gtsam/Contents.m
@@ -94,6 +94,7 @@
 %   Rot2                              - class Rot2, see Doxygen page for details
 %   Rot3                              - class Rot3, see Doxygen page for details
 %   SimpleCamera                      - class SimpleCamera, see Doxygen page for details
+%   PinholeCameraCal3_S2              - class PinholeCameraCal3_S2, see Doxygen page for details
 %   StereoPoint2                      - class StereoPoint2, see Doxygen page for details
 % 
 %% SLAM and SFM
diff --git a/matlab/+gtsam/VisualISAMGenerateData.m b/matlab/+gtsam/VisualISAMGenerateData.m
index ab47e92db..57f9439a4 100644
--- a/matlab/+gtsam/VisualISAMGenerateData.m
+++ b/matlab/+gtsam/VisualISAMGenerateData.m
@@ -31,7 +31,7 @@ data.K = truth.K;
 for i=1:options.nrCameras
     theta = (i-1)*2*pi/options.nrCameras;
     t = Point3([r*cos(theta), r*sin(theta), height]');
-    truth.cameras{i} = SimpleCamera.Lookat(t, Point3, Point3([0,0,1]'), truth.K);
+    truth.cameras{i} = PinholeCameraCal3_S2.Lookat(t, Point3, Point3([0,0,1]'), truth.K);
     % Create measurements
     for j=1:nrPoints
         % All landmarks seen in every frame
diff --git a/matlab/+gtsam/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m
index 4364e0fe4..51026f698 100644
--- a/matlab/+gtsam/covarianceEllipse3D.m
+++ b/matlab/+gtsam/covarianceEllipse3D.m
@@ -22,8 +22,8 @@ end
 % rotate data with orientation matrix U and center M
 data = kron(e(:,1),xc) + kron(e(:,2),yc) + kron(e(:,3),zc);
 n = size(data,2);
-x = data(1:n,:)+c(1); 
-y = data(n+1:2*n,:)+c(2); 
+x = data(1:n,:)+c(1);
+y = data(n+1:2*n,:)+c(2);
 z = data(2*n+1:end,:)+c(3);
 
 % now plot the rotated ellipse
diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m
index 71f72f6b9..2b913b52d 100644
--- a/matlab/+gtsam/cylinderSampleProjection.m
+++ b/matlab/+gtsam/cylinderSampleProjection.m
@@ -13,7 +13,7 @@ function [visiblePoints] = cylinderSampleProjection(K, pose, imageSize, cylinder
 
 import gtsam.*
 
-camera = SimpleCamera(pose, K);
+camera = PinholeCameraCal3_S2(pose, K);
 
 %% memory allocation
 cylinderNum = length(cylinders);
diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m
index 36b7635e2..add2bc75a 100644
--- a/matlab/gtsam_examples/CameraFlyingExample.m
+++ b/matlab/gtsam_examples/CameraFlyingExample.m
@@ -92,7 +92,7 @@ if options.enableTests
         cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
     end
 
-    camera = SimpleCamera.Lookat(Point3(10, 50, 10), ... 
+    camera = PinholeCameraCal3_S2.Lookat(Point3(10, 50, 10), ... 
         Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
         Point3([0,0,1]'), options.monoK); 
 
@@ -154,7 +154,7 @@ while 1
     
     %t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ...
     %    15, 10]');
-    camera = SimpleCamera.Lookat(t, ... 
+    camera = PinholeCameraCal3_S2.Lookat(t, ... 
         Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
         Point3([0,0,1]'), options.camera.monoK);    
     cameraPoses{end+1} = camera.pose;
diff --git a/matlab/gtsam_examples/IMUKittiExampleGPS.m b/matlab/gtsam_examples/IMUKittiExampleGPS.m
index c80b6ec3e..530c3382c 100755
--- a/matlab/gtsam_examples/IMUKittiExampleGPS.m
+++ b/matlab/gtsam_examples/IMUKittiExampleGPS.m
@@ -99,7 +99,7 @@ for measurementIndex = firstGPSPose:length(GPS_data)
     newFactors.add(ImuFactor( ...
       currentPoseKey-1, currentVelKey-1, ...
       currentPoseKey, currentVelKey, ...
-      currentBiasKey, currentSummarizedMeasurement));
+      currentBiasKey-1, currentSummarizedMeasurement));
 
     % Bias evolution as given in the IMU metadata
     newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
diff --git a/matlab/gtsam_examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m
index 7f50f2db8..584ace09a 100644
--- a/matlab/gtsam_examples/SBAExample.m
+++ b/matlab/gtsam_examples/SBAExample.m
@@ -46,7 +46,7 @@ end
 
 %% Add Gaussian priors for a pose and a landmark to constrain the system
 cameraPriorNoise  = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas);
-firstCamera = SimpleCamera(truth.cameras{1}.pose, truth.K);
+firstCamera = PinholeCameraCal3_S2(truth.cameras{1}.pose, truth.K);
 graph.add(PriorFactorSimpleCamera(symbol('c',1), firstCamera, cameraPriorNoise));
 
 pointPriorNoise  = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
@@ -60,7 +60,7 @@ graph.print(sprintf('\nFactor graph:\n'));
 initialEstimate = Values;
 for i=1:size(truth.cameras,2)
     pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1));
-    camera_i = SimpleCamera(pose_i, truth.K);
+    camera_i = PinholeCameraCal3_S2(pose_i, truth.K);
     initialEstimate.insert(symbol('c',i), camera_i);
 end
 for j=1:size(truth.points,2)
diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m
new file mode 100644
index 000000000..8a0485334
--- /dev/null
+++ b/matlab/gtsam_examples/VisualizeMEstimators.m
@@ -0,0 +1,73 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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
+%
+% @brief Plot visualizations of residuals, residual derivatives, and weights for the various mEstimators.
+% @author Varun Agrawal
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% import gtsam.*
+
+close all;
+
+x = linspace(-10, 10, 1000);
+
+%% Define all the mEstimator models and plot them
+
+c = 1.3998;
+fairNoiseModel = gtsam.noiseModel.mEstimator.Fair(c);
+plot_m_estimator(x, fairNoiseModel, 'Fair', 1, 'fair.png')
+
+c = 1.345;
+huberNoiseModel = gtsam.noiseModel.mEstimator.Huber(c);
+plot_m_estimator(x, huberNoiseModel, 'Huber', 2, 'huber.png')
+
+c = 0.1;
+cauchyNoiseModel = gtsam.noiseModel.mEstimator.Cauchy(c);
+plot_m_estimator(x, cauchyNoiseModel, 'Cauchy', 3, 'cauchy.png')
+
+c = 1.0;
+gemanmcclureNoiseModel = gtsam.noiseModel.mEstimator.GemanMcClure(c);
+plot_m_estimator(x, gemanmcclureNoiseModel, 'Geman-McClure', 4, 'gemanmcclure.png')
+
+c = 2.9846;
+welschNoiseModel = gtsam.noiseModel.mEstimator.Welsch(c);
+plot_m_estimator(x, welschNoiseModel, 'Welsch', 5, 'welsch.png')
+
+c = 4.6851;
+tukeyNoiseModel = gtsam.noiseModel.mEstimator.Tukey(c);
+plot_m_estimator(x, tukeyNoiseModel, 'Tukey', 6, 'tukey.png')
+
+%% Plot rho, psi and weights of the mEstimator.
+function plot_m_estimator(x, model, plot_title, fig_id, filename)
+    w = zeros(size(x));
+    rho = zeros(size(x));
+    for i = 1:size(x, 2)
+        w(i) = model.weight(x(i));
+        rho(i) = model.residual(x(i));
+    end
+
+    psi = w .* x;
+
+    figure(fig_id);
+    subplot(3, 1, 1);
+    plot(x, rho, 'LineWidth',2);
+    title('rho function');
+    xlim([-5, 5]);
+    subplot(3, 1, 2);
+    plot(x, psi, 'LineWidth',2);
+    title('influence function');
+    xlim([-5, 5]);
+    subplot(3, 1, 3);
+    plot(x, w, 'LineWidth',2);
+    title('weight function');
+    xlim([-5, 5]);
+
+    sgtitle(plot_title, 'FontSize', 26);
+
+    saveas(figure(fig_id), filename);
+end
diff --git a/matlab/gtsam_tests/testTriangulation.m b/matlab/gtsam_tests/testTriangulation.m
index d46493328..7116d3838 100644
--- a/matlab/gtsam_tests/testTriangulation.m
+++ b/matlab/gtsam_tests/testTriangulation.m
@@ -18,11 +18,11 @@ sharedCal = Cal3_S2(1500, 1200, 0, 640, 480);
 %% Looking along X-axis, 1 meter above ground plane (x-y)
 upright = Rot3.Ypr(-pi / 2, 0., -pi / 2);
 pose1 = Pose3(upright, Point3(0, 0, 1));
-camera1 = SimpleCamera(pose1, sharedCal);
+camera1 = PinholeCameraCal3_S2(pose1, sharedCal);
 
 %% create second camera 1 meter to the right of first camera
 pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)));
-camera2 = SimpleCamera(pose2, sharedCal);
+camera2 = PinholeCameraCal3_S2(pose2, sharedCal);
 
 %% landmark ~5 meters infront of camera
 landmark =Point3 (5, 0.5, 1.2);
diff --git a/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m b/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m
index d28d3c2cb..71ff20ec2 100644
--- a/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m
+++ b/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m
@@ -25,12 +25,12 @@ graph = NonlinearFactorGraph;
 
 %% Add a Gaussian prior on pose x_1
 priorMean = Pose3();%Pose3.Expmap([0.0; 0.0; 0.0; 0.0; 0.0; 0.0]); % prior mean is at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.3; 0.1; 0.1; 0.1]); % 30cm std on x,y, 0.1 rad on theta
+priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.1; 0.3; 0.3; 0.3]); % 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
 graph.add(PriorFactorPose3(1, priorMean, priorNoise)); % add directly to graph
 
 %% Add two odometry factors
 odometry = Pose3.Expmap([0.0; 0.0; 0.0; 2.0; 0.0; 0.0]); % create a measurement for both factors (the same in this case)
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.2; 0.1; 0.1; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+odometryNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.1; 0.2; 0.2; 0.2]); % 20cm std on x,y,z 0.1 rad on roll,pitch,yaw
 graph.add(BetweenFactorPose3(1, 2, odometry, odometryNoise));
 graph.add(BetweenFactorPose3(2, 3, odometry, odometryNoise));
 
diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
index 2cceea753..195b7ff69 100644
--- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
+++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
@@ -97,8 +97,8 @@ if options.useRealData == 1
       % Create the camera based on the current pose and the pose of the
       % camera in the body
       cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera);
-      camera = SimpleCamera(cameraPose, metadata.camera.calibration);
-      %camera = SimpleCamera(currentPose, metadata.camera.calibration);
+      camera = PinholeCameraCal3_S2(cameraPose, metadata.camera.calibration);
+      %camera = PinholeCameraCal3_S2(currentPose, metadata.camera.calibration);
       
       % Record measurements if the landmark is within visual range
       for j=1:length(metadata.camera.gtLandmarkPoints)
diff --git a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m
index c9e912ea4..129b8c176 100644
--- a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m
+++ b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m
@@ -108,7 +108,7 @@ for i=1:20
     % generate some camera measurements
     cam_pose = initial.atPose3(i).compose(actual_transform);
 %     gtsam.plotPose3(cam_pose);
-    cam = SimpleCamera(cam_pose,K);
+    cam = PinholeCameraCal3_S2(cam_pose,K);
     i
 %     result
     for j=1:nrPoints
diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
index fd4a17469..4557d711f 100644
--- a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
+++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
@@ -182,7 +182,7 @@ for i=1:steps
     % generate some camera measurements
     cam_pose = currentIMUPoseGlobal.compose(actual_transform);
 %     gtsam.plotPose3(cam_pose);
-    cam = SimpleCamera(cam_pose,K);
+    cam = PinholeCameraCal3_S2(cam_pose,K);
     i
 %     result
     for j=1:nrPoints
diff --git a/matlab/unstable_examples/TransformProjectionFactorExample.m b/matlab/unstable_examples/TransformProjectionFactorExample.m
index 669073e56..79a96209d 100644
--- a/matlab/unstable_examples/TransformProjectionFactorExample.m
+++ b/matlab/unstable_examples/TransformProjectionFactorExample.m
@@ -73,7 +73,7 @@ for i=1:20
     % generate some camera measurements
     cam_pose = initial.atPose3(i).compose(actual_transform);
     gtsam.plotPose3(cam_pose);
-    cam = SimpleCamera(cam_pose,K);
+    cam = PinholeCameraCal3_S2(cam_pose,K);
     i
     for j=1:nrPoints
         % All landmarks seen in every frame
diff --git a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m
index 8edcfade7..ca5b70c62 100644
--- a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m
+++ b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m
@@ -98,7 +98,7 @@ for i=1:20
     % generate some camera measurements
     cam_pose = initial.atPose3(i).compose(actual_transform);
 %     gtsam.plotPose3(cam_pose);
-    cam = SimpleCamera(cam_pose,K);
+    cam = PinholeCameraCal3_S2(cam_pose,K);
     i
 %     result
     for j=1:nrPoints
diff --git a/matlab/unstable_examples/project_landmarks.m b/matlab/unstable_examples/project_landmarks.m
index 629c6d994..aaccc9248 100644
--- a/matlab/unstable_examples/project_landmarks.m
+++ b/matlab/unstable_examples/project_landmarks.m
@@ -4,7 +4,7 @@ function [ measurements ] = project_landmarks( pose, landmarks, K )
 
     import gtsam.*;
 
-    camera = SimpleCamera(pose,K);    
+    camera = PinholeCameraCal3_S2(pose,K);    
     measurements = Values;
     
     for i=1:size(landmarks)-1
diff --git a/package.xml b/package.xml
index e14e77f2a..f8554729e 100644
--- a/package.xml
+++ b/package.xml
@@ -1,13 +1,18 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>gtsam</name>
-  <version>4.0.0</version>
+  <version>4.0.2</version>
   <description>gtsam</description>
 
   <maintainer email="gtsam@lists.gatech.edu">Frank Dellaert</maintainer>
   <license>BSD</license>
 
   <buildtool_depend>cmake</buildtool_depend>
+
+  <build_depend>boost</build_depend>
+  <build_depend>eigen</build_depend>
+  <build_depend>tbb</build_depend>
+
   <export>
     <!-- Specify that this is not really a Catkin package-->
     <build_type>cmake</build_type>
diff --git a/package_scripts/README.md b/package_scripts/README.md
deleted file mode 100644
index 1e33369aa..000000000
--- a/package_scripts/README.md
+++ /dev/null
@@ -1,14 +0,0 @@
-
-# How to generate Debian packages
-
-    cd [GTSAM_SOURCE_ROOT]
-    bash package_scripts/prepare_debian.sh
-
-
-# How to generate Ubuntu packages for a PPA
-
-    cd [GTSAM_SOURCE_ROOT]
-    bash package_scripts/prepare_ubuntu_pkgs_for_ppa.sh
-    cd ~/gtsam_ubuntu
-    bash [GTSAM_SOURCE_ROOT]/upload_all_gtsam_ppa.sh
-
diff --git a/package_scripts/compile_static_boost.sh b/package_scripts/compile_static_boost.sh
deleted file mode 100755
index ca3b99e09..000000000
--- a/package_scripts/compile_static_boost.sh
+++ /dev/null
@@ -1,8 +0,0 @@
-#!/bin/sh
-
-# Compile boost statically, with -fPIC to allow linking it into the mex
-# module (which is a dynamic library).  --disable-icu prevents depending
-# on libicu, which is unneeded and would require then linking the mex
-# module with it as well.  We just stage instead of install, then the
-# toolbox_package_unix.sh script uses the staged boost.
-./b2 link=static threading=multi cxxflags=-fPIC cflags=-fPIC --disable-icu -a -j4 stage
diff --git a/package_scripts/prepare_debian.sh b/package_scripts/prepare_debian.sh
deleted file mode 100755
index 7b083d8bb..000000000
--- a/package_scripts/prepare_debian.sh
+++ /dev/null
@@ -1,179 +0,0 @@
-#!/bin/bash
-# Prepare to build a Debian package.
-# Jose Luis Blanco Claraco, 2019 (for GTSAM)
-# Jose Luis Blanco Claraco, 2008-2018 (for MRPT)
-
-set -e   # end on error
-#set -x  # for debugging
-
-APPEND_SNAPSHOT_NUM=0
-IS_FOR_UBUNTU=0
-APPEND_LINUX_DISTRO=""
-VALUE_EXTRA_CMAKE_PARAMS=""
-while getopts "sud:c:" OPTION
-do
-     case $OPTION in
-         s)
-             APPEND_SNAPSHOT_NUM=1
-             ;;
-         u)
-             IS_FOR_UBUNTU=1
-             ;;
-         d)
-             APPEND_LINUX_DISTRO=$OPTARG
-             ;;
-         c)
-             VALUE_EXTRA_CMAKE_PARAMS=$OPTARG
-             ;;
-         ?)
-             echo "Unknown command line argument!"
-             exit 1
-             ;;
-     esac
-done
-
-if [ -f CMakeLists.txt ];
-then
-  source package_scripts/prepare_debian_gen_snapshot_version.sh
-else
-	echo "Error: cannot find CMakeList.txt. This script is intended to be run from the root of the source tree."
-	exit 1
-fi
-
-# Append snapshot?
-if [ $APPEND_SNAPSHOT_NUM == "1" ];
-then
-        CUR_SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
-        source $CUR_SCRIPT_DIR/prepare_debian_gen_snapshot_version.sh  # populate GTSAM_SNAPSHOT_VERSION
-
-        GTSAM_VERSION_STR="${GTSAM_VERSION_STR}~snapshot${GTSAM_SNAPSHOT_VERSION}${APPEND_LINUX_DISTRO}"
-else
-        GTSAM_VERSION_STR="${GTSAM_VERSION_STR}${APPEND_LINUX_DISTRO}"
-fi
-
-# Call prepare_release
-GTSAMSRC=`pwd`
-
-if [ -f $HOME/gtsam_release/gtsam*.tar.gz ];
-then
-  echo "## release file already exists. Reusing it."
-else
-  source package_scripts/prepare_release.sh
-  echo
-  echo "## Done prepare_release.sh"
-fi
-
-echo "=========== Generating GTSAM ${GTSAM_VER_MMP} Debian package =============="
-cd $GTSAMSRC
-
-set -x
-if [ -z "$GTSAM_DEB_DIR" ]; then
-        GTSAM_DEB_DIR="$HOME/gtsam_debian"
-fi
-GTSAM_EXTERN_DEBIAN_DIR="$GTSAMSRC/debian/"
-GTSAM_EXTERN_UBUNTU_PPA_DIR="$GTSAMSRC/debian/"
-
-if [ -f ${GTSAM_EXTERN_DEBIAN_DIR}/control ];
-then
-	echo "Using debian dir: ${GTSAM_EXTERN_DEBIAN_DIR}"
-else
-	echo "ERROR: Cannot find ${GTSAM_EXTERN_DEBIAN_DIR}"
-	exit 1
-fi
-
-GTSAM_DEBSRC_DIR=$GTSAM_DEB_DIR/gtsam-${GTSAM_VERSION_STR}
-
-echo "GTSAM_VERSION_STR: ${GTSAM_VERSION_STR}"
-echo "GTSAM_DEBSRC_DIR: ${GTSAM_DEBSRC_DIR}"
-
-# Prepare a directory for building the debian package:
-#
-rm -fR $GTSAM_DEB_DIR || true
-mkdir -p $GTSAM_DEB_DIR  || true
-
-# Orig tarball:
-echo "Copying orig tarball: gtsam_${GTSAM_VERSION_STR}.orig.tar.gz"
-cp $HOME/gtsam_release/gtsam*.tar.gz $GTSAM_DEB_DIR/gtsam_${GTSAM_VERSION_STR}.orig.tar.gz
-cd ${GTSAM_DEB_DIR}
-tar -xf gtsam_${GTSAM_VERSION_STR}.orig.tar.gz
-
-if [ ! -d "${GTSAM_DEBSRC_DIR}" ];
-then
-  mv gtsam-* ${GTSAM_DEBSRC_DIR}  # fix different dir names for Ubuntu PPA packages
-fi
-
-if [ ! -f "${GTSAM_DEBSRC_DIR}/CMakeLists.txt" ];
-then
-	echo "*ERROR*: Seems there was a problem copying sources to ${GTSAM_DEBSRC_DIR}... aborting script."
-	exit 1
-fi
-
-cd ${GTSAM_DEBSRC_DIR}
-
-# Copy debian directory:
-#mkdir debian
-cp -r ${GTSAM_EXTERN_DEBIAN_DIR}/* debian
-
-# Use modified control & rules files for Ubuntu PPA packages:
-#if [ $IS_FOR_UBUNTU == "1" ];
-#then
-	# already done: cp ${GTSAM_EXTERN_UBUNTU_PPA_DIR}/control.in debian/
-  # Ubuntu: force use of gcc-7:
-  #sed -i '9i\export CXX=/usr/bin/g++-7\' debian/rules
-  #sed -i '9i\export CC=/usr/bin/gcc-7\' debian/rules7
-#fi
-
-# Export signing pub key:
-mkdir debian/upstream/
-gpg --export --export-options export-minimal --armor > debian/upstream/signing-key.asc
-
-# Parse debian/ control.in --> control
-#mv debian/control.in debian/control
-#sed -i "s/@GTSAM_VER_MM@/${GTSAM_VER_MM}/g" debian/control
-
-# Replace the text "REPLACE_HERE_EXTRA_CMAKE_PARAMS" in the "debian/rules" file
-# with: ${${VALUE_EXTRA_CMAKE_PARAMS}}
-RULES_FILE=debian/rules
-sed -i -e "s/REPLACE_HERE_EXTRA_CMAKE_PARAMS/${VALUE_EXTRA_CMAKE_PARAMS}/g" $RULES_FILE
-echo "Using these extra parameters for CMake: '${VALUE_EXTRA_CMAKE_PARAMS}'"
-
-# Strip my custom files...
-rm debian/*.new || true
-
-
-# Figure out the next Debian version number:
-echo "Detecting next Debian version number..."
-
-CHANGELOG_UPSTREAM_VER=$( dpkg-parsechangelog | sed -n 's/Version:.*\([0-9]\.[0-9]*\.[0-9]*.*snapshot.*\)-.*/\1/p' )
-CHANGELOG_LAST_DEBIAN_VER=$( dpkg-parsechangelog | sed -n 's/Version:.*\([0-9]\.[0-9]*\.[0-9]*\).*-\([0-9]*\).*/\2/p' )
-
-echo " -> PREVIOUS UPSTREAM: $CHANGELOG_UPSTREAM_VER -> New: ${GTSAM_VERSION_STR}"
-echo " -> PREVIOUS DEBIAN VERSION: $CHANGELOG_LAST_DEBIAN_VER"
-
-# If we have the same upstream versions, increase the Debian version, otherwise create a new entry:
-if [ "$CHANGELOG_UPSTREAM_VER" = "$GTSAM_VERSION_STR" ];
-then
-	NEW_DEBIAN_VER=$[$CHANGELOG_LAST_DEBIAN_VER + 1]
-	echo "Changing to a new Debian version: ${GTSAM_VERSION_STR}-${NEW_DEBIAN_VER}"
-	DEBCHANGE_CMD="--newversion ${GTSAM_VERSION_STR}-${NEW_DEBIAN_VER}"
-else
-	DEBCHANGE_CMD="--newversion ${GTSAM_VERSION_STR}-1"
-fi
-
-echo "Adding a new entry to debian/changelog..."
-
-DEBEMAIL="Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>" debchange $DEBCHANGE_CMD -b --distribution unstable --force-distribution New version of upstream sources.
-
-echo "Copying back the new changelog to a temporary file in: ${GTSAM_EXTERN_DEBIAN_DIR}changelog.new"
-cp debian/changelog ${GTSAM_EXTERN_DEBIAN_DIR}changelog.new
-
-set +x
-
-echo "=============================================================="
-echo "Now, you can build the source Deb package with 'debuild -S -sa'"
-echo "=============================================================="
-
-cd ..
-ls -lh
-
-exit 0
diff --git a/package_scripts/prepare_debian_gen_snapshot_version.sh b/package_scripts/prepare_debian_gen_snapshot_version.sh
deleted file mode 100755
index 589d422fe..000000000
--- a/package_scripts/prepare_debian_gen_snapshot_version.sh
+++ /dev/null
@@ -1,25 +0,0 @@
-#!/bin/bash
-
-# See https://reproducible-builds.org/specs/source-date-epoch/
-# get SOURCE_DATE_EPOCH with UNIX time_t
-if [ -d ".git" ];
-then
-  SOURCE_DATE_EPOCH=$(git log -1 --pretty=%ct)
-else
-  echo "Error: intended for use from within a git repository"
-  exit 1
-fi
-GTSAM_SNAPSHOT_VERSION=$(date -d @$SOURCE_DATE_EPOCH +%Y%m%d-%H%M)
-
-GTSAM_SNAPSHOT_VERSION+="-git-"
-GTSAM_SNAPSHOT_VERSION+=`git rev-parse --short=8 HEAD`
-GTSAM_SNAPSHOT_VERSION+="-"
-
-# x.y.z version components:
-GTSAM_VERSION_MAJOR=$(grep "(GTSAM_VERSION_MAJOR" CMakeLists.txt | sed -r 's/^.*GTSAM_VERSION_MAJOR\s*([0-9])*.*$/\1/g')
-GTSAM_VERSION_MINOR=$(grep "(GTSAM_VERSION_MINOR" CMakeLists.txt | sed -r 's/^.*GTSAM_VERSION_MINOR\s*([0-9])*.*$/\1/g')
-GTSAM_VERSION_PATCH=$(grep "(GTSAM_VERSION_PATCH" CMakeLists.txt | sed -r 's/^.*GTSAM_VERSION_PATCH\s*([0-9])*.*$/\1/g')
-
-GTSAM_VER_MM="${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}"
-GTSAM_VER_MMP="${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}"
-GTSAM_VERSION_STR=$GTSAM_VER_MMP
diff --git a/package_scripts/prepare_release.sh b/package_scripts/prepare_release.sh
deleted file mode 100755
index 750fc27b3..000000000
--- a/package_scripts/prepare_release.sh
+++ /dev/null
@@ -1,71 +0,0 @@
-#!/bin/bash
-# Export sources from a git tree and prepare it for a public release.
-# Jose Luis Blanco Claraco, 2019 (for GTSAM)
-# Jose Luis Blanco Claraco, 2008-2018 (for MRPT)
-
-set -e  # exit on error
-#set -x  # for debugging
-
-# Checks
-# --------------------------------
-if [ -f version_prefix.txt ];
-then
-	if [ -z ${GTSAM_VERSION_STR+x} ];
-	then
-		source package_scripts/prepare_debian_gen_snapshot_version.sh
-	fi
-	echo "ERROR: Run this script from the GTSAM source tree root directory."
-	exit 1
-fi
-
-GTSAM_SRC=`pwd`
-OUT_RELEASES_DIR="$HOME/gtsam_release"
-
-OUT_DIR=$OUT_RELEASES_DIR/gtsam-${GTSAM_VERSION_STR}
-
-echo "=========== Generating GTSAM release ${GTSAM_VER_MMP} =================="
-echo "GTSAM_VERSION_STR   : ${GTSAM_VERSION_STR}"
-echo "OUT_DIR            : ${OUT_DIR}"
-echo "============================================================"
-echo
-
-# Prepare output directory:
-rm -fR $OUT_RELEASES_DIR  || true
-mkdir -p ${OUT_DIR}
-
-# Export / copy sources to target dir:
-if [ -d "$GTSAM_SRC/.git" ];
-then
-	echo "# Exporting git source tree to ${OUT_DIR}"
-	git archive --format=tar HEAD | tar -x -C ${OUT_DIR}
-
-	# Remove VCS control files:
-	find ${OUT_DIR} -name '.gitignore' | xargs rm
-
-	# Generate ./SOURCE_DATE_EPOCH with UNIX time_t
-	SOURCE_DATE_EPOCH=$(git log -1 --pretty=%ct)
-else
-	echo "# Copying sources to ${OUT_DIR}"
-	cp -R . ${OUT_DIR}
-
-	# Generate ./SOURCE_DATE_EPOCH with UNIX time_t
-	SOURCE_DATE_EPOCH=$(date +%s)
-fi
-
-# See https://reproducible-builds.org/specs/source-date-epoch/
-echo $SOURCE_DATE_EPOCH > ${OUT_DIR}/SOURCE_DATE_EPOCH
-
-cd ${OUT_DIR}
-
-# Dont include Debian files in releases:
-rm -fR package_scripts
-
-# Orig tarball:
-cd ..
-echo "# Creating orig tarball: gtsam-${GTSAM_VERSION_STR}.tar.gz"
-tar czf gtsam-${GTSAM_VERSION_STR}.tar.gz gtsam-${GTSAM_VERSION_STR}
-
-rm -fr gtsam-${GTSAM_VERSION_STR}
-
-# GPG signature:
-gpg --armor --detach-sign gtsam-${GTSAM_VERSION_STR}.tar.gz
diff --git a/package_scripts/prepare_ubuntu_pkgs_for_ppa.sh b/package_scripts/prepare_ubuntu_pkgs_for_ppa.sh
deleted file mode 100755
index 553e1884d..000000000
--- a/package_scripts/prepare_ubuntu_pkgs_for_ppa.sh
+++ /dev/null
@@ -1,90 +0,0 @@
-#!/bin/bash
-# Creates a set of packages for each different Ubuntu distribution, with the
-# intention of uploading them to:
-#   https://launchpad.net/~joseluisblancoc/
-#
-# JLBC, 2010
-# [Addition 2012:]
-#
-# You can declare a variable (in the caller shell) with extra flags for the
-# CMake in the final ./configure like:
-#  GTSAM_PKG_CUSTOM_CMAKE_PARAMS="\"-DDISABLE_SSE3=ON\""
-#
-
-set -e
-
-# List of distributions to create PPA packages for:
-LST_DISTROS=(xenial bionic disco eoan)
-
-# Checks
-# --------------------------------
-if [ -f CMakeLists.txt ];
-then
-	source package_scripts/prepare_debian_gen_snapshot_version.sh
-	echo "GTSAM version: ${GTSAM_VER_MMP}"
-else
-	echo "ERROR: Run this script from the GTSAM root directory."
-	exit 1
-fi
-
-if [ -z "${gtsam_ubuntu_OUT_DIR}" ]; then
-       export gtsam_ubuntu_OUT_DIR="$HOME/gtsam_ubuntu"
-fi
-GTSAMSRC=`pwd`
-if [ -z "${GTSAM_DEB_DIR}" ]; then
-       export GTSAM_DEB_DIR="$HOME/gtsam_debian"
-fi
-GTSAM_EXTERN_DEBIAN_DIR="$GTSAMSRC/debian/"
-EMAIL4DEB="Jose Luis Blanco (University of Malaga) <joseluisblancoc@gmail.com>"
-
-# Clean out dirs:
-rm -fr $gtsam_ubuntu_OUT_DIR/
-
-# -------------------------------------------------------------------
-# And now create the custom packages for each Ubuntu distribution:
-# -------------------------------------------------------------------
-count=${#LST_DISTROS[@]}
-IDXS=$(seq 0 $(expr $count - 1))
-
-cp ${GTSAM_EXTERN_DEBIAN_DIR}/changelog /tmp/my_changelog
-
-for IDX in ${IDXS};
-do
-	DEBIAN_DIST=${LST_DISTROS[$IDX]}
-
-	# -------------------------------------------------------------------
-	# Call the standard "prepare_debian.sh" script:
-	# -------------------------------------------------------------------
-	cd ${GTSAMSRC}
-	bash package_scripts/prepare_debian.sh -s -u -d ${DEBIAN_DIST}   -c "${GTSAM_PKG_CUSTOM_CMAKE_PARAMS}"
-
-	CUR_SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
-	source $CUR_SCRIPT_DIR/prepare_debian_gen_snapshot_version.sh # populate GTSAM_SNAPSHOT_VERSION
-
-	echo "===== Distribution: ${DEBIAN_DIST}  ========="
-	cd ${GTSAM_DEB_DIR}/gtsam-${GTSAM_VER_MMP}~snapshot${GTSAM_SNAPSHOT_VERSION}${DEBIAN_DIST}/debian
-	#cp ${GTSAM_EXTERN_DEBIAN_DIR}/changelog changelog
-	cp /tmp/my_changelog changelog
-	DEBCHANGE_CMD="--newversion ${GTSAM_VERSION_STR}~snapshot${GTSAM_SNAPSHOT_VERSION}${DEBIAN_DIST}-1"
-	echo "Changing to a new Debian version: ${DEBCHANGE_CMD}"
-	echo "Adding a new entry to debian/changelog for distribution ${DEBIAN_DIST}"
-	DEBEMAIL="Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>" debchange $DEBCHANGE_CMD -b --distribution ${DEBIAN_DIST} --force-distribution New version of upstream sources.
-
-	cp changelog /tmp/my_changelog
-
-	echo "Now, let's build the source Deb package with 'debuild -S -sa':"
-	cd ..
-	# -S: source package
-	# -sa: force inclusion of sources
-	# -d: don't check dependencies in this system
-	debuild -S -sa -d
-
-	# Make a copy of all these packages:
-	cd ..
-	mkdir -p $gtsam_ubuntu_OUT_DIR/$DEBIAN_DIST
-	cp gtsam_* $gtsam_ubuntu_OUT_DIR/${DEBIAN_DIST}/
-	echo ">>>>>> Saving packages to: $gtsam_ubuntu_OUT_DIR/$DEBIAN_DIST/"
-done
-
-
-exit 0
diff --git a/package_scripts/toolbox_package_unix.sh b/package_scripts/toolbox_package_unix.sh
deleted file mode 100755
index 28de2572a..000000000
--- a/package_scripts/toolbox_package_unix.sh
+++ /dev/null
@@ -1,64 +0,0 @@
-#!/bin/sh
-
-# Script to build a tarball with the matlab toolbox
-
-# Detect platform
-os=`uname -s`
-arch=`uname -m`
-if [ "$os" = "Linux" -a "$arch" = "x86_64" ]; then
-	platform=lin64
-elif [ "$os" = "Linux" -a "$arch" = "i686" ]; then
-	platform=lin32
-elif [ "$os" = "Darwin" -a "$arch" = "x86_64" ]; then
-	platform=mac64
-else
-	echo "Unrecognized platform"
-	exit 1
-fi
-
-echo "Platform is ${platform}"
-
-# Check for empty diectory
-if [ ! -z "`ls`" ]; then
-	echo "Please run this script from an empty build directory"
-	exit 1
-fi
-
-# Check for boost
-if [ -z "$1" ]; then
-	echo "Usage: $0 BOOSTTREE"
-	echo "BOOSTTREE should be a boost source tree compiled with toolbox_build_boost."
-	exit 1
-fi
-
-# Run cmake
-cmake -DCMAKE_BUILD_TYPE=Release \
--DGTSAM_INSTALL_MATLAB_TOOLBOX:BOOL=ON \
--DCMAKE_INSTALL_PREFIX="$PWD/stage" \
--DBoost_NO_SYSTEM_PATHS:BOOL=ON \
--DBoost_USE_STATIC_LIBS:BOOL=ON \
--DBOOST_ROOT="$1" \
--DGTSAM_BUILD_TESTS:BOOL=OFF \
--DGTSAM_BUILD_TIMING:BOOL=OFF \
--DGTSAM_BUILD_EXAMPLES_ALWAYS:BOOL=OFF \
--DGTSAM_WITH_TBB:BOOL=OFF \
--DGTSAM_SUPPORT_NESTED_DISSECTION:BOOL=OFF \
--DGTSAM_INSTALL_GEOGRAPHICLIB:BOOL=OFF \
--DGTSAM_BUILD_UNSTABLE:BOOL=OFF \
--DGTSAM_MEX_BUILD_STATIC_MODULE:BOOL=ON ..
-
-if [ $? -ne 0 ]; then
-	echo "CMake failed"
-	exit 1
-fi
-
-# Compile
-make -j8 install
-
-if [ $? -ne 0 ]; then
-    echo "Compile failed"
-    exit 1
-fi
-
-# Create package
-tar czf gtsam-toolbox-3.2.0-$platform.tgz -C stage/gtsam_toolbox toolbox
diff --git a/package_scripts/upload_all_gtsam_ppa.sh b/package_scripts/upload_all_gtsam_ppa.sh
deleted file mode 100755
index c9d113f00..000000000
--- a/package_scripts/upload_all_gtsam_ppa.sh
+++ /dev/null
@@ -1,3 +0,0 @@
-#!/bin/bash
-
-find . -name '*.changes' | xargs -I FIL dput ppa:joseluisblancoc/gtsam-develop FIL
diff --git a/tests/smallExample.h b/tests/smallExample.h
index 146289dac..2b29a6d10 100644
--- a/tests/smallExample.h
+++ b/tests/smallExample.h
@@ -159,10 +159,10 @@ namespace example {
 namespace impl {
 typedef boost::shared_ptr<NonlinearFactor> shared_nlf;
 
-static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
-static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
-static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
-static SharedDiagonal constraintModel = noiseModel::Constrained::All(2);
+static SharedDiagonal kSigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
+static SharedDiagonal kSigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
+static SharedDiagonal kSigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
+static SharedDiagonal kConstrainedModel = noiseModel::Constrained::All(2);
 
 static const Key _l1_=0, _x1_=1, _x2_=2;
 static const Key _x_=0, _y_=1, _z_=2;
@@ -170,40 +170,43 @@ static const Key _x_=0, _y_=1, _z_=2;
 
 
 /* ************************************************************************* */
-inline boost::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph() {
+inline boost::shared_ptr<const NonlinearFactorGraph>
+sharedNonlinearFactorGraph(const SharedNoiseModel &noiseModel1 = impl::kSigma0_1,
+                           const SharedNoiseModel &noiseModel2 = impl::kSigma0_2) {
   using namespace impl;
-  using symbol_shorthand::X;
   using symbol_shorthand::L;
+  using symbol_shorthand::X;
   // Create
-  boost::shared_ptr<NonlinearFactorGraph> nlfg(
-      new NonlinearFactorGraph);
+  boost::shared_ptr<NonlinearFactorGraph> nlfg(new NonlinearFactorGraph);
 
   // prior on x1
-  Point2 mu(0,0);
-  shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1)));
+  Point2 mu(0, 0);
+  shared_nlf f1(new simulated2D::Prior(mu, noiseModel1, X(1)));
   nlfg->push_back(f1);
 
   // odometry between x1 and x2
   Point2 z2(1.5, 0);
-  shared_nlf f2(new simulated2D::Odometry(z2, sigma0_1, X(1), X(2)));
+  shared_nlf f2(new simulated2D::Odometry(z2, noiseModel1, X(1), X(2)));
   nlfg->push_back(f2);
 
   // measurement between x1 and l1
   Point2 z3(0, -1);
-  shared_nlf f3(new simulated2D::Measurement(z3, sigma0_2, X(1), L(1)));
+  shared_nlf f3(new simulated2D::Measurement(z3, noiseModel2, X(1), L(1)));
   nlfg->push_back(f3);
 
   // measurement between x2 and l1
   Point2 z4(-1.5, -1.);
-  shared_nlf f4(new simulated2D::Measurement(z4, sigma0_2, X(2), L(1)));
+  shared_nlf f4(new simulated2D::Measurement(z4, noiseModel2, X(2), L(1)));
   nlfg->push_back(f4);
 
   return nlfg;
 }
 
 /* ************************************************************************* */
-inline NonlinearFactorGraph createNonlinearFactorGraph() {
-  return *sharedNonlinearFactorGraph();
+inline NonlinearFactorGraph
+createNonlinearFactorGraph(const SharedNoiseModel &noiseModel1 = impl::kSigma0_1,
+                           const SharedNoiseModel &noiseModel2 = impl::kSigma0_2) {
+  return *sharedNonlinearFactorGraph(noiseModel1, noiseModel2);
 }
 
 /* ************************************************************************* */
@@ -372,19 +375,19 @@ inline std::pair<NonlinearFactorGraph, Values> createNonlinearSmoother(int T) {
 
   // prior on x1
   Point2 x1(1.0, 0.0);
-  shared_nlf prior(new simulated2D::Prior(x1, sigma1_0, X(1)));
+  shared_nlf prior(new simulated2D::Prior(x1, kSigma1_0, X(1)));
   nlfg.push_back(prior);
   poses.insert(X(1), x1);
 
   for (int t = 2; t <= T; t++) {
     // odometry between x_t and x_{t-1}
     Point2 odo(1.0, 0.0);
-    shared_nlf odometry(new simulated2D::Odometry(odo, sigma1_0, X(t - 1), X(t)));
+    shared_nlf odometry(new simulated2D::Odometry(odo, kSigma1_0, X(t - 1), X(t)));
     nlfg.push_back(odometry);
 
     // measurement on x_t is like perfect GPS
     Point2 xt(t, 0);
-    shared_nlf measurement(new simulated2D::Prior(xt, sigma1_0, X(t)));
+    shared_nlf measurement(new simulated2D::Prior(xt, kSigma1_0, X(t)));
     nlfg.push_back(measurement);
 
     // initial estimate
@@ -412,7 +415,7 @@ inline GaussianFactorGraph createSimpleConstraintGraph() {
   Vector b1(2);
   b1(0) = 1.0;
   b1(1) = -1.0;
-  JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));
+  JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, kSigma0_1));
 
   // create binary constraint factor
   // between _x_ and _y_, that is going to be the only factor on _y_
@@ -422,7 +425,7 @@ inline GaussianFactorGraph createSimpleConstraintGraph() {
   Matrix Ay1 = I_2x2 * -1;
   Vector b2 = Vector2(0.0, 0.0);
   JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
-      constraintModel));
+      kConstrainedModel));
 
   // construct the graph
   GaussianFactorGraph fg;
@@ -453,8 +456,8 @@ inline GaussianFactorGraph createSingleConstraintGraph() {
   Vector b1(2);
   b1(0) = 1.0;
   b1(1) = -1.0;
-  //GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1));
-  JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));
+  //GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, kSigma0_1->Whiten(Ax), kSigma0_1->whiten(b1), kSigma0_1));
+  JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, kSigma0_1));
 
   // create binary constraint factor
   // between _x_ and _y_, that is going to be the only factor on _y_
@@ -468,7 +471,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() {
   Matrix Ay1 = I_2x2 * 10;
   Vector b2 = Vector2(1.0, 2.0);
   JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
-      constraintModel));
+      kConstrainedModel));
 
   // construct the graph
   GaussianFactorGraph fg;
@@ -493,7 +496,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() {
   // unary factor 1
   Matrix A = I_2x2;
   Vector b = Vector2(-2.0, 2.0);
-  JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1));
+  JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, kSigma0_1));
 
   // constraint 1
   Matrix A11(2, 2);
@@ -512,7 +515,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() {
   b1(0) = 1.0;
   b1(1) = 2.0;
   JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1,
-      constraintModel));
+      kConstrainedModel));
 
   // constraint 2
   Matrix A21(2, 2);
@@ -531,7 +534,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() {
   b2(0) = 3.0;
   b2(1) = 4.0;
   JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2,
-      constraintModel));
+      kConstrainedModel));
 
   // construct the graph
   GaussianFactorGraph fg;
diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp
index d9a013a60..d33c7ba1d 100644
--- a/tests/testExpressionFactor.cpp
+++ b/tests/testExpressionFactor.cpp
@@ -20,7 +20,7 @@
 #include <gtsam/slam/expressions.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/nonlinear/expressionTesting.h>
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp
index b3e8a3449..d759e83e1 100644
--- a/tests/testExtendedKalmanFilter.cpp
+++ b/tests/testExtendedKalmanFilter.cpp
@@ -14,7 +14,7 @@
  * @author Stephen Williams
  */
 
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
@@ -162,7 +162,7 @@ public:
   /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
   virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*> (&f);
-    return (e != NULL) && (key1() == e->key1()) && (key2() == e->key2());
+    return (e != nullptr) && (key1() == e->key1()) && (key2() == e->key2());
   }
 
   /**
@@ -196,7 +196,7 @@ public:
     Vector b = -evaluateError(x1, x2, A1, A2);
     SharedDiagonal constrained =
         boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
-    if (constrained.get() != NULL) {
+    if (constrained.get() != nullptr) {
       return JacobianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(),
           A2, b, constrained));
     }
@@ -292,7 +292,7 @@ public:
   /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
   virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*> (&f);
-    return (e != NULL) && Base::equals(f);
+    return (e != nullptr) && Base::equals(f);
   }
 
   /**
@@ -325,7 +325,7 @@ public:
     Vector b = -evaluateError(x1, A1);
     SharedDiagonal constrained =
         boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
-    if (constrained.get() != NULL) {
+    if (constrained.get() != nullptr) {
       return JacobianFactor::shared_ptr(new JacobianFactor(key(), A1, b, constrained));
     }
     // "Whiten" the system before converting to a Gaussian Factor
diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp
index c4e9d26f5..1f5ec6350 100644
--- a/tests/testGaussianFactorGraphB.cpp
+++ b/tests/testGaussianFactorGraphB.cpp
@@ -206,7 +206,7 @@ TEST(GaussianFactorGraph, optimize_Cholesky) {
   GaussianFactorGraph fg = createGaussianFactorGraph();
 
   // optimize the graph
-  VectorValues actual = fg.optimize(boost::none, EliminateCholesky);
+  VectorValues actual = fg.optimize(EliminateCholesky);
 
   // verify
   VectorValues expected = createCorrectDelta();
@@ -220,7 +220,7 @@ TEST( GaussianFactorGraph, optimize_QR )
   GaussianFactorGraph fg = createGaussianFactorGraph();
 
   // optimize the graph
-  VectorValues actual = fg.optimize(boost::none, EliminateQR);
+  VectorValues actual = fg.optimize(EliminateQR);
 
   // verify
   VectorValues expected = createCorrectDelta();
@@ -401,7 +401,6 @@ TEST(GaussianFactorGraph, hasConstraints)
 
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/sam/RangeFactor.h>
 
 /* ************************************************************************* */
@@ -438,7 +437,7 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) {
   initValues.insert(l41,  Point3(1.61051523, 6.7373052, -0.231582751)   );
 
   NonlinearFactorGraph factors;
-  factors += PriorFactor<Pose3>(xC1,
+  factors.addPrior(xC1,
       Pose3(Rot3(
           -1.,           0.0,  1.2246468e-16,
           0.0,             1.,           0.0,
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index 68d10bb7b..e8e916f04 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -7,7 +7,6 @@
 #include <gtsam/nonlinear/ISAM2.h>
 
 #include <tests/smallExample.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/geometry/Point2.h>
@@ -63,7 +62,7 @@ ISAM2 createSlamlikeISAM2(
   // Add a prior at time 0 and update isam
   {
     NonlinearFactorGraph newfactors;
-    newfactors += PriorFactor<Pose2>(0, Pose2(0.0, 0.0, 0.0), odoNoise);
+    newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
     Values init;
@@ -499,7 +498,7 @@ TEST(ISAM2, constrained_ordering)
   // Add a prior at time 0 and update isam
   {
     NonlinearFactorGraph newfactors;
-    newfactors += PriorFactor<Pose2>(0, Pose2(0.0, 0.0, 0.0), odoNoise);
+    newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
     fullgraph.push_back(newfactors);
 
     Values init;
@@ -669,7 +668,7 @@ namespace {
 TEST(ISAM2, marginalizeLeaves1) {
   ISAM2 isam;
   NonlinearFactorGraph factors;
-  factors += PriorFactor<double>(0, 0.0, model);
+  factors.addPrior(0, 0.0, model);
 
   factors += BetweenFactor<double>(0, 1, 0.0, model);
   factors += BetweenFactor<double>(1, 2, 0.0, model);
@@ -696,7 +695,7 @@ TEST(ISAM2, marginalizeLeaves2) {
   ISAM2 isam;
 
   NonlinearFactorGraph factors;
-  factors += PriorFactor<double>(0, 0.0, model);
+  factors.addPrior(0, 0.0, model);
 
   factors += BetweenFactor<double>(0, 1, 0.0, model);
   factors += BetweenFactor<double>(1, 2, 0.0, model);
@@ -726,7 +725,7 @@ TEST(ISAM2, marginalizeLeaves3) {
   ISAM2 isam;
 
   NonlinearFactorGraph factors;
-  factors += PriorFactor<double>(0, 0.0, model);
+  factors.addPrior(0, 0.0, model);
 
   factors += BetweenFactor<double>(0, 1, 0.0, model);
   factors += BetweenFactor<double>(1, 2, 0.0, model);
@@ -765,7 +764,7 @@ TEST(ISAM2, marginalizeLeaves4) {
   ISAM2 isam;
 
   NonlinearFactorGraph factors;
-  factors += PriorFactor<double>(0, 0.0, model);
+  factors.addPrior(0, 0.0, model);
   factors += BetweenFactor<double>(0, 2, 0.0, model);
   factors += BetweenFactor<double>(1, 2, 0.0, model);
 
diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp
index e877e5a9d..dfdb32b46 100644
--- a/tests/testGaussianJunctionTreeB.cpp
+++ b/tests/testGaussianJunctionTreeB.cpp
@@ -18,7 +18,7 @@
 #include <tests/smallExample.h>
 #include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp
index 95caaaf9c..05b4c7f66 100644
--- a/tests/testGeneralSFMFactorB.cpp
+++ b/tests/testGeneralSFMFactorB.cpp
@@ -42,7 +42,7 @@ using symbol_shorthand::P;
 /* ************************************************************************* */
 TEST(PinholeCamera, BAL) {
   string filename = findExampleDataFile("dubrovnik-3-7-pre");
-  SfM_data db;
+  SfmData db;
   bool success = readBAL(filename, db);
   if (!success) throw runtime_error("Could not access file!");
 
@@ -50,7 +50,7 @@ TEST(PinholeCamera, BAL) {
   NonlinearFactorGraph graph;
 
   for (size_t j = 0; j < db.number_tracks(); j++) {
-    for (const SfM_Measurement& m: db.tracks[j].measurements)
+    for (const SfmMeasurement& m: db.tracks[j].measurements)
       graph.emplace_shared<sfmFactor>(m.second, unit2, m.first, P(j));
   }
 
diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp
index e45f234aa..a44a28273 100644
--- a/tests/testGradientDescentOptimizer.cpp
+++ b/tests/testGradientDescentOptimizer.cpp
@@ -12,7 +12,6 @@
  * @date   Jun 11, 2012
  */
 
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -38,7 +37,7 @@ boost::tuple<NonlinearFactorGraph, Values> generateProblem() {
   Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
   SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(
       Vector3(0.3, 0.3, 0.1));
-  graph += PriorFactor<Pose2>(1, priorMean, priorNoise);
+  graph.addPrior(1, priorMean, priorNoise);
 
   // 2b. Add odometry factors
   SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(
diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp
index 7afb4e178..9d6e58ac7 100644
--- a/tests/testIterative.cpp
+++ b/tests/testIterative.cpp
@@ -16,7 +16,6 @@
  **/
 
 #include <tests/smallExample.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/inference/Symbol.h>
@@ -117,7 +116,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
   config.insert(X(2), Pose2(1.5,0.,0.));
 
   NonlinearFactorGraph graph;
-  graph += PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
+  graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
   graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
 
   boost::shared_ptr<GaussianFactorGraph> fg = graph.linearize(config);
diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp
index 4bdbd9387..d7746e08d 100644
--- a/tests/testMarginals.cpp
+++ b/tests/testMarginals.cpp
@@ -29,7 +29,6 @@
 #include <gtsam/linear/NoiseModel.h>
 
 // add in headers for specific factors
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
 
@@ -53,7 +52,7 @@ TEST(Marginals, planarSLAMmarginals) {
   // gaussian for prior
   SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
-  graph += PriorFactor<Pose2>(x1, priorMean, priorNoise);  // add the factor to the graph
+  graph.addPrior(x1, priorMean, priorNoise);  // add the factor to the graph
 
   /* add odometry */
   // general noisemodel for odometry
@@ -87,6 +86,12 @@ TEST(Marginals, planarSLAMmarginals) {
   soln.insert(x3, Pose2(4.0, 0.0, 0.0));
   soln.insert(l1, Point2(2.0, 2.0));
   soln.insert(l2, Point2(4.0, 2.0));
+  VectorValues soln_lin;
+  soln_lin.insert(x1, Vector3(0.0, 0.0, 0.0));
+  soln_lin.insert(x2, Vector3(2.0, 0.0, 0.0));
+  soln_lin.insert(x3, Vector3(4.0, 0.0, 0.0));
+  soln_lin.insert(l1, Vector2(2.0, 2.0));
+  soln_lin.insert(l2, Vector2(4.0, 2.0));
 
   Matrix expectedx1(3,3);
   expectedx1 <<
@@ -110,77 +115,86 @@ TEST(Marginals, planarSLAMmarginals) {
   Matrix expectedl2(2,2);
   expectedl2 <<
       0.293870968, -0.104516129,
-     -0.104516129,  0.391935484;
+    -0.104516129,  0.391935484;
 
-  // Check marginals covariances for all variables (Cholesky mode)
-  Marginals marginals(graph, soln, Marginals::CHOLESKY);
-  EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8));
-  EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8));
-  EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8));
-  EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8));
-  EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8));
+  auto testMarginals = [&] (Marginals marginals) {
+    EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8));
+    EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8));
+    EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8));
+    EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8));
+    EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8));
+  };
 
-  // Check marginals covariances for all variables (QR mode)
+  auto testJointMarginals = [&] (Marginals marginals) {
+    // Check joint marginals for 3 variables
+    Matrix expected_l2x1x3(8,8);
+    expected_l2x1x3 <<
+        0.293871159514111,  -0.104516127560770,   0.090000180000270,  -0.000000000000000,  -0.020000000000000,   0.151935669757191,  -0.104516127560770,  -0.050967744878460,
+      -0.104516127560770,   0.391935664055174,   0.000000000000000,   0.090000180000270,   0.040000000000000,   0.007741936219615,   0.351935664055174,   0.056129031890193,
+        0.090000180000270,   0.000000000000000,   0.090000180000270,  -0.000000000000000,   0.000000000000000,   0.090000180000270,   0.000000000000000,   0.000000000000000,
+      -0.000000000000000,   0.090000180000270,  -0.000000000000000,   0.090000180000270,   0.000000000000000,  -0.000000000000000,   0.090000180000270,   0.000000000000000,
+      -0.020000000000000,   0.040000000000000,   0.000000000000000,   0.000000000000000,   0.010000000000000,   0.000000000000000,   0.040000000000000,   0.010000000000000,
+        0.151935669757191,   0.007741936219615,   0.090000180000270,  -0.000000000000000,   0.000000000000000,   0.160967924878730,   0.007741936219615,   0.004516127560770,
+      -0.104516127560770,   0.351935664055174,   0.000000000000000,   0.090000180000270,   0.040000000000000,   0.007741936219615,   0.351935664055174,   0.056129031890193,
+      -0.050967744878460,   0.056129031890193,   0.000000000000000,   0.000000000000000,   0.010000000000000,   0.004516127560770,   0.056129031890193,   0.027741936219615;
+    KeyVector variables {x1, l2, x3};
+    JointMarginal joint_l2x1x3 = marginals.jointMarginalCovariance(variables);
+    EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,0,2,2)), Matrix(joint_l2x1x3(l2,l2)), 1e-6));
+    EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,0,3,2)), Matrix(joint_l2x1x3(x1,l2)), 1e-6));
+    EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,0,3,2)), Matrix(joint_l2x1x3(x3,l2)), 1e-6));
+
+    EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,2,2,3)), Matrix(joint_l2x1x3(l2,x1)), 1e-6));
+    EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,2,3,3)), Matrix(joint_l2x1x3(x1,x1)), 1e-6));
+    EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,2,3,3)), Matrix(joint_l2x1x3(x3,x1)), 1e-6));
+
+    EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,5,2,3)), Matrix(joint_l2x1x3(l2,x3)), 1e-6));
+    EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,5,3,3)), Matrix(joint_l2x1x3(x1,x3)), 1e-6));
+    EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,5,3,3)), Matrix(joint_l2x1x3(x3,x3)), 1e-6));
+
+    // Check joint marginals for 2 variables (different code path than >2 variable case above)
+    Matrix expected_l2x1(5,5);
+    expected_l2x1 <<
+        0.293871159514111,  -0.104516127560770,   0.090000180000270,  -0.000000000000000,  -0.020000000000000,
+      -0.104516127560770,   0.391935664055174,   0.000000000000000,   0.090000180000270,   0.040000000000000,
+        0.090000180000270,   0.000000000000000,   0.090000180000270,  -0.000000000000000,   0.000000000000000,
+      -0.000000000000000,   0.090000180000270,  -0.000000000000000,   0.090000180000270,   0.000000000000000,
+      -0.020000000000000,   0.040000000000000,   0.000000000000000,   0.000000000000000,   0.010000000000000;
+    variables.resize(2);
+    variables[0] = l2;
+    variables[1] = x1;
+    JointMarginal joint_l2x1 = marginals.jointMarginalCovariance(variables);
+    EXPECT(assert_equal(Matrix(expected_l2x1.block(0,0,2,2)), Matrix(joint_l2x1(l2,l2)), 1e-6));
+    EXPECT(assert_equal(Matrix(expected_l2x1.block(2,0,3,2)), Matrix(joint_l2x1(x1,l2)), 1e-6));
+    EXPECT(assert_equal(Matrix(expected_l2x1.block(0,2,2,3)), Matrix(joint_l2x1(l2,x1)), 1e-6));
+    EXPECT(assert_equal(Matrix(expected_l2x1.block(2,2,3,3)), Matrix(joint_l2x1(x1,x1)), 1e-6));
+
+    // Check joint marginal for 1 variable (different code path than >1 variable cases above)
+    variables.resize(1);
+    variables[0] = x1;
+    JointMarginal joint_x1 = marginals.jointMarginalCovariance(variables);
+    EXPECT(assert_equal(expectedx1, Matrix(joint_l2x1(x1,x1)), 1e-6));
+  };
+
+  Marginals marginals;
+
+  marginals = Marginals(graph, soln, Marginals::CHOLESKY);
+  testMarginals(marginals);
   marginals = Marginals(graph, soln, Marginals::QR);
-  EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8));
-  EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8));
-  EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8));
-  EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8));
-  EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8));
+  testMarginals(marginals);
+  testJointMarginals(marginals);
 
-  // Check joint marginals for 3 variables
-  Matrix expected_l2x1x3(8,8);
-  expected_l2x1x3 <<
-      0.293871159514111,  -0.104516127560770,   0.090000180000270,  -0.000000000000000,  -0.020000000000000,   0.151935669757191,  -0.104516127560770,  -0.050967744878460,
-     -0.104516127560770,   0.391935664055174,   0.000000000000000,   0.090000180000270,   0.040000000000000,   0.007741936219615,   0.351935664055174,   0.056129031890193,
-      0.090000180000270,   0.000000000000000,   0.090000180000270,  -0.000000000000000,   0.000000000000000,   0.090000180000270,   0.000000000000000,   0.000000000000000,
-     -0.000000000000000,   0.090000180000270,  -0.000000000000000,   0.090000180000270,   0.000000000000000,  -0.000000000000000,   0.090000180000270,   0.000000000000000,
-     -0.020000000000000,   0.040000000000000,   0.000000000000000,   0.000000000000000,   0.010000000000000,   0.000000000000000,   0.040000000000000,   0.010000000000000,
-      0.151935669757191,   0.007741936219615,   0.090000180000270,  -0.000000000000000,   0.000000000000000,   0.160967924878730,   0.007741936219615,   0.004516127560770,
-     -0.104516127560770,   0.351935664055174,   0.000000000000000,   0.090000180000270,   0.040000000000000,   0.007741936219615,   0.351935664055174,   0.056129031890193,
-     -0.050967744878460,   0.056129031890193,   0.000000000000000,   0.000000000000000,   0.010000000000000,   0.004516127560770,   0.056129031890193,   0.027741936219615;
-  KeyVector variables {x1, l2, x3};
-  JointMarginal joint_l2x1x3 = marginals.jointMarginalCovariance(variables);
-  EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,0,2,2)), Matrix(joint_l2x1x3(l2,l2)), 1e-6));
-  EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,0,3,2)), Matrix(joint_l2x1x3(x1,l2)), 1e-6));
-  EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,0,3,2)), Matrix(joint_l2x1x3(x3,l2)), 1e-6));
-
-  EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,2,2,3)), Matrix(joint_l2x1x3(l2,x1)), 1e-6));
-  EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,2,3,3)), Matrix(joint_l2x1x3(x1,x1)), 1e-6));
-  EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,2,3,3)), Matrix(joint_l2x1x3(x3,x1)), 1e-6));
-
-  EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,5,2,3)), Matrix(joint_l2x1x3(l2,x3)), 1e-6));
-  EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,5,3,3)), Matrix(joint_l2x1x3(x1,x3)), 1e-6));
-  EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,5,3,3)), Matrix(joint_l2x1x3(x3,x3)), 1e-6));
-
-  // Check joint marginals for 2 variables (different code path than >2 variable case above)
-  Matrix expected_l2x1(5,5);
-  expected_l2x1 <<
-      0.293871159514111,  -0.104516127560770,   0.090000180000270,  -0.000000000000000,  -0.020000000000000,
-     -0.104516127560770,   0.391935664055174,   0.000000000000000,   0.090000180000270,   0.040000000000000,
-      0.090000180000270,   0.000000000000000,   0.090000180000270,  -0.000000000000000,   0.000000000000000,
-     -0.000000000000000,   0.090000180000270,  -0.000000000000000,   0.090000180000270,   0.000000000000000,
-     -0.020000000000000,   0.040000000000000,   0.000000000000000,   0.000000000000000,   0.010000000000000;
-  variables.resize(2);
-  variables[0] = l2;
-  variables[1] = x1;
-  JointMarginal joint_l2x1 = marginals.jointMarginalCovariance(variables);
-  EXPECT(assert_equal(Matrix(expected_l2x1.block(0,0,2,2)), Matrix(joint_l2x1(l2,l2)), 1e-6));
-  EXPECT(assert_equal(Matrix(expected_l2x1.block(2,0,3,2)), Matrix(joint_l2x1(x1,l2)), 1e-6));
-  EXPECT(assert_equal(Matrix(expected_l2x1.block(0,2,2,3)), Matrix(joint_l2x1(l2,x1)), 1e-6));
-  EXPECT(assert_equal(Matrix(expected_l2x1.block(2,2,3,3)), Matrix(joint_l2x1(x1,x1)), 1e-6));
-
-  // Check joint marginal for 1 variable (different code path than >1 variable cases above)
-  variables.resize(1);
-  variables[0] = x1;
-  JointMarginal joint_x1 = marginals.jointMarginalCovariance(variables);
-  EXPECT(assert_equal(expectedx1, Matrix(joint_l2x1(x1,x1)), 1e-6));
+  GaussianFactorGraph gfg = *graph.linearize(soln);
+  marginals = Marginals(gfg, soln_lin, Marginals::CHOLESKY);
+  testMarginals(marginals);
+  marginals = Marginals(gfg, soln_lin, Marginals::QR);
+  testMarginals(marginals);
+  testJointMarginals(marginals);
 }
 
 /* ************************************************************************* */
 TEST(Marginals, order) {
   NonlinearFactorGraph fg;
-  fg += PriorFactor<Pose2>(0, Pose2(), noiseModel::Unit::Create(3));
+  fg.addPrior(0, Pose2(), noiseModel::Unit::Create(3));
   fg += BetweenFactor<Pose2>(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3));
   fg += BetweenFactor<Pose2>(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3));
   fg += BetweenFactor<Pose2>(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3));
@@ -222,17 +236,26 @@ TEST(Marginals, order) {
     vals.at<Pose2>(3).bearing(vals.at<Point2>(101)),
     vals.at<Pose2>(3).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));
 
+  auto testMarginals = [&] (Marginals marginals, KeySet set) {
+    KeyVector keys(set.begin(), set.end());
+    JointMarginal joint = marginals.jointMarginalCovariance(keys);
+
+    LONGS_EQUAL(3, (long)joint(0,0).rows());
+    LONGS_EQUAL(3, (long)joint(1,1).rows());
+    LONGS_EQUAL(3, (long)joint(2,2).rows());
+    LONGS_EQUAL(3, (long)joint(3,3).rows());
+    LONGS_EQUAL(2, (long)joint(100,100).rows());
+    LONGS_EQUAL(2, (long)joint(101,101).rows());
+  };
+
   Marginals marginals(fg, vals);
   KeySet set = fg.keys();
-  KeyVector keys(set.begin(), set.end());
-  JointMarginal joint = marginals.jointMarginalCovariance(keys);
+  testMarginals(marginals, set);
 
-  LONGS_EQUAL(3, (long)joint(0,0).rows());
-  LONGS_EQUAL(3, (long)joint(1,1).rows());
-  LONGS_EQUAL(3, (long)joint(2,2).rows());
-  LONGS_EQUAL(3, (long)joint(3,3).rows());
-  LONGS_EQUAL(2, (long)joint(100,100).rows());
-  LONGS_EQUAL(2, (long)joint(101,101).rows());
+  GaussianFactorGraph gfg = *fg.linearize(vals);
+  marginals = Marginals(gfg, vals);
+  set = gfg.keys();
+  testMarginals(marginals, set);
 }
 
 /* ************************************************************************* */
diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp
index 95843e5ab..9d91a344b 100644
--- a/tests/testNonlinearEquality.cpp
+++ b/tests/testNonlinearEquality.cpp
@@ -16,7 +16,7 @@
 
 #include <tests/simulated2DConstraints.h>
 
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -28,7 +28,7 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -527,10 +527,10 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
   Rot3 faceTowardsY(Point3(1, 0, 0), Point3(0, 0, -1), Point3(0, 1, 0));
 
   Pose3 poseLeft(faceTowardsY, Point3(0, 0, 0));  // origin, left camera
-  SimpleCamera leftCamera(poseLeft, K);
+  PinholeCamera<Cal3_S2> leftCamera(poseLeft, K);
 
   Pose3 poseRight(faceTowardsY, Point3(2, 0, 0));  // 2 units to the right
-  SimpleCamera rightCamera(poseRight, K);
+  PinholeCamera<Cal3_S2> rightCamera(poseRight, K);
 
   Point3 landmark(1, 5, 0); //centered between the cameras, 5 units away
 
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index 290f0138e..fdb080a63 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/tests/testNonlinearFactorGraph.cpp
@@ -25,8 +25,8 @@
 #include <gtsam/symbolic/SymbolicFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/sam/RangeFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 
 #include <CppUnitLite/TestHarness.h>
@@ -198,7 +198,7 @@ TEST(NonlinearFactorGraph, UpdateCholesky) {
       }
     }
   };
-  EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6));
+  EXPECT(assert_equal(initial, fg.updateCholesky(initial, dampen), 1e-6));
 }
 
 /* ************************************************************************* */
@@ -216,8 +216,8 @@ TEST(testNonlinearFactorGraph, eliminate) {
 
   // Priors
   auto prior = noiseModel::Isotropic::Sigma(3, 1);
-  graph.add(PriorFactor<Pose2>(11, T11, prior));
-  graph.add(PriorFactor<Pose2>(21, T21, prior));
+  graph.addPrior(11, T11, prior);
+  graph.addPrior(21, T21, prior);
 
   // Odometry
   auto model = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.3));
@@ -242,6 +242,73 @@ TEST(testNonlinearFactorGraph, eliminate) {
   EXPECT_LONGS_EQUAL(4, bn->size());
 }
 
+/* ************************************************************************* */
+TEST(testNonlinearFactorGraph, addPrior) {
+  Key k(0);
+
+  // Factor graph.
+  auto graph = NonlinearFactorGraph();
+
+  // Add a prior factor for key k.
+  auto model_double = noiseModel::Isotropic::Sigma(1, 1);
+  graph.addPrior<double>(k, 10, model_double);
+
+  // Assert the graph has 0 error with the correct values.
+  Values values;
+  values.insert(k, 10.0);
+  EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16);
+
+  // Assert the graph has some error with incorrect values.
+  values.clear();
+  values.insert(k, 11.0);
+  EXPECT(0 != graph.error(values));
+
+  // Clear the factor graph and values.
+  values.clear();
+  graph.erase(graph.begin(), graph.end());
+
+  // Add a Pose3 prior to the factor graph. Use a gaussian noise model by
+  // providing the covariance matrix.
+  Eigen::DiagonalMatrix<double, 6, 6> covariance_pose3;
+  covariance_pose3.setIdentity();
+  Pose3 pose{Rot3(), Point3(0, 0, 0)};
+  graph.addPrior(k, pose, covariance_pose3);
+
+  // Assert the graph has 0 error with the correct values.
+  values.insert(k, pose);
+  EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16);
+
+  // Assert the graph has some error with incorrect values.
+  values.clear();
+  Pose3 pose_incorrect{Rot3::RzRyRx(-M_PI, M_PI, -M_PI / 8), Point3(1, 2, 3)};
+  values.insert(k, pose_incorrect);
+  EXPECT(0 != graph.error(values));
+}
+
+TEST(NonlinearFactorGraph, printErrors)
+{
+  const NonlinearFactorGraph fg = createNonlinearFactorGraph();
+  const Values c = createValues();
+
+  // Test that it builds with default parameters.
+  // We cannot check the output since (at present) output is fixed to std::cout.
+  fg.printErrors(c);
+
+  // Second round: using callback filter to check that we actually visit all factors:
+  std::vector<bool> visited;
+  visited.assign(fg.size(), false);
+  const auto testFilter =
+      [&](const gtsam::Factor *f, double error, size_t index) {
+        EXPECT(f!=nullptr);
+        EXPECT(error>=.0);
+        visited.at(index)=true;
+        return false; // do not print
+      };
+  fg.printErrors(c,"Test graph: ", gtsam::DefaultKeyFormatter,testFilter);
+
+  for (bool visit : visited) EXPECT(visit==true);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp
index c06d10beb..88ae508b6 100644
--- a/tests/testNonlinearISAM.cpp
+++ b/tests/testNonlinearISAM.cpp
@@ -8,7 +8,6 @@
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/slam/dataset.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearISAM.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -116,9 +115,9 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) {
 
     // Add a floating landmark constellation
     if (i == 7) {
-      new_factors += PriorFactor<Point2>(lm1, landmark1, model2);
-      new_factors += PriorFactor<Point2>(lm2, landmark2, model2);
-      new_factors += PriorFactor<Point2>(lm3, landmark3, model2);
+      new_factors.addPrior(lm1, landmark1, model2);
+      new_factors.addPrior(lm2, landmark2, model2);
+      new_factors.addPrior(lm3, landmark3, model2);
 
       // Initialize to origin
       new_init.insert(lm1, Point2(0,0));
@@ -193,9 +192,9 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) {
 
     // Add a floating landmark constellation
     if (i == 7) {
-      new_factors += PriorFactor<Point2>(lm1, landmark1, model2);
-      new_factors += PriorFactor<Point2>(lm2, landmark2, model2);
-      new_factors += PriorFactor<Point2>(lm3, landmark3, model2);
+      new_factors.addPrior(lm1, landmark1, model2);
+      new_factors.addPrior(lm2, landmark2, model2);
+      new_factors.addPrior(lm3, landmark3, model2);
 
       // Initialize to origin
       new_init.insert(lm1, Point2(0,0));
@@ -296,8 +295,7 @@ TEST(testNonlinearISAM, loop_closures ) {
       if (id == 0) {
         noiseModel::Diagonal::shared_ptr priorNoise =
             noiseModel::Diagonal::Sigmas(Vector3(0.001, 0.001, 0.001));
-        graph.emplace_shared<PriorFactor<Pose2> >(Symbol('x', id),
-            Pose2(0, 0, 0), priorNoise);
+        graph.addPrior(Symbol('x', id), Pose2(0, 0, 0), priorNoise);
       } else {
         isam.update(graph, initialEstimate);
 
diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp
index 0e7793552..a549dc726 100644
--- a/tests/testNonlinearOptimizer.cpp
+++ b/tests/testNonlinearOptimizer.cpp
@@ -16,10 +16,10 @@
  */
 
 #include <tests/smallExample.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/nonlinear/DoglegOptimizer.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -181,7 +181,7 @@ TEST( NonlinearOptimizer, Factorization )
   config.insert(X(2), Pose2(1.5,0.,0.));
 
   NonlinearFactorGraph graph;
-  graph += PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
+  graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
   graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
 
   Ordering ordering;
@@ -240,8 +240,7 @@ TEST(NonlinearOptimizer, NullFactor) {
 TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
 
   NonlinearFactorGraph fg;
-  fg += PriorFactor<Pose2>(0, Pose2(0, 0, 0),
-      noiseModel::Isotropic::Sigma(3, 1));
+  fg.addPrior(0, Pose2(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1));
   fg += BetweenFactor<Pose2>(0, 1, Pose2(1, 0, M_PI / 2),
       noiseModel::Isotropic::Sigma(3, 1));
   fg += BetweenFactor<Pose2>(1, 2, Pose2(1, 0, M_PI / 2),
@@ -339,31 +338,137 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
 }
 
 /* ************************************************************************* */
-TEST(NonlinearOptimizer, MoreOptimizationWithHuber) {
+TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) {
 
   NonlinearFactorGraph fg;
-  fg += PriorFactor<Pose2>(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1));
-  fg += BetweenFactor<Pose2>(0, 1, Pose2(1,0,M_PI/2),
+  fg.addPrior(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1));
+  fg += BetweenFactor<Pose2>(0, 1, Pose2(1,1.1,M_PI/4),
                               noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(2.0),
                                                          noiseModel::Isotropic::Sigma(3,1)));
-  fg += BetweenFactor<Pose2>(1, 2, Pose2(1,0,M_PI/2),
+  fg += BetweenFactor<Pose2>(0, 1, Pose2(1,0.9,M_PI/2),
                               noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(3.0),
                                                          noiseModel::Isotropic::Sigma(3,1)));
 
   Values init;
-  init.insert(0, Pose2(10,10,0));
-  init.insert(1, Pose2(1,0,M_PI));
-  init.insert(2, Pose2(1,1,-M_PI));
+  init.insert(0, Pose2(0,0,0));
+  init.insert(1, Pose2(0.961187, 0.99965, 1.1781));
 
   Values expected;
   expected.insert(0, Pose2(0,0,0));
-  expected.insert(1, Pose2(1,0,M_PI/2));
-  expected.insert(2, Pose2(1,1,M_PI));
+  expected.insert(1, Pose2(0.961187, 0.99965, 1.1781));
+
+  LevenbergMarquardtParams lm_params;
+
+  auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
+  auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize();
+  auto dl_result = DoglegOptimizer(fg, init).optimize();
+
+  EXPECT(assert_equal(expected, gn_result, 3e-2));
+  EXPECT(assert_equal(expected, lm_result, 3e-2));
+  EXPECT(assert_equal(expected, dl_result, 3e-2));
+}
+
+/* ************************************************************************* */
+TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) {
+
+  NonlinearFactorGraph fg;
+  fg.addPrior(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01));
+  fg += BetweenFactor<Point2>(0, 1, Point2(1,1.8),
+                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
+                                                         noiseModel::Isotropic::Sigma(2,1)));
+  fg += BetweenFactor<Point2>(0, 1, Point2(1,0.9),
+                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
+                                                         noiseModel::Isotropic::Sigma(2,1)));
+  fg += BetweenFactor<Point2>(0, 1, Point2(1,90),
+                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
+                                                         noiseModel::Isotropic::Sigma(2,1)));
+
+  Values init;
+  init.insert(0, Point2(1,1));
+  init.insert(1, Point2(1,0));
+
+  Values expected;
+  expected.insert(0, Point2(0,0));
+  expected.insert(1, Point2(1,1.85));
 
   LevenbergMarquardtParams params;
-  EXPECT(assert_equal(expected, GaussNewtonOptimizer(fg, init).optimize()));
-  EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init, params).optimize()));
-  EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize()));
+
+  auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
+  auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize();
+  auto dl_result = DoglegOptimizer(fg, init).optimize();
+
+  EXPECT(assert_equal(expected, gn_result, 1e-4));
+  EXPECT(assert_equal(expected, lm_result, 1e-4));
+  EXPECT(assert_equal(expected, dl_result, 1e-4));
+}
+
+/* ************************************************************************* */
+TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) {
+
+  NonlinearFactorGraph fg;
+  fg.addPrior(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1));
+  fg += BetweenFactor<Pose2>(0, 1, Pose2(0,9, M_PI/2),
+                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
+                                                         noiseModel::Isotropic::Sigma(3,1)));
+  fg += BetweenFactor<Pose2>(0, 1, Pose2(0, 11, M_PI/2),
+                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
+                                                         noiseModel::Isotropic::Sigma(3,1)));
+  fg += BetweenFactor<Pose2>(0, 1, Pose2(0, 10, M_PI/2),
+                             noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
+                                                        noiseModel::Isotropic::Sigma(3,1)));
+  fg += BetweenFactor<Pose2>(0, 1, Pose2(0,9, 0),
+                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
+                                                         noiseModel::Isotropic::Sigma(3,1)));
+
+  Values init;
+  init.insert(0, Pose2(0, 0, 0));
+  init.insert(1, Pose2(0, 10, M_PI/4));
+
+  Values expected;
+  expected.insert(0, Pose2(0, 0, 0));
+  expected.insert(1, Pose2(0, 10, 1.45212));
+
+  LevenbergMarquardtParams params;
+
+  auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
+  auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize();
+  auto dl_result = DoglegOptimizer(fg, init).optimize();
+
+  EXPECT(assert_equal(expected, gn_result, 1e-1));
+  EXPECT(assert_equal(expected, lm_result, 1e-1));
+  EXPECT(assert_equal(expected, dl_result, 1e-1));
+}
+
+/* ************************************************************************* */
+TEST(NonlinearOptimizer, RobustMeanCalculation) {
+
+  NonlinearFactorGraph fg;
+
+  Values init;
+
+  Values expected;
+
+  auto huber = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(20),
+                                          noiseModel::Isotropic::Sigma(1, 1));
+
+  vector<double> pts{-10,-3,-1,1,3,10,1000};
+  for(auto pt : pts) {
+    fg.addPrior(0, pt, huber);
+  }
+
+  init.insert(0, 100.0);
+  expected.insert(0, 3.33333333);
+
+  DoglegParams params_dl;
+  params_dl.setRelativeErrorTol(1e-10);
+
+  auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
+  auto lm_result = LevenbergMarquardtOptimizer(fg, init).optimize();
+  auto dl_result = DoglegOptimizer(fg, init, params_dl).optimize();
+
+  EXPECT(assert_equal(expected, gn_result, tol));
+  EXPECT(assert_equal(expected, lm_result, tol));
+  EXPECT(assert_equal(expected, dl_result, tol));
 }
 
 /* ************************************************************************* */
@@ -379,9 +484,9 @@ TEST(NonlinearOptimizer, disconnected_graph) {
   init.insert(X(3), Pose2(0.,0.,0.));
 
   NonlinearFactorGraph graph;
-  graph += PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
+  graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
   graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1));
-  graph += PriorFactor<Pose2>(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
+  graph.addPrior(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
 
   EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(graph, init).optimize()));
 }
@@ -389,23 +494,24 @@ TEST(NonlinearOptimizer, disconnected_graph) {
 /* ************************************************************************* */
 #include <gtsam/linear/iterative.h>
 
-class IterativeLM: public LevenbergMarquardtOptimizer {
-
+class IterativeLM : public LevenbergMarquardtOptimizer {
   /// Solver specific parameters
   ConjugateGradientParameters cgParams_;
   Values initial_;
 
-public:
+ public:
   /// Constructor
   IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues,
-      const ConjugateGradientParameters &p,
-      const LevenbergMarquardtParams& params = LevenbergMarquardtParams::LegacyDefaults()) :
-      LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p), initial_(initialValues) {
-  }
+              const ConjugateGradientParameters& p,
+              const LevenbergMarquardtParams& params =
+                  LevenbergMarquardtParams::LegacyDefaults())
+      : LevenbergMarquardtOptimizer(graph, initialValues, params),
+        cgParams_(p),
+        initial_(initialValues) {}
 
   /// Solve that uses conjugate gradient
-  virtual VectorValues solve(const GaussianFactorGraph &gfg,
-      const NonlinearOptimizerParams& params) const {
+  virtual VectorValues solve(const GaussianFactorGraph& gfg,
+                             const NonlinearOptimizerParams& params) const {
     VectorValues zeros = initial_.zeroVectors();
     return conjugateGradientDescent(gfg, zeros, cgParams_);
   }
@@ -414,19 +520,20 @@ public:
 /* ************************************************************************* */
 TEST(NonlinearOptimizer, subclass_solver) {
   Values expected;
-  expected.insert(X(1), Pose2(0.,0.,0.));
-  expected.insert(X(2), Pose2(1.5,0.,0.));
-  expected.insert(X(3), Pose2(3.0,0.,0.));
+  expected.insert(X(1), Pose2(0., 0., 0.));
+  expected.insert(X(2), Pose2(1.5, 0., 0.));
+  expected.insert(X(3), Pose2(3.0, 0., 0.));
 
   Values init;
-  init.insert(X(1), Pose2(0.,0.,0.));
-  init.insert(X(2), Pose2(0.,0.,0.));
-  init.insert(X(3), Pose2(0.,0.,0.));
+  init.insert(X(1), Pose2(0., 0., 0.));
+  init.insert(X(2), Pose2(0., 0., 0.));
+  init.insert(X(3), Pose2(0., 0., 0.));
 
   NonlinearFactorGraph graph;
-  graph += PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
-  graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1));
-  graph += PriorFactor<Pose2>(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
+  graph.addPrior(X(1), Pose2(0., 0., 0.), noiseModel::Isotropic::Sigma(3, 1));
+  graph += BetweenFactor<Pose2>(X(1), X(2), Pose2(1.5, 0., 0.),
+                                noiseModel::Isotropic::Sigma(3, 1));
+  graph.addPrior(X(3), Pose2(3., 0., 0.), noiseModel::Isotropic::Sigma(3, 1));
 
   ConjugateGradientParameters p;
   Values actual = IterativeLM(graph, init, p).optimize();
@@ -481,7 +588,7 @@ struct traits<MyType> {
 
 TEST(NonlinearOptimizer, Traits) {
   NonlinearFactorGraph fg;
-  fg += PriorFactor<MyType>(0, MyType(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1));
+  fg.addPrior(0, MyType(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1));
 
   Values init;
   init.insert(0, MyType(0,0,0));
diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp
index 66b022c82..9d6cc49ac 100644
--- a/tests/testPCGSolver.cpp
+++ b/tests/testPCGSolver.cpp
@@ -126,65 +126,63 @@ TEST( GaussianFactorGraphSystem, multiply_getb)
 
 /* ************************************************************************* */
 // Test Dummy Preconditioner
-TEST( PCGSolver, dummy )
-{
-  LevenbergMarquardtParams paramsPCG;
-  paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
-  PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
+TEST(PCGSolver, dummy) {
+  LevenbergMarquardtParams params;
+  params.linearSolverType = LevenbergMarquardtParams::Iterative;
+  auto pcg = boost::make_shared<PCGSolverParameters>();
   pcg->preconditioner_ = boost::make_shared<DummyPreconditionerParameters>();
-  paramsPCG.iterativeParams = pcg;
+  params.iterativeParams = pcg;
 
   NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
 
-  Point2 x0(10,10);
+  Point2 x0(10, 10);
   Values c0;
   c0.insert(X(1), x0);
 
-  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();
+  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize();
 
-  DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
+  DOUBLES_EQUAL(0, fg.error(actualPCG), tol);
 }
 
 /* ************************************************************************* */
 // Test Block-Jacobi Precondioner
-TEST( PCGSolver, blockjacobi )
-{
-  LevenbergMarquardtParams paramsPCG;
-  paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
-  PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
-  pcg->preconditioner_ = boost::make_shared<BlockJacobiPreconditionerParameters>();
-  paramsPCG.iterativeParams = pcg;
+TEST(PCGSolver, blockjacobi) {
+  LevenbergMarquardtParams params;
+  params.linearSolverType = LevenbergMarquardtParams::Iterative;
+  auto pcg = boost::make_shared<PCGSolverParameters>();
+  pcg->preconditioner_ =
+      boost::make_shared<BlockJacobiPreconditionerParameters>();
+  params.iterativeParams = pcg;
 
   NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
 
-  Point2 x0(10,10);
+  Point2 x0(10, 10);
   Values c0;
   c0.insert(X(1), x0);
 
-  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();
+  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize();
 
-  DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
+  DOUBLES_EQUAL(0, fg.error(actualPCG), tol);
 }
 
 /* ************************************************************************* */
 // Test Incremental Subgraph PCG Solver
-TEST( PCGSolver, subgraph )
-{
-  LevenbergMarquardtParams paramsPCG;
-  paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
-  PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
+TEST(PCGSolver, subgraph) {
+  LevenbergMarquardtParams params;
+  params.linearSolverType = LevenbergMarquardtParams::Iterative;
+  auto pcg = boost::make_shared<PCGSolverParameters>();
   pcg->preconditioner_ = boost::make_shared<SubgraphPreconditionerParameters>();
-  paramsPCG.iterativeParams = pcg;
+  params.iterativeParams = pcg;
 
   NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
 
-  Point2 x0(10,10);
+  Point2 x0(10, 10);
   Values c0;
   c0.insert(X(1), x0);
 
-  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();
+  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize();
 
-  DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
+  DOUBLES_EQUAL(0, fg.error(actualPCG), tol);
 }
 
 /* ************************************************************************* */
diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp
index 31cf68ebd..5746022a3 100644
--- a/tests/testRot3Optimization.cpp
+++ b/tests/testRot3Optimization.cpp
@@ -26,11 +26,9 @@
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 
 using namespace gtsam;
 
-typedef PriorFactor<Rot3> Prior;
 typedef BetweenFactor<Rot3> Between;
 typedef NonlinearFactorGraph Graph;
 
@@ -41,7 +39,7 @@ TEST(Rot3, optimize) {
   Values truth;
   Values initial;
   Graph fg;
-  fg += Prior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01));
+  fg.addPrior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01));
   for(int j=0; j<6; ++j) {
     truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j)));
     initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
@@ -59,4 +57,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-
diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp
index 64e111e94..e3252a90b 100644
--- a/tests/testSerializationSLAM.cpp
+++ b/tests/testSerializationSLAM.cpp
@@ -27,7 +27,7 @@
 //#include <gtsam/slam/BoundingConstraint.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 //#include <gtsam/slam/PartialPriorFactor.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/sam/RangeFactor.h>
 #include <gtsam/slam/StereoFactor.h>
@@ -47,7 +47,7 @@
 #include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/geometry/CalibratedCamera.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/StereoCamera.h>
 
 #include <gtsam/base/serializationTestHelpers.h>
@@ -57,20 +57,21 @@ using namespace gtsam;
 using namespace gtsam::serializationTestHelpers;
 
 // Creating as many permutations of factors as possible
-typedef PriorFactor<LieVector>         PriorFactorLieVector;
-typedef PriorFactor<LieMatrix>         PriorFactorLieMatrix;
-typedef PriorFactor<Point2>            PriorFactorPoint2;
-typedef PriorFactor<StereoPoint2>      PriorFactorStereoPoint2;
-typedef PriorFactor<Point3>            PriorFactorPoint3;
-typedef PriorFactor<Rot2>              PriorFactorRot2;
-typedef PriorFactor<Rot3>              PriorFactorRot3;
-typedef PriorFactor<Pose2>             PriorFactorPose2;
-typedef PriorFactor<Pose3>             PriorFactorPose3;
-typedef PriorFactor<Cal3_S2>           PriorFactorCal3_S2;
-typedef PriorFactor<Cal3DS2>           PriorFactorCal3DS2;
-typedef PriorFactor<CalibratedCamera>  PriorFactorCalibratedCamera;
-typedef PriorFactor<SimpleCamera>      PriorFactorSimpleCamera;
-typedef PriorFactor<StereoCamera>      PriorFactorStereoCamera;
+typedef PriorFactor<LieVector>              PriorFactorLieVector;
+typedef PriorFactor<LieMatrix>              PriorFactorLieMatrix;
+typedef PriorFactor<Point2>                 PriorFactorPoint2;
+typedef PriorFactor<StereoPoint2>           PriorFactorStereoPoint2;
+typedef PriorFactor<Point3>                 PriorFactorPoint3;
+typedef PriorFactor<Rot2>                   PriorFactorRot2;
+typedef PriorFactor<Rot3>                   PriorFactorRot3;
+typedef PriorFactor<Pose2>                  PriorFactorPose2;
+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;
 
 typedef BetweenFactor<LieVector>       BetweenFactorLieVector;
 typedef BetweenFactor<LieMatrix>       BetweenFactorLieMatrix;
@@ -81,29 +82,32 @@ typedef BetweenFactor<Rot3>            BetweenFactorRot3;
 typedef BetweenFactor<Pose2>           BetweenFactorPose2;
 typedef BetweenFactor<Pose3>           BetweenFactorPose3;
 
-typedef NonlinearEquality<LieVector>         NonlinearEqualityLieVector;
-typedef NonlinearEquality<LieMatrix>         NonlinearEqualityLieMatrix;
-typedef NonlinearEquality<Point2>            NonlinearEqualityPoint2;
-typedef NonlinearEquality<StereoPoint2>      NonlinearEqualityStereoPoint2;
-typedef NonlinearEquality<Point3>            NonlinearEqualityPoint3;
-typedef NonlinearEquality<Rot2>              NonlinearEqualityRot2;
-typedef NonlinearEquality<Rot3>              NonlinearEqualityRot3;
-typedef NonlinearEquality<Pose2>             NonlinearEqualityPose2;
-typedef NonlinearEquality<Pose3>             NonlinearEqualityPose3;
-typedef NonlinearEquality<Cal3_S2>           NonlinearEqualityCal3_S2;
-typedef NonlinearEquality<Cal3DS2>           NonlinearEqualityCal3DS2;
-typedef NonlinearEquality<CalibratedCamera>  NonlinearEqualityCalibratedCamera;
-typedef NonlinearEquality<SimpleCamera>      NonlinearEqualitySimpleCamera;
-typedef NonlinearEquality<StereoCamera>      NonlinearEqualityStereoCamera;
+typedef NonlinearEquality<LieVector>              NonlinearEqualityLieVector;
+typedef NonlinearEquality<LieMatrix>              NonlinearEqualityLieMatrix;
+typedef NonlinearEquality<Point2>                 NonlinearEqualityPoint2;
+typedef NonlinearEquality<StereoPoint2>           NonlinearEqualityStereoPoint2;
+typedef NonlinearEquality<Point3>                 NonlinearEqualityPoint3;
+typedef NonlinearEquality<Rot2>                   NonlinearEqualityRot2;
+typedef NonlinearEquality<Rot3>                   NonlinearEqualityRot3;
+typedef NonlinearEquality<Pose2>                  NonlinearEqualityPose2;
+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;
 
-typedef RangeFactor<Pose2, Point2>                      RangeFactor2D;
-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<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
-typedef RangeFactor<SimpleCamera, SimpleCamera>         RangeFactorSimpleCamera;
+typedef RangeFactor<Pose2, Point2>                              RangeFactor2D;
+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;
 typedef BearingRangeFactor<Pose3, Point3>  BearingRangeFactor3D;
@@ -111,7 +115,7 @@ typedef BearingRangeFactor<Pose3, Point3>  BearingRangeFactor3D;
 typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorCal3_S2;
 typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
 
-typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
 //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 
 typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
@@ -158,6 +162,7 @@ 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);
 
 /* Create GUIDs for factors */
@@ -294,7 +299,7 @@ TEST (testSerializationSLAM, factors) {
   Cal3DS2 cal3ds2(1.0, 2.0, 3.0, 4.0, 5.0,6.0, 7.0, 8.0, 9.0);
   Cal3_S2Stereo cal3_s2stereo(1.0, 2.0, 3.0, 4.0, 5.0, 1.0);
   CalibratedCamera calibratedCamera(pose3);
-  SimpleCamera simpleCamera(pose3, cal3_s2);
+  PinholeCamera<Cal3_S2> simpleCamera(pose3, cal3_s2);
   StereoCamera stereoCamera(pose3, boost::make_shared<Cal3_S2Stereo>(cal3_s2stereo));
 
 
diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp
new file mode 100644
index 000000000..5a98c3bf5
--- /dev/null
+++ b/tests/testTranslationRecovery.cpp
@@ -0,0 +1,87 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, 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 testTranslationRecovery.cpp
+ * @author Frank Dellaert
+ * @date March 2020
+ * @brief test recovering translations when rotations are given.
+ */
+
+#include <gtsam/sfm/TranslationRecovery.h>
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/slam/dataset.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+// We read the BAL file, which has 3 cameras in it, with poses. We then assume
+// the rotations are correct, but translations have to be estimated from
+// translation directions only. Since we have 3 cameras, A, B, and C, we can at
+// most create three relative measurements, let's call them w_aZb, w_aZc, and
+// bZc. These will be of type Unit3. We then call `recoverTranslations` which
+// sets up an optimization problem for the three unknown translations.
+TEST(TranslationRecovery, BAL) {
+  const string filename = findExampleDataFile("dubrovnik-3-7-pre");
+  SfmData db;
+  bool success = readBAL(filename, db);
+  if (!success) throw runtime_error("Could not access file!");
+
+  // Get camera poses, as Values
+  size_t j = 0;
+  Values poses;
+  for (auto camera : db.cameras) {
+    poses.insert(j++, camera.pose());
+  }
+
+  // Simulate measurements
+  const auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
+      poses, {{0, 1}, {0, 2}, {1, 2}});
+
+  // Check
+  const Pose3 wTa = poses.at<Pose3>(0), wTb = poses.at<Pose3>(1),
+              wTc = poses.at<Pose3>(2);
+  const Point3 Ta = wTa.translation(), Tb = wTb.translation(),
+               Tc = wTc.translation();
+  const Rot3 aRw = wTa.rotation().inverse();
+  const Unit3 w_aZb = relativeTranslations.at({0, 1});
+  EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb));
+  const Unit3 w_aZc = relativeTranslations.at({0, 2});
+  EXPECT(assert_equal(Unit3(Tc - Ta), w_aZc));
+
+  TranslationRecovery algorithm(relativeTranslations);
+  const auto graph = algorithm.buildGraph();
+  EXPECT_LONGS_EQUAL(3, graph.size());
+
+  // Translation recovery, version 1
+  const double scale = 2.0;
+  const auto result = algorithm.run(scale);
+
+  // Check result for first two translations, determined by prior
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
+  EXPECT(assert_equal(Point3(2 * w_aZb.point3()), result.at<Point3>(1)));
+
+  // Check that the third translations is correct
+  Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm());
+  EXPECT(assert_equal(expected, result.at<Point3>(2), 1e-4));
+
+  // TODO(frank): how to get stats back?
+  // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
diff --git a/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp
index 1ced2af23..9fedc853b 100644
--- a/tests/testVisualISAM2.cpp
+++ b/tests/testVisualISAM2.cpp
@@ -24,7 +24,6 @@
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 
 #include <vector>
@@ -59,7 +58,7 @@ TEST(testVisualISAM2, all)
         // Add factors for each landmark observation
         for (size_t j = 0; j < points.size(); ++j)
         {
-            SimpleCamera camera(poses[i], *K);
+            PinholeCamera<Cal3_S2> camera(poses[i], *K);
             Point2 measurement = camera.project(points[j]);
             graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
                 measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
@@ -76,15 +75,13 @@ TEST(testVisualISAM2, all)
         {
             // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
             static auto kPosePrior = noiseModel::Diagonal::Sigmas(
-                (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))
+                (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
                     .finished());
-            graph.emplace_shared<PriorFactor<Pose3>>(Symbol('x', 0), poses[0],
-                                                     kPosePrior);
+            graph.addPrior(Symbol('x', 0), poses[0], kPosePrior);
 
             // Add a prior on landmark l0
             static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
-            graph.emplace_shared<PriorFactor<Point3>>(Symbol('l', 0), points[0],
-                                                      kPointPrior);
+            graph.addPrior(Symbol('l', 0), points[0], kPointPrior);
 
             // Add initial guesses to all observed landmarks
             // Intentionally initialize the variables off from the ground truth
diff --git a/timing/timeFrobeniusFactor.cpp b/timing/timeFrobeniusFactor.cpp
new file mode 100644
index 000000000..c8bdd8fec
--- /dev/null
+++ b/timing/timeFrobeniusFactor.cpp
@@ -0,0 +1,110 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    timeFrobeniusFactor.cpp
+ * @brief   time FrobeniusFactor with BAL file
+ * @author  Frank Dellaert
+ * @date    June 6, 2015
+ */
+
+#include <gtsam/base/timing.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/SO4.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/linear/PCGSolver.h>
+#include <gtsam/linear/SubgraphPreconditioner.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/FrobeniusFactor.h>
+
+#include <iostream>
+#include <string>
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
+
+int main(int argc, char* argv[]) {
+  // primitive argument parsing:
+  if (argc > 3) {
+    throw runtime_error("Usage: timeFrobeniusFactor [g2oFile]");
+  }
+
+  string g2oFile;
+  try {
+    if (argc > 1)
+      g2oFile = argv[argc - 1];
+    else
+      g2oFile =
+          "/Users/dellaert/git/2019c-notes-shonanrotationaveraging/matlabCode/"
+          "datasets/randomTorus3D.g2o";
+    // g2oFile = findExampleDataFile("sphere_smallnoise.graph");
+  } catch (const exception& e) {
+    cerr << e.what() << '\n';
+    exit(1);
+  }
+
+  // Read G2O file
+  const auto factors = parse3DFactors(g2oFile);
+  const auto poses = parse3DPoses(g2oFile);
+
+  // Build graph
+  NonlinearFactorGraph graph;
+  // graph.add(NonlinearEquality<SO4>(0, SO4()));
+  auto priorModel = noiseModel::Isotropic::Sigma(6, 10000);
+  graph.add(PriorFactor<SO4>(0, SO4(), priorModel));
+  for (const auto& factor : factors) {
+    const auto& keys = factor->keys();
+    const auto& Tij = factor->measured();
+    const auto& model = factor->noiseModel();
+    graph.emplace_shared<FrobeniusWormholeFactor>(
+        keys[0], keys[1], SO3(Tij.rotation().matrix()), model);
+  }
+
+  boost::mt19937 rng(42);
+
+  // Set parameters to be similar to ceres
+  LevenbergMarquardtParams params;
+  LevenbergMarquardtParams::SetCeresDefaults(&params);
+  params.setLinearSolverType("MULTIFRONTAL_QR");
+  // params.setVerbosityLM("SUMMARY");
+  // params.linearSolverType = LevenbergMarquardtParams::Iterative;
+  // auto pcg = boost::make_shared<PCGSolverParameters>();
+  // pcg->preconditioner_ =
+  // boost::make_shared<SubgraphPreconditionerParameters>();
+  // boost::make_shared<BlockJacobiPreconditionerParameters>();
+  // params.iterativeParams = pcg;
+
+  // Optimize
+  for (size_t i = 0; i < 100; i++) {
+    gttic_(optimize);
+    Values initial;
+    initial.insert(0, SO4());
+    for (size_t j = 1; j < poses.size(); j++) {
+      initial.insert(j, SO4::Random(rng));
+    }
+    LevenbergMarquardtOptimizer lm(graph, initial, params);
+    Values result = lm.optimize();
+    cout << "cost = " << graph.error(result) << endl;
+  }
+
+  tictoc_finishedIteration_();
+  tictoc_print_();
+
+  return 0;
+}
diff --git a/timing/timeGaussianFactorGraph.cpp b/timing/timeGaussianFactorGraph.cpp
index 3258edb49..2c1e11586 100644
--- a/timing/timeGaussianFactorGraph.cpp
+++ b/timing/timeGaussianFactorGraph.cpp
@@ -40,7 +40,7 @@ double timeKalmanSmoother(int T) {
 /* ************************************************************************* */
 // Create a planar factor graph and optimize
 double timePlanarSmoother(int N, bool old = true) {
-  GaussianFactorGraph fg = planarGraph(N).get<0>();
+  GaussianFactorGraph fg = planarGraph(N).first;
   clock_t start = clock();
   fg.optimize();
   clock_t end = clock ();
@@ -51,7 +51,7 @@ double timePlanarSmoother(int N, bool old = true) {
 /* ************************************************************************* */
 // Create a planar factor graph and eliminate
 double timePlanarSmootherEliminate(int N, bool old = true) {
-  GaussianFactorGraph fg = planarGraph(N).get<0>();
+  GaussianFactorGraph fg = planarGraph(N).first;
   clock_t start = clock();
   fg.eliminateMultifrontal();
   clock_t end = clock ();
diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp
index ec5fc3fa5..6e0f4ccdf 100644
--- a/timing/timeIncremental.cpp
+++ b/timing/timeIncremental.cpp
@@ -15,7 +15,6 @@
  */
 
 #include <gtsam/slam/dataset.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/geometry/Pose2.h>
@@ -98,7 +97,7 @@ int main(int argc, char *argv[]) {
       //      cout << "Initializing " << 0 << endl;
       newVariables.insert(0, Pose());
       // Add prior
-      newFactors.add(PriorFactor<Pose>(0, Pose(), noiseModel::Unit::Create(3)));
+      newFactors.addPrior(0, Pose(), noiseModel::Unit::Create(3));
     }
     while(nextMeasurement < measurements.size()) {
 
diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp
index d0b6b263c..7aaf37e92 100644
--- a/timing/timeLago.cpp
+++ b/timing/timeLago.cpp
@@ -17,7 +17,6 @@
  */
 
 #include <gtsam/slam/dataset.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/lago.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@@ -51,7 +50,7 @@ int main(int argc, char *argv[]) {
   // Add prior on the pose having index (key) = 0
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8));
-  g->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+  g->addPrior(0, Pose2(), priorModel);
 
   // LAGO
   for (size_t i = 0; i < trials; i++) {
diff --git a/timing/timeMatrix.cpp b/timing/timeMatrix.cpp
index c3e9f32f4..e4dec40d0 100644
--- a/timing/timeMatrix.cpp
+++ b/timing/timeMatrix.cpp
@@ -16,7 +16,7 @@
  */
 
 #include <iostream>
-#include <boost/timer.hpp>
+#include <gtsam/base/timing.h>
 #include <gtsam/base/Matrix.h>
 
 using namespace std;
@@ -54,7 +54,7 @@ double timeCollect(size_t p, size_t m, size_t n, bool passDims, size_t reps) {
   Matrix result;
   double elapsed;
   {
-    boost::timer t;
+    gttic_(elapsed);
 
     if (passDims)
       for (size_t i=0; i<reps; ++i)
@@ -62,7 +62,11 @@ double timeCollect(size_t p, size_t m, size_t n, bool passDims, size_t reps) {
     else
       for (size_t i=0; i<reps; ++i)
         result = collect(matrices);
-    elapsed = t.elapsed();
+
+    gttoc_(elapsed);
+    tictoc_getNode(elapsedNode, elapsed);
+    elapsed = elapsedNode->secs();
+    tictoc_reset_();
   }
   // delete the matrices
   for (size_t i=0; i<p;++i) {
@@ -95,10 +99,15 @@ double timeVScaleColumn(size_t m, size_t n, size_t reps) {
   double elapsed;
   Matrix result;
   {
-    boost::timer t;
+    gttic_(elapsed);
+
     for (size_t i=0; i<reps; ++i)
       Matrix result = vector_scale(M,V);
-    elapsed = t.elapsed();
+
+    gttoc_(elapsed);
+    tictoc_getNode(elapsedNode, elapsed);
+    elapsed = elapsedNode->secs();
+    tictoc_reset_();
   }
 
   return elapsed;
@@ -126,10 +135,15 @@ double timeVScaleRow(size_t m, size_t n, size_t reps) {
   double elapsed;
   Matrix result;
   {
-    boost::timer t;
+    gttic_(elapsed);
+
     for (size_t i=0; i<reps; ++i)
       result = vector_scale(V,M);
-    elapsed = t.elapsed();
+
+    gttoc_(elapsed);
+    tictoc_getNode(elapsedNode, elapsed);
+    elapsed = elapsedNode->secs();
+    tictoc_reset_();
   }
 
   return elapsed;
@@ -156,12 +170,17 @@ double timeColumn(size_t reps) {
   double elapsed;
   Vector result;
   {
-    boost::timer t;
+    gttic_(elapsed);
+
     for (size_t i=0; i<reps; ++i)
       for (size_t j = 0; j<n; ++j)
         //result = ublas::matrix_column<Matrix>(M, j);
         result = column(M, j);
-    elapsed = t.elapsed();
+
+    gttoc_(elapsed);
+    tictoc_getNode(elapsedNode, elapsed);
+    elapsed = elapsedNode->secs();
+    tictoc_reset_();
   }
   return elapsed;
 }
@@ -198,12 +217,17 @@ double timeHouseholder(size_t reps) {
   // perform timing
   double elapsed;
   {
-    boost::timer t;
+    gttic_(elapsed);
+
     for (size_t i=0; i<reps; ++i) {
       Matrix A = Abase;
       householder_(A,3);
     }
-    elapsed = t.elapsed();
+
+    gttoc_(elapsed);
+    tictoc_getNode(elapsedNode, elapsed);
+    elapsed = elapsedNode->secs();
+    tictoc_reset_();
   }
   return elapsed;
 }
@@ -222,13 +246,18 @@ double timeMatrixInsert(size_t reps) {
   // perform timing
   double elapsed;
   {
-    boost::timer t;
+    gttic_(elapsed);
+
     Matrix big = bigBase;
     for (size_t rep=0; rep<reps; ++rep)
       for (size_t i=0; i<100; i += 5)
         for (size_t j=0; j<100; j += 5)
           insertSub(big, small, i,j);
-    elapsed = t.elapsed();
+
+    gttoc_(elapsed);
+    tictoc_getNode(elapsedNode, elapsed);
+    elapsed = elapsedNode->secs();
+    tictoc_reset_();
   }
   return elapsed;
 }
diff --git a/timing/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp
index f16be40f6..d9f2ffaf6 100644
--- a/timing/timeMatrixOps.cpp
+++ b/timing/timeMatrixOps.cpp
@@ -17,10 +17,10 @@
  */
 
 #include <boost/random.hpp>
-#include <boost/timer.hpp>
 #include <boost/format.hpp>
 #include <boost/lambda/lambda.hpp>
 
+#include <gtsam/base/timing.h>
 #include <gtsam/base/Matrix.h>
 
 #include <iostream>
@@ -30,7 +30,6 @@
 using namespace std;
 //namespace ublas = boost::numeric::ublas;
 //using namespace Eigen;
-using boost::timer;
 using boost::format;
 using namespace boost::lambda;
 
@@ -68,7 +67,6 @@ int main(int argc, char* argv[]) {
     cout << endl;
 
     {
-      timer tim;
       double basicTime, fullTime, topTime, blockTime;
 
       cout << "Row-major matrix, row-major assignment:" << endl;
@@ -79,43 +77,54 @@ int main(int argc, char* argv[]) {
           for(size_t j=0; j<(size_t)mat.cols(); ++j)
             mat(i,j) = rng();
 
-      tim.restart();
+      gttic_(basicTime);
       for(size_t rep=0; rep<nReps; ++rep)
         for(size_t i=0; i<(size_t)mat.rows(); ++i)
           for(size_t j=0; j<(size_t)mat.cols(); ++j)
             mat(i,j) = rng();
-      basicTime = tim.elapsed();
+      gttoc_(basicTime);
+      tictoc_getNode(basicTimeNode, basicTime);
+      basicTime = basicTimeNode->secs();
+      gtsam::tictoc_reset_();
       cout << format("  Basic: %1% mus/element") % double(1000000 * basicTime / double(mat.rows()*mat.cols()*nReps)) << endl;
 
-      tim.restart();
+      gttic_(fullTime);
       for(size_t rep=0; rep<nReps; ++rep)
         for(size_t i=0; i<(size_t)full.rows(); ++i)
           for(size_t j=0; j<(size_t)full.cols(); ++j)
             full(i,j) = rng();
-      fullTime = tim.elapsed();
+      gttoc_(fullTime);
+      tictoc_getNode(fullTimeNode, fullTime);
+      fullTime = fullTimeNode->secs();
+      gtsam::tictoc_reset_();
       cout << format("  Full:  %1% mus/element") % double(1000000 * fullTime / double(full.rows()*full.cols()*nReps)) << endl;
 
-      tim.restart();
+      gttic_(topTime);
       for(size_t rep=0; rep<nReps; ++rep)
         for(size_t i=0; i<(size_t)top.rows(); ++i)
           for(size_t j=0; j<(size_t)top.cols(); ++j)
             top(i,j) = rng();
-      topTime = tim.elapsed();
+      gttoc_(topTime);
+      tictoc_getNode(topTimeNode, topTime);
+      topTime = topTimeNode->secs();
+      gtsam::tictoc_reset_();
       cout << format("  Top:   %1% mus/element") % double(1000000 * topTime / double(top.rows()*top.cols()*nReps)) << endl;
 
-      tim.restart();
+      gttic_(blockTime);
       for(size_t rep=0; rep<nReps; ++rep)
         for(size_t i=0; i<(size_t)block.rows(); ++i)
           for(size_t j=0; j<(size_t)block.cols(); ++j)
             block(i,j) = rng();
-      blockTime = tim.elapsed();
+      gttoc_(blockTime);
+      tictoc_getNode(blockTimeNode, blockTime);
+      blockTime = blockTimeNode->secs();
+      gtsam::tictoc_reset_();
       cout << format("  Block: %1% mus/element") % double(1000000 * blockTime / double(block.rows()*block.cols()*nReps)) << endl;
 
       cout << endl;
     }
 
     {
-      timer tim;
       double basicTime, fullTime, topTime, blockTime;
 
       cout << "Row-major matrix, column-major assignment:" << endl;
@@ -126,43 +135,54 @@ int main(int argc, char* argv[]) {
           for(size_t i=0; i<(size_t)mat.rows(); ++i)
             mat(i,j) = rng();
 
-      tim.restart();
+      gttic_(basicTime);
       for(size_t rep=0; rep<nReps; ++rep)
         for(size_t j=0; j<(size_t)mat.cols(); ++j)
           for(size_t i=0; i<(size_t)mat.rows(); ++i)
             mat(i,j) = rng();
-      basicTime = tim.elapsed();
+      gttoc_(basicTime);
+      tictoc_getNode(basicTimeNode, basicTime);
+      basicTime = basicTimeNode->secs();
+      gtsam::tictoc_reset_();
       cout << format("  Basic: %1% mus/element") % double(1000000 * basicTime / double(mat.rows()*mat.cols()*nReps)) << endl;
 
-      tim.restart();
+      gttic_(fullTime);
       for(size_t rep=0; rep<nReps; ++rep)
         for(size_t j=0; j<(size_t)full.cols(); ++j)
           for(size_t i=0; i<(size_t)full.rows(); ++i)
             full(i,j) = rng();
-      fullTime = tim.elapsed();
+      gttoc_(fullTime);
+      tictoc_getNode(fullTimeNode, fullTime);
+      fullTime = fullTimeNode->secs();
+      gtsam::tictoc_reset_();
       cout << format("  Full:  %1% mus/element") % double(1000000 * fullTime / double(full.rows()*full.cols()*nReps)) << endl;
 
-      tim.restart();
+      gttic_(topTime);
       for(size_t rep=0; rep<nReps; ++rep)
         for(size_t j=0; j<(size_t)top.cols(); ++j)
           for(size_t i=0; i<(size_t)top.rows(); ++i)
             top(i,j) = rng();
-      topTime = tim.elapsed();
+      gttoc_(topTime);
+      tictoc_getNode(topTimeNode, topTime);
+      topTime = topTimeNode->secs();
+      gtsam::tictoc_reset_();
       cout << format("  Top:   %1% mus/element") % double(1000000 * topTime / double(top.rows()*top.cols()*nReps)) << endl;
 
-      tim.restart();
+      gttic_(blockTime);
       for(size_t rep=0; rep<nReps; ++rep)
         for(size_t j=0; j<(size_t)block.cols(); ++j)
           for(size_t i=0; i<(size_t)block.rows(); ++i)
             block(i,j) = rng();
-      blockTime = tim.elapsed();
+      gttoc_(blockTime);
+      tictoc_getNode(blockTimeNode, blockTime);
+      blockTime = blockTimeNode->secs();
+      gtsam::tictoc_reset_();
       cout << format("  Block: %1% mus/element") % double(1000000 * blockTime / double(block.rows()*block.cols()*nReps)) << endl;
 
       cout << endl;
     }
 
     {
-      timer tim;
       double basicTime, fullTime, topTime, blockTime;
       typedef pair<size_t,size_t> ij_t;
       vector<ij_t> ijs(100000);
@@ -174,28 +194,44 @@ int main(int argc, char* argv[]) {
       for(size_t rep=0; rep<1000; ++rep)
         for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rng(); }
 
+      gttic_(basicTime);
       for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj()));
       for(size_t rep=0; rep<1000; ++rep)
         for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rng(); }
-      basicTime = tim.elapsed();
+      gttoc_(basicTime);
+      tictoc_getNode(basicTimeNode, basicTime);
+      basicTime = basicTimeNode->secs();
+      gtsam::tictoc_reset_();
       cout << format("  Basic: %1% mus/element") % double(1000000 * basicTime / double(ijs.size()*nReps)) << endl;
 
+      gttic_(fullTime);
       for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj()));
       for(size_t rep=0; rep<1000; ++rep)
         for(const ij_t& ij: ijs) { full(ij.first, ij.second) = rng(); }
-      fullTime = tim.elapsed();
+      gttoc_(fullTime);
+      tictoc_getNode(fullTimeNode, fullTime);
+      fullTime = fullTimeNode->secs();
+      gtsam::tictoc_reset_();
       cout << format("  Full:  %1% mus/element") % double(1000000 * fullTime / double(ijs.size()*nReps)) << endl;
 
+      gttic_(topTime);
       for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%top.rows(),rnj()));
       for(size_t rep=0; rep<1000; ++rep)
         for(const ij_t& ij: ijs) { top(ij.first, ij.second) = rng(); }
-      topTime = tim.elapsed();
+      gttoc_(topTime);
+      tictoc_getNode(topTimeNode, topTime);
+      topTime = topTimeNode->secs();
+      gtsam::tictoc_reset_();
       cout << format("  Top:   %1% mus/element") % double(1000000 * topTime / double(ijs.size()*nReps)) << endl;
 
+      gttic_(blockTime);
       for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%block.rows(),rnj()%block.cols()));
       for(size_t rep=0; rep<1000; ++rep)
         for(const ij_t& ij: ijs) { block(ij.first, ij.second) = rng(); }
-      blockTime = tim.elapsed();
+      gttoc_(blockTime);
+      tictoc_getNode(blockTimeNode, blockTime);
+      blockTime = blockTimeNode->secs();
+      gtsam::tictoc_reset_();
       cout << format("  Block: %1% mus/element") % double(1000000 * blockTime / double(ijs.size()*nReps)) << endl;
 
       cout << endl;
diff --git a/timing/timeRot3.cpp b/timing/timeRot3.cpp
index 00c02c250..3a6a156d5 100644
--- a/timing/timeRot3.cpp
+++ b/timing/timeRot3.cpp
@@ -23,33 +23,33 @@
 using namespace std;
 using namespace gtsam;
 
-#define TEST(TITLE,STATEMENT) \
-  cout << endl << TITLE << endl;\
-  timeLog = clock();\
-  for(int i = 0; i < n; i++)\
-  STATEMENT;\
-  timeLog2 = clock();\
-  seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;\
-  cout << seconds << " seconds" << endl;\
-  cout << ((double)n/seconds) << " calls/second" << endl;
+#define TEST(TITLE, STATEMENT)                                        \
+  cout << endl << TITLE << endl;                                      \
+  timeLog = clock();                                                  \
+  for (int i = 0; i < n; i++) STATEMENT;                              \
+  timeLog2 = clock();                                                 \
+  seconds = static_cast<double>(timeLog2 - timeLog) / CLOCKS_PER_SEC; \
+  cout << 1000 * seconds << " milliseconds" << endl;                  \
+  cout << (1e9 * seconds / static_cast<double>(n)) << " nanosecs/call" << endl;
 
-int main()
-{
-  int n = 100000; long timeLog, timeLog2; double seconds;
+int main() {
+  int n = 100000;
+  clock_t timeLog, timeLog2;
+  double seconds;
   // create a random direction:
-  double norm=sqrt(1.0+16.0+4.0);
-  double x=1.0/norm, y=4.0/norm, z=2.0/norm;
+  double norm = sqrt(1.0 + 16.0 + 4.0);
+  double x = 1.0 / norm, y = 4.0 / norm, z = 2.0 / norm;
   Vector v = (Vector(3) << x, y, z).finished();
   Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2), R2 = R.retract(v);
 
-  TEST("Rodriguez formula given axis angle", Rot3::AxisAngle(v,0.001))
+  TEST("Rodriguez formula given axis angle", Rot3::AxisAngle(v, 0.001))
   TEST("Rodriguez formula given canonical coordinates", Rot3::Rodrigues(v))
-  TEST("Expmap", R*Rot3::Expmap(v))
+  TEST("Expmap", R * Rot3::Expmap(v))
   TEST("Retract", R.retract(v))
   TEST("Logmap", Rot3::Logmap(R.between(R2)))
   TEST("localCoordinates", R.localCoordinates(R2))
-  TEST("Slow rotation matrix",Rot3::Rz(z)*Rot3::Ry(y)*Rot3::Rx(x))
-  TEST("Fast Rotation matrix", Rot3::RzRyRx(x,y,z))
+  TEST("Slow rotation matrix", Rot3::Rz(z) * Rot3::Ry(y) * Rot3::Rx(x))
+  TEST("Fast Rotation matrix", Rot3::RzRyRx(x, y, z))
 
   return 0;
 }
diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp
index be1104b1a..4a58a57a6 100644
--- a/timing/timeSFMBAL.cpp
+++ b/timing/timeSFMBAL.cpp
@@ -32,12 +32,12 @@ typedef GeneralSFMFactor<Camera, Point3> SfmFactor;
 
 int main(int argc, char* argv[]) {
   // parse options and read BAL file
-  SfM_data db = preamble(argc, argv);
+  SfmData db = preamble(argc, argv);
 
   // Build graph using conventional GeneralSFMFactor
   NonlinearFactorGraph graph;
   for (size_t j = 0; j < db.number_tracks(); j++) {
-    for (const SfM_Measurement& m: db.tracks[j].measurements) {
+    for (const SfmMeasurement& m: db.tracks[j].measurements) {
       size_t i = m.first;
       Point2 z = m.second;
       graph.emplace_shared<SfmFactor>(z, gNoiseModel, C(i), P(j));
@@ -46,9 +46,9 @@ int main(int argc, char* argv[]) {
 
   Values initial;
   size_t i = 0, j = 0;
-  for (const SfM_Camera& camera: db.cameras)
+  for (const SfmCamera& camera: db.cameras)
     initial.insert(C(i++), camera);
-  for (const SfM_Track& track: db.tracks)
+  for (const SfmTrack& track: db.tracks)
     initial.insert(P(j++), track.p);
 
   return optimize(db, graph, initial);
diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h
index 1ca9f82d2..548c4de70 100644
--- a/timing/timeSFMBAL.h
+++ b/timing/timeSFMBAL.h
@@ -38,7 +38,7 @@ static bool gUseSchur = true;
 static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
 
 // parse options and read BAL file
-SfM_data preamble(int argc, char* argv[]) {
+SfmData preamble(int argc, char* argv[]) {
   // primitive argument parsing:
   if (argc > 2) {
     if (strcmp(argv[1], "--colamd"))
@@ -48,7 +48,7 @@ SfM_data preamble(int argc, char* argv[]) {
   }
 
   // Load BAL file
-  SfM_data db;
+  SfmData db;
   string filename;
   if (argc > 1)
     filename = argv[argc - 1];
@@ -60,7 +60,7 @@ SfM_data preamble(int argc, char* argv[]) {
 }
 
 // Create ordering and optimize
-int optimize(const SfM_data& db, const NonlinearFactorGraph& graph,
+int optimize(const SfmData& db, const NonlinearFactorGraph& graph,
              const Values& initial, bool separateCalibration = false) {
   using symbol_shorthand::P;
 
diff --git a/timing/timeSFMBALautodiff.cpp b/timing/timeSFMBALautodiff.cpp
index eb1a46606..2d0f4a1fe 100644
--- a/timing/timeSFMBALautodiff.cpp
+++ b/timing/timeSFMBALautodiff.cpp
@@ -38,14 +38,14 @@ typedef PinholeCamera<Cal3Bundler> Camera;
 
 int main(int argc, char* argv[]) {
   // parse options and read BAL file
-  SfM_data db = preamble(argc, argv);
+  SfmData db = preamble(argc, argv);
 
   AdaptAutoDiff<SnavelyProjection, 2, 9, 3> snavely;
 
   // Build graph
   NonlinearFactorGraph graph;
   for (size_t j = 0; j < db.number_tracks(); j++) {
-    for (const SfM_Measurement& m: db.tracks[j].measurements) {
+    for (const SfmMeasurement& m: db.tracks[j].measurements) {
       size_t i = m.first;
       Point2 z = m.second;
       Expression<Vector9> camera_(C(i));
@@ -58,14 +58,14 @@ int main(int argc, char* argv[]) {
 
   Values initial;
   size_t i = 0, j = 0;
-  for (const SfM_Camera& camera: db.cameras) {
+  for (const SfmCamera& camera: db.cameras) {
     // readBAL converts to GTSAM format, so we need to convert back !
     Pose3 openGLpose = gtsam2openGL(camera.pose());
     Vector9 v9;
     v9 << Pose3::Logmap(openGLpose), camera.calibration();
     initial.insert(C(i++), v9);
   }
-  for (const SfM_Track& track: db.tracks) {
+  for (const SfmTrack& track: db.tracks) {
     Vector3 v3 = track.p;
     initial.insert(P(j++), v3);
   }
diff --git a/timing/timeSFMBALcamTnav.cpp b/timing/timeSFMBALcamTnav.cpp
index 0475253ec..355defed9 100644
--- a/timing/timeSFMBALcamTnav.cpp
+++ b/timing/timeSFMBALcamTnav.cpp
@@ -29,12 +29,12 @@ using namespace gtsam;
 
 int main(int argc, char* argv[]) {
   // parse options and read BAL file
-  SfM_data db = preamble(argc, argv);
+  SfmData db = preamble(argc, argv);
 
   // Build graph using conventional GeneralSFMFactor
   NonlinearFactorGraph graph;
   for (size_t j = 0; j < db.number_tracks(); j++) {
-    for (const SfM_Measurement& m: db.tracks[j].measurements) {
+    for (const SfmMeasurement& m: db.tracks[j].measurements) {
       size_t i = m.first;
       Point2 z = m.second;
       Pose3_ camTnav_(C(i));
@@ -49,12 +49,12 @@ int main(int argc, char* argv[]) {
 
   Values initial;
   size_t i = 0, j = 0;
-  for (const SfM_Camera& camera: db.cameras) {
+  for (const SfmCamera& camera: db.cameras) {
     initial.insert(C(i), camera.pose().inverse());  // inverse !!!
     initial.insert(K(i), camera.calibration());
     i += 1;
   }
-  for (const SfM_Track& track: db.tracks)
+  for (const SfmTrack& track: db.tracks)
     initial.insert(P(j++), track.p);
 
   bool separateCalibration = true;
diff --git a/timing/timeSFMBALnavTcam.cpp b/timing/timeSFMBALnavTcam.cpp
index fc9568626..e602ef241 100644
--- a/timing/timeSFMBALnavTcam.cpp
+++ b/timing/timeSFMBALnavTcam.cpp
@@ -29,13 +29,13 @@ using namespace gtsam;
 
 int main(int argc, char* argv[]) {
   // parse options and read BAL file
-  SfM_data db = preamble(argc, argv);
+  SfmData db = preamble(argc, argv);
 
   // Build graph using conventional GeneralSFMFactor
   NonlinearFactorGraph graph;
   for (size_t j = 0; j < db.number_tracks(); j++) {
     Point3_ nav_point_(P(j));
-    for (const SfM_Measurement& m: db.tracks[j].measurements) {
+    for (const SfmMeasurement& m: db.tracks[j].measurements) {
       size_t i = m.first;
       Point2 z = m.second;
       Pose3_ navTcam_(C(i));
@@ -49,12 +49,12 @@ int main(int argc, char* argv[]) {
 
   Values initial;
   size_t i = 0, j = 0;
-  for (const SfM_Camera& camera: db.cameras) {
+  for (const SfmCamera& camera: db.cameras) {
     initial.insert(C(i), camera.pose());
     initial.insert(K(i), camera.calibration());
     i += 1;
   }
-  for (const SfM_Track& track: db.tracks)
+  for (const SfmTrack& track: db.tracks)
     initial.insert(P(j++), track.p);
 
   bool separateCalibration = true;
diff --git a/timing/timeSFMBALsmart.cpp b/timing/timeSFMBALsmart.cpp
index 708c25695..a69d895a5 100644
--- a/timing/timeSFMBALsmart.cpp
+++ b/timing/timeSFMBALsmart.cpp
@@ -31,13 +31,13 @@ typedef SmartProjectionFactor<Camera> SfmFactor;
 
 int main(int argc, char* argv[]) {
   // parse options and read BAL file
-  SfM_data db = preamble(argc, argv);
+  SfmData db = preamble(argc, argv);
 
   // Add smart factors to graph
   NonlinearFactorGraph graph;
   for (size_t j = 0; j < db.number_tracks(); j++) {
     auto smartFactor = boost::make_shared<SfmFactor>(gNoiseModel);
-    for (const SfM_Measurement& m : db.tracks[j].measurements) {
+    for (const SfmMeasurement& m : db.tracks[j].measurements) {
       size_t i = m.first;
       Point2 z = m.second;
       smartFactor->add(z, C(i));
@@ -48,7 +48,7 @@ int main(int argc, char* argv[]) {
   Values initial;
   size_t i = 0;
   gUseSchur = false;
-  for (const SfM_Camera& camera : db.cameras)
+  for (const SfmCamera& camera : db.cameras)
     initial.insert(C(i++), camera);
 
   return optimize(db, graph, initial);
diff --git a/timing/timeiSAM2Chain.cpp b/timing/timeiSAM2Chain.cpp
index 7a4413ac7..a056bde24 100644
--- a/timing/timeiSAM2Chain.cpp
+++ b/timing/timeiSAM2Chain.cpp
@@ -17,7 +17,6 @@
 #include <gtsam/base/timing.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/ISAM2.h>
@@ -61,7 +60,7 @@ int main(int argc, char *argv[]) {
     gttic_(Create_measurements);
     if(step == 0) {
       // Add prior
-      newFactors.add(PriorFactor<Pose>(0, Pose(), noiseModel::Unit::Create(3)));
+      newFactors.addPrior(0, Pose(), noiseModel::Unit::Create(3));
       newVariables.insert(0, Pose());
     } else {
       Vector eta = Vector::Random(3) * 0.1;
diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt
index 6e046e3ab..c04a44edb 100644
--- a/wrap/CMakeLists.txt
+++ b/wrap/CMakeLists.txt
@@ -1,14 +1,9 @@
 # Build/install Wrap
 
 set(WRAP_BOOST_LIBRARIES
-  optimized
-    ${Boost_FILESYSTEM_LIBRARY_RELEASE}
-    ${Boost_SYSTEM_LIBRARY_RELEASE}
-    ${Boost_THREAD_LIBRARY_RELEASE}
-  debug
-    ${Boost_FILESYSTEM_LIBRARY_DEBUG}
-    ${Boost_SYSTEM_LIBRARY_DEBUG}
-    ${Boost_THREAD_LIBRARY_DEBUG}
+  Boost::system
+  Boost::filesystem
+  Boost::thread
 )
 
 # Allow for disabling serialization to handle errors related to Clang's linker
@@ -30,8 +25,6 @@ endif()
 gtsam_apply_build_flags(wrap_lib)
 
 target_link_libraries(wrap_lib ${WRAP_BOOST_LIBRARIES})
-target_include_directories(wrap_lib PUBLIC ${Boost_INCLUDE_DIR})
-
 gtsam_assign_source_folders(${wrap_srcs} ${wrap_headers})
 add_executable(wrap wrap.cpp)
 target_link_libraries(wrap PRIVATE wrap_lib)
@@ -41,12 +34,12 @@ file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE
 set_target_properties(wrap_lib wrap PROPERTIES FOLDER "${relative_path}")
 
 # Install wrap binary and export target
-install(TARGETS wrap EXPORT GTSAM-exports DESTINATION bin)
+install(TARGETS wrap EXPORT GTSAM-exports DESTINATION ${CMAKE_INSTALL_BINDIR})
 list(APPEND GTSAM_EXPORTED_TARGETS wrap)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
 
 # Install matlab header
-install(FILES matlab.h DESTINATION include/wrap)
+install(FILES matlab.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wrap)
 
 # Build tests
 add_subdirectory(tests)
diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h
index 9742733f7..099cefa70 100644
--- a/wrap/GlobalFunction.h
+++ b/wrap/GlobalFunction.h
@@ -54,7 +54,16 @@ struct GlobalFunction: public FullyOverloadedFunction {
   // function name in Cython pxd
   std::string pxdName() const { return "pxd_" + pyRename(name_); }
   // function name in Python pyx
-  std::string pyxName() const { return pyRename(name_); }
+  std::string pyxName() const {
+    std::string result = "";
+    for(size_t i=0; i<overloads[0].namespaces_.size(); i++){
+      if (i >= 1) {
+        result += (overloads[0].namespaces_[i] + "_");
+      }
+    }
+    result += pyRename(name_);
+    return result;
+  }
 
   // emit cython wrapper
   void emit_cython_pxd(FileWriter& pxdFile) const;
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
index a7db9e1f6..ec02dc412 100644
--- a/wrap/Module.cpp
+++ b/wrap/Module.cpp
@@ -350,7 +350,10 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const {
                  "        T* get()\n"
                  "        long use_count() const\n"
                  "        T& operator*()\n\n"
-                 "    cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n"
+                 "    cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n\n";
+
+  // gtsam alignment-friendly shared_ptr
+  pxdFile.oss << "cdef extern from \"gtsam/base/make_shared.h\" namespace \"gtsam\":\n"
                  "    cdef shared_ptr[T] make_shared[T](const T& r)\n\n";
 
   for(const TypedefPair& types: typedefs)
diff --git a/wrap/tests/expected-cython/geometry.pxd b/wrap/tests/expected-cython/geometry.pxd
index 0d9adac5f..3527840a3 100644
--- a/wrap/tests/expected-cython/geometry.pxd
+++ b/wrap/tests/expected-cython/geometry.pxd
@@ -16,6 +16,8 @@ cdef extern from "boost/shared_ptr.hpp" namespace "boost":
         T& operator*()
 
     cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)
+
+cdef extern from "gtsam/base/make_shared.h" namespace "gtsam":
     cdef shared_ptr[T] make_shared[T](const T& r)
 
 cdef extern from "gtsam/geometry/Point2.h" namespace "gtsam":