diff --git a/.travis.python.sh b/.travis.python.sh
index 1ef5799aa..772311f38 100644
--- a/.travis.python.sh
+++ b/.travis.python.sh
@@ -34,7 +34,7 @@ cmake $CURRDIR -DCMAKE_BUILD_TYPE=Release \
 
 make -j$(nproc) install
 
-cd $CURRDIR/../gtsam_install/cython
+cd cython
 
 sudo $PYTHON setup.py install
 
diff --git a/.travis.yml b/.travis.yml
index d8094ef4d..0c8c4bee5 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -1,6 +1,5 @@
 language: cpp
 cache: ccache
-sudo: required
 dist: xenial
 
 addons:
@@ -33,7 +32,7 @@ stages:
 
 env:
   global:
-    - MAKEFLAGS="-j2"
+    - MAKEFLAGS="-j3"
     - CCACHE_SLOPPINESS=pch_defines,time_macros
 
 # Compile stage without building examples/tests to populate the caches.
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a810ac9df..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}")
 
@@ -68,8 +68,8 @@ 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)
@@ -454,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()
@@ -537,54 +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}")
+message(STATUS "  Default allocator               : ${GTSAM_DEFAULT_ALLOCATOR}")
 
 if(GTSAM_THROW_CHEIRALITY_EXCEPTION)
-	message(STATUS "  Cheirality exceptions enabled  : YES")
+	message(STATUS "  Cheirality exceptions enabled   : YES")
 else()
-	message(STATUS "  Cheirality exceptions enabled  : NO")
+	message(STATUS "  Cheirality exceptions enabled   : NO")
 endif()
 
 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     ")
@@ -595,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/Test.h b/CppUnitLite/Test.h
index b1fb0cf08..a898c83ef 100644
--- a/CppUnitLite/Test.h
+++ b/CppUnitLite/Test.h
@@ -64,7 +64,7 @@ protected:
   class testGroup##testName##Test : public Test \
   { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \
             virtual ~testGroup##testName##Test () {};\
-            void run (TestResult& result_);} \
+            void run (TestResult& result_) override;} \
     testGroup##testName##Instance; \
   void testGroup##testName##Test::run (TestResult& result_)
 
@@ -82,7 +82,7 @@ protected:
   class testGroup##testName##Test : public Test \
   { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \
             virtual ~testGroup##testName##Test () {};\
-            void run (TestResult& result_);} \
+            void run (TestResult& result_) override;} \
     testGroup##testName##Instance; \
   void testGroup##testName##Test::run (TestResult& result_)
 
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/appveyor.yml b/appveyor.yml
index 2c78ca1f2..3747354cf 100644
--- a/appveyor.yml
+++ b/appveyor.yml
@@ -1,5 +1,5 @@
 # version format
-version: 4.0.2-{branch}-build{build}
+version: 4.0.3-{branch}-build{build}
 
 os: Visual Studio 2019
 
diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake
index 15a02b6e8..53dacd3f5 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.
@@ -99,8 +104,24 @@ 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:
-  # "-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.")
+
+  if (
+      ((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR
+      (CMAKE_CXX_COMPILER_ID MATCHES "GNU")
+     )
+    set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday
+  endif()
+
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON
+    -Wall                                          # Enable common warnings
+    -fPIC                                          # ensure proper code generation for shared libraries
+    $<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr -Werror=return-local-addr>            # Error: return local address
+    $<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address   -Werror=return-stack-address>  # Error: return local address
+    -Wreturn-type  -Werror=return-type             # Error on missing return()
+    -Wformat -Werror=format-security               # Error on wrong printf() arguments
+    $<$<COMPILE_LANGUAGE:CXX>:${flag_override_}>   # Enforce the use of the override keyword
+    #
+    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 f1382729f..c8f876895 100644
--- a/cmake/GtsamCythonWrap.cmake
+++ b/cmake/GtsamCythonWrap.cmake
@@ -41,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)
@@ -138,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
@@ -150,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
@@ -175,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
 #
@@ -232,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/cython/CMakeLists.txt b/cython/CMakeLists.txt
index 4cc9d2f5d..221025575 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -3,16 +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})
 
   # Fix for error "C1128: number of sections exceeded object file format limit"
   if(MSVC)
     add_compile_options(/bigobj)
   endif()
 
-  # wrap gtsam
+  # 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
@@ -20,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
@@ -30,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/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_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_logging_optimizer.py b/cython/gtsam/tests/test_logging_optimizer.py
index c857a6f7d..2560a72a2 100644
--- a/cython/gtsam/tests/test_logging_optimizer.py
+++ b/cython/gtsam/tests/test_logging_optimizer.py
@@ -4,6 +4,12 @@ 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
 
@@ -37,11 +43,24 @@ class TestOptimizeComet(GtsamTestCase):
         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: float):
+        def hook(_, error):
             print(error)
 
         # Only thing we require from optimizer is an iterate method
@@ -51,6 +70,16 @@ class TestOptimizeComet(GtsamTestCase):
         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."""
@@ -65,7 +94,7 @@ class TestOptimizeComet(GtsamTestCase):
                        + str(time.hour)+":"+str(time.minute)+":"+str(time.second))
 
         # I want to do some comet thing here
-        def hook(optimizer, error: float):
+        def hook(optimizer, error):
             comet.log_metric("Karcher error",
                              error, optimizer.iterations())
 
@@ -76,4 +105,4 @@ class TestOptimizeComet(GtsamTestCase):
         self.gtsamAssertEquals(actual.atRot3(KEY), self.expected)
 
 if __name__ == "__main__":
-    unittest.main()
\ No newline at end of file
+    unittest.main()
diff --git a/cython/gtsam/utils/logging_optimizer.py b/cython/gtsam/utils/logging_optimizer.py
index b201bb8aa..3d9175951 100644
--- a/cython/gtsam/utils/logging_optimizer.py
+++ b/cython/gtsam/utils/logging_optimizer.py
@@ -4,15 +4,11 @@ Author: Jing Wu and Frank Dellaert
 """
 # pylint: disable=invalid-name
 
-from typing import TypeVar
-
 from gtsam import NonlinearOptimizer, NonlinearOptimizerParams
 import gtsam
 
-T = TypeVar('T')
 
-
-def optimize(optimizer: T, check_convergence, hook):
+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.
@@ -36,8 +32,8 @@ def optimize(optimizer: T, check_convergence, hook):
         current_error = new_error
 
 
-def gtsam_optimize(optimizer: NonlinearOptimizer,
-                   params: NonlinearOptimizerParams,
+def gtsam_optimize(optimizer,
+                   params,
                    hook):
     """ Given an optimizer and params, iterate until convergence.
         After each iteration, hook(optimizer) is called.
@@ -50,5 +46,7 @@ def gtsam_optimize(optimizer: NonlinearOptimizer,
     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))
+                                   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 cbc7c227a..f69987235 100644
--- a/cython/gtsam/utils/plot.py
+++ b/cython/gtsam/utils/plot.py
@@ -135,7 +135,8 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
         axes.add_patch(e1)
 
 
-def plot_pose2(fignum, pose, axis_length=0.1, covariance=None):
+def plot_pose2(fignum, pose, axis_length=0.1, covariance=None,
+               axis_labels=('X axis', 'Y axis', 'Z axis')):
     """
     Plot a 2D pose on given figure with given `axis_length`.
 
@@ -144,6 +145,7 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None):
         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.
+        axis_labels (iterable[string]): List of axis labels to set.
     """
     # get figure object
     fig = plt.figure(fignum)
@@ -151,6 +153,12 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None):
     plot_pose2_on_axes(axes, pose, axis_length=axis_length,
                        covariance=covariance)
 
+    axes.set_xlabel(axis_labels[0])
+    axes.set_ylabel(axis_labels[1])
+    axes.set_zlabel(axis_labels[2])
+
+    return fig
+
 
 def plot_point3_on_axes(axes, point, linespec, P=None):
     """
@@ -167,7 +175,8 @@ def plot_point3_on_axes(axes, point, linespec, P=None):
         plot_covariance_ellipse_3d(axes, point.vector(), P)
 
 
-def plot_point3(fignum, point, linespec, P=None):
+def plot_point3(fignum, point, linespec, P=None,
+                axis_labels=('X axis', 'Y axis', 'Z axis')):
     """
     Plot a 3D point on given figure with given `linespec`.
 
@@ -176,13 +185,25 @@ def plot_point3(fignum, point, linespec, P=None):
         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.
+        axis_labels (iterable[string]): List of axis labels to set.
+
+    Returns:
+        fig: The matplotlib figure.
+
     """
     fig = plt.figure(fignum)
     axes = fig.gca(projection='3d')
     plot_point3_on_axes(axes, point, linespec, P)
 
+    axes.set_xlabel(axis_labels[0])
+    axes.set_ylabel(axis_labels[1])
+    axes.set_zlabel(axis_labels[2])
 
-def plot_3d_points(fignum, values, linespec="g*", marginals=None):
+    return fig
+
+
+def plot_3d_points(fignum, values, linespec="g*", marginals=None,
+                   title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')):
     """
     Plots the Point3s in `values`, with optional covariances.
     Finds all the Point3 objects in the given Values object and plots them.
@@ -193,7 +214,9 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None):
         fignum (int): Integer representing the figure number to use for plotting.
         values (gtsam.Values): Values dictionary consisting of points to be plotted.
         linespec (string): String representing formatting options for Matplotlib.
-        covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+        marginals (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+        title (string): The title of the plot.
+        axis_labels (iterable[string]): List of axis labels to set.
     """
 
     keys = values.keys()
@@ -208,12 +231,16 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None):
             else:
                 covariance = None
 
-            plot_point3(fignum, point, linespec, covariance)
+            fig = plot_point3(fignum, point, linespec, covariance,
+                              axis_labels=axis_labels)
 
         except RuntimeError:
             continue
             # I guess it's not a Point3
 
+    fig.suptitle(title)
+    fig.canvas.set_window_title(title.lower())
+
 
 def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
     """
@@ -251,7 +278,8 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
         plot_covariance_ellipse_3d(axes, origin, gPp)
 
 
-def plot_pose3(fignum, pose, axis_length=0.1, P=None):
+def plot_pose3(fignum, pose, axis_length=0.1, P=None,
+               axis_labels=('X axis', 'Y axis', 'Z axis')):
     """
     Plot a 3D pose on given figure with given `axis_length`.
 
@@ -260,6 +288,10 @@ def plot_pose3(fignum, pose, axis_length=0.1, P=None):
         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.
+        axis_labels (iterable[string]): List of axis labels to set.
+
+    Returns:
+        fig: The matplotlib figure.
     """
     # get figure object
     fig = plt.figure(fignum)
@@ -267,9 +299,15 @@ def plot_pose3(fignum, pose, axis_length=0.1, P=None):
     plot_pose3_on_axes(axes, pose, P=P,
                        axis_length=axis_length)
 
+    axes.set_xlabel(axis_labels[0])
+    axes.set_ylabel(axis_labels[1])
+    axes.set_zlabel(axis_labels[2])
+
+    return fig
+
 
 def plot_trajectory(fignum, values, scale=1, marginals=None,
-                    animation_interval=0.0):
+                    title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')):
     """
     Plot a complete 3D trajectory using poses in `values`.
 
@@ -279,6 +317,8 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
         scale (float): Value to scale the poses by.
         marginals (gtsam.Marginals): Marginalized probability values of the estimation.
             Used to plot uncertainty bounds.
+        title (string): The title of the plot.
+        axis_labels (iterable[string]): List of axis labels to set.
     """
     pose3Values = gtsam.utilities_allPose3s(values)
     keys = gtsam.KeyVector(pose3Values.keys())
@@ -304,8 +344,8 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
             else:
                 covariance = None
 
-            plot_pose3(fignum, lastPose,  P=covariance,
-                       axis_length=scale)
+            fig = plot_pose3(fignum, lastPose,  P=covariance,
+                             axis_length=scale, axis_labels=axis_labels)
 
         lastIndex = i
 
@@ -319,12 +359,15 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
             else:
                 covariance = None
 
-            plot_pose3(fignum, lastPose, P=covariance,
-                       axis_length=scale)
+            fig = plot_pose3(fignum, lastPose, P=covariance,
+                             axis_length=scale, axis_labels=axis_labels)
 
         except:
             pass
 
+    fig.suptitle(title)
+    fig.canvas.set_window_title(title.lower())
+
 
 def plot_incremental_trajectory(fignum, values, start=0,
                                 scale=1, marginals=None,
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/setup.py.in b/cython/setup.py.in
index df92b564c..98a05c9f6 100644
--- a/cython/setup.py.in
+++ b/cython/setup.py.in
@@ -7,6 +7,22 @@ except ImportError:
 
 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',
@@ -16,7 +32,7 @@ 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
@@ -34,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', '.pyd')]
-        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/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 b12418098..7ac2de8b1 100644
--- a/examples/CameraResectioning.cpp
+++ b/examples/CameraResectioning.cpp
@@ -46,8 +46,8 @@ public:
   }
 
   /// evaluate the error
-  virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
-      boost::none) const {
+  Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
+      boost::none) const override {
     PinholeCamera<Cal3_S2> camera(pose, *K_);
     return camera.project(P_, H, boost::none, boost::none) - p_;
   }
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/example_with_vertices.g2o b/examples/Data/example_with_vertices.g2o
new file mode 100644
index 000000000..6c2f1a530
--- /dev/null
+++ b/examples/Data/example_with_vertices.g2o
@@ -0,0 +1,16 @@
+VERTEX_SE3:QUAT 8646911284551352320 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162
+VERTEX_SE3:QUAT 8646911284551352321 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508
+VERTEX_SE3:QUAT 8646911284551352322 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10
+VERTEX_SE3:QUAT 8646911284551352323 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508
+VERTEX_SE3:QUAT 8646911284551352324 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162
+VERTEX_SE3:QUAT 8646911284551352325 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567
+VERTEX_SE3:QUAT 8646911284551352326 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412
+VERTEX_SE3:QUAT 8646911284551352327 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567
+VERTEX_TRACKXYZ 7782220156096217088 10 10 10
+VERTEX_TRACKXYZ 7782220156096217089 -10 10 10
+VERTEX_TRACKXYZ 7782220156096217090 -10 -10 10
+VERTEX_TRACKXYZ 7782220156096217091 10 -10 10
+VERTEX_TRACKXYZ 7782220156096217092 10 10 -10
+VERTEX_TRACKXYZ 7782220156096217093 -10 10 -10
+VERTEX_TRACKXYZ 7782220156096217094 -10 -10 -10
+VERTEX_TRACKXYZ 7782220156096217095 10 -10 -10
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/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/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp
index 668b48934..315f7b8e1 100644
--- a/examples/ImuFactorsExample.cpp
+++ b/examples/ImuFactorsExample.cpp
@@ -35,22 +35,28 @@
  *  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/inference/Symbol.h>
 #include <gtsam/navigation/CombinedImuFactor.h>
 #include <gtsam/navigation/GPSFactor.h>
 #include <gtsam/navigation/ImuFactor.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/slam/BetweenFactor.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 the following to use Levenberg Marquardt Optimizer
+// #define USE_LM
+
 using namespace gtsam;
 using namespace std;
 
@@ -64,6 +70,17 @@ static const char use_combined_imu_flag[3] = "-c";
 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");
@@ -252,9 +269,19 @@ int main(int argc, char* argv[]) {
       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)));
diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp
index a28ead5fe..e6a0f6622 100644
--- a/examples/LocalizationExample.cpp
+++ b/examples/LocalizationExample.cpp
@@ -85,7 +85,7 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
   // 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 {
+                       boost::optional<Matrix&> H = boost::none) const override {
     // The measurement function for a GPS-like measurement is simple:
     // error_x = pose.x - measurement.x
     // error_y = pose.y - measurement.y
@@ -99,7 +99,7 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
   // The second is a 'clone' function that allows the factor to be copied. Under most
   // circumstances, the following code that employs the default copy constructor should
   // work fine.
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); }
 
diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp
index 7f0c79e0e..fddca8169 100644
--- a/examples/SFMExample.cpp
+++ b/examples/SFMExample.cpp
@@ -109,7 +109,7 @@ int main(int argc, char* argv[]) {
         Symbol('x', i), corrupted_pose);
   }
   for (size_t j = 0; j < points.size(); ++j) {
-    auto corrupted_point = points[j] + Point3(-0.25, 0.20, 0.15);
+    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");
diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp
index d1155fe4c..718ae6286 100644
--- a/examples/SolverComparer.cpp
+++ b/examples/SolverComparer.cpp
@@ -50,11 +50,11 @@
 #include <boost/program_options.hpp>
 #include <boost/range/algorithm/set_algorithm.hpp>
 #include <boost/range/adaptor/reversed.hpp>
-#include <boost/random.hpp>
 #include <boost/serialization/export.hpp>
 
 #include <fstream>
 #include <iostream>
+#include <random>
 
 #ifdef GTSAM_USE_TBB
 #include <tbb/task_arena.h> // tbb::task_arena
@@ -554,8 +554,8 @@ void runCompare()
 void runPerturb()
 {
   // Set up random number generator
-  boost::mt19937 rng;
-  boost::normal_distribution<double> normal(0.0, perturbationNoise);
+  std::mt19937 rng;
+  std::normal_distribution<double> normal(0.0, perturbationNoise);
 
   // Perturb values
   VectorValues noise;
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/gtsam.h b/gtsam.h
index cf6f658a1..2cd30be42 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -598,6 +598,7 @@ 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;
@@ -1458,7 +1459,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class Fair: gtsam::noiseModel::mEstimator::Base {
@@ -1469,7 +1470,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class Huber: gtsam::noiseModel::mEstimator::Base {
@@ -1480,7 +1481,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
@@ -1491,7 +1492,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
@@ -1502,7 +1503,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
@@ -1513,7 +1514,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
@@ -1524,7 +1525,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class DCS: gtsam::noiseModel::mEstimator::Base {
@@ -1535,7 +1536,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
@@ -1546,7 +1547,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 }///\namespace mEstimator
@@ -1937,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();
@@ -2835,6 +2852,34 @@ 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
 //*************************************************************************
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 3d1bbd2a7..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)
@@ -186,11 +186,17 @@ install(
 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/GenericValue.h b/gtsam/base/GenericValue.h
index 2ac3eb80c..98425adde 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -70,7 +70,7 @@ public:
   }
 
   /// equals implementing generic Value interface
-  virtual bool equals_(const Value& p, double tol = 1e-9) const {
+  bool equals_(const Value& p, double tol = 1e-9) const override {
     // Cast the base class Value pointer to a templated generic class pointer
     const GenericValue& genericValue2 = static_cast<const GenericValue&>(p);
     // Return the result of using the equals traits for the derived class
@@ -83,7 +83,7 @@ public:
   }
 
   /// Virtual print function, uses traits
-  virtual void print(const std::string& str) const {
+  void print(const std::string& str) const override {
     std::cout << "(" << demangle(typeid(T).name()) << ") ";
     traits<T>::Print(value_, str);
   }
@@ -91,7 +91,7 @@ public:
     /**
      * Create a duplicate object returned as a pointer to the generic Value interface.
      */
-    virtual Value* clone_() const {
+    Value* clone_() const override {
       GenericValue* ptr = new GenericValue(*this); // calls copy constructor to fill in
       return ptr;
     }
@@ -99,19 +99,19 @@ public:
     /**
      * Destroy and deallocate this object, only if it was originally allocated using clone_().
      */
-    virtual void deallocate_() const {
+    void deallocate_() const override {
       delete this;
     }
 
     /**
      * Clone this value (normal clone on the heap, delete with 'delete' operator)
      */
-    virtual boost::shared_ptr<Value> clone() const {
+    boost::shared_ptr<Value> clone() const override {
 		return boost::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
     }
 
     /// Generic Value interface version of retract
-    virtual Value* retract_(const Vector& delta) const {
+    Value* retract_(const Vector& delta) const override {
       // Call retract on the derived class using the retract trait function
       const T retractResult = traits<T>::Retract(GenericValue<T>::value(), delta);
 
@@ -122,7 +122,7 @@ public:
     }
 
     /// Generic Value interface version of localCoordinates
-    virtual Vector localCoordinates_(const Value& value2) const {
+    Vector localCoordinates_(const Value& value2) const override {
       // Cast the base class Value pointer to a templated generic class pointer
       const GenericValue<T>& genericValue2 =
           static_cast<const GenericValue<T>&>(value2);
@@ -142,12 +142,12 @@ public:
     }
 
     /// Return run-time dimensionality
-    virtual size_t dim() const {
+    size_t dim() const override {
       return traits<T>::GetDimension(value_);
     }
 
     /// Assignment operator
-    virtual Value& operator=(const Value& rhs) {
+    Value& operator=(const Value& rhs) override {
       // Cast the base class Value pointer to a derived class pointer
       const GenericValue& derivedRhs = static_cast<const GenericValue&>(rhs);
 
diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index fa70e5b00..1c1138438 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -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/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h
index 0f7c6131d..744759f5b 100644
--- a/gtsam/base/ThreadsafeException.h
+++ b/gtsam/base/ThreadsafeException.h
@@ -71,12 +71,12 @@ protected:
           String(description.begin(), description.end())) {
   }
 
-  /// Default destructor doesn't have the throw()
-  virtual ~ThreadsafeException() throw () {
+  /// Default destructor doesn't have the noexcept
+  virtual ~ThreadsafeException() noexcept {
   }
 
 public:
-  virtual const char* what() const throw () {
+  const char* what() const noexcept override {
     return description_ ? description_->c_str() : "";
   }
 };
@@ -113,8 +113,8 @@ public:
 class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
 {
 public:
-  CholeskyFailed() throw() {}
-  virtual ~CholeskyFailed() throw() {}
+  CholeskyFailed() noexcept {}
+  virtual ~CholeskyFailed() noexcept {}
 };
 
 } // namespace gtsam
diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp
index b33f06045..4b41ea937 100644
--- a/gtsam/base/timing.cpp
+++ b/gtsam/base/timing.cpp
@@ -89,7 +89,7 @@ void TimingOutline::print(const std::string& outline) const {
     childOrder[child.second->myOrder_] = child.second;
   }
   // Print children
-  for(const ChildOrder::value_type order_child: childOrder) {
+  for(const ChildOrder::value_type& order_child: childOrder) {
     std::string childOutline(outline);
     childOutline += "|   ";
     order_child.second->print(childOutline);
diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h
index b52d44e2a..87d5b0d4c 100644
--- a/gtsam/base/treeTraversal/parallelTraversalTasks.h
+++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h
@@ -57,7 +57,7 @@ namespace gtsam {
               makeNewTasks(makeNewTasks),
               isPostOrderPhase(false) {}
 
-        tbb::task* execute()
+        tbb::task* execute() override
         {
           if(isPostOrderPhase)
           {
@@ -144,7 +144,7 @@ namespace gtsam {
           roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
           problemSizeThreshold(problemSizeThreshold) {}
 
-        tbb::task* execute()
+        tbb::task* execute() override
         {
           typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
           // Create data and tasks for our children
diff --git a/gtsam/base/types.h b/gtsam/base/types.h
index c5dac9ab7..aaada3cee 100644
--- a/gtsam/base/types.h
+++ b/gtsam/base/types.h
@@ -28,6 +28,7 @@
 #include <cstdint>
 
 #include <exception>
+#include <string>
 
 #ifdef GTSAM_USE_TBB
 #include <tbb/scalable_allocator.h>
@@ -54,7 +55,7 @@
 namespace gtsam {
 
   /// Function to demangle type name of variable, e.g. demangle(typeid(x).name())
-  std::string demangle(const char* name);
+  std::string GTSAM_EXPORT demangle(const char* name);
 
   /// Integer nonlinear key type
   typedef std::uint64_t Key;
diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index 2efd069cc..dd10500e6 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -66,42 +66,42 @@ namespace gtsam {
     }
 
     /// Leaf-Leaf equality
-    bool sameLeaf(const Leaf& q) const {
+    bool sameLeaf(const Leaf& q) const override {
       return constant_ == q.constant_;
     }
 
     /// polymorphic equality: is q is a leaf, could be
-    bool sameLeaf(const Node& q) const {
+    bool sameLeaf(const Node& q) const override {
       return (q.isLeaf() && q.sameLeaf(*this));
     }
 
     /** equality up to tolerance */
-    bool equals(const Node& q, double tol) const {
+    bool equals(const Node& q, double tol) const override {
       const Leaf* other = dynamic_cast<const Leaf*> (&q);
       if (!other) return false;
       return std::abs(double(this->constant_ - other->constant_)) < tol;
     }
 
     /** print */
-    void print(const std::string& s) const {
+    void print(const std::string& s) const override {
       bool showZero = true;
       if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl;
     }
 
     /** to graphviz file */
-    void dot(std::ostream& os, bool showZero) const {
+    void dot(std::ostream& os, bool showZero) const override {
       if (showZero || constant_) os << "\"" << this->id() << "\" [label=\""
           << boost::format("%4.2g") % constant_
           << "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; // width=0.55,
     }
 
     /** evaluate */
-    const Y& operator()(const Assignment<L>& x) const {
+    const Y& operator()(const Assignment<L>& x) const override {
       return constant_;
     }
 
     /** apply unary operator */
-    NodePtr apply(const Unary& op) const {
+    NodePtr apply(const Unary& op) const override {
       NodePtr f(new Leaf(op(constant_)));
       return f;
     }
@@ -111,27 +111,27 @@ namespace gtsam {
     // Simply calls apply on argument to call correct virtual method:
     // fL.apply_f_op_g(gL) -> gL.apply_g_op_fL(fL) (below)
     // fL.apply_f_op_g(gC) -> gC.apply_g_op_fL(fL) (Choice)
-    NodePtr apply_f_op_g(const Node& g, const Binary& op) const {
+    NodePtr apply_f_op_g(const Node& g, const Binary& op) const override {
       return g.apply_g_op_fL(*this, op);
     }
 
     // Applying binary operator to two leaves results in a leaf
-    NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const {
+    NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const override {
       NodePtr h(new Leaf(op(fL.constant_, constant_))); // fL op gL
       return h;
     }
 
     // If second argument is a Choice node, call it's apply with leaf as second
-    NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const {
+    NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const override {
       return fC.apply_fC_op_gL(*this, op); // operand order back to normal
     }
 
     /** choose a branch, create new memory ! */
-    NodePtr choose(const L& label, size_t index) const {
+    NodePtr choose(const L& label, size_t index) const override {
       return NodePtr(new Leaf(constant()));
     }
 
-    bool isLeaf() const { return true; }
+    bool isLeaf() const override { return true; }
 
   }; // Leaf
 
@@ -175,7 +175,7 @@ namespace gtsam {
         return f;
     }
 
-    bool isLeaf() const { return false; }
+    bool isLeaf() const override { return false; }
 
     /** Constructor, given choice label and mandatory expected branch count */
     Choice(const L& label, size_t count) :
@@ -236,7 +236,7 @@ namespace gtsam {
     }
 
     /** print (as a tree) */
-    void print(const std::string& s) const {
+    void print(const std::string& s) const override {
       std::cout << s << " Choice(";
       //        std::cout << this << ",";
       std::cout << label_ << ") " << std::endl;
@@ -245,7 +245,7 @@ namespace gtsam {
     }
 
     /** output to graphviz (as a a graph) */
-    void dot(std::ostream& os, bool showZero) const {
+    void dot(std::ostream& os, bool showZero) const override {
       os << "\"" << this->id() << "\" [shape=circle, label=\"" << label_
           << "\"]\n";
       for (size_t i = 0; i < branches_.size(); i++) {
@@ -266,17 +266,17 @@ namespace gtsam {
     }
 
     /// Choice-Leaf equality: always false
-    bool sameLeaf(const Leaf& q) const {
+    bool sameLeaf(const Leaf& q) const override {
       return false;
     }
 
     /// polymorphic equality: if q is a leaf, could be...
-    bool sameLeaf(const Node& q) const {
+    bool sameLeaf(const Node& q) const override {
       return (q.isLeaf() && q.sameLeaf(*this));
     }
 
     /** equality up to tolerance */
-    bool equals(const Node& q, double tol) const {
+    bool equals(const Node& q, double tol) const override {
       const Choice* other = dynamic_cast<const Choice*> (&q);
       if (!other) return false;
       if (this->label_ != other->label_) return false;
@@ -288,7 +288,7 @@ namespace gtsam {
     }
 
     /** evaluate */
-    const Y& operator()(const Assignment<L>& x) const {
+    const Y& operator()(const Assignment<L>& x) const override {
 #ifndef NDEBUG
       typename Assignment<L>::const_iterator it = x.find(label_);
       if (it == x.end()) {
@@ -314,7 +314,7 @@ namespace gtsam {
     }
 
     /** apply unary operator */
-    NodePtr apply(const Unary& op) const {
+    NodePtr apply(const Unary& op) const override {
       boost::shared_ptr<Choice> r(new Choice(label_, *this, op));
       return Unique(r);
     }
@@ -324,12 +324,12 @@ namespace gtsam {
     // Simply calls apply on argument to call correct virtual method:
     // fC.apply_f_op_g(gL) -> gL.apply_g_op_fC(fC) -> (Leaf)
     // fC.apply_f_op_g(gC) -> gC.apply_g_op_fC(fC) -> (below)
-    NodePtr apply_f_op_g(const Node& g, const Binary& op) const {
+    NodePtr apply_f_op_g(const Node& g, const Binary& op) const override {
       return g.apply_g_op_fC(*this, op);
     }
 
     // If second argument of binary op is Leaf node, recurse on branches
-    NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const {
+    NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const override {
       boost::shared_ptr<Choice> h(new Choice(label(), nrChoices()));
       for(NodePtr branch: branches_)
               h->push_back(fL.apply_f_op_g(*branch, op));
@@ -337,7 +337,7 @@ namespace gtsam {
     }
 
     // If second argument of binary op is Choice, call constructor
-    NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const {
+    NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const override {
       boost::shared_ptr<Choice> h(new Choice(fC, *this, op));
       return Unique(h);
     }
@@ -352,7 +352,7 @@ namespace gtsam {
     }
 
     /** choose a branch, recursively */
-    NodePtr choose(const L& label, size_t index) const {
+    NodePtr choose(const L& label, size_t index) const override {
       if (label_ == label)
         return branches_[index]; // choose branch
 
diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index f83349436..b7b9d7034 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -69,7 +69,7 @@ namespace gtsam {
     for(Key j: f.keys()) cs[j] = f.cardinality(j);
     // Convert map into keys
     DiscreteKeys keys;
-    for(const DiscreteKey& key: cs)
+    for(const std::pair<const Key,size_t>& key: cs)
       keys.push_back(key);
     // apply operand
     ADT result = ADT::apply(f, op);
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 54cce56be..d1696a281 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -69,23 +69,23 @@ namespace gtsam {
     /// @{
 
     /// equality
-    bool equals(const DiscreteFactor& other, double tol = 1e-9) const;
+    bool equals(const DiscreteFactor& other, double tol = 1e-9) const override;
 
     // print
-    virtual void print(const std::string& s = "DecisionTreeFactor:\n",
-        const KeyFormatter& formatter = DefaultKeyFormatter) const;
+    void print(const std::string& s = "DecisionTreeFactor:\n",
+        const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
     /// @}
     /// @name Standard Interface
     /// @{
 
     /// Value is just look up in AlgebraicDecisonTree
-    virtual double operator()(const Values& values) const {
+    double operator()(const Values& values) const override {
       return Potentials::operator()(values);
     }
 
     /// multiply two factors
-    DecisionTreeFactor operator*(const DecisionTreeFactor& f) const {
+    DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
       return apply(f, ADT::Ring::mul);
     }
 
@@ -95,7 +95,7 @@ namespace gtsam {
     }
 
     /// Convert into a decisiontree
-    virtual DecisionTreeFactor toDecisionTreeFactor() const {
+    DecisionTreeFactor toDecisionTreeFactor() const override {
       return *this;
     }
 
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..29da5817e 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,63 @@
 #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() {}
+  virtual ~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 2ab3054a8..ac7c58405 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -27,6 +27,7 @@
 #include <algorithm>
 #include <random>
 #include <stdexcept>
+#include <string>
 #include <vector>
 
 using namespace std;
@@ -61,16 +62,26 @@ DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
 }
 
 /* ******************************************************************************** */
-DiscreteConditional::DiscreteConditional(const Signature& signature) :
-        BaseFactor(signature.discreteKeysParentsFirst(), signature.cpt()), BaseConditional(
-            1) {
-}
+DiscreteConditional::DiscreteConditional(const Signature& signature)
+    : BaseFactor(signature.discreteKeys(), signature.cpt()),
+      BaseConditional(1) {}
 
 /* ******************************************************************************** */
-void DiscreteConditional::print(const std::string& s,
-    const KeyFormatter& formatter) const {
-  std::cout << s << std::endl;
-  Potentials::print(s);
+void DiscreteConditional::print(const string& s,
+                                const KeyFormatter& formatter) const {
+  cout << s << " P( ";
+  for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
+    cout << formatter(*it) << " ";
+  }
+  if (nrParents()) {
+    cout << "| ";
+    for (const_iterator it = beginParents(); it != endParents(); ++it) {
+      cout << formatter(*it) << " ";
+    }
+  }
+  cout << ")";
+  Potentials::print("");
+  cout << endl;
 }
 
 /* ******************************************************************************** */
@@ -173,55 +184,28 @@ size_t DiscreteConditional::solve(const Values& parentsValues) const {
 
 /* ******************************************************************************** */
 size_t DiscreteConditional::sample(const Values& parentsValues) const {
-
-  static mt19937 rng(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_distribution<double> dist(0, cdf.back());
-  size_t sampled = lower_bound(cdf.begin(), cdf.end(), dist(rng)) - 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 3da8d0a82..8299fab2c 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 {
 
 /**
@@ -83,17 +85,24 @@ public:
 
   /// GTSAM-style print
   void print(const std::string& s = "Discrete Conditional: ",
-      const KeyFormatter& formatter = DefaultKeyFormatter) const;
+      const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
   /// GTSAM-style equals
-  bool equals(const DiscreteFactor& other, double tol = 1e-9) const;
+  bool equals(const DiscreteFactor& other, double tol = 1e-9) const override;
 
   /// @}
   /// @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 {
+  double operator()(const Values& values) const override {
     return Potentials::operator()(values);
   }
 
diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp
index c4cdbe0ef..331a76c13 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 std::pair<const Key,size_t>& 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 753af16d8..0261ef833 100644
--- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
+++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
@@ -132,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();
@@ -181,19 +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_);
diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp
index 5ed662332..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, 1)(T.first, 0)(
-      S.first, 1)(E.first, 1)(L.first, 1)(B.first, 0);
+  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/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index 3e62f758d..7fd453d45 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -60,7 +60,7 @@ public:
   /// @{
 
   /// print with optional string
-  virtual void print(const std::string& s = "") const ;
+  void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3DS2& K, double tol = 10e-9) const;
@@ -86,7 +86,7 @@ public:
   /// @{
 
   /// @return a deep copy of this object
-  virtual boost::shared_ptr<Base> clone() const {
+  boost::shared_ptr<Base> clone() const override {
     return boost::shared_ptr<Base>(new Cal3DS2(*this));
   }
 
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index 4da5d1360..a0ece8bdb 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -69,7 +69,7 @@ public:
   /// @{
 
   /// print with optional string
-  virtual void print(const std::string& s = "") const ;
+  virtual void print(const std::string& s = "") const;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index ae75c3916..381405d20 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -75,7 +75,7 @@ public:
   /// @{
 
   /// print with optional string
-  virtual void print(const std::string& s = "") const ;
+  void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3Unified& K, double tol = 10e-9) const;
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index 9a80b937b..cf449ce8c 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -175,7 +175,7 @@ public:
   }
 
   /// return calibration
-  const Calibration& calibration() const {
+  const Calibration& calibration() const override {
     return K_;
   }
 
diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h
index 60088577c..3bf96c08b 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;
   }
@@ -361,7 +361,7 @@ public:
   }
 
   /// return calibration
-  virtual const CALIBRATION& calibration() const {
+  const CALIBRATION& calibration() const override {
     return *K_;
   }
 
diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h
index a2896ca8d..7e3dae56f 100644
--- a/gtsam/geometry/PinholeSet.h
+++ b/gtsam/geometry/PinholeSet.h
@@ -45,7 +45,7 @@ public:
   /// @{
 
   /// print
-  virtual void print(const std::string& s = "") const {
+  void print(const std::string& s = "") const override {
     Base::print(s);
   }
 
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 31033a027..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);
 }
 
@@ -72,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;
@@ -88,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;
@@ -116,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));
@@ -125,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 {
@@ -135,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;
@@ -158,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
 }
@@ -261,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_;
 }
@@ -284,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);
 }
 
 /* ************************************************************************* */
@@ -299,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);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index ced3b904b..3825b6241 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -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. */
diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp
index 8a78bb83f..1c4920af8 100644
--- a/gtsam/geometry/SO4.cpp
+++ b/gtsam/geometry/SO4.cpp
@@ -35,7 +35,7 @@ namespace gtsam {
 
 // TODO(frank): is this better than SOn::Random?
 // /* *************************************************************************
-// */ static Vector3 randomOmega(boost::mt19937 &rng) {
+// */ static Vector3 randomOmega(std::mt19937 &rng) {
 //   static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
 //   return Unit3::Random(rng).unitVector() * randomAngle(rng);
 // }
@@ -43,7 +43,7 @@ namespace gtsam {
 // /* *************************************************************************
 // */
 // // Create random SO(4) element using direct product of lie algebras.
-// SO4 SO4::Random(boost::mt19937 &rng) {
+// SO4 SO4::Random(std::mt19937 &rng) {
 //   Vector6 delta;
 //   delta << randomOmega(rng), randomOmega(rng);
 //   return SO4::Expmap(delta);
diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h
index 5014414c2..33bd8c161 100644
--- a/gtsam/geometry/SO4.h
+++ b/gtsam/geometry/SO4.h
@@ -34,7 +34,7 @@ namespace gtsam {
 using SO4 = SO<4>;
 
 // /// Random SO(4) element (no big claims about uniformity)
-// static SO4 Random(boost::mt19937 &rng);
+// static SO4 Random(std::mt19937 &rng);
 
 // Below are all declarations of SO<4> specializations.
 // They are *defined* in SO4.cpp.
diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h
index a08f87783..a6392c2f9 100644
--- a/gtsam/geometry/SOn.h
+++ b/gtsam/geometry/SOn.h
@@ -292,6 +292,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
                    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;
@@ -329,6 +333,16 @@ 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
  */
diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h
index b80e6e4d8..27d41a014 100644
--- a/gtsam/geometry/Unit3.h
+++ b/gtsam/geometry/Unit3.h
@@ -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;
   }
 
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 5808f36f8..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));
diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp
index a25ab25c7..ddf60a256 100644
--- a/gtsam/geometry/tests/testUnit3.cpp
+++ b/gtsam/geometry/tests/testUnit3.cpp
@@ -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);
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.h b/gtsam/inference/BayesTree.h
index 9d632ff06..10fc513ee 100644
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@@ -299,7 +299,7 @@ namespace gtsam {
       this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents());
     }
 
-    void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const {
+    void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const override {
       clique->print(s + "stored clique", formatter);
     }
   };
diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h
index e762786f5..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), 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 6266e961c..7aca84635 100644
--- a/gtsam/inference/BayesTreeCliqueBase.h
+++ b/gtsam/inference/BayesTreeCliqueBase.h
@@ -100,7 +100,7 @@ namespace gtsam {
     bool equals(const DERIVED& other, double tol = 1e-9) const;
 
     /** print this node */
-    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+    virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
     /// @}
     /// @name Standard Interface
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/inferenceExceptions.h b/gtsam/inference/inferenceExceptions.h
index edd0e0aa5..4409b16c7 100644
--- a/gtsam/inference/inferenceExceptions.h
+++ b/gtsam/inference/inferenceExceptions.h
@@ -28,9 +28,9 @@ namespace gtsam {
    *  with an ordering that does not include all of the variables. */
   class InconsistentEliminationRequested : public std::exception {
   public:
-    InconsistentEliminationRequested() throw() {}
-    virtual ~InconsistentEliminationRequested() throw() {}
-    virtual const char* what() const throw() {
+    InconsistentEliminationRequested() noexcept {}
+    virtual ~InconsistentEliminationRequested() noexcept {}
+    const char* what() const noexcept override {
       return
         "An inference algorithm was called with inconsistent arguments.  The\n"
         "factor graph, ordering, or variable index were inconsistent with each\n"
diff --git a/gtsam/linear/BinaryJacobianFactor.h b/gtsam/linear/BinaryJacobianFactor.h
index 343548613..8b53c34dd 100644
--- a/gtsam/linear/BinaryJacobianFactor.h
+++ b/gtsam/linear/BinaryJacobianFactor.h
@@ -49,7 +49,7 @@ struct BinaryJacobianFactor: JacobianFactor {
 
   // Fixed-size matrix update
   void updateHessian(const KeyVector& infoKeys,
-      SymmetricBlockMatrix* info) const {
+      SymmetricBlockMatrix* info) const override {
     gttic(updateHessian_BinaryJacobianFactor);
     // Whiten the factor if it has a noise model
     const SharedDiagonal& model = get_model();
diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h
index 2596a7403..c92390491 100644
--- a/gtsam/linear/ConjugateGradientSolver.h
+++ b/gtsam/linear/ConjugateGradientSolver.h
@@ -80,7 +80,7 @@ public:
 
 
   void print() const { Base::print(); }
-  virtual void print(std::ostream &os) const;
+  void print(std::ostream &os) const override;
 
   static std::string blasTranslator(const BLASKernel k) ;
   static BLASKernel blasTranslator(const std::string &s) ;
diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h
index b6f5acd52..e2d8ae87a 100644
--- a/gtsam/linear/GaussianBayesTree.h
+++ b/gtsam/linear/GaussianBayesTree.h
@@ -41,6 +41,7 @@ namespace gtsam {
     typedef boost::shared_ptr<This> shared_ptr;
     typedef boost::weak_ptr<This> weak_ptr;
     GaussianBayesTreeClique() {}
+    virtual ~GaussianBayesTreeClique() {}
     GaussianBayesTreeClique(const boost::shared_ptr<GaussianConditional>& conditional) : Base(conditional) {}
   };
 
diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h
index 8b41a4def..fdbe511f8 100644
--- a/gtsam/linear/GaussianConditional.h
+++ b/gtsam/linear/GaussianConditional.h
@@ -88,10 +88,10 @@ namespace gtsam {
 
     /** print */
     void print(const std::string& = "GaussianConditional",
-      const KeyFormatter& formatter = DefaultKeyFormatter) const;
+      const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
     /** equals function */
-    bool equals(const GaussianFactor&cg, double tol = 1e-9) const;
+    bool equals(const GaussianFactor&cg, double tol = 1e-9) const override;
 
     /** Return a view of the upper-triangular R block of the conditional */
     constABlock R() const { return Ab_.range(0, nrFrontals()); }
diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h
index 03ffe9326..71af704ab 100644
--- a/gtsam/linear/GaussianDensity.h
+++ b/gtsam/linear/GaussianDensity.h
@@ -57,7 +57,7 @@ namespace gtsam {
 
     /// print
     void print(const std::string& = "GaussianDensity",
-      const KeyFormatter& formatter = DefaultKeyFormatter) const;
+      const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
     /// Mean \f$ \mu = R^{-1} d \f$
     Vector mean() const;
diff --git a/gtsam/linear/GaussianISAM.h b/gtsam/linear/GaussianISAM.h
index 55208d4d1..b77b26240 100644
--- a/gtsam/linear/GaussianISAM.h
+++ b/gtsam/linear/GaussianISAM.h
@@ -20,6 +20,7 @@
 
 #include <gtsam/linear/GaussianBayesTree.h>
 #include <gtsam/inference/ISAM.h>
+#include <gtsam/base/Testable.h>
 
 namespace gtsam {
 
@@ -43,4 +44,8 @@ namespace gtsam {
 
   };
 
+  /// traits
+  template <>
+  struct traits<GaussianISAM> : public Testable<GaussianISAM> {};
+
 }
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 09a9a6103..b8a08be9e 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -515,7 +515,7 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const {
 double JacobianFactor::error(const VectorValues& c) const {
   Vector e = unweighted_error(c);
   // Use the noise model distance function to get the correct error if available.
-  if (model_) return 0.5 * model_->distance(e);
+  if (model_) return 0.5 * model_->squaredMahalanobisDistance(e);
   return 0.5 * e.dot(e);
 }
 
diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp
index de2a5142d..bf799a2ba 100644
--- a/gtsam/linear/LossFunctions.cpp
+++ b/gtsam/linear/LossFunctions.cpp
@@ -137,12 +137,12 @@ Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) {
   }
 }
 
-double Fair::weight(double error) const {
-  return 1.0 / (1.0 + std::abs(error) / c_);
+double Fair::weight(double distance) const {
+  return 1.0 / (1.0 + std::abs(distance) / c_);
 }
 
-double Fair::residual(double error) const {
-  const double absError = std::abs(error);
+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));
@@ -170,15 +170,15 @@ Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) {
   }
 }
 
-double Huber::weight(double error) const {
-  const double absError = std::abs(error);
+double Huber::weight(double distance) const {
+  const double absError = std::abs(distance);
   return (absError <= k_) ? (1.0) : (k_ / absError);
 }
 
-double Huber::residual(double error) const {
-  const double absError = std::abs(error);
+double Huber::loss(double distance) const {
+  const double absError = std::abs(distance);
   if (absError <= k_) {  // |x| <= k
-    return error*error / 2;
+    return distance*distance / 2;
   } else { // |x| > k
     return k_ * (absError - (k_/2));
   }
@@ -208,12 +208,12 @@ Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k),
   }
 }
 
-double Cauchy::weight(double error) const {
-  return ksquared_ / (ksquared_ + error*error);
+double Cauchy::weight(double distance) const {
+  return ksquared_ / (ksquared_ + distance*distance);
 }
 
-double Cauchy::residual(double error) const {
-  const double val = std::log1p(error * error / ksquared_);
+double Cauchy::loss(double distance) const {
+  const double val = std::log1p(distance * distance / ksquared_);
   return ksquared_ * val * 0.5;
 }
 
@@ -241,18 +241,18 @@ Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), c
   }
 }
 
-double Tukey::weight(double error) const {
-  if (std::abs(error) <= c_) {
-    const double one_minus_xc2 = 1.0 - error*error/csquared_;
+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::residual(double error) const {
-  double absError = std::abs(error);
+double Tukey::loss(double distance) const {
+  double absError = std::abs(distance);
   if (absError <= c_) {
-    const double one_minus_xc2 = 1.0 - error*error/csquared_;
+    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 {
@@ -280,13 +280,13 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) {
 
 Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
 
-double Welsch::weight(double error) const {
-  const double xc2 = (error*error)/csquared_;
+double Welsch::weight(double distance) const {
+  const double xc2 = (distance*distance)/csquared_;
   return std::exp(-xc2);
 }
 
-double Welsch::residual(double error) const {
-  const double xc2 = (error*error)/csquared_;
+double Welsch::loss(double distance) const {
+  const double xc2 = (distance*distance)/csquared_;
   return csquared_ * 0.5 * -std::expm1(-xc2);
 }
 
@@ -311,16 +311,16 @@ GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight)
   : Base(reweight), c_(c) {
 }
 
-double GemanMcClure::weight(double error) const {
+double GemanMcClure::weight(double distance) const {
   const double c2 = c_*c_;
   const double c4 = c2*c2;
-  const double c2error = c2 + error*error;
+  const double c2error = c2 + distance*distance;
   return c4/(c2error*c2error);
 }
 
-double GemanMcClure::residual(double error) const {
+double GemanMcClure::loss(double distance) const {
   const double c2 = c_*c_;
-  const double error2 = error*error;
+  const double error2 = distance*distance;
   return 0.5 * (c2 * error2) / (c2 + error2);
 }
 
@@ -345,8 +345,8 @@ DCS::DCS(double c, const ReweightScheme reweight)
   : Base(reweight), c_(c) {
 }
 
-double DCS::weight(double error) const {
-  const double e2 = error*error;
+double DCS::weight(double distance) const {
+  const double e2 = distance*distance;
   if (e2 > c_)
   {
     const double w = 2.0*c_/(c_ + e2);
@@ -356,10 +356,10 @@ double DCS::weight(double error) const {
   return 1.0;
 }
 
-double DCS::residual(double error) const {
+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 = error*error;
+  const double e2 = distance*distance;
   const double e4 = e2*e2;
   const double c2 = c_*c_;
 
@@ -391,17 +391,17 @@ L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight)
   }
 }
 
-double L2WithDeadZone::weight(double error) const {
+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(error) <= k_) return 0.0;
-  else if (error > k_) return (-k_+error)/error;
-  else return (k_+error)/error;
+  if (std::abs(distance) <= k_) return 0.0;
+  else if (distance > k_) return (-k_+distance)/distance;
+  else return (k_+distance)/distance;
 }
 
-double L2WithDeadZone::residual(double error) const {
-  const double abs_error = std::abs(error);
+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);
 }
 
diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h
index 1f7cc1377..a126b37db 100644
--- a/gtsam/linear/LossFunctions.h
+++ b/gtsam/linear/LossFunctions.h
@@ -36,12 +36,12 @@ namespace noiseModel {
  * 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.
+ * 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
- * Residual    \rho(x)         0.5*x^2         |x|        0.5*x^2 if |x|<k, 0.5*k^2 + k|x-k| otherwise
+ * 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
  *
@@ -75,27 +75,31 @@ class GTSAM_EXPORT Base {
    * the quadratic function for an L2 penalty, the absolute value function for
    * an L1 penalty, etc.
    *
-   * TODO(mikebosse): When the residual function has as input the norm of the
-   * residual vector, then it prevents implementations of asymmeric loss
+   * 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 residual(double error) const { return 0; };
+  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 residual function. See
+   * 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 error) const = 0;
+  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 error) const { return std::sqrt(weight(error)); }
+  double sqrtWeight(double distance) const { return std::sqrt(weight(distance)); }
 
   /** produce a weight vector according to an error vector and the implemented
    * robust function */
@@ -123,17 +127,17 @@ class GTSAM_EXPORT Base {
   }
 };
 
-/// Null class is not robust so is a Gaussian ?
+/// 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 residual(double error) const { return error; }
-  void print(const std::string &s) const;
-  bool equals(const Base & /*expected*/, double /*tol*/) const { return true; }
+  double weight(double /*error*/) const override { return 1.0; }
+  double loss(double distance) const override { return 0.5 * distance * distance; }
+  void print(const std::string &s) const override;
+  bool equals(const Base & /*expected*/, double /*tol*/) const override { return true; }
   static shared_ptr Create();
 
  private:
@@ -154,8 +158,8 @@ class GTSAM_EXPORT Fair : public Base {
   typedef boost::shared_ptr<Fair> shared_ptr;
 
   Fair(double c = 1.3998, const ReweightScheme reweight = Block);
-  double weight(double error) const override;
-  double residual(double error) const override;
+  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);
@@ -179,8 +183,8 @@ class GTSAM_EXPORT Huber : public Base {
   typedef boost::shared_ptr<Huber> shared_ptr;
 
   Huber(double k = 1.345, const ReweightScheme reweight = Block);
-  double weight(double error) const override;
-  double residual(double error) const override;
+  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);
@@ -209,8 +213,8 @@ class GTSAM_EXPORT Cauchy : public Base {
   typedef boost::shared_ptr<Cauchy> shared_ptr;
 
   Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
-  double weight(double error) const override;
-  double residual(double error) const override;
+  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);
@@ -234,8 +238,8 @@ class GTSAM_EXPORT Tukey : public Base {
   typedef boost::shared_ptr<Tukey> shared_ptr;
 
   Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
-  double weight(double error) const override;
-  double residual(double error) const override;
+  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);
@@ -259,8 +263,8 @@ class GTSAM_EXPORT Welsch : public Base {
   typedef boost::shared_ptr<Welsch> shared_ptr;
 
   Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
-  double weight(double error) const override;
-  double residual(double error) const override;
+  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);
@@ -295,8 +299,8 @@ class GTSAM_EXPORT GemanMcClure : public Base {
 
   GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
   ~GemanMcClure() {}
-  double weight(double error) const override;
-  double residual(double error) const override;
+  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);
@@ -325,8 +329,8 @@ class GTSAM_EXPORT DCS : public Base {
 
   DCS(double c = 1.0, const ReweightScheme reweight = Block);
   ~DCS() {}
-  double weight(double error) const override;
-  double residual(double error) const override;
+  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);
@@ -358,8 +362,8 @@ class GTSAM_EXPORT L2WithDeadZone : public Base {
   typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
 
   L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block);
-  double weight(double error) const override;
-  double residual(double error) const override;
+  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);
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index d7fd2d1ea..f5ec95696 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -74,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();
@@ -164,13 +171,6 @@ Vector Gaussian::unwhiten(const Vector& v) const {
   return backSubstituteUpper(thisR(), v);
 }
 
-/* ************************************************************************* */
-double Gaussian::squaredMahalanobisDistance(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;
@@ -376,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
@@ -663,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 ce2c84425..7494e0501 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -90,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;
@@ -200,39 +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;
-
-      /**
-       * 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));
-      }
+      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);
       }
-#endif
 
-      inline virtual double distance(const Vector& v) const {
-        return squaredMahalanobisDistance(v);
+      /**
+       * error value 0.5 * v'*R'*R*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
@@ -247,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]
@@ -335,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)
@@ -363,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();
       }
 
@@ -417,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;
@@ -472,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) {
@@ -494,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
@@ -514,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
@@ -576,14 +591,14 @@ namespace gtsam {
         return Variance(dim, 1.0/precision, smart);
       }
 
-      virtual void print(const std::string& name) const;
-      virtual double squaredMahalanobisDistance(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
@@ -616,7 +631,7 @@ namespace gtsam {
 
       typedef boost::shared_ptr<Unit> shared_ptr;
 
-      virtual ~Unit() {}
+      ~Unit() {}
 
       /**
        * Create a unit covariance noise model
@@ -626,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 squaredMahalanobisDistance(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 */
@@ -687,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_; }
@@ -699,28 +714,36 @@ 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."); }
-      // Fold the use of the m-estimator residual(...) function into distance(...)
-      inline virtual double distance(const Vector& v) const
-      { return robust_->residual(this->unweightedWhiten(v).norm()); }
-      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;
 
-      virtual Vector unweightedWhiten(const Vector& v) const {
+      Vector unweightedWhiten(const Vector& v) const override {
         return noise_->unweightedWhiten(v);
       }
-      virtual double weight(const Vector& v) const {
+      double weight(const Vector& v) const override {
         // Todo(mikebosse): make the robust weight function input a vector.
         return robust_->weight(v.norm());
       }
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..515f2eed2 100644
--- a/gtsam/linear/PCGSolver.h
+++ b/gtsam/linear/PCGSolver.h
@@ -41,14 +41,19 @@ public:
   PCGSolverParameters() {
   }
 
-  virtual void print(std::ostream &os) const;
+  void print(std::ostream &os) const override;
 
   /* interface to preconditioner parameters */
   inline const PreconditionerParameters& preconditioner() const {
     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);
 };
 
 /**
@@ -72,9 +77,9 @@ public:
 
   using IterativeSolver::optimize;
 
-  virtual VectorValues optimize(const GaussianFactorGraph &gfg,
+  VectorValues optimize(const GaussianFactorGraph &gfg,
       const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
-      const VectorValues &initial);
+      const VectorValues &initial) override;
 
 };
 
diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp
index 1c602a963..24a42afb8 100644
--- a/gtsam/linear/Preconditioner.cpp
+++ b/gtsam/linear/Preconditioner.cpp
@@ -145,7 +145,7 @@ void BlockJacobiPreconditioner::build(
 
   /* getting the block diagonals over the factors */
   std::map<Key, Matrix> hessianMap =gfg.hessianBlockDiagonal();
-  for ( const Matrix hessian: hessianMap | boost::adaptors::map_values)
+  for (const Matrix& hessian: hessianMap | boost::adaptors::map_values)
     blocks.push_back(hessian);
 
   /* if necessary, allocating the memory for cacheing the factorization results */
diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h
index 31901db3f..eceb19982 100644
--- a/gtsam/linear/Preconditioner.h
+++ b/gtsam/linear/Preconditioner.h
@@ -111,13 +111,13 @@ public:
   virtual ~DummyPreconditioner() {}
 
   /* Computation Interfaces for raw vector */
-  virtual void solve(const Vector& y, Vector &x) const { x = y; }
-  virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; }
-  virtual void build(
+  void solve(const Vector& y, Vector &x) const override { x = y; }
+  void transposeSolve(const Vector& y, Vector& x) const  override { x = y; }
+  void build(
     const GaussianFactorGraph &gfg,
     const KeyInfo &info,
     const std::map<Key,Vector> &lambda
-    )  {}
+    ) override {}
 };
 
 /*******************************************************************************************/
@@ -135,13 +135,13 @@ public:
   virtual ~BlockJacobiPreconditioner() ;
 
   /* Computation Interfaces for raw vector */
-  virtual void solve(const Vector& y, Vector &x) const;
-  virtual void transposeSolve(const Vector& y, Vector& x) const ;
-  virtual void build(
+  void solve(const Vector& y, Vector &x) const override;
+  void transposeSolve(const Vector& y, Vector& x) const override;
+  void build(
     const GaussianFactorGraph &gfg,
     const KeyInfo &info,
     const std::map<Key,Vector> &lambda
-    ) ;
+    ) override;
 
 protected:
 
diff --git a/gtsam/linear/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h
index a24acfacd..707f519ca 100644
--- a/gtsam/linear/RegularHessianFactor.h
+++ b/gtsam/linear/RegularHessianFactor.h
@@ -109,8 +109,8 @@ private:
 public:
 
   /** y += alpha * A'*A*x */
-  virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
-      VectorValues& y) const {
+  void multiplyHessianAdd(double alpha, const VectorValues& x,
+      VectorValues& y) const override {
     HessianFactor::multiplyHessianAdd(alpha, x, y);
   }
 
@@ -182,7 +182,7 @@ public:
   }
 
   /** Return the diagonal of the Hessian for this factor (raw memory version) */
-  virtual void hessianDiagonal(double* d) const {
+  void hessianDiagonal(double* d) const override {
 
     // Loop over all variables in the factor
     for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
@@ -193,7 +193,7 @@ public:
   }
 
   /// Add gradient at zero to d TODO: is it really the goal to add ??
-  virtual void gradientAtZero(double* d) const {
+  void gradientAtZero(double* d) const override {
 
     // Loop over all variables in the factor
     for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
diff --git a/gtsam/linear/RegularJacobianFactor.h b/gtsam/linear/RegularJacobianFactor.h
index 0312efe78..1c465da03 100644
--- a/gtsam/linear/RegularJacobianFactor.h
+++ b/gtsam/linear/RegularJacobianFactor.h
@@ -70,8 +70,8 @@ public:
   using JacobianFactor::multiplyHessianAdd;
 
   /** y += alpha * A'*A*x */
-  virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
-      VectorValues& y) const {
+  void multiplyHessianAdd(double alpha, const VectorValues& x,
+      VectorValues& y) const override {
     JacobianFactor::multiplyHessianAdd(alpha, x, y);
   }
 
@@ -106,7 +106,7 @@ public:
   using GaussianFactor::hessianDiagonal;
 
   /// Raw memory access version of hessianDiagonal
-  void hessianDiagonal(double* d) const {
+  void hessianDiagonal(double* d) const override {
     // Loop over all variables in the factor
     for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
       // Get the diagonal block, and insert its diagonal
@@ -125,12 +125,12 @@ public:
   }
 
   /// Expose base class gradientAtZero
-  virtual VectorValues gradientAtZero() const {
+  VectorValues gradientAtZero() const override {
     return JacobianFactor::gradientAtZero();
   }
 
   /// Raw memory access version of gradientAtZero
-  void gradientAtZero(double* d) const {
+  void gradientAtZero(double* d) const override {
 
     // Get vector b not weighted
     Vector b = getb();
diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h
index 5ab1a8520..0f7eea06f 100644
--- a/gtsam/linear/SubgraphSolver.h
+++ b/gtsam/linear/SubgraphSolver.h
@@ -38,7 +38,7 @@ struct GTSAM_EXPORT SubgraphSolverParameters
   explicit SubgraphSolverParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
     : builderParams(p) {}
   void print() const { Base::print(); }
-  virtual void print(std::ostream &os) const {
+  void print(std::ostream &os) const override {
     Base::print(os);
   }
 };
diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp
index 0a25617e4..9b5868744 100644
--- a/gtsam/linear/VectorValues.cpp
+++ b/gtsam/linear/VectorValues.cpp
@@ -45,7 +45,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   VectorValues::VectorValues(const Vector& x, const Dims& dims) {
-    typedef pair<Key, size_t> Pair;
+    using Pair = pair<const Key, size_t>;
     size_t j = 0;
     for (const Pair& v : dims) {
       Key key;
diff --git a/gtsam/linear/linearExceptions.cpp b/gtsam/linear/linearExceptions.cpp
index a4168af2d..ada3a298c 100644
--- a/gtsam/linear/linearExceptions.cpp
+++ b/gtsam/linear/linearExceptions.cpp
@@ -24,7 +24,7 @@
 namespace gtsam {
 
   /* ************************************************************************* */
-  const char* IndeterminantLinearSystemException::what() const throw()
+  const char* IndeterminantLinearSystemException::what() const noexcept
   {
     if(!description_) {
       description_ = String(
@@ -43,7 +43,7 @@ more information.");
   }
 
   /* ************************************************************************* */
-  const char* InvalidNoiseModel::what() const throw() {
+  const char* InvalidNoiseModel::what() const noexcept {
     if(description_.empty())
       description_ = (boost::format(
       "A JacobianFactor was attempted to be constructed or modified to use a\n"
@@ -54,7 +54,7 @@ more information.");
   }
 
   /* ************************************************************************* */
-  const char* InvalidMatrixBlock::what() const throw() {
+  const char* InvalidMatrixBlock::what() const noexcept {
     if(description_.empty())
       description_ = (boost::format(
       "A JacobianFactor was attempted to be constructed with a matrix block of\n"
diff --git a/gtsam/linear/linearExceptions.h b/gtsam/linear/linearExceptions.h
index 32db27fc9..f0ad0be39 100644
--- a/gtsam/linear/linearExceptions.h
+++ b/gtsam/linear/linearExceptions.h
@@ -94,10 +94,10 @@ namespace gtsam {
   class GTSAM_EXPORT IndeterminantLinearSystemException : public ThreadsafeException<IndeterminantLinearSystemException> {
     Key j_;
   public:
-    IndeterminantLinearSystemException(Key j) throw() : j_(j) {}
-    virtual ~IndeterminantLinearSystemException() throw() {}
+    IndeterminantLinearSystemException(Key j) noexcept : j_(j) {}
+    virtual ~IndeterminantLinearSystemException() noexcept {}
     Key nearbyVariable() const { return j_; }
-    virtual const char* what() const throw();
+    const char* what() const noexcept override;
   };
 
   /* ************************************************************************* */
@@ -110,9 +110,9 @@ namespace gtsam {
 
     InvalidNoiseModel(DenseIndex factorDims, DenseIndex noiseModelDims) :
       factorDims(factorDims), noiseModelDims(noiseModelDims) {}
-    virtual ~InvalidNoiseModel() throw() {}
+    virtual ~InvalidNoiseModel() noexcept {}
 
-    virtual const char* what() const throw();
+    const char* what() const noexcept override;
 
   private:
     mutable std::string description_;
@@ -128,9 +128,9 @@ namespace gtsam {
 
     InvalidMatrixBlock(DenseIndex factorRows, DenseIndex blockRows) :
       factorRows(factorRows), blockRows(blockRows) {}
-    virtual ~InvalidMatrixBlock() throw() {}
+    virtual ~InvalidMatrixBlock() noexcept {}
 
-    virtual const char* what() const throw();
+    const char* what() const noexcept override;
 
   private:
     mutable std::string description_;
diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index 488368c72..eafefb3cb 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -137,7 +137,7 @@ TEST( GaussianBayesNet, optimize3 )
 }
 
 /* ************************************************************************* */
-TEST(GaussianBayesNet, ordering) 
+TEST(GaussianBayesNet, ordering)
 {
   Ordering expected;
   expected += _x_, _y_;
@@ -155,7 +155,7 @@ TEST( GaussianBayesNet, MatrixStress )
   bn.emplace_shared<GC>(_z_, Vector2(5, 6), 6 * I_2x2);
 
   const VectorValues expected = bn.optimize();
-  for (const auto keys :
+  for (const auto& keys :
        {KeyVector({_x_, _y_, _z_}), KeyVector({_x_, _z_, _y_}),
         KeyVector({_y_, _x_, _z_}), KeyVector({_y_, _z_, _x_}),
         KeyVector({_z_, _x_, _y_}), KeyVector({_z_, _y_, _x_})}) {
@@ -183,7 +183,7 @@ TEST( GaussianBayesNet, backSubstituteTranspose )
 
   VectorValues actual = smallBayesNet.backSubstituteTranspose(x);
   EXPECT(assert_equal(expected, actual));
-  
+
   const auto ordering = noisyBayesNet.ordering();
   const Matrix R = smallBayesNet.matrix(ordering).first;
   const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);
@@ -206,7 +206,7 @@ TEST( GaussianBayesNet, backSubstituteTransposeNoisy )
 
   VectorValues actual = noisyBayesNet.backSubstituteTranspose(x);
   EXPECT(assert_equal(expected, actual));
-  
+
   const auto ordering = noisyBayesNet.ordering();
   const Matrix R = noisyBayesNet.matrix(ordering).first;
   const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);
diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp
index 3f6550b9f..42d68a603 100644
--- a/gtsam/linear/tests/testNoiseModel.cpp
+++ b/gtsam/linear/tests/testNoiseModel.cpp
@@ -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,7 +453,7 @@ 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.
@@ -467,10 +469,10 @@ TEST(NoiseModel, robustFunctionFair)
   DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8);
   DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8);
 
-  DOUBLES_EQUAL(0.441961080151135, fair->residual(error1), 1e-8);
-  DOUBLES_EQUAL(22.534692783297260, fair->residual(error2), 1e-8);
-  DOUBLES_EQUAL(22.534692783297260, fair->residual(error3), 1e-8);
-  DOUBLES_EQUAL(0.441961080151135, fair->residual(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)
@@ -483,10 +485,10 @@ TEST(NoiseModel, robustFunctionHuber)
   DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8);
   DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8);
 
-  DOUBLES_EQUAL(0.5000, huber->residual(error1), 1e-8);
-  DOUBLES_EQUAL(37.5000, huber->residual(error2), 1e-8);
-  DOUBLES_EQUAL(37.5000, huber->residual(error3), 1e-8);
-  DOUBLES_EQUAL(0.5000, huber->residual(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)
@@ -499,10 +501,10 @@ TEST(NoiseModel, robustFunctionCauchy)
   DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8);
   DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8);
 
-  DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error1), 1e-8);
-  DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error2), 1e-8);
-  DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error3), 1e-8);
-  DOUBLES_EQUAL(0.490258914416017, cauchy->residual(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)
@@ -514,10 +516,10 @@ TEST(NoiseModel, robustFunctionGemanMcClure)
   DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8);
   DOUBLES_EQUAL(0.25      , gmc->weight(error4), 1e-8);
 
-  DOUBLES_EQUAL(0.2500, gmc->residual(error1), 1e-8);
-  DOUBLES_EQUAL(0.495049504950495, gmc->residual(error2), 1e-8);
-  DOUBLES_EQUAL(0.495049504950495, gmc->residual(error3), 1e-8);
-  DOUBLES_EQUAL(0.2500, gmc->residual(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)
@@ -530,10 +532,10 @@ TEST(NoiseModel, robustFunctionWelsch)
   DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8);
   DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8);
 
-  DOUBLES_EQUAL(0.490132010595960, welsch->residual(error1), 1e-8);
-  DOUBLES_EQUAL(12.271054513890823, welsch->residual(error2), 1e-8);
-  DOUBLES_EQUAL(12.271054513890823, welsch->residual(error3), 1e-8);
-  DOUBLES_EQUAL(0.490132010595960, welsch->residual(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)
@@ -546,10 +548,10 @@ TEST(NoiseModel, robustFunctionTukey)
   DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8);
   DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8);
 
-  DOUBLES_EQUAL(0.480266666666667, tukey->residual(error1), 1e-8);
-  DOUBLES_EQUAL(4.166666666666667, tukey->residual(error2), 1e-8);
-  DOUBLES_EQUAL(4.166666666666667, tukey->residual(error3), 1e-8);
-  DOUBLES_EQUAL(0.480266666666667, tukey->residual(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)
@@ -560,8 +562,8 @@ TEST(NoiseModel, robustFunctionDCS)
   DOUBLES_EQUAL(1.0       , dcs->weight(error1), 1e-8);
   DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8);
 
-  DOUBLES_EQUAL(0.5         , dcs->residual(error1), 1e-8);
-  DOUBLES_EQUAL(0.9900990099, dcs->residual(error2), 1e-8);
+  DOUBLES_EQUAL(0.5         , dcs->loss(error1), 1e-8);
+  DOUBLES_EQUAL(0.9900990099, dcs->loss(error2), 1e-8);
 }
 
 TEST(NoiseModel, robustFunctionL2WithDeadZone)
@@ -576,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);
 }
 
 /* ************************************************************************* */
@@ -665,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.
  *
@@ -681,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/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h
index 1418ab687..c3765b8cf 100644
--- a/gtsam/navigation/AHRSFactor.h
+++ b/gtsam/navigation/AHRSFactor.h
@@ -158,14 +158,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const;
+  gtsam::NonlinearFactor::shared_ptr clone() const override;
 
   /// print
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const override;
 
   /// equals
-  virtual bool equals(const NonlinearFactor&, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor&, double tol = 1e-9) const override;
 
   /// Access the preintegrated measurements.
   const PreintegratedAhrsMeasurements& preintegratedMeasurements() const {
@@ -178,7 +178,7 @@ public:
   Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
       const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
-          boost::none) const;
+          boost::none) const override;
 
   /// predicted states from IMU
   /// TODO(frank): relationship with PIM predict ??
diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h
index 5a0031caf..9a0a11cfb 100644
--- a/gtsam/navigation/AttitudeFactor.h
+++ b/gtsam/navigation/AttitudeFactor.h
@@ -108,21 +108,21 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /** print */
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const override;
 
   /** equals */
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /** vector of errors */
-  virtual Vector evaluateError(const Rot3& nRb, //
-      boost::optional<Matrix&> H = boost::none) const {
+  Vector evaluateError(const Rot3& nRb, //
+      boost::optional<Matrix&> H = boost::none) const override {
     return attitudeError(nRb, H);
   }
 
@@ -182,21 +182,21 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /** print */
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const override;
 
   /** equals */
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /** vector of errors */
-  virtual Vector evaluateError(const Pose3& nTb, //
-      boost::optional<Matrix&> H = boost::none) const {
+  Vector evaluateError(const Pose3& nTb, //
+      boost::optional<Matrix&> H = boost::none) const override {
     Vector e = attitudeError(nTb.rotation(), H);
     if (H) {
       Matrix H23 = *H;
diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp
index 149067269..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
 //------------------------------------------------------------------------------
@@ -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 6b3bf979a..6181f3d0d 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 {
 
@@ -61,10 +63,18 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
   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) {
+  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
@@ -77,6 +87,9 @@ PreintegrationCombinedParams(const Vector3& n_gravity) :
     return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
   }
 
+  void print(const std::string& s="") const override;
+  bool equals(const PreintegratedRotationParams& other, double tol) const override;
+
   void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
   void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
   void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
@@ -86,24 +99,22 @@ PreintegrationCombinedParams(const Vector3& n_gravity) :
   const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
   
 private:
-  /// Default constructor makes unitialized params struct
-  PreintegrationCombinedParams() {}
 
   /** 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);
+  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.
@@ -128,7 +139,6 @@ public:
    */
   Eigen::Matrix<double, 15, 15> preintMeasCov_;
 
-
   friend class CombinedImuFactor;
 
  public:
@@ -136,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,
@@ -149,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
@@ -158,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
   /// @{
 
@@ -205,6 +236,7 @@ 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_);
   }
@@ -244,9 +276,6 @@ private:
 
   PreintegratedCombinedMeasurements _PIM_;
 
-  /** Default constructor - only use for serialization */
-  CombinedImuFactor() {}
-
 public:
 
   /** Shorthand for a smart pointer to a factor */
@@ -256,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
@@ -273,16 +305,21 @@ public:
   virtual ~CombinedImuFactor() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const;
+  gtsam::NonlinearFactor::shared_ptr clone() const override;
 
   /** 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;
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const override;
 
   /// equals
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
+  /// @}
 
   /** Access the preintegrated measurements. */
 
@@ -299,7 +336,7 @@ public:
       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
           boost::none, boost::optional<Matrix&> H3 = boost::none,
       boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
-          boost::none, boost::optional<Matrix&> H6 = boost::none) const;
+          boost::none, boost::optional<Matrix&> H6 = boost::none) const override;
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
   /// @deprecated typename
@@ -321,14 +358,12 @@ 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:
@@ -336,4 +371,18 @@ public:
 };
 // 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.h b/gtsam/navigation/GPSFactor.h
index 2b5ea1f2f..e6d5b88a9 100644
--- a/gtsam/navigation/GPSFactor.h
+++ b/gtsam/navigation/GPSFactor.h
@@ -65,21 +65,21 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /// print
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const override;
 
   /// equals
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /// vector of errors
   Vector evaluateError(const Pose3& p,
-      boost::optional<Matrix&> H = boost::none) const;
+      boost::optional<Matrix&> H = boost::none) const override;
 
   inline const Point3 & measurementIn() const {
     return nT_;
@@ -137,21 +137,21 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /// print
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const override;
 
   /// equals
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /// vector of errors
   Vector evaluateError(const NavState& p,
-      boost::optional<Matrix&> H = boost::none) const;
+      boost::optional<Matrix&> H = boost::none) const override;
 
   inline const Point3 & measurementIn() const {
     return nT_;
diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h
index 8e3f8f0a4..65142af24 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_);
   }
 };
 
@@ -217,14 +217,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const;
+  gtsam::NonlinearFactor::shared_ptr clone() const override;
 
   /// @name Testable
   /// @{
   GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const override;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
   /// @}
 
   /** Access the preintegrated measurements. */
@@ -241,7 +241,7 @@ public:
       const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1 =
           boost::none, boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
-          boost::none, boost::optional<Matrix&> H5 = boost::none) const;
+          boost::none, boost::optional<Matrix&> H5 = boost::none) const override;
 
 #ifdef GTSAM_TANGENT_PREINTEGRATION
   /// Merge two pre-integrated measurement classes
@@ -315,14 +315,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const;
+  gtsam::NonlinearFactor::shared_ptr clone() const override;
 
   /// @name Testable
   /// @{
   GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const override;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
   /// @}
 
   /** Access the preintegrated measurements. */
@@ -338,7 +338,7 @@ public:
                        const imuBias::ConstantBias& bias_i,  //
                        boost::optional<Matrix&> H1 = boost::none,
                        boost::optional<Matrix&> H2 = boost::none,
-                       boost::optional<Matrix&> H3 = boost::none) const;
+                       boost::optional<Matrix&> H3 = boost::none) const override;
 
 private:
 
diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h
index 3875391d0..97a4c70ce 100644
--- a/gtsam/navigation/MagFactor.h
+++ b/gtsam/navigation/MagFactor.h
@@ -53,7 +53,7 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual NonlinearFactor::shared_ptr clone() const {
+  NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<NonlinearFactor>(
         NonlinearFactor::shared_ptr(new MagFactor(*this)));
   }
@@ -102,7 +102,7 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual NonlinearFactor::shared_ptr clone() const {
+  NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<NonlinearFactor>(
         NonlinearFactor::shared_ptr(new MagFactor1(*this)));
   }
@@ -138,7 +138,7 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual NonlinearFactor::shared_ptr clone() const {
+  NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<NonlinearFactor>(
         NonlinearFactor::shared_ptr(new MagFactor2(*this)));
   }
@@ -179,7 +179,7 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual NonlinearFactor::shared_ptr clone() const {
+  NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<NonlinearFactor>(
         NonlinearFactor::shared_ptr(new MagFactor3(*this)));
   }
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/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 12938a625..0e0559a32 100644
--- a/gtsam/navigation/PreintegratedRotation.h
+++ b/gtsam/navigation/PreintegratedRotation.h
@@ -61,9 +61,15 @@ 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
diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index eb30c1f13..29d7814b5 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@@ -213,6 +213,16 @@ 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:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
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 4bff625ca..ce1f0e734 100644
--- a/gtsam/navigation/PreintegrationParams.h
+++ b/gtsam/navigation/PreintegrationParams.h
@@ -31,7 +31,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
 
   /// Default constructor for serialization only
   PreintegrationParams()
-      : accelerometerCovariance(I_3x3),
+      : PreintegratedRotationParams(),
+        accelerometerCovariance(I_3x3),
         integrationCovariance(I_3x3),
         use2ndOrderCoriolis(false),
         n_gravity(0, 0, -1) {}
@@ -39,7 +40,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
   /// 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) {}
@@ -54,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 override;
+  bool equals(const PreintegratedRotationParams& other, double tol) const override;
 
   void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
   void setIntegrationCovariance(const Matrix3& cov)   { integrationCovariance = cov; }
@@ -73,10 +75,9 @@ protected:
   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);
   }
diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp
index 55efdb151..56d7aa6d3 100644
--- a/gtsam/navigation/TangentPreintegration.cpp
+++ b/gtsam/navigation/TangentPreintegration.cpp
@@ -68,7 +68,7 @@ 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 Rot3 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;
 
@@ -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 edf76e562..1b51b4e1e 100644
--- a/gtsam/navigation/TangentPreintegration.h
+++ b/gtsam/navigation/TangentPreintegration.h
@@ -132,12 +132,10 @@ 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:
diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp
index 59d0ac199..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,6 +66,16 @@ 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<ImuFactor>(factor));
@@ -69,6 +83,30 @@ TEST(ImuFactor, serialization) {
   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));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;
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/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h
index c42b2bdfc..4bb8da685 100644
--- a/gtsam/nonlinear/ExpressionFactor.h
+++ b/gtsam/nonlinear/ExpressionFactor.h
@@ -27,8 +27,13 @@
 namespace gtsam {
 
 /**
-
- * Factor that supports arbitrary expressions via AD
+ * Factor that supports arbitrary expressions via AD.
+ *
+ * Arbitrary instances of this template can be directly inserted into a factor
+ * graph for optimization. However, to enable the correct (de)serialization of
+ * such instances, the user should declare derived classes from this template,
+ * implementing expresion(), serialize(), clone(), print(), and defining the
+ * corresponding `struct traits<NewFactor> : public Testable<NewFactor> {}`.
  */
 template<typename T>
 class ExpressionFactor: public NoiseModelFactor {
@@ -68,13 +73,13 @@ protected:
 
   /// print relies on Testable traits being defined for T
   void print(const std::string& s = "",
-             const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+             const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     NoiseModelFactor::print(s, keyFormatter);
     traits<T>::Print(measured_, "ExpressionFactor with measurement: ");
   }
 
   /// equals relies on Testable traits being defined for T
-  bool equals(const NonlinearFactor& f, double tol) const {
+  bool equals(const NonlinearFactor& f, double tol) const override {
     const ExpressionFactor* p = dynamic_cast<const ExpressionFactor*>(&f);
     return p && NoiseModelFactor::equals(f, tol) &&
            traits<T>::Equals(measured_, p->measured_, tol) &&
@@ -86,8 +91,8 @@ protected:
    * We override this method to provide
    * both the function evaluation and its derivative(s) in H.
    */
-  virtual Vector unwhitenedError(const Values& x,
-                                 boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  Vector unwhitenedError(const Values& x,
+    boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     if (H) {
       const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
       // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here
@@ -99,7 +104,7 @@ protected:
     }
   }
 
-  virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
+  boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
     // Only linearize if the factor is active
     if (!active(x))
       return boost::shared_ptr<JacobianFactor>();
@@ -138,7 +143,7 @@ protected:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
@@ -263,7 +268,7 @@ class ExpressionFactor2 : public ExpressionFactor<T> {
 
  private:
   /// Return an expression that predicts the measurement given Values
-  virtual Expression<T> expression() const {
+  Expression<T> expression() const override {
     return expression(this->keys_[0], this->keys_[1]);
   }
 
diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h
new file mode 100644
index 000000000..688b3aaa2
--- /dev/null
+++ b/gtsam/nonlinear/FunctorizedFactor.h
@@ -0,0 +1,147 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+  NonlinearFactor::shared_ptr clone() const override {
+    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 override {
+    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 override {
+    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;
+  }
+
+  bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
+    const FunctorizedFactor<R, T> *e =
+        dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
+    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/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h
index 53bdaec81..9b40c02c6 100644
--- a/gtsam/nonlinear/ISAM2Clique.h
+++ b/gtsam/nonlinear/ISAM2Clique.h
@@ -51,6 +51,7 @@ class GTSAM_EXPORT ISAM2Clique
 
   /// Default constructor
   ISAM2Clique() : Base() {}
+  virtual ~ISAM2Clique() = default;
 
   /// Copy constructor, does *not* copy solution pointers as these are invalid
   /// in different trees.
@@ -85,7 +86,7 @@ class GTSAM_EXPORT ISAM2Clique
 
   /** print this node */
   void print(const std::string& s = "",
-             const KeyFormatter& formatter = DefaultKeyFormatter) const;
+             const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
   void optimizeWildfire(const KeySet& replaced, double threshold,
                         KeySet* changed, VectorValues* delta,
diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h
index 8a1f600ff..4540117b6 100644
--- a/gtsam/nonlinear/LinearContainerFactor.h
+++ b/gtsam/nonlinear/LinearContainerFactor.h
@@ -59,10 +59,10 @@ public:
   // Testable
 
   /** print */
-  GTSAM_EXPORT void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
+  GTSAM_EXPORT void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
   /** Check if two factors are equal */
-  GTSAM_EXPORT bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
+  GTSAM_EXPORT bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
 
   // NonlinearFactor
 
@@ -74,10 +74,10 @@ public:
    *
    * @return nonlinear error if linearizationPoint present, zero otherwise
    */
-  GTSAM_EXPORT double error(const Values& c) const;
+  GTSAM_EXPORT double error(const Values& c) const override;
 
   /** get the dimension of the factor: rows of linear factor */
-  GTSAM_EXPORT size_t dim() const;
+  GTSAM_EXPORT size_t dim() const override;
 
   /** Extract the linearization point used in recalculating error */
   const boost::optional<Values>& linearizationPoint() const { return linearizationPoint_; }
@@ -98,7 +98,7 @@ public:
    * TODO: better approximation of relinearization
    * TODO: switchable modes for approximation technique
    */
-  GTSAM_EXPORT GaussianFactor::shared_ptr linearize(const Values& c) const;
+  GTSAM_EXPORT GaussianFactor::shared_ptr linearize(const Values& c) const override;
 
   /**
    * Creates an anti-factor directly
@@ -116,7 +116,7 @@ public:
    *
    * Clones the underlying linear factor
    */
-  NonlinearFactor::shared_ptr clone() const {
+  NonlinearFactor::shared_ptr clone() const override {
     return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_));
   }
 
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index 1bba57051..6286b73da 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -107,15 +107,15 @@ public:
   /// @name Testable
   /// @{
 
-  virtual void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n";
     traits<VALUE>::Print(feasible_, "Feasible Point:\n");
     std::cout << "Variable Dimension: " << traits<T>::GetDimension(feasible_) << std::endl;
   }
 
   /** Check if two factors are equal */
-  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
     const This* e = dynamic_cast<const This*>(&f);
     return e && Base::equals(f) && traits<T>::Equals(feasible_,e->feasible_, tol)
         && std::abs(error_gain_ - e->error_gain_) < tol;
@@ -126,7 +126,7 @@ public:
   /// @{
 
   /** actual error function calculation */
-  virtual double error(const Values& c) const {
+  double error(const Values& c) const override {
     const T& xj = c.at<T>(this->key());
     Vector e = this->unwhitenedError(c);
     if (allow_error_ || !compare_(xj, feasible_)) {
@@ -138,7 +138,7 @@ public:
 
   /** error function */
   Vector evaluateError(const T& xj,
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> H = boost::none) const override {
     const size_t nj = traits<T>::GetDimension(feasible_);
     if (allow_error_) {
       if (H)
@@ -158,7 +158,7 @@ public:
   }
 
   // Linearize is over-written, because base linearization tries to whiten
-  virtual GaussianFactor::shared_ptr linearize(const Values& x) const {
+  GaussianFactor::shared_ptr linearize(const Values& x) const override {
     const T& xj = x.at<T>(this->key());
     Matrix A;
     Vector b = evaluateError(xj, A);
@@ -168,7 +168,7 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
@@ -242,14 +242,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /** g(x) with optional derivative */
   Vector evaluateError(const X& x1,
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> H = boost::none) const override {
     if (H)
       (*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
     // manifold equivalent of h(x)-z -> log(z,h(x))
@@ -257,8 +257,8 @@ public:
   }
 
   /** Print */
-  virtual void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
         << ")," << "\n";
     this->noiseModel_->print();
@@ -317,14 +317,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /** g(x) with optional derivative2 */
   Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 =
-      boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+      boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
     static const size_t p = traits<X>::dimension;
     if (H1) *H1 = -Matrix::Identity(p,p);
     if (H2) *H2 =  Matrix::Identity(p,p);
diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp
index ee14e8073..1cfcba274 100644
--- a/gtsam/nonlinear/NonlinearFactor.cpp
+++ b/gtsam/nonlinear/NonlinearFactor.cpp
@@ -121,7 +121,7 @@ double NoiseModelFactor::error(const Values& c) const {
     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 63547a248..7bbd51236 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -31,7 +31,7 @@
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 #define ADD_CLONE_NONLINEAR_FACTOR(Derived) \
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const { \
+  gtsam::NonlinearFactor::shared_ptr clone() const override { \
   return boost::static_pointer_cast<gtsam::NonlinearFactor>( \
       gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); }
 #endif
@@ -195,14 +195,14 @@ protected:
 public:
 
   /** Print */
-  virtual void print(const std::string& s = "",
-    const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+  void print(const std::string& s = "",
+    const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /** Check if two factors are equal */
-  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
 
   /** get the dimension of the factor (number of rows on linearization) */
-  virtual size_t dim() const {
+  size_t dim() const override {
     return noiseModel_->dim();
   }
 
@@ -242,14 +242,14 @@ public:
    * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
    * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
    */
-  virtual double error(const Values& c) const;
+  double error(const Values& c) const override;
 
   /**
    * Linearize a non-linearFactorN to get a GaussianFactor,
    * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
    * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
    */
-  boost::shared_ptr<GaussianFactor> linearize(const Values& x) const;
+  boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
   /// @name Deprecated
@@ -315,7 +315,7 @@ public:
   /** Calls the 1-key specific version of evaluateError, which is pure virtual
    *  so must be implemented in the derived class.
    */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     if(this->active(x)) {
       const X& x1 = x.at<X>(keys_[0]);
       if(H) {
@@ -389,7 +389,7 @@ public:
 
   /** Calls the 2-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     if(this->active(x)) {
       const X1& x1 = x.at<X1>(keys_[0]);
       const X2& x2 = x.at<X2>(keys_[1]);
@@ -467,7 +467,7 @@ public:
 
   /** Calls the 3-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
@@ -547,7 +547,7 @@ public:
 
   /** Calls the 4-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
@@ -631,7 +631,7 @@ public:
 
   /** Calls the 5-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
@@ -719,7 +719,7 @@ public:
 
   /** Calls the 6-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h
index 0afbaa588..191a7eeaa 100644
--- a/gtsam/nonlinear/PriorFactor.h
+++ b/gtsam/nonlinear/PriorFactor.h
@@ -65,15 +65,15 @@ namespace gtsam {
     }
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       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 {
+    void print(const std::string& s,
+       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
       traits<T>::Print(prior_, "  prior mean: ");
       if (this->noiseModel_)
@@ -83,7 +83,7 @@ namespace gtsam {
     }
 
     /** equals */
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
       const This* e = dynamic_cast<const This*> (&expected);
       return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
     }
@@ -91,7 +91,7 @@ namespace gtsam {
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
-    Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const {
+    Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override {
       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.
@@ -117,4 +117,9 @@ namespace gtsam {
 	GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   };
 
+  /// traits
+  template<class VALUE>
+  struct traits<PriorFactor<VALUE> > : public Testable<PriorFactor<VALUE> > {};
+
+
 } /// namespace gtsam
diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp
index 98790ccd9..7e13a072a 100644
--- a/gtsam/nonlinear/Values.cpp
+++ b/gtsam/nonlinear/Values.cpp
@@ -214,7 +214,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  const char* ValuesKeyAlreadyExists::what() const throw() {
+  const char* ValuesKeyAlreadyExists::what() const noexcept {
     if(message_.empty())
       message_ =
           "Attempting to add a key-value pair with key \"" + DefaultKeyFormatter(key_) + "\", key already exists.";
@@ -222,7 +222,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  const char* ValuesKeyDoesNotExist::what() const throw() {
+  const char* ValuesKeyDoesNotExist::what() const noexcept {
     if(message_.empty())
       message_ =
           "Attempting to " + std::string(operation_) + " the key \"" +
@@ -231,7 +231,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  const char* ValuesIncorrectType::what() const throw() {
+  const char* ValuesIncorrectType::what() const noexcept {
     if(message_.empty()) {
       std::string storedTypeName = demangle(storedTypeId_.name());
       std::string requestedTypeName = demangle(requestedTypeId_.name());
@@ -251,7 +251,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  const char* NoMatchFoundForFixed::what() const throw() {
+  const char* NoMatchFoundForFixed::what() const noexcept {
     if(message_.empty()) {
       ostringstream oss;
     oss
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 041aa3441..bc64f2612 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -432,16 +432,16 @@ namespace gtsam {
 
   public:
     /// Construct with the key-value pair attempted to be added
-    ValuesKeyAlreadyExists(Key key) throw() :
+    ValuesKeyAlreadyExists(Key key) noexcept :
       key_(key) {}
 
-    virtual ~ValuesKeyAlreadyExists() throw() {}
+    virtual ~ValuesKeyAlreadyExists() noexcept {}
 
     /// The duplicate key that was attempted to be added
-    Key key() const throw() { return key_; }
+    Key key() const noexcept { return key_; }
 
     /// The message to be displayed to the user
-    GTSAM_EXPORT virtual const char* what() const throw();
+    GTSAM_EXPORT const char* what() const noexcept override;
   };
 
   /* ************************************************************************* */
@@ -455,16 +455,16 @@ namespace gtsam {
 
   public:
     /// Construct with the key that does not exist in the values
-    ValuesKeyDoesNotExist(const char* operation, Key key) throw() :
+    ValuesKeyDoesNotExist(const char* operation, Key key) noexcept :
       operation_(operation), key_(key) {}
 
-    virtual ~ValuesKeyDoesNotExist() throw() {}
+    virtual ~ValuesKeyDoesNotExist() noexcept {}
 
     /// The key that was attempted to be accessed that does not exist
-    Key key() const throw() { return key_; }
+    Key key() const noexcept { return key_; }
 
     /// The message to be displayed to the user
-    GTSAM_EXPORT virtual const char* what() const throw();
+    GTSAM_EXPORT const char* what() const noexcept override;
   };
 
   /* ************************************************************************* */
@@ -480,13 +480,13 @@ namespace gtsam {
   public:
     /// Construct with the key that does not exist in the values
     ValuesIncorrectType(Key key,
-        const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() :
+        const std::type_info& storedTypeId, const std::type_info& requestedTypeId) noexcept :
       key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
 
-    virtual ~ValuesIncorrectType() throw() {}
+    virtual ~ValuesIncorrectType() noexcept {}
 
     /// The key that was attempted to be accessed that does not exist
-    Key key() const throw() { return key_; }
+    Key key() const noexcept { return key_; }
 
     /// The typeid of the value stores in the Values
     const std::type_info& storedTypeId() const { return storedTypeId_; }
@@ -495,18 +495,18 @@ namespace gtsam {
     const std::type_info& requestedTypeId() const { return requestedTypeId_; }
 
     /// The message to be displayed to the user
-    GTSAM_EXPORT virtual const char* what() const throw();
+    GTSAM_EXPORT const char* what() const noexcept override;
   };
 
   /* ************************************************************************* */
   class DynamicValuesMismatched : public std::exception {
 
   public:
-    DynamicValuesMismatched() throw() {}
+    DynamicValuesMismatched() noexcept {}
 
-    virtual ~DynamicValuesMismatched() throw() {}
+    virtual ~DynamicValuesMismatched() noexcept {}
 
-    virtual const char* what() const throw() {
+    const char* what() const noexcept override {
       return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values";
     }
   };
@@ -522,14 +522,14 @@ namespace gtsam {
     mutable std::string message_;
 
   public:
-    NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) throw () :
+    NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) noexcept :
         M1_(M1), N1_(N1), M2_(M2), N2_(N2) {
     }
 
-    virtual ~NoMatchFoundForFixed() throw () {
+    virtual ~NoMatchFoundForFixed() noexcept {
     }
 
-    GTSAM_EXPORT virtual const char* what() const throw ();
+    GTSAM_EXPORT const char* what() const noexcept override;
   };
 
   /* ************************************************************************* */
diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h
index 634736800..974998830 100644
--- a/gtsam/nonlinear/WhiteNoiseFactor.h
+++ b/gtsam/nonlinear/WhiteNoiseFactor.h
@@ -109,7 +109,7 @@ namespace gtsam {
 
     /// Print
     void print(const std::string& p = "WhiteNoiseFactor",
-        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       Base::print(p, keyFormatter);
       std::cout << p + ".z: " << z_ << std::endl;
     }
@@ -119,12 +119,12 @@ namespace gtsam {
     /// @{
 
     /// get the dimension of the factor (number of rows on linearization)
-    virtual size_t dim() const {
+    size_t dim() const override {
       return 2;
     }
 
     /// Calculate the error of the factor, typically equal to log-likelihood
-    inline double error(const Values& x) const {
+    double error(const Values& x) const override {
       return f(z_, x.at<double>(meanKey_), x.at<double>(precisionKey_));
     }
 
@@ -153,7 +153,7 @@ namespace gtsam {
     /// @{
 
     /// linearize returns a Hessianfactor that is an approximation of error(p)
-    virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
+    boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
       double u = x.at<double>(meanKey_);
       double p = x.at<double>(precisionKey_);
       Key j1 = meanKey_;
@@ -163,7 +163,7 @@ namespace gtsam {
 
     // TODO: Frank commented this out for now, can it go?
     //    /// @return a deep copy of this factor
-    //    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    //    gtsam::NonlinearFactor::shared_ptr clone() const override {
     //      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
     //          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
diff --git a/gtsam/nonlinear/internal/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h
index 8bdc0652e..707c617ee 100644
--- a/gtsam/nonlinear/internal/CallRecord.h
+++ b/gtsam/nonlinear/internal/CallRecord.h
@@ -144,43 +144,43 @@ private:
     return static_cast<const Derived&>(*this);
   }
 
-  virtual void _print(const std::string& indent) const {
+  void _print(const std::string& indent) const override {
     derived().print(indent);
   }
 
   // Called from base class non-virtual inline method startReverseAD2
   // Calls non-virtual function startReverseAD4, implemented in Derived (ExpressionNode::Record)
-  virtual void _startReverseAD3(JacobianMap& jacobians) const {
+  void _startReverseAD3(JacobianMap& jacobians) const override {
     derived().startReverseAD4(jacobians);
   }
 
-  virtual void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const {
+  void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const override {
     derived().reverseAD4(dFdT, jacobians);
   }
 
-  virtual void _reverseAD3(
+  void _reverseAD3(
       const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
-      JacobianMap& jacobians) const {
+      JacobianMap& jacobians) const override {
     derived().reverseAD4(dFdT, jacobians);
   }
-  virtual void _reverseAD3(const Eigen::Matrix<double, 1, Cols> & dFdT,
-      JacobianMap& jacobians) const {
+  void _reverseAD3(const Eigen::Matrix<double, 1, Cols> & dFdT,
+      JacobianMap& jacobians) const override {
     derived().reverseAD4(dFdT, jacobians);
   }
-  virtual void _reverseAD3(const Eigen::Matrix<double, 2, Cols> & dFdT,
-      JacobianMap& jacobians) const {
+  void _reverseAD3(const Eigen::Matrix<double, 2, Cols> & dFdT,
+      JacobianMap& jacobians) const override {
     derived().reverseAD4(dFdT, jacobians);
   }
-  virtual void _reverseAD3(const Eigen::Matrix<double, 3, Cols> & dFdT,
-      JacobianMap& jacobians) const {
+  void _reverseAD3(const Eigen::Matrix<double, 3, Cols> & dFdT,
+      JacobianMap& jacobians) const override {
     derived().reverseAD4(dFdT, jacobians);
   }
-  virtual void _reverseAD3(const Eigen::Matrix<double, 4, Cols> & dFdT,
-      JacobianMap& jacobians) const {
+  void _reverseAD3(const Eigen::Matrix<double, 4, Cols> & dFdT,
+      JacobianMap& jacobians) const override {
     derived().reverseAD4(dFdT, jacobians);
   }
-  virtual void _reverseAD3(const Eigen::Matrix<double, 5, Cols> & dFdT,
-      JacobianMap& jacobians) const {
+  void _reverseAD3(const Eigen::Matrix<double, 5, Cols> & dFdT,
+      JacobianMap& jacobians) const override {
     derived().reverseAD4(dFdT, jacobians);
   }
 };
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 0ae37f130..ee3dc8929 100644
--- a/gtsam/nonlinear/internal/ExpressionNode.h
+++ b/gtsam/nonlinear/internal/ExpressionNode.h
@@ -135,18 +135,18 @@ public:
   }
 
   /// Print
-  virtual void print(const std::string& indent = "") const {
+  void print(const std::string& indent = "") const override {
     std::cout << indent << "Constant" << std::endl;
   }
 
   /// Return value
-  virtual T value(const Values& values) const {
+  T value(const Values& values) const override {
     return constant_;
   }
 
   /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* traceStorage) const {
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+      ExecutionTraceStorage* traceStorage) const override {
     return constant_;
   }
 
@@ -176,30 +176,30 @@ public:
   }
 
   /// Print
-  virtual void print(const std::string& indent = "") const {
+  void print(const std::string& indent = "") const override {
     std::cout << indent << "Leaf, key = " << DefaultKeyFormatter(key_) << std::endl;
   }
 
   /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
+  std::set<Key> keys() const override {
     std::set<Key> keys;
     keys.insert(key_);
     return keys;
   }
 
   /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
+  void dims(std::map<Key, int>& map) const override {
     map[key_] = traits<T>::dimension;
   }
 
   /// Return value
-  virtual T value(const Values& values) const {
+  T value(const Values& values) const override {
     return values.at<T>(key_);
   }
 
   /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* traceStorage) const {
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+      ExecutionTraceStorage* traceStorage) const override {
     trace.setLeaf(key_);
     return values.at<T>(key_);
   }
@@ -248,23 +248,23 @@ public:
   }
 
   /// Print
-  virtual void print(const std::string& indent = "") const {
+  void print(const std::string& indent = "") const override {
     std::cout << indent << "UnaryExpression" << std::endl;
     expression1_->print(indent + "  ");
   }
 
   /// Return value
-  virtual T value(const Values& values) const {
+  T value(const Values& values) const override {
     return function_(expression1_->value(values), boost::none);
   }
 
   /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
+  std::set<Key> keys() const override {
     return expression1_->keys();
   }
 
   /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
+  void dims(std::map<Key, int>& map) const override {
     expression1_->dims(map);
   }
 
@@ -307,8 +307,8 @@ public:
   };
 
   /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* ptr) const {
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+      ExecutionTraceStorage* ptr) const override {
     assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
 
     // Create a Record in the memory pointed to by ptr
@@ -357,21 +357,21 @@ public:
   }
 
   /// Print
-  virtual void print(const std::string& indent = "") const {
+  void print(const std::string& indent = "") const override {
     std::cout << indent << "BinaryExpression" << std::endl;
     expression1_->print(indent + "  ");
     expression2_->print(indent + "  ");
   }
 
   /// Return value
-  virtual T value(const Values& values) const {
+  T value(const Values& values) const override {
     using boost::none;
     return function_(expression1_->value(values), expression2_->value(values),
         none, none);
   }
 
   /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
+  std::set<Key> keys() const override {
     std::set<Key> keys = expression1_->keys();
     std::set<Key> myKeys = expression2_->keys();
     keys.insert(myKeys.begin(), myKeys.end());
@@ -379,7 +379,7 @@ public:
   }
 
   /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
+  void dims(std::map<Key, int>& map) const override {
     expression1_->dims(map);
     expression2_->dims(map);
   }
@@ -426,8 +426,8 @@ public:
   };
 
   /// Construct an execution trace for reverse AD, see UnaryExpression for explanation
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* ptr) const {
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+      ExecutionTraceStorage* ptr) const override {
     assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
     Record* record = new (ptr) Record(values, *expression1_, *expression2_, ptr);
     trace.setFunction(record);
@@ -464,7 +464,7 @@ public:
   }
 
   /// Print
-  virtual void print(const std::string& indent = "") const {
+  void print(const std::string& indent = "") const override {
     std::cout << indent << "TernaryExpression" << std::endl;
     expression1_->print(indent + "  ");
     expression2_->print(indent + "  ");
@@ -472,14 +472,14 @@ public:
   }
 
   /// Return value
-  virtual T value(const Values& values) const {
+  T value(const Values& values) const override {
     using boost::none;
     return function_(expression1_->value(values), expression2_->value(values),
         expression3_->value(values), none, none, none);
   }
 
   /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
+  std::set<Key> keys() const override {
     std::set<Key> keys = expression1_->keys();
     std::set<Key> myKeys = expression2_->keys();
     keys.insert(myKeys.begin(), myKeys.end());
@@ -489,7 +489,7 @@ public:
   }
 
   /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
+  void dims(std::map<Key, int>& map) const override {
     expression1_->dims(map);
     expression2_->dims(map);
     expression3_->dims(map);
@@ -544,8 +544,8 @@ public:
   };
 
   /// Construct an execution trace for reverse AD, see UnaryExpression for explanation
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-                           ExecutionTraceStorage* ptr) const {
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+                           ExecutionTraceStorage* ptr) const override {
     assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
     Record* record = new (ptr) Record(values, *expression1_, *expression2_, *expression3_, ptr);
     trace.setFunction(record);
@@ -574,23 +574,23 @@ class ScalarMultiplyNode : public ExpressionNode<T> {
   virtual ~ScalarMultiplyNode() {}
 
   /// Print
-  virtual void print(const std::string& indent = "") const {
+  void print(const std::string& indent = "") const override {
     std::cout << indent << "ScalarMultiplyNode" << std::endl;
     expression_->print(indent + "  ");
   }
 
   /// Return value
-  virtual T value(const Values& values) const {
+  T value(const Values& values) const override {
     return scalar_ * expression_->value(values);
   }
 
   /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
+  std::set<Key> keys() const override {
     return expression_->keys();
   }
 
   /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
+  void dims(std::map<Key, int>& map) const override {
     expression_->dims(map);
   }
 
@@ -624,8 +624,8 @@ class ScalarMultiplyNode : public ExpressionNode<T> {
   };
 
   /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-                           ExecutionTraceStorage* ptr) const {
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+                           ExecutionTraceStorage* ptr) const override {
     assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
     Record* record = new (ptr) Record();
     ptr += upAligned(sizeof(Record));
@@ -662,19 +662,19 @@ class BinarySumNode : public ExpressionNode<T> {
   virtual ~BinarySumNode() {}
 
   /// Print
-  virtual void print(const std::string& indent = "") const {
+  void print(const std::string& indent = "") const override {
     std::cout << indent << "BinarySumNode" << std::endl;
     expression1_->print(indent + "  ");
     expression2_->print(indent + "  ");
   }
 
   /// Return value
-  virtual T value(const Values& values) const {
+  T value(const Values& values) const override {
     return expression1_->value(values) + expression2_->value(values);
   }
 
   /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
+  std::set<Key> keys() const override {
     std::set<Key> keys = expression1_->keys();
     std::set<Key> myKeys = expression2_->keys();
     keys.insert(myKeys.begin(), myKeys.end());
@@ -682,7 +682,7 @@ class BinarySumNode : public ExpressionNode<T> {
   }
 
   /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
+  void dims(std::map<Key, int>& map) const override {
     expression1_->dims(map);
     expression2_->dims(map);
   }
@@ -717,8 +717,8 @@ class BinarySumNode : public ExpressionNode<T> {
   };
 
   /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-                           ExecutionTraceStorage* ptr) const {
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+                           ExecutionTraceStorage* ptr) const override {
     assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
     Record* record = new (ptr) Record();
     trace.setFunction(record);
diff --git a/gtsam/nonlinear/nonlinearExceptions.h b/gtsam/nonlinear/nonlinearExceptions.h
index 8b32b6fdc..7b704ac39 100644
--- a/gtsam/nonlinear/nonlinearExceptions.h
+++ b/gtsam/nonlinear/nonlinearExceptions.h
@@ -35,11 +35,11 @@ namespace gtsam {
     KeyFormatter formatter_;
     mutable std::string what_;
   public:
-    MarginalizeNonleafException(Key key, KeyFormatter formatter = DefaultKeyFormatter) throw() :
+    MarginalizeNonleafException(Key key, KeyFormatter formatter = DefaultKeyFormatter) noexcept :
       key_(key), formatter_(formatter) {}
-    virtual ~MarginalizeNonleafException() throw() {}
+    virtual ~MarginalizeNonleafException() noexcept {}
     Key key() const { return key_; }
-    virtual const char* what() const throw() {
+    const char* what() const noexcept override {
       if(what_.empty())
         what_ =
 "\nRequested to marginalize out variable " + formatter_(key_) + ", but this variable\n\
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/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/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h
index a9ed5ef4b..530fcfdcd 100644
--- a/gtsam/sam/BearingFactor.h
+++ b/gtsam/sam/BearingFactor.h
@@ -48,7 +48,7 @@ struct BearingFactor : public ExpressionFactor2<T, A1, A2> {
   }
 
   // Return measurement expression
-  virtual Expression<T> expression(Key key1, Key key2) const {
+  Expression<T> expression(Key key1, Key key2) const override {
     Expression<A1> a1_(key1);
     Expression<A2> a2_(key2);
     return Expression<T>(Bearing<A1, A2>(), a1_, a2_);
@@ -56,7 +56,7 @@ struct BearingFactor : public ExpressionFactor2<T, A1, A2> {
 
   /// print
   void print(const std::string& s = "",
-             const KeyFormatter& kf = DefaultKeyFormatter) const {
+             const KeyFormatter& kf = DefaultKeyFormatter) const override {
     std::cout << s << "BearingFactor" << std::endl;
     Base::print(s, kf);
   }
diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h
index 44740f8ff..be645ef94 100644
--- a/gtsam/sam/BearingRangeFactor.h
+++ b/gtsam/sam/BearingRangeFactor.h
@@ -55,20 +55,20 @@ class BearingRangeFactor
   virtual ~BearingRangeFactor() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   // Return measurement expression
-  virtual Expression<T> expression(Key key1, Key key2) const {
+  Expression<T> expression(Key key1, Key key2) const override {
     return Expression<T>(T::Measure, Expression<A1>(key1),
                          Expression<A2>(key2));
   }
 
   /// print
-  virtual void print(const std::string& s = "",
-                     const KeyFormatter& kf = DefaultKeyFormatter) const {
+  void print(const std::string& s = "",
+                     const KeyFormatter& kf = DefaultKeyFormatter) const override {
     std::cout << s << "BearingRangeFactor" << std::endl;
     Base::print(s, kf);
   }
diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h
index 40a9cf758..80b02404e 100644
--- a/gtsam/sam/RangeFactor.h
+++ b/gtsam/sam/RangeFactor.h
@@ -47,13 +47,13 @@ class RangeFactor : public ExpressionFactor2<T, A1, A2> {
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   // Return measurement expression
-  virtual Expression<T> expression(Key key1, Key key2) const {
+  Expression<T> expression(Key key1, Key key2) const override {
     Expression<A1> a1_(key1);
     Expression<A2> a2_(key2);
     return Expression<T>(Range<A1, A2>(), a1_, a2_);
@@ -61,7 +61,7 @@ class RangeFactor : public ExpressionFactor2<T, A1, A2> {
 
   /// print
   void print(const std::string& s = "",
-             const KeyFormatter& kf = DefaultKeyFormatter) const {
+             const KeyFormatter& kf = DefaultKeyFormatter) const override {
     std::cout << s << "RangeFactor" << std::endl;
     Base::print(s, kf);
   }
@@ -107,13 +107,13 @@ class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
   virtual ~RangeFactorWithTransform() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   // Return measurement expression
-  virtual Expression<T> expression(Key key1, Key key2) const {
+  Expression<T> expression(Key key1, Key key2) const override {
     Expression<A1> body_T_sensor__(body_T_sensor_);
     Expression<A1> nav_T_body_(key1);
     Expression<A1> nav_T_sensor_(traits<A1>::Compose, nav_T_body_,
@@ -124,7 +124,7 @@ class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
 
   /** print contents */
   void print(const std::string& s = "",
-             const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+             const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "RangeFactorWithTransform" << std::endl;
     this->body_T_sensor_.print("  sensor pose in body frame: ");
     Base::print(s, keyFormatter);
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 9e4f7db5a..277080b6b 100644
--- a/gtsam/slam/AntiFactor.h
+++ b/gtsam/slam/AntiFactor.h
@@ -52,20 +52,20 @@ namespace gtsam {
     virtual ~AntiFactor() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       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 {
+    void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "AntiFactor version of:" << std::endl;
       factor_->print(s, keyFormatter);
     }
 
     /** equals */
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
       const This *e =  dynamic_cast<const This*> (&expected);
       return e != nullptr && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol);
     }
@@ -77,16 +77,16 @@ namespace gtsam {
      * For the AntiFactor, this will have the same magnitude of the original factor,
      * but the opposite sign.
      */
-    double error(const Values& c) const { return -factor_->error(c); }
+    double error(const Values& c) const override { return -factor_->error(c); }
 
     /** get the dimension of the factor (same as the original factor) */
-    size_t dim() const { return factor_->dim(); }
+    size_t dim() const override { return factor_->dim(); }
 
     /**
      * Checks whether this factor should be used based on a set of values.
      * The AntiFactor will have the same 'active' profile as the original factor.
      */
-    bool active(const Values& c) const { return factor_->active(c); }
+    bool active(const Values& c) const override { return factor_->active(c); }
 
     /**
      * Linearize to a GaussianFactor. The AntiFactor always returns a Hessian Factor
@@ -94,7 +94,7 @@ namespace gtsam {
      * returns a Jacobian instead of a full Hessian), but with the opposite sign. This
      * effectively cancels the effect of the original factor on the factor graph.
      */
-    boost::shared_ptr<GaussianFactor> linearize(const Values& c) const {
+    boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
 
       // Generate the linearized factor from the contained nonlinear factor
       GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c);
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index b1d4904aa..0f5aa6a4c 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -63,14 +63,14 @@ namespace gtsam {
     virtual ~BetweenFactor() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       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 {
+    void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "BetweenFactor("
           << keyFormatter(this->key1()) << ","
           << keyFormatter(this->key2()) << ")\n";
@@ -79,7 +79,7 @@ namespace gtsam {
     }
 
     /** equals */
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
       const This *e =  dynamic_cast<const This*> (&expected);
       return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
     }
@@ -87,8 +87,8 @@ namespace gtsam {
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
-  Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
-      boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+    Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
+      boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
       T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
       // manifold equivalent of h(x)-z -> log(z,h(x))
 #ifdef SLOW_BUT_CORRECT_BETWEENFACTOR
diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h
index f6561807e..43cc6d292 100644
--- a/gtsam/slam/BoundingConstraint.h
+++ b/gtsam/slam/BoundingConstraint.h
@@ -58,14 +58,14 @@ struct BoundingConstraint1: public NoiseModelFactor1<VALUE> {
       boost::none) const = 0;
 
   /** active when constraint *NOT* met */
-  bool active(const Values& c) const {
+  bool active(const Values& c) const override {
     // note: still active at equality to avoid zigzagging
     double x = value(c.at<X>(this->key()));
     return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
   }
 
   Vector evaluateError(const X& x, boost::optional<Matrix&> H =
-      boost::none) const {
+      boost::none) const override {
     Matrix D;
     double error = value(x, D) - threshold_;
     if (H) {
@@ -126,7 +126,7 @@ struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
       boost::optional<Matrix&> H2 = boost::none) const = 0;
 
   /** active when constraint *NOT* met */
-  bool active(const Values& c) const {
+  bool active(const Values& c) const override {
     // note: still active at equality to avoid zigzagging
     double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
     return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
@@ -134,7 +134,7 @@ struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
 
   Vector evaluateError(const X1& x1, const X2& x2,
       boost::optional<Matrix&> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const {
+      boost::optional<Matrix&> H2 = boost::none) const override {
     Matrix D1, D2;
     double error = value(x1, x2, D1, D2) - threshold_;
     if (H1) {
diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h
index e474ce5b3..943db7207 100644
--- a/gtsam/slam/EssentialMatrixConstraint.h
+++ b/gtsam/slam/EssentialMatrixConstraint.h
@@ -61,7 +61,7 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
@@ -69,18 +69,18 @@ public:
   /** implement functions needed for Testable */
 
   /** print */
-  virtual void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+  void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /** equals */
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /** implement functions needed to derive from Factor */
 
   /** vector of errors */
-  virtual Vector evaluateError(const Pose3& p1, const Pose3& p2,
+  Vector evaluateError(const Pose3& p1, const Pose3& p2,
       boost::optional<Matrix&> Hp1 = boost::none, //
-      boost::optional<Matrix&> Hp2 = boost::none) const;
+      boost::optional<Matrix&> Hp2 = boost::none) const override;
 
   /** return the measured */
   const EssentialMatrix& measured() const {
diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index c214a2f48..a99ac9512 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -58,14 +58,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /// print
-  virtual void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s);
     std::cout << "  EssentialMatrixFactor with measurements\n  ("
         << vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
@@ -74,7 +74,7 @@ public:
 
   /// vector of errors returns 1D vector
   Vector evaluateError(const EssentialMatrix& E, boost::optional<Matrix&> H =
-      boost::none) const {
+      boost::none) const override {
     Vector error(1);
     error << E.error(vA_, vB_, H);
     return error;
@@ -131,14 +131,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /// print
-  virtual void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s);
     std::cout << "  EssentialMatrixFactor2 with measurements\n  ("
         << dP1_.transpose() << ")' and (" << pn_.transpose()
@@ -152,7 +152,7 @@ public:
    */
   Vector evaluateError(const EssentialMatrix& E, const double& d,
       boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
-          boost::none) const {
+          boost::none) const override {
 
     // We have point x,y in image 1
     // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
@@ -250,14 +250,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /// print
-  virtual void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s);
     std::cout << "  EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
   }
@@ -269,7 +269,7 @@ public:
    */
   Vector evaluateError(const EssentialMatrix& E, const double& d,
       boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
-          boost::none) const {
+          boost::none) const override {
     if (!DE) {
       // Convert E from body to camera frame
       EssentialMatrix cameraE = cRb_ * E;
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..6b2ef67fc
--- /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 override {
+    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 override {
+    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 override {
+    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 override;
+};
+
+}  // namespace gtsam
diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h
index 0eb489f35..f848a56ca 100644
--- a/gtsam/slam/GeneralSFMFactor.h
+++ b/gtsam/slam/GeneralSFMFactor.h
@@ -96,7 +96,7 @@ public:
   virtual ~GeneralSFMFactor() {} ///< destructor
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
 
@@ -105,7 +105,7 @@ public:
    * @param s optional string naming the factor
    * @param keyFormatter optional formatter for printing out Symbols
    */
-  void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     traits<Point2>::Print(measured_, s + ".z");
   }
@@ -113,14 +113,14 @@ public:
   /**
    * equals
    */
-  bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
     const This* e = dynamic_cast<const This*>(&p);
     return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
   }
 
   /** h(x)-z */
   Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
-      boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
+      boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const override {
     try {
       return camera.project2(point,H1,H2) - measured_;
     }
@@ -133,7 +133,7 @@ public:
   }
 
   /// Linearize using fixed-size matrices
-  boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
+  boost::shared_ptr<GaussianFactor> linearize(const Values& values) const override {
     // Only linearize if the factor is active
     if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
 
@@ -230,7 +230,7 @@ public:
   virtual ~GeneralSFMFactor2() {} ///< destructor
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
 
@@ -239,7 +239,7 @@ public:
    * @param s optional string naming the factor
    * @param keyFormatter optional formatter useful for printing Symbols
    */
-  void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     traits<Point2>::Print(measured_, s + ".z");
   }
@@ -247,7 +247,7 @@ public:
   /**
    * equals
    */
-  bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
     const This* e = dynamic_cast<const This*>(&p);
     return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
   }
@@ -256,7 +256,7 @@ public:
   Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
       boost::optional<Matrix&> H1=boost::none,
       boost::optional<Matrix&> H2=boost::none,
-      boost::optional<Matrix&> H3=boost::none) const
+      boost::optional<Matrix&> H3=boost::none) const override
   {
     try {
       Camera camera(pose3,calib);
diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h
index 1f56adc45..e83464b1e 100644
--- a/gtsam/slam/OrientedPlane3Factor.h
+++ b/gtsam/slam/OrientedPlane3Factor.h
@@ -41,13 +41,13 @@ public:
   }
 
   /// print
-  virtual void print(const std::string& s = "OrientedPlane3Factor",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+  void print(const std::string& s = "OrientedPlane3Factor",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /// evaluateError
-  virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
+  Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
-          boost::none) const {
+          boost::none) const override {
     OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1,
         H2);
     Vector err(3);
@@ -78,14 +78,14 @@ public:
   }
 
   /// print
-  virtual void print(const std::string& s = "OrientedPlane3DirectionPrior",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+  void print(const std::string& s = "OrientedPlane3DirectionPrior",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /// equals
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
-  virtual Vector evaluateError(const OrientedPlane3& plane,
-      boost::optional<Matrix&> H = boost::none) const;
+  Vector evaluateError(const OrientedPlane3& plane,
+      boost::optional<Matrix&> H = boost::none) const override;
 };
 
 } // gtsam
diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h
index f4ce1520a..7e8bdaa46 100644
--- a/gtsam/slam/PoseRotationPrior.h
+++ b/gtsam/slam/PoseRotationPrior.h
@@ -50,7 +50,7 @@ public:
   virtual ~PoseRotationPrior() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
@@ -60,19 +60,19 @@ public:
   // testable
 
   /** equals specialized to this factor */
-  virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
     const This *e = dynamic_cast<const This*> (&expected);
     return e != nullptr && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
   }
 
   /** print contents */
-  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s + "PoseRotationPrior", keyFormatter);
     measured_.print("Measured Rotation");
   }
 
   /** h(x)-z */
-  Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const {
+  Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
     const Rotation& newR = pose.rotation();
     if (H) {
       *H = Matrix::Zero(rDim, xDim);
diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h
index 516582d83..0c029c501 100644
--- a/gtsam/slam/PoseTranslationPrior.h
+++ b/gtsam/slam/PoseTranslationPrior.h
@@ -54,12 +54,12 @@ public:
   const Translation& measured() const { return measured_; }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
   /** h(x)-z */
-  Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const {
+  Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
     const Translation& newTrans = pose.translation();
     const Rotation& R = pose.rotation();
     const int tDim = traits<Translation>::GetDimension(newTrans);
@@ -74,13 +74,13 @@ public:
   }
 
   /** equals specialized to this factor */
-  virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
     const This *e = dynamic_cast<const This*> (&expected);
     return e != nullptr && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
   }
 
   /** print contents */
-  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s + "PoseTranslationPrior", keyFormatter);
     traits<Translation>::Print(measured_, "Measured Translation");
   }
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index 0bed15fdc..266037469 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -100,7 +100,7 @@ namespace gtsam {
     virtual ~GenericProjectionFactor() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       return boost::static_pointer_cast<gtsam::NonlinearFactor>(
           gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
@@ -109,7 +109,7 @@ namespace gtsam {
      * @param s optional string naming the factor
      * @param keyFormatter optional formatter useful for printing Symbols
      */
-    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "GenericProjectionFactor, z = ";
       traits<Point2>::Print(measured_);
       if(this->body_P_sensor_)
@@ -118,7 +118,7 @@ namespace gtsam {
     }
 
     /// equals
-    virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+    bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
       const This *e = dynamic_cast<const This*>(&p);
       return e
           && Base::equals(p, tol)
@@ -129,7 +129,7 @@ namespace gtsam {
 
     /// Evaluate error h(x)-z and optionally derivatives
     Vector evaluateError(const Pose3& pose, const Point3& point,
-        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
       try {
         if(body_P_sensor_) {
           if(H1) {
diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h
index 30f761934..e41b5f908 100644
--- a/gtsam/slam/ReferenceFrameFactor.h
+++ b/gtsam/slam/ReferenceFrameFactor.h
@@ -89,23 +89,23 @@ public:
 
   virtual ~ReferenceFrameFactor(){}
 
-  virtual NonlinearFactor::shared_ptr clone() const {
+  NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<NonlinearFactor>(
         NonlinearFactor::shared_ptr(new This(*this))); }
 
   /** Combined cost and derivative function using boost::optional */
-  virtual Vector evaluateError(const Point& global, const Transform& trans, const Point& local,
+  Vector evaluateError(const Point& global, const Transform& trans, const Point& local,
         boost::optional<Matrix&> Dforeign = boost::none,
         boost::optional<Matrix&> Dtrans = boost::none,
-        boost::optional<Matrix&> Dlocal = boost::none) const  {
+        boost::optional<Matrix&> Dlocal = boost::none) const override {
     Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
     if (Dlocal)
       *Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension);
     return traits<Point>::Local(local,newlocal);
   }
 
-  virtual void print(const std::string& s="",
-      const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s="",
+      const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << ": ReferenceFrameFactor("
         << "Global: " << keyFormatter(this->key1()) << ","
         << " Transform: " << keyFormatter(this->key2()) << ","
diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h
index 948fffe13..9e4091111 100644
--- a/gtsam/slam/RotateFactor.h
+++ b/gtsam/slam/RotateFactor.h
@@ -36,13 +36,13 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
   /// print
-  virtual void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s);
     std::cout << "RotateFactor:]\n";
     std::cout << "p: " << p_.transpose() << std::endl;
@@ -51,7 +51,7 @@ public:
 
   /// vector of errors returns 2D vector
   Vector evaluateError(const Rot3& R,
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> H = boost::none) const override {
     // predict p_ as q = R*z_, derivative H will be filled if not none
     Point3 q = R.rotate(z_,H);
     // error is just difference, and note derivative of that wrpt q is I3
@@ -88,13 +88,13 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
   /// print
-  virtual void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s);
     std::cout << "RotateDirectionsFactor:" << std::endl;
     i_p_.print("p");
@@ -102,7 +102,7 @@ public:
   }
 
   /// vector of errors returns 2D vector
-  Vector evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) const {
+  Vector evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) const override {
     Unit3 i_q = iRc * c_z_;
     Vector error = i_p_.error(i_q, H);
     if (H) {
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 717a9c1f2..d9f2b9c3d 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -150,7 +150,7 @@ protected:
   }
 
   /// get the dimension (number of rows!) of the factor
-  virtual size_t dim() const {
+  size_t dim() const override {
     return ZDim * this->measured_.size();
   }
 
@@ -173,7 +173,7 @@ protected:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const {
+      DefaultKeyFormatter) const override {
     std::cout << s << "SmartFactorBase, z = \n";
     for (size_t k = 0; k < measured_.size(); ++k) {
       std::cout << "measurement, p = " << measured_[k] << "\t";
@@ -185,7 +185,7 @@ protected:
   }
 
   /// equals
-  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
     const This *e = dynamic_cast<const This*>(&p);
 
     bool areMeasurementsEqual = true;
@@ -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/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h
index 15d632cda..a7d85dd08 100644
--- a/gtsam/slam/SmartProjectionFactor.h
+++ b/gtsam/slam/SmartProjectionFactor.h
@@ -99,7 +99,7 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const {
+      DefaultKeyFormatter) const override {
     std::cout << s << "SmartProjectionFactor\n";
     std::cout << "linearizationMode:\n" << params_.linearizationMode
         << std::endl;
@@ -110,7 +110,7 @@ public:
   }
 
   /// equals
-  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
     const This *e = dynamic_cast<const This*>(&p);
     return e && params_.linearizationMode == e->params_.linearizationMode
         && Base::equals(p, tol);
@@ -305,8 +305,8 @@ public:
   }
 
   /// linearize
-  virtual boost::shared_ptr<GaussianFactor> linearize(
-      const Values& values) const {
+  boost::shared_ptr<GaussianFactor> linearize(
+      const Values& values) const override {
     return linearizeDamped(values);
   }
 
@@ -409,7 +409,7 @@ public:
   }
 
   /// Calculate total reprojection error
-  virtual double error(const Values& values) const {
+  double error(const Values& values) const override {
     if (this->active(values)) {
       return totalReprojectionError(Base::cameras(values));
     } else { // else of active flag
diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h
index b5be46258..cccdf20d6 100644
--- a/gtsam/slam/SmartProjectionPoseFactor.h
+++ b/gtsam/slam/SmartProjectionPoseFactor.h
@@ -103,13 +103,13 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const {
+      DefaultKeyFormatter) const override {
     std::cout << s << "SmartProjectionPoseFactor, z = \n ";
     Base::print("", keyFormatter);
   }
 
   /// equals
-  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
     const This *e = dynamic_cast<const This*>(&p);
     return e && Base::equals(p, tol);
   }
@@ -117,7 +117,7 @@ public:
   /**
    * error calculates the error of the factor.
    */
-  virtual double error(const Values& values) const {
+  double error(const Values& values) const override {
     if (this->active(values)) {
       return this->totalReprojectionError(cameras(values));
     } else { // else of active flag
@@ -136,7 +136,7 @@ public:
    * to keys involved in this factor
    * @return vector of Values
    */
-  typename Base::Cameras cameras(const Values& values) const {
+  typename Base::Cameras cameras(const Values& values) const override {
     typename Base::Cameras cameras;
     for (const Key& k : this->keys_) {
       const Pose3 world_P_sensor_k =
diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
index 8828f5de7..6556723bd 100644
--- a/gtsam/slam/StereoFactor.h
+++ b/gtsam/slam/StereoFactor.h
@@ -91,7 +91,7 @@ public:
   virtual ~GenericStereoFactor() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
@@ -100,7 +100,7 @@ public:
    * @param s optional string naming the factor
    * @param keyFormatter optional formatter useful for printing Symbols
    */
-  void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     measured_.print(s + ".z");
     if(this->body_P_sensor_)
@@ -110,7 +110,7 @@ public:
   /**
    * equals
    */
-  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
     const GenericStereoFactor* e = dynamic_cast<const GenericStereoFactor*> (&f);
     return e
         && Base::equals(f)
@@ -120,7 +120,7 @@ public:
 
   /** h(x)-z */
   Vector evaluateError(const Pose3& pose, const Point3& point,
-      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
     try {
       if(body_P_sensor_) {
         if(H1) {
diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h
index 811d92fbc..9d02ad321 100644
--- a/gtsam/slam/TriangulationFactor.h
+++ b/gtsam/slam/TriangulationFactor.h
@@ -90,7 +90,7 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
@@ -101,7 +101,7 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const {
+      DefaultKeyFormatter) const override {
     std::cout << s << "TriangulationFactor,";
     camera_.print("camera");
     traits<Measurement>::Print(measured_, "z");
@@ -109,7 +109,7 @@ public:
   }
 
   /// equals
-  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
     const This *e = dynamic_cast<const This*>(&p);
     return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol)
         && traits<Measurement>::Equals(this->measured_, e->measured_, tol);
@@ -117,7 +117,7 @@ public:
 
   /// Evaluate error h(x)-z and optionally derivatives
   Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
-      boost::none) const {
+      boost::none) const override {
     try {
       return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2));
     } catch (CheiralityException& e) {
@@ -143,7 +143,7 @@ public:
    * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
    * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
    */
-  boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
+  boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
     // Only linearize if the factor is active
     if (!this->active(x))
       return boost::shared_ptr<JacobianFactor>();
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 66e8fc4c0..3a2d04127 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -11,7 +11,10 @@
 /**
  * @file dataset.cpp
  * @date Jan 22, 2010
- * @author Kai Ni, Luca Carlone, Frank Dellaert
+ * @author Kai Ni
+ * @author Luca Carlone
+ * @author Frank Dellaert
+ * @author Varun Agrawal
  * @brief utility functions for loading datasets
  */
 
@@ -70,8 +73,8 @@ string findExampleDataFile(const string& name) {
   namesToSearch.push_back(name + ".xml");
 
   // Find first name that exists
-  for(const fs::path& root: rootsToSearch) {
-    for(const fs::path& name: namesToSearch) {
+  for(const fs::path root: rootsToSearch) {
+    for(const fs::path name: namesToSearch) {
       if (fs::is_regular_file(root / name))
         return (root / name).string();
     }
@@ -192,8 +195,15 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
   }
 }
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
 /* ************************************************************************* */
 boost::optional<IndexedPose> parseVertex(istream& is, const string& tag) {
+  return parseVertexPose(is, tag);
+}
+#endif
+
+/* ************************************************************************* */
+boost::optional<IndexedPose> parseVertexPose(istream& is, const string& tag) {
   if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
     Key id;
     double x, y, yaw;
@@ -204,6 +214,18 @@ boost::optional<IndexedPose> parseVertex(istream& is, const string& tag) {
   }
 }
 
+/* ************************************************************************* */
+boost::optional<IndexedLandmark> parseVertexLandmark(istream& is, const string& tag) {
+  if (tag == "VERTEX_XY") {
+    Key id;
+    double x, y;
+    is >> id >> x >> y;
+    return IndexedLandmark(id, Point2(x, y));
+  } else {
+    return boost::none;
+  }
+}
+
 /* ************************************************************************* */
 boost::optional<IndexedEdge> parseEdge(istream& is, const string& tag) {
   if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
@@ -232,12 +254,12 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
 
   string tag;
 
-  // load the poses
+  // load the poses and landmarks
   while (!is.eof()) {
     if (!(is >> tag))
       break;
 
-    const auto indexed_pose = parseVertex(is, tag);
+    const auto indexed_pose = parseVertexPose(is, tag);
     if (indexed_pose) {
       Key id = indexed_pose->first;
 
@@ -247,6 +269,16 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
 
       initial->insert(id, indexed_pose->second);
     }
+    const auto indexed_landmark = parseVertexLandmark(is, tag);
+    if (indexed_landmark) {
+      Key id = indexed_landmark->first;
+
+      // optional filter
+      if (maxID && id >= maxID)
+        continue;
+
+      initial->insert(id, indexed_landmark->second);
+    }
     is.ignore(LINESIZE, '\n');
   }
   is.clear(); /* clears the end-of-file and error flags */
@@ -429,7 +461,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
     const string& filename) {
   fstream stream(filename.c_str(), fstream::out);
 
-  // save 2D & 3D poses
+  // save 2D poses
   for (const auto& key_value : estimate) {
     auto p = dynamic_cast<const GenericValue<Pose2>*>(&key_value.value);
     if (!p) continue;
@@ -438,15 +470,34 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
         << pose.y() << " " << pose.theta() << endl;
   }
 
+  // save 3D poses
   for(const auto& key_value: 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;
+    auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
+    if (!p) continue;
+    const Pose3& pose = p->value();
+    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 2D landmarks
+  for(const auto& key_value: estimate) {
+    auto p = dynamic_cast<const GenericValue<Point2>*>(&key_value.value);
+    if (!p) continue;
+    const Point2& point = p->value();
+    stream << "VERTEX_XY " << key_value.key << " " << point.x() << " "
+        << point.y() << endl;
+  }
+
+  // save 3D landmarks
+  for(const auto& key_value: estimate) {
+    auto p = dynamic_cast<const GenericValue<Point3>*>(&key_value.value);
+    if (!p) continue;
+    const Point3& point = p->value();
+    stream << "VERTEX_TRACKXYZ " << key_value.key << " " << point.x() << " "
+        << point.y() << " " << point.z() << endl;
   }
 
   // save edges (2D or 3D)
@@ -486,13 +537,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
@@ -511,6 +561,12 @@ 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());
@@ -535,14 +591,39 @@ 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, 
+std::map<Key, Point3> parse3DLandmarks(const string& filename) {
+  ifstream is(filename.c_str());
+  if (!is)
+    throw invalid_argument("parse3DLandmarks: can not find file " + filename);
+
+  std::map<Key, Point3> landmarks;
+  while (!is.eof()) {
+    char buf[LINESIZE];
+    is.getline(buf, LINESIZE);
+    istringstream ls(buf);
+    string tag;
+    ls >> tag;
+
+    if (tag == "VERTEX_TRACKXYZ") {
+      Key id;
+      double x, y, z;
+      ls >> id >> x >> y >> z;
+      landmarks.emplace(id, Point3(x, y, z));
+    }
+  }
+  return landmarks;
+}
+
+/* ************************************************************************* */
+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);
@@ -592,7 +673,7 @@ 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 = Rot3::Quaternion(qw, qx, qy, qz);
+      auto R12 = NormalizedRot3(qw, qx, qy, qz);
       if (sampler) {
         R12 = R12.retract(sampler->sample());
       }
@@ -612,11 +693,16 @@ GraphAndValues load3D(const string& filename) {
     graph->push_back(factor);
   }
 
-  const auto poses = parse3DPoses(filename);
   Values::shared_ptr initial(new Values);
+
+  const auto poses = parse3DPoses(filename);
   for (const auto& key_pose : poses) {
     initial->insert(key_pose.first, key_pose.second);
   }
+  const auto landmarks = parse3DLandmarks(filename);
+  for (const auto& key_landmark : landmarks) {
+    initial->insert(key_landmark.first, key_landmark.second);
+  }
 
   return make_pair(graph, initial);
 }
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index 032799429..7029a7a9c 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -76,8 +76,10 @@ enum KernelFunctionType {
 
 /// Return type for auxiliary functions
 typedef std::pair<Key, Pose2> IndexedPose;
+typedef std::pair<Key, Point2> IndexedLandmark;
 typedef std::pair<std::pair<Key, Key>, Pose2> IndexedEdge;
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
 /**
  * Parse TORO/G2O vertex "id x y yaw"
  * @param is input stream
@@ -85,6 +87,24 @@ typedef std::pair<std::pair<Key, Key>, Pose2> IndexedEdge;
  */
 GTSAM_EXPORT boost::optional<IndexedPose> parseVertex(std::istream& is,
     const std::string& tag);
+#endif
+
+/**
+ * Parse TORO/G2O vertex "id x y yaw"
+ * @param is input stream
+ * @param tag string parsed from input stream, will only parse if vertex type
+ */
+GTSAM_EXPORT boost::optional<IndexedPose> parseVertexPose(std::istream& is,
+    const std::string& tag);
+
+/**
+ * Parse G2O landmark vertex "id x y"
+ * @param is input stream
+ * @param tag string parsed from input stream, will only parse if vertex type
+ */
+
+GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream& is,
+    const std::string& tag);
 
 /**
  * Parse TORO/G2O edge "id1 id2 x y yaw"
@@ -162,9 +182,12 @@ using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
 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.
+/// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s.
 GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
 
+/// Parse landmarks in 3D g2o graph file into a map of Point3s.
+GTSAM_EXPORT std::map<Key, Point3> parse3DLandmarks(const string& filename);
+
 /// Load TORO 3D Graph
 GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
 
diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp
index 9a3c797b2..b6b1776d2 100644
--- a/gtsam/slam/tests/testDataset.cpp
+++ b/gtsam/slam/tests/testDataset.cpp
@@ -43,13 +43,13 @@ TEST(dataSet, findExampleDataFile) {
 }
 
 /* ************************************************************************* */
-TEST( dataSet, parseVertex)
+TEST( dataSet, parseVertexPose)
 {
   const string str = "VERTEX2 1 2.000000 3.000000 4.000000";
   istringstream is(str);
   string tag;
   EXPECT(is >> tag);
-  const auto actual = parseVertex(is, tag);
+  const auto actual = parseVertexPose(is, tag);
   EXPECT(actual);
   if (actual) {
     EXPECT_LONGS_EQUAL(1, actual->first);
@@ -57,6 +57,21 @@ TEST( dataSet, parseVertex)
   }
 }
 
+/* ************************************************************************* */
+TEST( dataSet, parseVertexLandmark)
+{
+  const string str = "VERTEX_XY 1 2.000000 3.000000";
+  istringstream is(str);
+  string tag;
+  EXPECT(is >> tag);
+  const auto actual = parseVertexLandmark(is, tag);
+  EXPECT(actual);
+  if (actual) {
+    EXPECT_LONGS_EQUAL(1, actual->first);
+    EXPECT(assert_equal(Point2(2, 3), actual->second));
+  }
+}
+
 /* ************************************************************************* */
 TEST( dataSet, parseEdge)
 {
@@ -122,45 +137,6 @@ TEST( dataSet, Balbianello)
   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");
@@ -221,6 +197,12 @@ TEST(dataSet, readG2o3D) {
     EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
   }
 
+  // Check landmark parsing
+  const auto actualLandmarks = parse3DLandmarks(g2oFile);
+  for (size_t j : {0, 1, 2, 3, 4}) {
+    EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
+  }
+
   // Check graph version
   NonlinearFactorGraph::shared_ptr actualGraph;
   Values::shared_ptr actualValues;
@@ -273,59 +255,116 @@ 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);
+  }
+  const map<Key, Point3> landmarks = parse3DLandmarks(g2oFile);
+  EXPECT_LONGS_EQUAL(0, landmarks.size());
 }
 
 /* ************************************************************************* */
-TEST( dataSet, readG2oTukey)
-{
+TEST(dataSet, readG2oLandmarks) {
+  const string g2oFile = findExampleDataFile("example_with_vertices.g2o");
+
+  // Check number of poses and landmarks. Should be 8 each.
+  const map<Key, Pose3> poses = parse3DPoses(g2oFile);
+  EXPECT_LONGS_EQUAL(8, poses.size());
+  const map<Key, Point3> landmarks = parse3DLandmarks(g2oFile);
+  EXPECT_LONGS_EQUAL(8, landmarks.size());
+}
+
+/* ************************************************************************* */
+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));
 }
 
 /* ************************************************************************* */
@@ -495,7 +534,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
   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
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/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp
index f69f4c113..951cbf8f4 100644
--- a/gtsam/slam/tests/testSmartFactorBase.cpp
+++ b/gtsam/slam/tests/testSmartFactorBase.cpp
@@ -37,13 +37,13 @@ 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;
-  }
-  virtual boost::shared_ptr<GaussianFactor> linearize(
-      const Values& values) const {
+  PinholeFactor(const SharedNoiseModel& sharedNoiseModel,
+                boost::optional<Pose3> body_P_sensor = boost::none,
+                size_t expectedNumberCameras = 10)
+      : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {}
+  double error(const Values& values) const override { return 0.0; }
+  boost::shared_ptr<GaussianFactor> linearize(
+      const Values& values) const override {
     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>
 
@@ -71,11 +105,11 @@ public:
   StereoFactor() {}
   StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
   }
-  virtual double error(const Values& values) const {
+  double error(const Values& values) const override {
     return 0.0;
   }
-  virtual boost::shared_ptr<GaussianFactor> linearize(
-      const Values& values) const {
+  boost::shared_ptr<GaussianFactor> linearize(
+      const Values& values) const override {
     return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
   }
 };
diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h
index e28f28764..3fdf1011b 100644
--- a/gtsam/symbolic/SymbolicBayesTree.h
+++ b/gtsam/symbolic/SymbolicBayesTree.h
@@ -39,6 +39,7 @@ namespace gtsam {
     typedef boost::shared_ptr<This> shared_ptr;
     typedef boost::weak_ptr<This> weak_ptr;
     SymbolicBayesTreeClique() {}
+    virtual ~SymbolicBayesTreeClique() {}
     SymbolicBayesTreeClique(const boost::shared_ptr<SymbolicConditional>& conditional) : Base(conditional) {}
   };
 
diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h
index 0530af556..9163cdba6 100644
--- a/gtsam/symbolic/SymbolicConditional.h
+++ b/gtsam/symbolic/SymbolicConditional.h
@@ -105,7 +105,7 @@ namespace gtsam {
     /// @name Testable
 
     /** Print with optional formatter */
-    void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+    virtual void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
     /** Check equality */
     bool equals(const This& c, double tol = 1e-9) const;
diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h
index 3e7296593..80e700b29 100644
--- a/gtsam_unstable/discrete/AllDiff.h
+++ b/gtsam_unstable/discrete/AllDiff.h
@@ -34,11 +34,11 @@ namespace gtsam {
     AllDiff(const DiscreteKeys& dkeys);
 
     // print
-    virtual void print(const std::string& s = "",
-        const KeyFormatter& formatter = DefaultKeyFormatter) const;
+    void print(const std::string& s = "",
+        const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
     /// equals
-    bool equals(const DiscreteFactor& other, double tol) const {
+    bool equals(const DiscreteFactor& other, double tol) const override {
       if(!dynamic_cast<const AllDiff*>(&other))
         return false;
       else {
@@ -50,13 +50,13 @@ namespace gtsam {
     }
 
     /// Calculate value = expensive !
-    virtual double operator()(const Values& values) const;
+    double operator()(const Values& values) const override;
 
     /// Convert into a decisiontree, can be *very* expensive !
-    virtual DecisionTreeFactor toDecisionTreeFactor() const;
+    DecisionTreeFactor toDecisionTreeFactor() const override;
 
     /// Multiply into a decisiontree
-    virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const;
+    DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
 
     /*
      * Ensure Arc-consistency
@@ -65,13 +65,13 @@ namespace gtsam {
      * @param j domain to be checked
      * @param domains all other domains
      */
-    bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const;
+    bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const override;
 
     /// Partially apply known values
-    virtual Constraint::shared_ptr partiallyApply(const Values&) const;
+    Constraint::shared_ptr partiallyApply(const Values&) const override;
 
     /// Partially apply known values, domain version
-    virtual Constraint::shared_ptr partiallyApply(const std::vector<Domain>&) const;
+    Constraint::shared_ptr partiallyApply(const std::vector<Domain>&) const override;
   };
 
 } // namespace gtsam
diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h
index 1ae5a008a..bbb60e2f1 100644
--- a/gtsam_unstable/discrete/BinaryAllDiff.h
+++ b/gtsam_unstable/discrete/BinaryAllDiff.h
@@ -33,14 +33,14 @@ namespace gtsam {
     }
 
     // print
-    virtual void print(const std::string& s = "",
-        const KeyFormatter& formatter = DefaultKeyFormatter) const {
+    void print(const std::string& s = "",
+        const KeyFormatter& formatter = DefaultKeyFormatter) const override {
       std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and "
           << formatter(keys_[1]) << std::endl;
     }
 
     /// equals
-    bool equals(const DiscreteFactor& other, double tol) const {
+    bool equals(const DiscreteFactor& other, double tol) const override {
       if(!dynamic_cast<const BinaryAllDiff*>(&other))
         return false;
       else {
@@ -50,12 +50,12 @@ namespace gtsam {
     }
 
     /// Calculate value
-    virtual double operator()(const Values& values) const {
+    double operator()(const Values& values) const override {
       return (double) (values.at(keys_[0]) != values.at(keys_[1]));
     }
 
     /// Convert into a decisiontree
-    virtual DecisionTreeFactor toDecisionTreeFactor() const {
+    DecisionTreeFactor toDecisionTreeFactor() const override {
       DiscreteKeys keys;
       keys.push_back(DiscreteKey(keys_[0],cardinality0_));
       keys.push_back(DiscreteKey(keys_[1],cardinality1_));
@@ -68,7 +68,7 @@ namespace gtsam {
     }
 
     /// Multiply into a decisiontree
-    virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const {
+    DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
       // TODO: can we do this more efficiently?
       return toDecisionTreeFactor() * f;
     }
@@ -79,20 +79,20 @@ namespace gtsam {
      * @param domains all other domains
      */
     ///
-    bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const {
+    bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const override {
 //      throw std::runtime_error(
 //          "BinaryAllDiff::ensureArcConsistency not implemented");
       return false;
     }
 
     /// Partially apply known values
-    virtual Constraint::shared_ptr partiallyApply(const Values&) const {
+    Constraint::shared_ptr partiallyApply(const Values&) const override {
       throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented");
     }
 
     /// Partially apply known values, domain version
-    virtual Constraint::shared_ptr partiallyApply(
-        const std::vector<Domain>&) const {
+    Constraint::shared_ptr partiallyApply(
+        const std::vector<Domain>&) const override {
       throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented");
     }
   };
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index 5dd597e20..5acc5a08f 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -66,11 +66,11 @@ namespace gtsam {
     }
 
     // print
-    virtual void print(const std::string& s = "",
-        const KeyFormatter& formatter = DefaultKeyFormatter) const;
+    void print(const std::string& s = "",
+        const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
     /// equals
-    bool equals(const DiscreteFactor& other, double tol) const {
+    bool equals(const DiscreteFactor& other, double tol) const override {
       if(!dynamic_cast<const Domain*>(&other))
         return false;
       else {
@@ -84,20 +84,20 @@ namespace gtsam {
     }
 
     /// Calculate value
-    virtual double operator()(const Values& values) const;
+    double operator()(const Values& values) const override;
 
     /// Convert into a decisiontree
-    virtual DecisionTreeFactor toDecisionTreeFactor() const;
+    DecisionTreeFactor toDecisionTreeFactor() const override;
 
     /// Multiply into a decisiontree
-    virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const;
+    DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
 
     /*
      * Ensure Arc-consistency
      * @param j domain to be checked
      * @param domains all other domains
      */
-    bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const;
+    bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const override;
 
     /**
      *  Check for a value in domain that does not occur in any other connected domain.
@@ -107,12 +107,11 @@ namespace gtsam {
     bool checkAllDiff(const KeyVector keys, std::vector<Domain>& domains);
 
     /// Partially apply known values
-    virtual Constraint::shared_ptr partiallyApply(
-        const Values& values) const;
+    Constraint::shared_ptr partiallyApply(const Values& values) const override;
 
     /// Partially apply known values, domain version
-    virtual Constraint::shared_ptr partiallyApply(
-        const std::vector<Domain>& domains) const;
+    Constraint::shared_ptr partiallyApply(
+        const std::vector<Domain>& domains) const override;
   };
 
 } // namespace gtsam
diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h
index 43f06956d..c4d2addec 100644
--- a/gtsam_unstable/discrete/SingleValue.h
+++ b/gtsam_unstable/discrete/SingleValue.h
@@ -42,11 +42,11 @@ namespace gtsam {
     }
 
     // print
-    virtual void print(const std::string& s = "",
-        const KeyFormatter& formatter = DefaultKeyFormatter) const;
+    void print(const std::string& s = "",
+        const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
     /// equals
-    bool equals(const DiscreteFactor& other, double tol) const {
+    bool equals(const DiscreteFactor& other, double tol) const override {
       if(!dynamic_cast<const SingleValue*>(&other))
         return false;
       else {
@@ -56,28 +56,27 @@ namespace gtsam {
     }
 
     /// Calculate value
-    virtual double operator()(const Values& values) const;
+    double operator()(const Values& values) const override;
 
     /// Convert into a decisiontree
-    virtual DecisionTreeFactor toDecisionTreeFactor() const;
+    DecisionTreeFactor toDecisionTreeFactor() const override;
 
     /// Multiply into a decisiontree
-    virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const;
+    DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
 
     /*
      * Ensure Arc-consistency
      * @param j domain to be checked
      * @param domains all other domains
      */
-    bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const;
+    bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const override;
 
     /// Partially apply known values
-    virtual Constraint::shared_ptr partiallyApply(
-        const Values& values) const;
+    Constraint::shared_ptr partiallyApply(const Values& values) const override;
 
     /// Partially apply known values, domain version
-    virtual Constraint::shared_ptr partiallyApply(
-        const std::vector<Domain>& domains) const;
+    Constraint::shared_ptr partiallyApply(
+        const std::vector<Domain>& domains) const override;
   };
 
 } // namespace gtsam
diff --git a/gtsam_unstable/discrete/tests/testPlanning.cpp b/gtsam_unstable/discrete/tests/testPlanning.cpp
deleted file mode 100644
index 431d7a4ef..000000000
--- a/gtsam_unstable/discrete/tests/testPlanning.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
- * testPlanning.cpp
- * @brief develop code for planning example
- * @date Feb 13, 2012
- * @author Frank Dellaert
- */
-
-//#include <gtsam/discrete/Planner.h>
-#include <gtsam/discrete/DiscreteKey.h>
-#include <CppUnitLite/TestHarness.h>
-#include <iostream>
-#include <fstream>
-
-using namespace std;
-using namespace gtsam;
-
-/* ************************************************************************* */
-TEST_UNSAFE(Planner, allInOne)
-{
-  // A small planning problem
-  // First variable is location
-  DiscreteKey location(0,3),
-      haveRock(1,2), haveSoil(2,2), haveImage(3,2),
-      commRock(4,2), commSoil(5,2), commImage(6,2);
-
-  // There are 3 actions:
-  // Drive, communicate, sample
-}
-
-
-/* ************************************************************************* */
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-/* ************************************************************************* */
-
diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp
index 0be4e4b1f..3f6c6a1e0 100644
--- a/gtsam_unstable/discrete/tests/testScheduler.cpp
+++ b/gtsam_unstable/discrete/tests/testScheduler.cpp
@@ -75,7 +75,7 @@ DiscreteFactorGraph createExpected() {
   // Mutual exclusion for students
   expected.addAllDiff(A, J);
 
-  return expected;
+  return std::move(expected);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h
index 1c5ade5b6..c8c351d7a 100644
--- a/gtsam_unstable/dynamics/FullIMUFactor.h
+++ b/gtsam_unstable/dynamics/FullIMUFactor.h
@@ -51,12 +51,12 @@ public:
   virtual ~FullIMUFactor() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
   /** Check if two factors are equal */
-  virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& e, double tol = 1e-9) const override {
     const This* const f = dynamic_cast<const This*>(&e);
     return f && Base::equals(e) &&
         equal_with_abs_tol(accel_, f->accel_, tol) &&
@@ -64,7 +64,7 @@ public:
         std::abs(dt_ - f->dt_) < tol;
   }
 
-  void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
+  void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const override {
     std::string a = "FullIMUFactor: " + s;
     Base::print(a, formatter);
     gtsam::print((Vector)accel_, "accel");
@@ -81,9 +81,9 @@ public:
    * Error evaluation with optional derivatives - calculates
    *  z - h(x1,x2)
    */
-  virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
+  Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
       boost::optional<Matrix&> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const {
+      boost::optional<Matrix&> H2 = boost::none) const override {
     Vector9 z;
     z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang
     z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h
index bb0a354ee..fb864a78d 100644
--- a/gtsam_unstable/dynamics/IMUFactor.h
+++ b/gtsam_unstable/dynamics/IMUFactor.h
@@ -44,12 +44,12 @@ public:
   virtual ~IMUFactor() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
   /** Check if two factors are equal */
-  virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& e, double tol = 1e-9) const override {
     const This* const f = dynamic_cast<const This*>(&e);
     return f && Base::equals(e) &&
         equal_with_abs_tol(accel_, f->accel_, tol) &&
@@ -57,7 +57,7 @@ public:
         std::abs(dt_ - f->dt_) < tol;
   }
 
-  void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
+  void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const override {
     std::string a = "IMUFactor: " + s;
     Base::print(a, formatter);
     gtsam::print((Vector)accel_, "accel");
@@ -74,9 +74,9 @@ public:
    * Error evaluation with optional derivatives - calculates
    *  z - h(x1,x2)
    */
-  virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
+  Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
       boost::optional<Matrix&> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const {
+      boost::optional<Matrix&> H2 = boost::none) const override {
     const Vector6 meas = z();
     if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>(
         boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h
index 209456a62..470d7108b 100644
--- a/gtsam_unstable/dynamics/Pendulum.h
+++ b/gtsam_unstable/dynamics/Pendulum.h
@@ -40,7 +40,7 @@ public:
   : Base(noiseModel::Constrained::All(1, std::abs(mu)), k1, k, velKey), h_(h) {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); }
 
@@ -48,7 +48,7 @@ public:
   Vector evaluateError(const double& qk1, const double& qk, const double& v,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
-      boost::optional<Matrix&> H3 = boost::none) const {
+      boost::optional<Matrix&> H3 = boost::none) const override {
     const size_t p = 1;
     if (H1) *H1 = -Matrix::Identity(p,p);
     if (H2) *H2 = Matrix::Identity(p,p);
@@ -88,7 +88,7 @@ public:
   : Base(noiseModel::Constrained::All(1, std::abs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); }
 
@@ -96,7 +96,7 @@ public:
   Vector evaluateError(const double & vk1, const double & vk, const double & q,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
-      boost::optional<Matrix&> H3 = boost::none) const {
+      boost::optional<Matrix&> H3 = boost::none) const override {
     const size_t p = 1;
     if (H1) *H1 = -Matrix::Identity(p,p);
     if (H2) *H2 = Matrix::Identity(p,p);
@@ -139,7 +139,7 @@ public:
     h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); }
 
@@ -147,7 +147,7 @@ public:
   Vector evaluateError(const double & pk, const double & qk, const double & qk1,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
-      boost::optional<Matrix&> H3 = boost::none) const {
+      boost::optional<Matrix&> H3 = boost::none) const override {
     const size_t p = 1;
 
     double qmid = (1-alpha_)*qk + alpha_*qk1;
@@ -195,7 +195,7 @@ public:
     h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); }
 
@@ -203,7 +203,7 @@ public:
   Vector evaluateError(const double & pk1, const double & qk, const double & qk1,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
-      boost::optional<Matrix&> H3 = boost::none) const {
+      boost::optional<Matrix&> H3 = boost::none) const override {
     const size_t p = 1;
 
     double qmid = (1-alpha_)*qk + alpha_*qk1;
diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h
index f38c256b1..42ef04f84 100644
--- a/gtsam_unstable/dynamics/SimpleHelicopter.h
+++ b/gtsam_unstable/dynamics/SimpleHelicopter.h
@@ -36,7 +36,7 @@ public:
   virtual ~Reconstruction() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new Reconstruction(*this))); }
 
@@ -44,7 +44,7 @@ public:
   Vector evaluateError(const Pose3& gk1, const Pose3& gk, const Vector6& xik,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
-      boost::optional<Matrix&> H3 = boost::none) const {
+      boost::optional<Matrix&> H3 = boost::none) const override {
 
     Matrix6 D_exphxi_xi;
     Pose3 exphxi = Pose3::Expmap(h_ * xik, H3 ? &D_exphxi_xi : 0);
@@ -98,7 +98,7 @@ public:
   virtual ~DiscreteEulerPoincareHelicopter() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new DiscreteEulerPoincareHelicopter(*this))); }
 
@@ -110,7 +110,7 @@ public:
   Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
-      boost::optional<Matrix&> H3 = boost::none) const {
+      boost::optional<Matrix&> H3 = boost::none) const override {
 
     Vector muk = Inertia_*xik;
     Vector muk_1 = Inertia_*xik_1;
diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h
index ed3797015..986d8e271 100644
--- a/gtsam_unstable/dynamics/VelocityConstraint.h
+++ b/gtsam_unstable/dynamics/VelocityConstraint.h
@@ -73,16 +73,16 @@ public:
   virtual ~VelocityConstraint() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint(*this))); }
 
   /**
    * Calculates the error for trapezoidal model given
    */
-  virtual gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
+  gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
       boost::optional<gtsam::Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      boost::optional<gtsam::Matrix&> H2=boost::none) const override {
     if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>(
         boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5);
     if (H2) *H2 = gtsam::numericalDerivative22<gtsam::Vector,PoseRTV,PoseRTV>(
@@ -90,7 +90,7 @@ public:
     return evaluateError_(x1, x2, dt_, integration_mode_);
   }
 
-  virtual void print(const std::string& s = "", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
+  void print(const std::string& s = "", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const override {
     std::string a = "VelocityConstraint: " + s;
     Base::print(a, formatter);
     switch(integration_mode_) {
diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h
index 721d0265b..f6cd8ccc4 100644
--- a/gtsam_unstable/dynamics/VelocityConstraint3.h
+++ b/gtsam_unstable/dynamics/VelocityConstraint3.h
@@ -31,7 +31,7 @@ public:
   virtual ~VelocityConstraint3() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); }
 
@@ -39,7 +39,7 @@ public:
   Vector evaluateError(const double& x1, const double& x2, const double& v,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
-      boost::optional<Matrix&> H3 = boost::none) const {
+      boost::optional<Matrix&> H3 = boost::none) const override {
     const size_t p = 1;
     if (H1) *H1 = Matrix::Identity(p,p);
     if (H2) *H2 = -Matrix::Identity(p,p);
diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp
index b07b5acd6..d23346896 100644
--- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp
+++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp
@@ -244,7 +244,7 @@ TEST(Similarity3, GroupAction) {
       &Similarity3::transformFrom, _1, _2, boost::none, boost::none);
 
   Point3 q(1, 2, 3);
-  for (const auto T : { T1, T2, T3, T4, T5, T6 }) {
+  for (const auto& T : { T1, T2, T3, T4, T5, T6 }) {
     Point3 q(1, 0, 0);
     Matrix H1 = numericalDerivative21<Point3, Similarity3, Point3>(f, T, q);
     Matrix H2 = numericalDerivative22<Point3, Similarity3, Point3>(f, T, q);
diff --git a/gtsam_unstable/linear/InfeasibleInitialValues.h b/gtsam_unstable/linear/InfeasibleInitialValues.h
index 60adb872e..6f589d0c3 100644
--- a/gtsam_unstable/linear/InfeasibleInitialValues.h
+++ b/gtsam_unstable/linear/InfeasibleInitialValues.h
@@ -29,10 +29,10 @@ public:
   InfeasibleInitialValues() {
   }
 
-  virtual ~InfeasibleInitialValues() throw () {
+  virtual ~InfeasibleInitialValues() noexcept {
   }
 
-  virtual const char *what() const throw () {
+  const char *what() const noexcept override {
     if (description_.empty())
       description_ =
           "An infeasible initial value was provided for the solver.\n";
diff --git a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h
index 2184e9f52..25742d61f 100644
--- a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h
+++ b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h
@@ -25,10 +25,10 @@ class InfeasibleOrUnboundedProblem: public ThreadsafeException<
 public:
   InfeasibleOrUnboundedProblem() {
   }
-  virtual ~InfeasibleOrUnboundedProblem() throw () {
+  virtual ~InfeasibleOrUnboundedProblem() noexcept {
   }
 
-  virtual const char* what() const throw () {
+  const char* what() const noexcept override {
     if (description_.empty())
       description_ = "The problem is either infeasible or unbounded.\n";
     return description_.c_str();
diff --git a/gtsam_unstable/linear/LinearCost.h b/gtsam_unstable/linear/LinearCost.h
index b489510af..c22c389be 100644
--- a/gtsam_unstable/linear/LinearCost.h
+++ b/gtsam_unstable/linear/LinearCost.h
@@ -88,18 +88,18 @@ public:
   }
 
   /** equals */
-  virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const {
+  bool equals(const GaussianFactor& lf, double tol = 1e-9) const override {
     return Base::equals(lf, tol);
   }
 
   /** print */
-  virtual void print(const std::string& s = "", const KeyFormatter& formatter =
-      DefaultKeyFormatter) const {
+  void print(const std::string& s = "", const KeyFormatter& formatter =
+      DefaultKeyFormatter) const override {
     Base::print(s + " LinearCost: ", formatter);
   }
 
   /** Clone this LinearCost */
-  virtual GaussianFactor::shared_ptr clone() const {
+  GaussianFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast < GaussianFactor
         > (boost::make_shared < LinearCost > (*this));
   }
@@ -110,7 +110,7 @@ public:
   }
 
   /** Special error for single-valued inequality constraints. */
-  virtual double error(const VectorValues& c) const {
+  double error(const VectorValues& c) const override {
     return error_vector(c)[0];
   }
 };
diff --git a/gtsam_unstable/linear/LinearEquality.h b/gtsam_unstable/linear/LinearEquality.h
index 2463ef31c..d960d8336 100644
--- a/gtsam_unstable/linear/LinearEquality.h
+++ b/gtsam_unstable/linear/LinearEquality.h
@@ -89,18 +89,18 @@ public:
   }
 
   /** equals */
-  virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const {
+  bool equals(const GaussianFactor& lf, double tol = 1e-9) const override {
     return Base::equals(lf, tol);
   }
 
   /** print */
-  virtual void print(const std::string& s = "", const KeyFormatter& formatter =
-      DefaultKeyFormatter) const {
+  void print(const std::string& s = "", const KeyFormatter& formatter =
+      DefaultKeyFormatter) const override {
     Base::print(s, formatter);
   }
 
   /** Clone this LinearEquality */
-  virtual GaussianFactor::shared_ptr clone() const {
+  GaussianFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast < GaussianFactor
         > (boost::make_shared < LinearEquality > (*this));
   }
@@ -124,7 +124,7 @@ public:
    * I think it should be zero, as this function is meant for objective cost.
    * But the name "error" can be misleading.
    * TODO: confirm with Frank!! */
-  virtual double error(const VectorValues& c) const {
+  double error(const VectorValues& c) const override {
     return 0.0;
   }
 
diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h
index 1a31bd4e4..d353afc46 100644
--- a/gtsam_unstable/linear/LinearInequality.h
+++ b/gtsam_unstable/linear/LinearInequality.h
@@ -100,13 +100,13 @@ public:
   }
 
   /** equals */
-  virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const {
+  bool equals(const GaussianFactor& lf, double tol = 1e-9) const override {
     return Base::equals(lf, tol);
   }
 
   /** print */
-  virtual void print(const std::string& s = "", const KeyFormatter& formatter =
-      DefaultKeyFormatter) const {
+  void print(const std::string& s = "", const KeyFormatter& formatter =
+      DefaultKeyFormatter) const override {
     if (active())
       Base::print(s + "  Active", formatter);
     else
@@ -114,7 +114,7 @@ public:
   }
 
   /** Clone this LinearInequality */
-  virtual GaussianFactor::shared_ptr clone() const {
+  GaussianFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast < GaussianFactor
         > (boost::make_shared < LinearInequality > (*this));
   }
@@ -145,7 +145,7 @@ public:
   }
 
   /** Special error for single-valued inequality constraints. */
-  virtual double error(const VectorValues& c) const {
+  double error(const VectorValues& c) const override {
     return error_vector(c)[0];
   }
 
diff --git a/gtsam_unstable/linear/QPSParserException.h b/gtsam_unstable/linear/QPSParserException.h
index ed4c79bdd..ef14ed430 100644
--- a/gtsam_unstable/linear/QPSParserException.h
+++ b/gtsam_unstable/linear/QPSParserException.h
@@ -25,10 +25,10 @@ public:
   QPSParserException() {
   }
 
-  virtual ~QPSParserException() throw () {
+  virtual ~QPSParserException() noexcept {
   }
 
-  virtual const char *what() const throw () {
+  const char *what() const noexcept override {
     if (description_.empty())
       description_ = "There is a problem parsing the QPS file.\n";
     return description_.c_str();
diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
index d71cdd409..2291642aa 100644
--- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
+++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
@@ -41,10 +41,10 @@ public:
   virtual ~BatchFixedLagSmoother() { };
 
   /** Print the factor for debugging and testing (implementing Testable) */
-  virtual void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
+  void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /** Check if two IncrementalFixedLagSmoother Objects are equal */
-  virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const override;
+  bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const override;
 
   /** Add new factors, updating the solution and relinearizing as needed. */
   Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
index d04f579eb..c99c67492 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
@@ -67,10 +67,10 @@ public:
   virtual ~ConcurrentBatchFilter() {};
 
   /** Implement a GTSAM standard 'print' function */
-  virtual void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+  void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /** Check if two Concurrent Filters are equal */
-  virtual bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const;
+  bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const override;
 
   /** Access the current set of factors */
   const NonlinearFactorGraph& getFactors() const {
@@ -130,7 +130,7 @@ public:
    * Perform any required operations before the synchronization process starts.
    * Called by 'synchronize'
    */
-  virtual void presync();
+  virtual void presync() override;
 
   /**
    * Populate the provided containers with factors that constitute the filter branch summarization
@@ -139,7 +139,7 @@ public:
    * @param summarizedFactors The summarized factors for the filter branch
    * @param rootValues The linearization points of the root clique variables
    */
-  virtual void getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues);
+  void getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues) override;
 
   /**
    * Populate the provided containers with factors being sent to the smoother from the filter. These
@@ -149,20 +149,20 @@ public:
    * @param smootherFactors The new factors to be added to the smoother
    * @param smootherValues The linearization points of any new variables
    */
-  virtual void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues);
+  void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) override;
 
   /**
    * Apply the updated version of the smoother branch summarized factors.
    *
    * @param summarizedFactors An updated version of the smoother branch summarized factors
    */
-  virtual void synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues);
+  void synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues) override;
 
   /**
    * Perform any required operations after the synchronization process finishes.
    * Called by 'synchronize'
    */
-  virtual void postsync();
+  virtual void postsync() override;
 
 protected:
 
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h
index 67a1ef4f3..364d02caf 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h
@@ -60,10 +60,10 @@ public:
   virtual ~ConcurrentBatchSmoother() {};
 
   /** Implement a GTSAM standard 'print' function */
-  virtual void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+  void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /** Check if two Concurrent Smoothers are equal */
-  virtual bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const;
+  bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const override;
 
   /** Access the current set of factors */
   const NonlinearFactorGraph& getFactors() const {
@@ -124,7 +124,7 @@ public:
    * Perform any required operations before the synchronization process starts.
    * Called by 'synchronize'
    */
-  virtual void presync();
+  virtual void presync() override;
 
   /**
    * Populate the provided containers with factors that constitute the smoother branch summarization
@@ -132,7 +132,7 @@ public:
    *
    * @param summarizedFactors The summarized factors for the filter branch
    */
-  virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues);
+  void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) override;
 
   /**
    * Apply the new smoother factors sent by the filter, and the updated version of the filter
@@ -143,14 +143,14 @@ public:
    * @param summarizedFactors An updated version of the filter branch summarized factors
    * @param rootValues The linearization point of the root variables
    */
-  virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
-      const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues);
+  void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
+      const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) override;
 
   /**
    * Perform any required operations after the synchronization process finishes.
    * Called by 'synchronize'
    */
-  virtual void postsync();
+  virtual void postsync() override;
 
 protected:
 
diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h
index e01919048..d0525a4da 100644
--- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h
+++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h
@@ -67,10 +67,10 @@ public:
   virtual ~ConcurrentIncrementalFilter() {};
 
   /** Implement a GTSAM standard 'print' function */
-  virtual void print(const std::string& s = "Concurrent Incremental Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+  void print(const std::string& s = "Concurrent Incremental Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /** Check if two Concurrent Filters are equal */
-  virtual bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const;
+  bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const override;
 
   /** Access the current set of factors */
   const NonlinearFactorGraph& getFactors() const {
@@ -130,7 +130,7 @@ public:
    * Perform any required operations before the synchronization process starts.
    * Called by 'synchronize'
    */
-  virtual void presync();
+  void presync() override;
 
   /**
    * Populate the provided containers with factors that constitute the filter branch summarization
@@ -139,7 +139,7 @@ public:
    * @param summarizedFactors The summarized factors for the filter branch
    * @param rootValues The linearization points of the root clique variables
    */
-  virtual void getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues);
+  void getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues) override;
 
   /**
    * Populate the provided containers with factors being sent to the smoother from the filter. These
@@ -149,20 +149,20 @@ public:
    * @param smootherFactors The new factors to be added to the smoother
    * @param smootherValues The linearization points of any new variables
    */
-  virtual void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues);
+  void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) override;
 
   /**
    * Apply the updated version of the smoother branch summarized factors.
    *
    * @param summarizedFactors An updated version of the smoother branch summarized factors
    */
-  virtual void synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues);
+  void synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues) override;
 
   /**
    * Perform any required operations after the synchronization process finishes.
    * Called by 'synchronize'
    */
-  virtual void postsync();
+  void postsync() override;
 
 protected:
 
diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h
index ec95e1ec8..8848b6a43 100644
--- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h
+++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h
@@ -57,10 +57,10 @@ public:
   virtual ~ConcurrentIncrementalSmoother() {};
 
   /** Implement a GTSAM standard 'print' function */
-  virtual void print(const std::string& s = "Concurrent Incremental Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+  void print(const std::string& s = "Concurrent Incremental Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /** Check if two Concurrent Smoothers are equal */
-  virtual bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const;
+  bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const override;
 
   /** Access the current set of factors */
   const NonlinearFactorGraph& getFactors() const {
@@ -115,7 +115,7 @@ public:
    * Perform any required operations before the synchronization process starts.
    * Called by 'synchronize'
    */
-  virtual void presync();
+  void presync() override;
 
   /**
    * Populate the provided containers with factors that constitute the smoother branch summarization
@@ -123,7 +123,7 @@ public:
    *
    * @param summarizedFactors The summarized factors for the filter branch
    */
-  virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues);
+  void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) override;
 
   /**
    * Apply the new smoother factors sent by the filter, and the updated version of the filter
@@ -134,14 +134,14 @@ public:
    * @param summarizedFactors An updated version of the filter branch summarized factors
    * @param rootValues The linearization point of the root variables
    */
-  virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
-      const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues);
+  void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
+      const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) override;
 
   /**
    * Perform any required operations after the synchronization process finishes.
    * Called by 'synchronize'
    */
-  virtual void postsync();
+  void postsync() override;
 
 protected:
 
diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h
index 128889b82..caf67de21 100644
--- a/gtsam_unstable/nonlinear/LinearizedFactor.h
+++ b/gtsam_unstable/nonlinear/LinearizedFactor.h
@@ -112,17 +112,17 @@ public:
   virtual ~LinearizedJacobianFactor() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
   // Testable
 
   /** print function */
-  virtual void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /** equals function with optional tolerance */
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   // access functions
   const constBVector b() const { return Ab_(size()).col(0); }
@@ -130,17 +130,17 @@ public:
   const constABlock A(Key key) const { return Ab_(std::find(begin(), end(), key) - begin()); }
 
   /** get the dimension of the factor (number of rows on linearization) */
-  size_t dim() const { return Ab_.rows(); };
+  size_t dim() const override { return Ab_.rows(); };
 
   /** Calculate the error of the factor */
-  double error(const Values& c) const;
+  double error(const Values& c) const override;
 
   /**
    * linearize to a GaussianFactor
    * Reimplemented from NoiseModelFactor to directly copy out the
    * matrices and only update the RHS b with an updated residual
    */
-  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const;
+  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override;
 
   /** (A*x-b) */
   Vector error_vector(const Values& c) const;
@@ -202,17 +202,17 @@ public:
   virtual ~LinearizedHessianFactor() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
   // Testable
 
   /** print function */
-  virtual void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /** equals function with optional tolerance */
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /** Return the constant term \f$ f \f$ as described above
    * @return The constant term \f$ f \f$
@@ -261,17 +261,17 @@ public:
   }
 
   /** get the dimension of the factor (number of rows on linearization) */
-  size_t dim() const { return info_.rows() - 1; }
+  size_t dim() const override { return info_.rows() - 1; }
 
   /** Calculate the error of the factor */
-  double error(const Values& c) const;
+  double error(const Values& c) const override;
 
   /**
    * linearize to a GaussianFactor
    * Reimplemented from NoiseModelFactor to directly copy out the
    * matrices and only update the RHS b with an updated residual
    */
-  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const;
+  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override;
 
 private:
   /** Serialization function */
diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h
index 104b3209d..4104ba653 100644
--- a/gtsam_unstable/slam/BetweenFactorEM.h
+++ b/gtsam_unstable/slam/BetweenFactorEM.h
@@ -84,8 +84,8 @@ public:
   /** implement functions needed for Testable */
 
   /** print */
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const {
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const override {
     std::cout << s << "BetweenFactorEM(" << keyFormatter(key1_) << ","
         << keyFormatter(key2_) << ")\n";
     measured_.print("  measured: ");
@@ -97,7 +97,7 @@ public:
   }
 
   /** equals */
-  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
     const This *t = dynamic_cast<const This*>(&f);
 
     if (t && Base::equals(f))
@@ -408,7 +408,7 @@ public:
     return 2;
   }
 
-  virtual size_t dim() const {
+  size_t dim() const override {
     return model_inlier_->R().rows() + model_inlier_->R().cols();
   }
 
diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h
index cf56afab2..c8bbdd78a 100644
--- a/gtsam_unstable/slam/BiasedGPSFactor.h
+++ b/gtsam_unstable/slam/BiasedGPSFactor.h
@@ -55,7 +55,7 @@ namespace gtsam {
     /** implement functions needed for Testable */
 
     /** print */
-    virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "BiasedGPSFactor("
           << keyFormatter(this->key1()) << ","
           << keyFormatter(this->key2()) << ")\n"
@@ -64,7 +64,7 @@ namespace gtsam {
     }
 
     /** equals */
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
       const This *e =  dynamic_cast<const This*> (&expected);
       return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(this->measured_, e->measured_, tol);
     }
@@ -74,7 +74,7 @@ namespace gtsam {
     /** vector of errors */
     Vector evaluateError(const Pose3& pose, const Point3& bias,
         boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
-            boost::none) const {
+            boost::none) const override {
 
       if (H1 || H2){
         H1->resize(3,6); // jacobian wrt pose
diff --git a/gtsam_unstable/slam/DummyFactor.h b/gtsam_unstable/slam/DummyFactor.h
index 574efabea..08b1c800a 100644
--- a/gtsam_unstable/slam/DummyFactor.h
+++ b/gtsam_unstable/slam/DummyFactor.h
@@ -34,10 +34,10 @@ public:
   // testable
 
   /** print */
-  virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /** Check if two factors are equal */
-  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
 
   // access
 
@@ -48,13 +48,13 @@ public:
   /**
    * Calculate the error of the factor - zero for dummy factors
    */
-  virtual double error(const Values& c) const { return 0.0; }
+  double error(const Values& c) const override { return 0.0; }
 
   /** get the dimension of the factor (number of rows on linearization) */
-  virtual size_t dim() const { return rowDim_; }
+  size_t dim() const override { return rowDim_; }
 
   /** linearize to a GaussianFactor */
-  virtual boost::shared_ptr<GaussianFactor> linearize(const Values& c) const;
+  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override;
 
   /**
    * Creates a shared_ptr clone of the factor - needs to be specialized to allow
@@ -62,7 +62,7 @@ public:
    *
    * By default, throws exception if subclass does not implement the function.
    */
-  virtual NonlinearFactor::shared_ptr clone() const {
+  NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<NonlinearFactor>(
         NonlinearFactor::shared_ptr(new DummyFactor(*this)));
   }
diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
index f499a0f4b..745605325 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
@@ -133,7 +133,7 @@ public:
   /** implement functions needed for Testable */
 
   /** print */
-  virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "("
         << keyFormatter(this->key1()) << ","
         << keyFormatter(this->key2()) << ","
@@ -153,7 +153,7 @@ public:
   }
 
   /** equals */
-  virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
     const This *e =  dynamic_cast<const This*> (&expected);
     return e != nullptr && Base::equals(*e, tol)
     && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
@@ -302,7 +302,7 @@ public:
       boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none,
       boost::optional<Matrix&> H4 = boost::none,
-      boost::optional<Matrix&> H5 = boost::none) const {
+      boost::optional<Matrix&> H5 = boost::none) const override {
 
     // TODO: Write analytical derivative calculations
     // Jacobian w.r.t. Pose1
diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
index 86efd10c1..9f5d800db 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
@@ -151,7 +151,7 @@ public:
   }
 
   /** equals */
-  virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
     const This *e =  dynamic_cast<const This*> (&expected);
     return e != nullptr && Base::equals(*e, tol)
     && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
index 762199753..f6de48625 100644
--- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
+++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
@@ -71,7 +71,7 @@ public:
   /** implement functions needed for Testable */
 
   /** print */
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "GaussMarkov1stOrderFactor("
         << keyFormatter(this->key1()) << ","
         << keyFormatter(this->key2()) << ")\n";
@@ -79,7 +79,7 @@ public:
   }
 
   /** equals */
-  virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
     const This *e =  dynamic_cast<const This*> (&expected);
     return e != nullptr && Base::equals(*e, tol);
   }
@@ -89,7 +89,7 @@ public:
   /** vector of errors */
   Vector evaluateError(const VALUE& p1, const VALUE& p2,
       boost::optional<Matrix&> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const {
+      boost::optional<Matrix&> H2 = boost::none) const override {
 
     Vector v1( traits<VALUE>::Logmap(p1) );
     Vector v2( traits<VALUE>::Logmap(p2) );
diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
index 4763c4263..e5a3e9118 100644
--- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
+++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
@@ -114,7 +114,7 @@ public:
   /** implement functions needed for Testable */
 
   /** print */
-  virtual void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "("
         << keyFormatter(this->key1()) << ","
         << keyFormatter(this->key2()) << ","
@@ -133,7 +133,7 @@ public:
   }
 
   /** equals */
-  virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
     const This *e =  dynamic_cast<const This*> (&expected);
     return e != nullptr && Base::equals(*e, tol)
       && (measurement_acc_ - e->measurement_acc_).norm() < tol
@@ -229,7 +229,7 @@ public:
       boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none,
       boost::optional<Matrix&> H4 = boost::none,
-      boost::optional<Matrix&> H5 = boost::none) const {
+      boost::optional<Matrix&> H5 = boost::none) const override {
 
     // TODO: Write analytical derivative calculations
     // Jacobian w.r.t. Pose1
diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h
index 0d10b0123..50b0c5980 100644
--- a/gtsam_unstable/slam/InvDepthFactor3.h
+++ b/gtsam_unstable/slam/InvDepthFactor3.h
@@ -70,13 +70,13 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "InvDepthFactor3",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     traits<Point2>::Print(measured_, s + ".z");
   }
 
   /// equals
-  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
     const This *e = dynamic_cast<const This*>(&p);
     return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol);
   }
@@ -85,7 +85,7 @@ public:
   Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
       boost::optional<Matrix&> H1=boost::none,
       boost::optional<Matrix&> H2=boost::none,
-      boost::optional<Matrix&> H3=boost::none) const {
+      boost::optional<Matrix&> H3=boost::none) const override {
     try {
       InvDepthCamera3<Cal3_S2> camera(pose, K_);
       return camera.project(point, invDepth, H1, H2, H3) - measured_;
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h
index ad66eee5b..785556750 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant1.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h
@@ -66,13 +66,13 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "InvDepthFactorVariant1",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     traits<Point2>::Print(measured_, s + ".z");
   }
 
   /// equals
-  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
     const This *e = dynamic_cast<const This*>(&p);
     return e
         && Base::equals(p, tol)
@@ -102,7 +102,7 @@ public:
   /// Evaluate error h(x)-z and optionally derivatives
   Vector evaluateError(const Pose3& pose, const Vector6& landmark,
       boost::optional<Matrix&> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const {
+      boost::optional<Matrix&> H2=boost::none) const override {
 
     if (H1) {
       (*H1) = numericalDerivative11<Vector, Pose3>(
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h
index c6b5d5af8..97ceb8a8b 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant2.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h
@@ -69,13 +69,13 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "InvDepthFactorVariant2",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     traits<Point2>::Print(measured_, s + ".z");
   }
 
   /// equals
-  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
     const This *e = dynamic_cast<const This*>(&p);
     return e
         && Base::equals(p, tol)
@@ -105,7 +105,7 @@ public:
   /// Evaluate error h(x)-z and optionally derivatives
   Vector evaluateError(const Pose3& pose, const Vector3& landmark,
       boost::optional<Matrix&> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const {
+      boost::optional<Matrix&> H2=boost::none) const override {
 
     if (H1) {
       (*H1) = numericalDerivative11<Vector, Pose3>(
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h
index 83df9e13b..21c7aa6de 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant3.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h
@@ -68,13 +68,13 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "InvDepthFactorVariant3a",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     traits<Point2>::Print(measured_, s + ".z");
   }
 
   /// equals
-  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
     const This *e = dynamic_cast<const This*>(&p);
     return e
         && Base::equals(p, tol)
@@ -105,7 +105,7 @@ public:
   /// Evaluate error h(x)-z and optionally derivatives
   Vector evaluateError(const Pose3& pose, const Vector3& landmark,
       boost::optional<Matrix&> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const {
+      boost::optional<Matrix&> H2=boost::none) const override {
 
     if(H1) {
       (*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose);
@@ -188,13 +188,13 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "InvDepthFactorVariant3",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     traits<Point2>::Print(measured_, s + ".z");
   }
 
   /// equals
-  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
     const This *e = dynamic_cast<const This*>(&p);
     return e
         && Base::equals(p, tol)
@@ -226,7 +226,7 @@ public:
   Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark,
       boost::optional<Matrix&> H1=boost::none,
       boost::optional<Matrix&> H2=boost::none,
-      boost::optional<Matrix&> H3=boost::none) const {
+      boost::optional<Matrix&> H3=boost::none) const override {
 
     if(H1)
       (*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1);
diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h
index 5bef97bcf..b6bf2a9c7 100644
--- a/gtsam_unstable/slam/MultiProjectionFactor.h
+++ b/gtsam_unstable/slam/MultiProjectionFactor.h
@@ -101,7 +101,7 @@ namespace gtsam {
     virtual ~MultiProjectionFactor() {}
 
     /// @return a deep copy of this factor
-    virtual NonlinearFactor::shared_ptr clone() const {
+    NonlinearFactor::shared_ptr clone() const override {
       return boost::static_pointer_cast<NonlinearFactor>(
           NonlinearFactor::shared_ptr(new This(*this))); }
 
@@ -110,7 +110,7 @@ namespace gtsam {
      * @param s optional string naming the factor
      * @param keyFormatter optional formatter useful for printing Symbols
      */
-    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "MultiProjectionFactor, z = ";
       std::cout << measured_ << "measurements, z = ";
       if(this->body_P_sensor_)
@@ -119,7 +119,7 @@ namespace gtsam {
     }
 
     /// equals
-    virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+    bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
       const This *e = dynamic_cast<const This*>(&p);
       return e
           && Base::equals(p, tol)
@@ -129,7 +129,7 @@ namespace gtsam {
     }
 
     /// Evaluate error h(x)-z and optionally derivatives
-    Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const{
+    Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
 
       Vector a;
       return a;
diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h
index c6d892e93..d284366e8 100644
--- a/gtsam_unstable/slam/PartialPriorFactor.h
+++ b/gtsam_unstable/slam/PartialPriorFactor.h
@@ -85,20 +85,20 @@ namespace gtsam {
     }
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       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 {
+    void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       Base::print(s, keyFormatter);
       gtsam::print(prior_, "prior");
     }
 
     /** equals */
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
       const This *e = dynamic_cast<const This*> (&expected);
       return e != nullptr && Base::equals(*e, tol) &&
           gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) &&
@@ -108,7 +108,7 @@ namespace gtsam {
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
-    Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
+    Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const override {
       if (H) (*H) = H_;
       // FIXME: this was originally the generic retraction - may not produce same results
       Vector full_logmap = T::Logmap(p);
diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h
index bb5835f80..e20792e25 100644
--- a/gtsam_unstable/slam/PoseBetweenFactor.h
+++ b/gtsam_unstable/slam/PoseBetweenFactor.h
@@ -59,14 +59,14 @@ namespace gtsam {
     virtual ~PoseBetweenFactor() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       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 {
+    void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "BetweenFactor("
           << keyFormatter(this->key1()) << ","
           << keyFormatter(this->key2()) << ")\n";
@@ -77,7 +77,7 @@ namespace gtsam {
     }
 
     /** equals */
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
       const This *e =  dynamic_cast<const This*> (&expected);
       return e != nullptr
           && Base::equals(*e, tol)
@@ -90,7 +90,7 @@ namespace gtsam {
     /** vector of errors */
     Vector evaluateError(const POSE& p1, const POSE& p2,
         boost::optional<Matrix&> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none) const {
+        boost::optional<Matrix&> H2 = boost::none) const override {
       if(body_P_sensor_) {
         POSE hx;
         if(H1 || H2) {
diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h
index ce9861160..c9965f9bf 100644
--- a/gtsam_unstable/slam/PosePriorFactor.h
+++ b/gtsam_unstable/slam/PosePriorFactor.h
@@ -56,14 +56,14 @@ namespace gtsam {
     }
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       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 {
+    void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
       prior_.print("  prior mean: ");
       if(this->body_P_sensor_)
@@ -72,7 +72,7 @@ namespace gtsam {
     }
 
     /** equals */
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
       const This* e = dynamic_cast<const This*> (&expected);
       return e != nullptr
           && Base::equals(*e, tol)
@@ -83,7 +83,7 @@ namespace gtsam {
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
-    Vector evaluateError(const POSE& p, boost::optional<Matrix&> H = boost::none) const {
+    Vector evaluateError(const POSE& p, boost::optional<Matrix&> H = boost::none) const override {
       if(body_P_sensor_) {
         // manifold equivalent of h(x)-z -> log(z,h(x))
         return prior_.localCoordinates(p.compose(*body_P_sensor_, H));
diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h
index 19b8d56e6..caa388e2f 100644
--- a/gtsam_unstable/slam/ProjectionFactorPPP.h
+++ b/gtsam_unstable/slam/ProjectionFactorPPP.h
@@ -96,7 +96,7 @@ namespace gtsam {
     virtual ~ProjectionFactorPPP() {}
 
     /// @return a deep copy of this factor
-    virtual NonlinearFactor::shared_ptr clone() const {
+    NonlinearFactor::shared_ptr clone() const override {
       return boost::static_pointer_cast<NonlinearFactor>(
           NonlinearFactor::shared_ptr(new This(*this))); }
 
@@ -105,14 +105,14 @@ namespace gtsam {
      * @param s optional string naming the factor
      * @param keyFormatter optional formatter useful for printing Symbols
      */
-    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "ProjectionFactorPPP, z = ";
       traits<Point2>::Print(measured_);
       Base::print("", keyFormatter);
     }
 
     /// equals
-    virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+    bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
       const This *e = dynamic_cast<const This*>(&p);
       return e
           && Base::equals(p, tol)
@@ -124,7 +124,7 @@ namespace gtsam {
     Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point,
         boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none,
-        boost::optional<Matrix&> H3 = boost::none) const {
+        boost::optional<Matrix&> H3 = boost::none) const override {
       try {
           if(H1 || H2 || H3) {
             Matrix H0, H02;
diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h
index 369075abf..af3294bce 100644
--- a/gtsam_unstable/slam/ProjectionFactorPPPC.h
+++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h
@@ -91,7 +91,7 @@ namespace gtsam {
     virtual ~ProjectionFactorPPPC() {}
 
     /// @return a deep copy of this factor
-    virtual NonlinearFactor::shared_ptr clone() const {
+    NonlinearFactor::shared_ptr clone() const override {
       return boost::static_pointer_cast<NonlinearFactor>(
           NonlinearFactor::shared_ptr(new This(*this))); }
 
@@ -100,14 +100,14 @@ namespace gtsam {
      * @param s optional string naming the factor
      * @param keyFormatter optional formatter useful for printing Symbols
      */
-    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "ProjectionFactorPPPC, z = ";
       traits<Point2>::Print(measured_);
       Base::print("", keyFormatter);
     }
 
     /// equals
-    virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+    bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
       const This *e = dynamic_cast<const This*>(&p);
       return e
           && Base::equals(p, tol)
@@ -119,7 +119,7 @@ namespace gtsam {
         boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none,
         boost::optional<Matrix&> H3 = boost::none,
-        boost::optional<Matrix&> H4 = boost::none) const {
+        boost::optional<Matrix&> H4 = boost::none) const override {
       try {
           if(H1 || H2 || H3 || H4) {
             Matrix H0, H02;
diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h
index 3507a4492..b713d45dc 100644
--- a/gtsam_unstable/slam/RelativeElevationFactor.h
+++ b/gtsam_unstable/slam/RelativeElevationFactor.h
@@ -43,22 +43,22 @@ public:
   virtual ~RelativeElevationFactor() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
   /** h(x)-z */
   Vector evaluateError(const Pose3& pose, const Point3& point,
-      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const;
+      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override;
 
   /** return the measured */
   inline double measured() const { return measured_; }
 
   /** equals specialized to this factor */
-  virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override;
 
   /** print contents */
-  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
 private:
 
diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h
index 5511a0209..6bab3f334 100644
--- a/gtsam_unstable/slam/SmartRangeFactor.h
+++ b/gtsam_unstable/slam/SmartRangeFactor.h
@@ -73,14 +73,14 @@ class SmartRangeFactor: public NoiseModelFactor {
   // Testable
 
   /** print */
-  virtual void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "SmartRangeFactor with " << size() << " measurements\n";
     NoiseModelFactor::print(s);
   }
 
   /** Check if two factors are equal */
-  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
     return false;
   }
 
@@ -143,8 +143,8 @@ class SmartRangeFactor: public NoiseModelFactor {
   /**
    * Error function *without* the NoiseModel, \f$ z-h(x) \f$.
    */
-  virtual Vector unwhitenedError(const Values& x,
-      boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  Vector unwhitenedError(const Values& x,
+      boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     size_t n = size();
     if (n < 3) {
       if (H) {
@@ -175,7 +175,7 @@ class SmartRangeFactor: public NoiseModelFactor {
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
index db80956fa..ac063eff9 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
@@ -102,7 +102,7 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const {
+      DefaultKeyFormatter) const override {
     std::cout << s << "SmartStereoProjectionFactor\n";
     std::cout << "linearizationMode:\n" << params_.linearizationMode << std::endl;
     std::cout << "triangulationParameters:\n" << params_.triangulation << std::endl;
@@ -111,7 +111,7 @@ public:
   }
 
   /// equals
-  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
     const SmartStereoProjectionFactor *e =
         dynamic_cast<const SmartStereoProjectionFactor*>(&p);
     return e && params_.linearizationMode == e->params_.linearizationMode
@@ -327,8 +327,8 @@ public:
   }
 
   /// linearize
-  virtual boost::shared_ptr<GaussianFactor> linearize(
-      const Values& values) const {
+  boost::shared_ptr<GaussianFactor> linearize(
+      const Values& values) const override {
     return linearizeDamped(values);
   }
 
@@ -438,7 +438,7 @@ public:
   }
 
   /// Calculate total reprojection error
-  virtual double error(const Values& values) const {
+  double error(const Values& values) const override {
     if (this->active(values)) {
       return totalReprojectionError(Base::cameras(values));
     } else { // else of active flag
@@ -449,9 +449,9 @@ public:
   /**
    * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)
    */
-  virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
+  void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
       boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
-      boost::optional<Matrix&> E = boost::none) const
+      boost::optional<Matrix&> E = boost::none) const override
   {
     // when using stereo cameras, some of the measurements might be missing:
     for(size_t i=0; i < cameras.size(); i++){
diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
index 7ea5a4c2f..124e45005 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
@@ -120,7 +120,7 @@ public:
    * @param keyFormatter optional formatter useful for printing Symbols
    */
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const {
+      DefaultKeyFormatter) const override {
     std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
     for(const boost::shared_ptr<Cal3_S2Stereo>& K: K_all_)
     K->print("calibration = ");
@@ -128,7 +128,7 @@ public:
   }
 
   /// equals
-  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
     const SmartStereoProjectionPoseFactor *e =
         dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
 
@@ -138,7 +138,7 @@ public:
   /**
    * error calculates the error of the factor.
    */
-  virtual double error(const Values& values) const {
+  double error(const Values& values) const override {
     if (this->active(values)) {
       return this->totalReprojectionError(cameras(values));
     } else { // else of active flag
@@ -157,7 +157,7 @@ public:
    * to keys involved in this factor
    * @return vector of Values
    */
-   Base::Cameras cameras(const Values& values) const {
+   Base::Cameras cameras(const Values& values) const override {
     Base::Cameras cameras;
     size_t i=0;
     for(const Key& k: this->keys_) {
diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h
index 0a456e59c..6f98a2dbd 100644
--- a/gtsam_unstable/slam/TSAMFactors.h
+++ b/gtsam_unstable/slam/TSAMFactors.h
@@ -47,7 +47,7 @@ public:
   /// Evaluate measurement error h(x)-z
   Vector evaluateError(const Pose2& pose, const Point2& point,
       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
-          boost::none) const {
+          boost::none) const override {
     return pose.transformTo(point, H1, H2) - measured_;
   }
 };
@@ -79,7 +79,7 @@ public:
       boost::optional<Matrix&> H1 = boost::none, //
       boost::optional<Matrix&> H2 = boost::none, //
       boost::optional<Matrix&> H3 = boost::none, //
-      boost::optional<Matrix&> H4 = boost::none) const {
+      boost::optional<Matrix&> H4 = boost::none) const override {
     if (H1 || H2 || H3 || H4) {
       // TODO use fixed-size matrices
       Matrix D_pose_g_base1, D_pose_g_pose;
@@ -134,7 +134,7 @@ public:
       boost::optional<Matrix&> H1 = boost::none, //
       boost::optional<Matrix&> H2 = boost::none, //
       boost::optional<Matrix&> H3 = boost::none, //
-      boost::optional<Matrix&> H4 = boost::none) const {
+      boost::optional<Matrix&> H4 = boost::none) const override {
     if (H1 || H2 || H3 || H4) {
       // TODO use fixed-size matrices
       Matrix D_pose1_g_base1, D_pose1_g_pose1;
diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
index c29e3e794..35080bd42 100644
--- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
+++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
@@ -81,13 +81,13 @@ namespace gtsam {
 
 
     /** Clone */
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
+    gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::make_shared<This>(*this); }
 
 
     /** implement functions needed for Testable */
 
     /** print */
-    virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "TransformBtwRobotsUnaryFactor("
           << keyFormatter(key_) << ")\n";
       std::cout << "MR between factor keys: "
@@ -99,7 +99,7 @@ namespace gtsam {
     }
 
     /** equals */
-    virtual bool equals(const NonlinearFactor& f, double tol=1e-9) const {
+    bool equals(const NonlinearFactor& f, double tol=1e-9) const override {
       const This *t =  dynamic_cast<const This*> (&f);
 
       if(t && Base::equals(f))
@@ -128,7 +128,7 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    virtual double error(const gtsam::Values& x) const {
+    double error(const gtsam::Values& x) const override {
       return whitenedError(x).squaredNorm();
     }
 
@@ -139,7 +139,7 @@ namespace gtsam {
      * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
      */
     /* This version of linearize recalculates the noise model each time */
-    virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x) const {
+    boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x) const override {
       // Only linearize if the factor is active
       if (!this->active(x))
         return boost::shared_ptr<gtsam::JacobianFactor>();
@@ -208,7 +208,7 @@ namespace gtsam {
       return 1;
     }
 
-    virtual size_t dim() const {
+    size_t dim() const override {
       return model_->R().rows() + model_->R().cols();
     }
 
diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
index 48aaa8ed7..2db2844ae 100644
--- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
+++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
@@ -97,13 +97,13 @@ namespace gtsam {
 
 
     /** Clone */
-    virtual NonlinearFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
+    NonlinearFactor::shared_ptr clone() const override { return boost::make_shared<This>(*this); }
 
 
     /** implement functions needed for Testable */
 
     /** print */
-    virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "TransformBtwRobotsUnaryFactorEM("
           << keyFormatter(key_) << ")\n";
       std::cout << "MR between factor keys: "
@@ -119,7 +119,7 @@ namespace gtsam {
     }
 
     /** equals */
-    virtual bool equals(const NonlinearFactor& f, double tol=1e-9) const {
+    bool equals(const NonlinearFactor& f, double tol=1e-9) const override {
       const This *t =  dynamic_cast<const This*> (&f);
 
       if(t && Base::equals(f))
@@ -151,7 +151,7 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    virtual double error(const Values& x) const {
+    double error(const Values& x) const override {
       return whitenedError(x).squaredNorm();
     }
 
@@ -162,7 +162,7 @@ namespace gtsam {
      * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
      */
     /* This version of linearize recalculates the noise model each time */
-    virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
+    boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
       // Only linearize if the factor is active
       if (!this->active(x))
         return boost::shared_ptr<JacobianFactor>();
@@ -406,7 +406,7 @@ namespace gtsam {
       return 1;
     }
 
-    virtual size_t dim() const {
+    size_t dim() const override {
       return model_inlier_->R().rows() + model_inlier_->R().cols();
     }
 
diff --git a/tests/simulated2D.h b/tests/simulated2D.h
index c4930a55b..ed412bba8 100644
--- a/tests/simulated2D.h
+++ b/tests/simulated2D.h
@@ -139,14 +139,14 @@ namespace simulated2D {
     }
 
     /// Return error and optional derivative
-    Vector evaluateError(const Pose& x, boost::optional<Matrix&> H = boost::none) const {
+    Vector evaluateError(const Pose& x, boost::optional<Matrix&> H = boost::none) const override {
       return (prior(x, H) - measured_);
     }
 
     virtual ~GenericPrior() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       return boost::static_pointer_cast<gtsam::NonlinearFactor>(
           gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
@@ -185,14 +185,14 @@ namespace simulated2D {
     /// Evaluate error and optionally return derivatives
     Vector evaluateError(const Pose& x1, const Pose& x2,
         boost::optional<Matrix&> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none) const {
+        boost::optional<Matrix&> H2 = boost::none) const override {
       return (odo(x1, x2, H1, H2) - measured_);
     }
 
     virtual ~GenericOdometry() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       return boost::static_pointer_cast<gtsam::NonlinearFactor>(
           gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
@@ -232,14 +232,14 @@ namespace simulated2D {
     /// Evaluate error and optionally return derivatives
     Vector evaluateError(const Pose& x1, const Landmark& x2,
         boost::optional<Matrix&> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none) const {
+        boost::optional<Matrix&> H2 = boost::none) const override {
       return (mea(x1, x2, H1, H2) - measured_);
     }
 
     virtual ~GenericMeasurement() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       return boost::static_pointer_cast<gtsam::NonlinearFactor>(
           gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h
index deb9292f7..cb8c09fc8 100644
--- a/tests/simulated2DConstraints.h
+++ b/tests/simulated2DConstraints.h
@@ -60,7 +60,7 @@ namespace simulated2D {
       virtual ~ScalarCoordConstraint1() {}
 
       /// @return a deep copy of this factor
-      virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+      gtsam::NonlinearFactor::shared_ptr clone() const override {
         return boost::static_pointer_cast<gtsam::NonlinearFactor>(
             gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
@@ -87,8 +87,8 @@ namespace simulated2D {
        * @param x is the estimate of the constrained variable being evaluated
        * @param H is an optional Jacobian, linearized at x
        */
-      virtual double value(const Point& x, boost::optional<Matrix&> H =
-          boost::none) const {
+      double value(const Point& x, boost::optional<Matrix&> H =
+          boost::none) const override {
         if (H) {
           Matrix D = Matrix::Zero(1, traits<Point>::GetDimension(x));
           D(0, IDX) = 1.0;
@@ -128,7 +128,7 @@ namespace simulated2D {
       virtual ~MaxDistanceConstraint() {}
 
       /// @return a deep copy of this factor
-      virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+      gtsam::NonlinearFactor::shared_ptr clone() const override {
         return boost::static_pointer_cast<gtsam::NonlinearFactor>(
             gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
@@ -150,9 +150,9 @@ namespace simulated2D {
        * @param H2 is an optional Jacobian in x2
        * @return the distance between the variables
        */
-      virtual double value(const Point& x1, const Point& x2,
+      double value(const Point& x1, const Point& x2,
           boost::optional<Matrix&> H1 = boost::none,
-          boost::optional<Matrix&> H2 = boost::none) const {
+          boost::optional<Matrix&> H2 = boost::none) const override {
         if (H1) *H1 = numericalDerivative21(range_trait<Point,Point>, x1, x2, 1e-5);
         if (H1) *H2 = numericalDerivative22(range_trait<Point,Point>, x1, x2, 1e-5);
         return range_trait(x1, x2);
@@ -177,7 +177,7 @@ namespace simulated2D {
       virtual ~MinDistanceConstraint() {}
 
       /// @return a deep copy of this factor
-      virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+      gtsam::NonlinearFactor::shared_ptr clone() const override {
         return boost::static_pointer_cast<gtsam::NonlinearFactor>(
             gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
@@ -200,9 +200,9 @@ namespace simulated2D {
        * @param H2 is an optional Jacobian in x2
        * @return the distance between the variables
        */
-      virtual double value(const Pose& x1, const Point& x2,
+      double value(const Pose& x1, const Point& x2,
           boost::optional<Matrix&> H1 = boost::none,
-          boost::optional<Matrix&> H2 = boost::none) const {
+          boost::optional<Matrix&> H2 = boost::none) const override {
         if (H1) *H1 = numericalDerivative21(range_trait<Pose,Point>, x1, x2, 1e-5);
         if (H1) *H2 = numericalDerivative22(range_trait<Pose,Point>, x1, x2, 1e-5);
         return range_trait(x1, x2);
diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h
index 948a87ce5..27932415b 100644
--- a/tests/simulated2DOriented.h
+++ b/tests/simulated2DOriented.h
@@ -119,12 +119,12 @@ namespace simulated2DOriented {
     /// Evaluate error and optionally derivative
     Vector evaluateError(const VALUE& x1, const VALUE& x2,
         boost::optional<Matrix&> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none) const {
+        boost::optional<Matrix&> H2 = boost::none) const override {
       return measured_.localCoordinates(odo(x1, x2, H1, H2));
     }
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       return boost::static_pointer_cast<gtsam::NonlinearFactor>(
           gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
diff --git a/tests/simulated3D.h b/tests/simulated3D.h
index 3c92e733e..3b4afb106 100644
--- a/tests/simulated3D.h
+++ b/tests/simulated3D.h
@@ -90,7 +90,7 @@ struct PointPrior3D: public NoiseModelFactor1<Point3> {
    * @return Vector error between prior value and x (Dimension: 3)
    */
   Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
-      boost::none) const {
+      boost::none) const override {
     return prior(x, H) - measured_;
   }
 };
@@ -121,7 +121,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2<Point3, Point3> {
    * @return vector error between measurement and prediction (Dimension: 3)
    */
   Vector evaluateError(const Point3& x1, const Point3& x2,
-      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
     return mea(x1, x2, H1, H2) - measured_;
   }
 };
diff --git a/tests/smallExample.h b/tests/smallExample.h
index 2b29a6d10..0c933d106 100644
--- a/tests/smallExample.h
+++ b/tests/smallExample.h
@@ -337,7 +337,7 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> {
     gtsam::NoiseModelFactor1<Point2>(model, key), z_(z) {
   }
 
-  Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const {
+  Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const override {
     if (A) *A = H(x);
     return (h(x) - z_);
   }
diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp
index d759e83e1..0ea507a36 100644
--- a/tests/testExtendedKalmanFilter.cpp
+++ b/tests/testExtendedKalmanFilter.cpp
@@ -153,14 +153,14 @@ public:
   }
 
   /* print */
-  virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << ": NonlinearMotionModel\n";
     std::cout << "  TestKey1: " << keyFormatter(key1()) << std::endl;
     std::cout << "  TestKey2: " << keyFormatter(key2()) << std::endl;
   }
 
   /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
-  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
     const This *e = dynamic_cast<const This*> (&f);
     return (e != nullptr) && (key1() == e->key1()) && (key2() == e->key2());
   }
@@ -169,13 +169,13 @@ public:
    * calculate the error of the factor
    * Override for systems with unusual noise models
    */
-  virtual double error(const Values& c) const {
+  double error(const Values& c) const override {
     Vector w = whitenedError(c);
     return 0.5 * w.dot(w);
   }
 
   /** get the dimension of the factor (number of rows on linearization) */
-  size_t dim() const {
+  size_t dim() const override {
     return 2;
   }
 
@@ -189,7 +189,7 @@ public:
    * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z
    * Hence b = z - h(x1,x2) = - error_vector(x)
    */
-  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const {
+  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
     const X1& x1 = c.at<X1>(key1());
     const X2& x2 = c.at<X2>(key2());
     Matrix A1, A2;
@@ -212,7 +212,7 @@ public:
   /** vector of errors */
   Vector evaluateError(const Point2& p1, const Point2& p2,
       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
-          boost::none) const {
+          boost::none) const override {
 
     // error = p2 - f(p1)
     // H1 = d error / d p1 = -d f/ d p1 = -F
@@ -284,13 +284,13 @@ public:
   }
 
   /* print */
-  virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << ": NonlinearMeasurementModel\n";
     std::cout << "  TestKey: " << keyFormatter(key()) << std::endl;
   }
 
   /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
-  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
     const This *e = dynamic_cast<const This*> (&f);
     return (e != nullptr) && Base::equals(f);
   }
@@ -299,13 +299,13 @@ public:
    * calculate the error of the factor
    * Override for systems with unusual noise models
    */
-  virtual double error(const Values& c) const {
+  double error(const Values& c) const override {
     Vector w = whitenedError(c);
     return 0.5 * w.dot(w);
   }
 
   /** get the dimension of the factor (number of rows on linearization) */
-  size_t dim() const {
+  size_t dim() const override {
     return 1;
   }
 
@@ -319,7 +319,7 @@ public:
    * Ax-b \approx h(x1+dx1)-z = h(x1) + A1*dx1 - z
    * Hence b = z - h(x1) = - error_vector(x)
    */
-  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const {
+  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
     const X& x1 = c.at<X>(key());
     Matrix A1;
     Vector b = -evaluateError(x1, A1);
@@ -336,7 +336,7 @@ public:
   }
 
   /** vector of errors */
-  Vector evaluateError(const Point2& p, boost::optional<Matrix&> H1 = boost::none) const {
+  Vector evaluateError(const Point2& p, boost::optional<Matrix&> H1 = boost::none) const override {
     // error = z - h(p)
     // H = d error / d p = -d h/ d p = -H
     Vector z_hat = h(p);
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index e1156ceba..662b071df 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -239,12 +239,12 @@ public:
   typedef NoiseModelFactor4<double, double, double, double> Base;
   TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {}
 
-  virtual Vector
+  Vector
     evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,
         boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none,
         boost::optional<Matrix&> H3 = boost::none,
-        boost::optional<Matrix&> H4 = boost::none) const {
+        boost::optional<Matrix&> H4 = boost::none) const override {
     if(H1) {
       *H1 = (Matrix(1, 1) << 1.0).finished();
       *H2 = (Matrix(1, 1) << 2.0).finished();
@@ -254,7 +254,7 @@ public:
     return (Vector(1) << x1 + x2 + x3 + x4).finished();
   }
 
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new TestFactor4(*this))); }
 };
@@ -287,13 +287,13 @@ public:
   typedef NoiseModelFactor5<double, double, double, double, double> Base;
   TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5)) {}
 
-  virtual Vector
+  Vector
     evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
         boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none,
         boost::optional<Matrix&> H3 = boost::none,
         boost::optional<Matrix&> H4 = boost::none,
-        boost::optional<Matrix&> H5 = boost::none) const {
+        boost::optional<Matrix&> H5 = boost::none) const override {
     if(H1) {
       *H1 = (Matrix(1, 1) << 1.0).finished();
       *H2 = (Matrix(1, 1) << 2.0).finished();
@@ -336,14 +336,14 @@ public:
   typedef NoiseModelFactor6<double, double, double, double, double, double> Base;
   TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5), X(6)) {}
 
-  virtual Vector
+  Vector
     evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
         boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none,
         boost::optional<Matrix&> H3 = boost::none,
         boost::optional<Matrix&> H4 = boost::none,
         boost::optional<Matrix&> H5 = boost::none,
-        boost::optional<Matrix&> H6 = boost::none) const {
+        boost::optional<Matrix&> H6 = boost::none) const override {
     if(H1) {
       *H1 = (Matrix(1, 1) << 1.0).finished();
       *H2 = (Matrix(1, 1) << 2.0).finished();
diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp
index 88ae508b6..974806612 100644
--- a/tests/testNonlinearISAM.cpp
+++ b/tests/testNonlinearISAM.cpp
@@ -288,7 +288,7 @@ TEST(testNonlinearISAM, loop_closures ) {
       break;
 
     // Check if vertex
-    const auto indexedPose = parseVertex(is, tag);
+    const auto indexedPose = parseVertexPose(is, tag);
     if (indexedPose) {
       Key id = indexedPose->first;
       initialEstimate.insert(Symbol('x', id), indexedPose->second);
diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp
index 5d4c3f844..2616ab103 100644
--- a/tests/testNonlinearOptimizer.cpp
+++ b/tests/testNonlinearOptimizer.cpp
@@ -459,11 +459,12 @@ TEST(NonlinearOptimizer, RobustMeanCalculation) {
   init.insert(0, 100.0);
   expected.insert(0, 3.33333333);
 
-  LevenbergMarquardtParams params;
+  DoglegParams params_dl;
+  params_dl.setRelativeErrorTol(1e-10);
 
   auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
-  auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize();
-  auto dl_result = DoglegOptimizer(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));
@@ -509,8 +510,8 @@ class IterativeLM : public LevenbergMarquardtOptimizer {
         initial_(initialValues) {}
 
   /// Solve that uses conjugate gradient
-  virtual VectorValues solve(const GaussianFactorGraph& gfg,
-                             const NonlinearOptimizerParams& params) const {
+  VectorValues solve(const GaussianFactorGraph& gfg,
+                             const NonlinearOptimizerParams& params) const override {
     VectorValues zeros = initial_.zeroVectors();
     return conjugateGradientDescent(gfg, zeros, cgParams_);
   }
diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp
index e3252a90b..84e521156 100644
--- a/tests/testSerializationSLAM.cpp
+++ b/tests/testSerializationSLAM.cpp
@@ -18,15 +18,10 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#if 0
-
 #include <tests/smallExample.h>
-//#include <gtsam/slam/AntiFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-//#include <gtsam/slam/BoundingConstraint.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
-//#include <gtsam/slam/PartialPriorFactor.h>
 #include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/sam/RangeFactor.h>
@@ -34,8 +29,6 @@
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/GaussianISAM.h>
-#include <gtsam/base/LieVector.h>
-#include <gtsam/base/LieMatrix.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/StereoPoint2.h>
 #include <gtsam/geometry/Point3.h>
@@ -49,6 +42,7 @@
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/StereoCamera.h>
+#include <gtsam/geometry/SimpleCamera.h>
 
 #include <gtsam/base/serializationTestHelpers.h>
 
@@ -57,8 +51,6 @@ 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;
@@ -69,12 +61,9 @@ 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;
 typedef BetweenFactor<Point2>          BetweenFactorPoint2;
 typedef BetweenFactor<Point3>          BetweenFactorPoint3;
 typedef BetweenFactor<Rot2>            BetweenFactorRot2;
@@ -82,8 +71,6 @@ 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;
@@ -94,7 +81,6 @@ typedef NonlinearEquality<Pose3>                  NonlinearEqualityPose3;
 typedef NonlinearEquality<Cal3_S2>                NonlinearEqualityCal3_S2;
 typedef NonlinearEquality<Cal3DS2>                NonlinearEqualityCal3DS2;
 typedef NonlinearEquality<CalibratedCamera>       NonlinearEqualityCalibratedCamera;
-typedef NonlinearEquality<SimpleCamera>           NonlinearEqualitySimpleCamera;
 typedef NonlinearEquality<PinholeCameraCal3_S2>   NonlinearEqualityPinholeCameraCal3_S2;
 typedef NonlinearEquality<StereoCamera>           NonlinearEqualityStereoCamera;
 
@@ -148,8 +134,6 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
 
 /* Create GUIDs for geometry */
 /* ************************************************************************* */
-GTSAM_VALUE_EXPORT(gtsam::LieVector);
-GTSAM_VALUE_EXPORT(gtsam::LieMatrix);
 GTSAM_VALUE_EXPORT(gtsam::Point2);
 GTSAM_VALUE_EXPORT(gtsam::StereoPoint2);
 GTSAM_VALUE_EXPORT(gtsam::Point3);
@@ -170,8 +154,6 @@ GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
 BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
 BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
 
-BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector");
-BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix");
 BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2");
 BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2");
 BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3");
@@ -182,11 +164,8 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3");
 BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2");
 BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2");
 BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera");
-BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera");
 BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera");
 
-BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector");
-BOOST_CLASS_EXPORT_GUID(BetweenFactorLieMatrix, "gtsam::BetweenFactorLieMatrix");
 BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2");
 BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3");
 BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2");
@@ -194,8 +173,6 @@ BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3");
 BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2");
 BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3");
 
-BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieVector, "gtsam::NonlinearEqualityLieVector");
-BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieMatrix, "gtsam::NonlinearEqualityLieMatrix");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3");
@@ -206,7 +183,6 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3")
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera");
-BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera");
 
 BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D");
@@ -286,8 +262,6 @@ TEST (testSerializationSLAM, smallExample_nonlinear) {
 /* ************************************************************************* */
 TEST (testSerializationSLAM, factors) {
 
-  LieVector lieVector((Vector(4) << 1.0, 2.0, 3.0, 4.0).finished());
-  LieMatrix lieMatrix((Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0).finished());
   Point2 point2(1.0, 2.0);
   StereoPoint2 stereoPoint2(1.0, 2.0, 3.0);
   Point3 point3(1.0, 2.0, 3.0);
@@ -311,8 +285,6 @@ TEST (testSerializationSLAM, factors) {
           b11('b',11), b12('b',12), b13('b',13), b14('b',14), b15('b',15);
 
   Values values;
-  values.insert(a01, lieVector);
-  values.insert(a02, lieMatrix);
   values.insert(a03, point2);
   values.insert(a04, stereoPoint2);
   values.insert(a05, point3);
@@ -344,8 +316,6 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsDereferencedXML(robust1));
   EXPECT(equalsDereferencedBinary(robust1));
 
-  PriorFactorLieVector priorFactorLieVector(a01, lieVector, model4);
-  PriorFactorLieMatrix priorFactorLieMatrix(a02, lieMatrix, model6);
   PriorFactorPoint2 priorFactorPoint2(a03, point2, model2);
   PriorFactorStereoPoint2 priorFactorStereoPoint2(a04, stereoPoint2, model3);
   PriorFactorPoint3 priorFactorPoint3(a05, point3, model3);
@@ -356,11 +326,8 @@ TEST (testSerializationSLAM, factors) {
   PriorFactorCal3_S2 priorFactorCal3_S2(a10, cal3_s2, model5);
   PriorFactorCal3DS2 priorFactorCal3DS2(a11, cal3ds2, model9);
   PriorFactorCalibratedCamera priorFactorCalibratedCamera(a12, calibratedCamera, model6);
-  PriorFactorSimpleCamera priorFactorSimpleCamera(a13, simpleCamera, model11);
   PriorFactorStereoCamera priorFactorStereoCamera(a14, stereoCamera, model11);
 
-  BetweenFactorLieVector betweenFactorLieVector(a01, b01, lieVector, model4);
-  BetweenFactorLieMatrix betweenFactorLieMatrix(a02, b02, lieMatrix, model6);
   BetweenFactorPoint2 betweenFactorPoint2(a03, b03, point2, model2);
   BetweenFactorPoint3 betweenFactorPoint3(a05, b05, point3, model3);
   BetweenFactorRot2 betweenFactorRot2(a06, b06, rot2, model1);
@@ -368,8 +335,6 @@ TEST (testSerializationSLAM, factors) {
   BetweenFactorPose2 betweenFactorPose2(a08, b08, pose2, model3);
   BetweenFactorPose3 betweenFactorPose3(a09, b09, pose3, model6);
 
-  NonlinearEqualityLieVector nonlinearEqualityLieVector(a01, lieVector);
-  NonlinearEqualityLieMatrix nonlinearEqualityLieMatrix(a02, lieMatrix);
   NonlinearEqualityPoint2 nonlinearEqualityPoint2(a03, point2);
   NonlinearEqualityStereoPoint2 nonlinearEqualityStereoPoint2(a04, stereoPoint2);
   NonlinearEqualityPoint3 nonlinearEqualityPoint3(a05, point3);
@@ -380,7 +345,6 @@ TEST (testSerializationSLAM, factors) {
   NonlinearEqualityCal3_S2 nonlinearEqualityCal3_S2(a10, cal3_s2);
   NonlinearEqualityCal3DS2 nonlinearEqualityCal3DS2(a11, cal3ds2);
   NonlinearEqualityCalibratedCamera nonlinearEqualityCalibratedCamera(a12, calibratedCamera);
-  NonlinearEqualitySimpleCamera nonlinearEqualitySimpleCamera(a13, simpleCamera);
   NonlinearEqualityStereoCamera nonlinearEqualityStereoCamera(a14, stereoCamera);
 
   RangeFactor2D rangeFactor2D(a08, a03, 2.0, model1);
@@ -405,8 +369,6 @@ TEST (testSerializationSLAM, factors) {
 
 
   NonlinearFactorGraph graph;
-  graph += priorFactorLieVector;
-  graph += priorFactorLieMatrix;
   graph += priorFactorPoint2;
   graph += priorFactorStereoPoint2;
   graph += priorFactorPoint3;
@@ -417,11 +379,8 @@ TEST (testSerializationSLAM, factors) {
   graph += priorFactorCal3_S2;
   graph += priorFactorCal3DS2;
   graph += priorFactorCalibratedCamera;
-  graph += priorFactorSimpleCamera;
   graph += priorFactorStereoCamera;
 
-  graph += betweenFactorLieVector;
-  graph += betweenFactorLieMatrix;
   graph += betweenFactorPoint2;
   graph += betweenFactorPoint3;
   graph += betweenFactorRot2;
@@ -429,8 +388,6 @@ TEST (testSerializationSLAM, factors) {
   graph += betweenFactorPose2;
   graph += betweenFactorPose3;
 
-  graph += nonlinearEqualityLieVector;
-  graph += nonlinearEqualityLieMatrix;
   graph += nonlinearEqualityPoint2;
   graph += nonlinearEqualityStereoPoint2;
   graph += nonlinearEqualityPoint3;
@@ -441,7 +398,6 @@ TEST (testSerializationSLAM, factors) {
   graph += nonlinearEqualityCal3_S2;
   graph += nonlinearEqualityCal3DS2;
   graph += nonlinearEqualityCalibratedCamera;
-  graph += nonlinearEqualitySimpleCamera;
   graph += nonlinearEqualityStereoCamera;
 
   graph += rangeFactor2D;
@@ -471,8 +427,6 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsObj<Values>(values));
   EXPECT(equalsObj<NonlinearFactorGraph>(graph));
 
-  EXPECT(equalsObj<PriorFactorLieVector>(priorFactorLieVector));
-  EXPECT(equalsObj<PriorFactorLieMatrix>(priorFactorLieMatrix));
   EXPECT(equalsObj<PriorFactorPoint2>(priorFactorPoint2));
   EXPECT(equalsObj<PriorFactorStereoPoint2>(priorFactorStereoPoint2));
   EXPECT(equalsObj<PriorFactorPoint3>(priorFactorPoint3));
@@ -483,11 +437,8 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsObj<PriorFactorCal3_S2>(priorFactorCal3_S2));
   EXPECT(equalsObj<PriorFactorCal3DS2>(priorFactorCal3DS2));
   EXPECT(equalsObj<PriorFactorCalibratedCamera>(priorFactorCalibratedCamera));
-  EXPECT(equalsObj<PriorFactorSimpleCamera>(priorFactorSimpleCamera));
   EXPECT(equalsObj<PriorFactorStereoCamera>(priorFactorStereoCamera));
 
-  EXPECT(equalsObj<BetweenFactorLieVector>(betweenFactorLieVector));
-  EXPECT(equalsObj<BetweenFactorLieMatrix>(betweenFactorLieMatrix));
   EXPECT(equalsObj<BetweenFactorPoint2>(betweenFactorPoint2));
   EXPECT(equalsObj<BetweenFactorPoint3>(betweenFactorPoint3));
   EXPECT(equalsObj<BetweenFactorRot2>(betweenFactorRot2));
@@ -495,8 +446,6 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsObj<BetweenFactorPose2>(betweenFactorPose2));
   EXPECT(equalsObj<BetweenFactorPose3>(betweenFactorPose3));
 
-  EXPECT(equalsObj<NonlinearEqualityLieVector>(nonlinearEqualityLieVector));
-  EXPECT(equalsObj<NonlinearEqualityLieMatrix>(nonlinearEqualityLieMatrix));
   EXPECT(equalsObj<NonlinearEqualityPoint2>(nonlinearEqualityPoint2));
   EXPECT(equalsObj<NonlinearEqualityStereoPoint2>(nonlinearEqualityStereoPoint2));
   EXPECT(equalsObj<NonlinearEqualityPoint3>(nonlinearEqualityPoint3));
@@ -507,7 +456,6 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsObj<NonlinearEqualityCal3_S2>(nonlinearEqualityCal3_S2));
   EXPECT(equalsObj<NonlinearEqualityCal3DS2>(nonlinearEqualityCal3DS2));
   EXPECT(equalsObj<NonlinearEqualityCalibratedCamera>(nonlinearEqualityCalibratedCamera));
-  EXPECT(equalsObj<NonlinearEqualitySimpleCamera>(nonlinearEqualitySimpleCamera));
   EXPECT(equalsObj<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
 
   EXPECT(equalsObj<RangeFactor2D>(rangeFactor2D));
@@ -537,8 +485,6 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsXML<Values>(values));
   EXPECT(equalsXML<NonlinearFactorGraph>(graph));
 
-  EXPECT(equalsXML<PriorFactorLieVector>(priorFactorLieVector));
-  EXPECT(equalsXML<PriorFactorLieMatrix>(priorFactorLieMatrix));
   EXPECT(equalsXML<PriorFactorPoint2>(priorFactorPoint2));
   EXPECT(equalsXML<PriorFactorStereoPoint2>(priorFactorStereoPoint2));
   EXPECT(equalsXML<PriorFactorPoint3>(priorFactorPoint3));
@@ -549,11 +495,8 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsXML<PriorFactorCal3_S2>(priorFactorCal3_S2));
   EXPECT(equalsXML<PriorFactorCal3DS2>(priorFactorCal3DS2));
   EXPECT(equalsXML<PriorFactorCalibratedCamera>(priorFactorCalibratedCamera));
-  EXPECT(equalsXML<PriorFactorSimpleCamera>(priorFactorSimpleCamera));
   EXPECT(equalsXML<PriorFactorStereoCamera>(priorFactorStereoCamera));
 
-  EXPECT(equalsXML<BetweenFactorLieVector>(betweenFactorLieVector));
-  EXPECT(equalsXML<BetweenFactorLieMatrix>(betweenFactorLieMatrix));
   EXPECT(equalsXML<BetweenFactorPoint2>(betweenFactorPoint2));
   EXPECT(equalsXML<BetweenFactorPoint3>(betweenFactorPoint3));
   EXPECT(equalsXML<BetweenFactorRot2>(betweenFactorRot2));
@@ -561,8 +504,6 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsXML<BetweenFactorPose2>(betweenFactorPose2));
   EXPECT(equalsXML<BetweenFactorPose3>(betweenFactorPose3));
 
-  EXPECT(equalsXML<NonlinearEqualityLieVector>(nonlinearEqualityLieVector));
-  EXPECT(equalsXML<NonlinearEqualityLieMatrix>(nonlinearEqualityLieMatrix));
   EXPECT(equalsXML<NonlinearEqualityPoint2>(nonlinearEqualityPoint2));
   EXPECT(equalsXML<NonlinearEqualityStereoPoint2>(nonlinearEqualityStereoPoint2));
   EXPECT(equalsXML<NonlinearEqualityPoint3>(nonlinearEqualityPoint3));
@@ -573,7 +514,6 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsXML<NonlinearEqualityCal3_S2>(nonlinearEqualityCal3_S2));
   EXPECT(equalsXML<NonlinearEqualityCal3DS2>(nonlinearEqualityCal3DS2));
   EXPECT(equalsXML<NonlinearEqualityCalibratedCamera>(nonlinearEqualityCalibratedCamera));
-  EXPECT(equalsXML<NonlinearEqualitySimpleCamera>(nonlinearEqualitySimpleCamera));
   EXPECT(equalsXML<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
 
   EXPECT(equalsXML<RangeFactor2D>(rangeFactor2D));
@@ -603,8 +543,6 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsBinary<Values>(values));
   EXPECT(equalsBinary<NonlinearFactorGraph>(graph));
 
-  EXPECT(equalsBinary<PriorFactorLieVector>(priorFactorLieVector));
-  EXPECT(equalsBinary<PriorFactorLieMatrix>(priorFactorLieMatrix));
   EXPECT(equalsBinary<PriorFactorPoint2>(priorFactorPoint2));
   EXPECT(equalsBinary<PriorFactorStereoPoint2>(priorFactorStereoPoint2));
   EXPECT(equalsBinary<PriorFactorPoint3>(priorFactorPoint3));
@@ -615,11 +553,8 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsBinary<PriorFactorCal3_S2>(priorFactorCal3_S2));
   EXPECT(equalsBinary<PriorFactorCal3DS2>(priorFactorCal3DS2));
   EXPECT(equalsBinary<PriorFactorCalibratedCamera>(priorFactorCalibratedCamera));
-  EXPECT(equalsBinary<PriorFactorSimpleCamera>(priorFactorSimpleCamera));
   EXPECT(equalsBinary<PriorFactorStereoCamera>(priorFactorStereoCamera));
 
-  EXPECT(equalsBinary<BetweenFactorLieVector>(betweenFactorLieVector));
-  EXPECT(equalsBinary<BetweenFactorLieMatrix>(betweenFactorLieMatrix));
   EXPECT(equalsBinary<BetweenFactorPoint2>(betweenFactorPoint2));
   EXPECT(equalsBinary<BetweenFactorPoint3>(betweenFactorPoint3));
   EXPECT(equalsBinary<BetweenFactorRot2>(betweenFactorRot2));
@@ -627,8 +562,6 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsBinary<BetweenFactorPose2>(betweenFactorPose2));
   EXPECT(equalsBinary<BetweenFactorPose3>(betweenFactorPose3));
 
-  EXPECT(equalsBinary<NonlinearEqualityLieVector>(nonlinearEqualityLieVector));
-  EXPECT(equalsBinary<NonlinearEqualityLieMatrix>(nonlinearEqualityLieMatrix));
   EXPECT(equalsBinary<NonlinearEqualityPoint2>(nonlinearEqualityPoint2));
   EXPECT(equalsBinary<NonlinearEqualityStereoPoint2>(nonlinearEqualityStereoPoint2));
   EXPECT(equalsBinary<NonlinearEqualityPoint3>(nonlinearEqualityPoint3));
@@ -639,7 +572,6 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsBinary<NonlinearEqualityCal3_S2>(nonlinearEqualityCal3_S2));
   EXPECT(equalsBinary<NonlinearEqualityCal3DS2>(nonlinearEqualityCal3DS2));
   EXPECT(equalsBinary<NonlinearEqualityCalibratedCamera>(nonlinearEqualityCalibratedCamera));
-  EXPECT(equalsBinary<NonlinearEqualitySimpleCamera>(nonlinearEqualitySimpleCamera));
   EXPECT(equalsBinary<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
 
   EXPECT(equalsBinary<RangeFactor2D>(rangeFactor2D));
@@ -663,7 +595,6 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsBinary<GenericStereoFactor3D>(genericStereoFactor3D));
 }
 
-#endif
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
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/timing/timeFactorOverhead.cpp b/timing/timeFactorOverhead.cpp
index d9c19703c..038901388 100644
--- a/timing/timeFactorOverhead.cpp
+++ b/timing/timeFactorOverhead.cpp
@@ -22,13 +22,14 @@
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/linear/VectorValues.h>
 
-#include <boost/random.hpp>
+#include <random>
 #include <vector>
 
 using namespace gtsam;
 using namespace std;
 
-static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0));
+static std::mt19937 rng;
+static std::uniform_real_distribution<> uniform(0.0, 1.0);
 
 int main(int argc, char *argv[]) {
 
@@ -64,10 +65,10 @@ int main(int argc, char *argv[]) {
         Matrix A(blockdim, vardim);
         for(size_t j=0; j<blockdim; ++j)
           for(size_t k=0; k<vardim; ++k)
-            A(j,k) = rg();
+            A(j,k) = uniform(rng);
         Vector b(blockdim);
         for(size_t j=0; j<blockdim; ++j)
-          b(j) = rg();
+          b(j) = uniform(rng);
         blockGfgs[trial].push_back(boost::make_shared<JacobianFactor>(key, A, b, noise));
       }
     }
@@ -111,10 +112,10 @@ int main(int argc, char *argv[]) {
         // Generate a random Gaussian factor
         for(size_t j=0; j<blockdim; ++j)
           for(size_t k=0; k<vardim; ++k)
-            Acomb(blockdim*i+j, k) = rg();
+            Acomb(blockdim*i+j, k) = uniform(rng);
         Vector b(blockdim);
         for(size_t j=0; j<blockdim; ++j)
-          bcomb(blockdim*i+j) = rg();
+          bcomb(blockdim*i+j) = uniform(rng);
       }
       combGfgs[trial].push_back(boost::make_shared<JacobianFactor>(key, Acomb, bcomb,
           noiseModel::Isotropic::Sigma(blockdim*nBlocks, 1.0)));
diff --git a/timing/timeFrobeniusFactor.cpp b/timing/timeFrobeniusFactor.cpp
new file mode 100644
index 000000000..8bd754de6
--- /dev/null
+++ b/timing/timeFrobeniusFactor.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    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 <random>
+#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], Rot3(Tij.rotation().matrix()), 4, model);
+  }
+
+  std::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/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp
index d9f2ffaf6..95333fbf8 100644
--- a/timing/timeMatrixOps.cpp
+++ b/timing/timeMatrixOps.cpp
@@ -16,7 +16,6 @@
  * @date    Sep 18, 2010
  */
 
-#include <boost/random.hpp>
 #include <boost/format.hpp>
 #include <boost/lambda/lambda.hpp>
 
@@ -24,6 +23,7 @@
 #include <gtsam/base/Matrix.h>
 
 #include <iostream>
+#include <random>
 #include <vector>
 #include <utility>
 
@@ -33,7 +33,8 @@ using namespace std;
 using boost::format;
 using namespace boost::lambda;
 
-static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rng(boost::mt19937(), boost::uniform_real<>(-1.0, 0.0));
+static std::mt19937 rng;
+static std::uniform_real_distribution<> uniform(-1.0, 0.0);
 //typedef ublas::matrix<double> matrix;
 //typedef ublas::matrix_range<matrix> matrix_range;
 //typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix;
@@ -53,8 +54,8 @@ int main(int argc, char* argv[]) {
     volatile size_t n=300;
     volatile size_t nReps = 1000;
     assert(m > n);
-    boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rni(boost::mt19937(), boost::uniform_int<size_t>(0,m-1));
-    boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rnj(boost::mt19937(), boost::uniform_int<size_t>(0,n-1));
+    std::uniform_int_distribution<size_t> uniform_i(0,m-1);
+    std::uniform_int_distribution<size_t> uniform_j(0,n-1);
     gtsam::Matrix mat((int)m,(int)n);
     gtsam::SubMatrix full = mat.block(0, 0, m, n);
     gtsam::SubMatrix top = mat.block(0, 0, n, n);
@@ -75,13 +76,13 @@ int main(int argc, char* argv[]) {
       for(size_t rep=0; rep<1000; ++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();
+            mat(i,j) = uniform(rng);
 
       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();
+            mat(i,j) = uniform(rng);
       gttoc_(basicTime);
       tictoc_getNode(basicTimeNode, basicTime);
       basicTime = basicTimeNode->secs();
@@ -92,7 +93,7 @@ int main(int argc, char* argv[]) {
       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();
+            full(i,j) = uniform(rng);
       gttoc_(fullTime);
       tictoc_getNode(fullTimeNode, fullTime);
       fullTime = fullTimeNode->secs();
@@ -103,7 +104,7 @@ int main(int argc, char* argv[]) {
       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();
+            top(i,j) = uniform(rng);
       gttoc_(topTime);
       tictoc_getNode(topTimeNode, topTime);
       topTime = topTimeNode->secs();
@@ -114,7 +115,7 @@ int main(int argc, char* argv[]) {
       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();
+            block(i,j) = uniform(rng);
       gttoc_(blockTime);
       tictoc_getNode(blockTimeNode, blockTime);
       blockTime = blockTimeNode->secs();
@@ -133,13 +134,13 @@ int main(int argc, char* argv[]) {
       for(size_t rep=0; rep<1000; ++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();
+            mat(i,j) = uniform(rng);
 
       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();
+            mat(i,j) = uniform(rng);
       gttoc_(basicTime);
       tictoc_getNode(basicTimeNode, basicTime);
       basicTime = basicTimeNode->secs();
@@ -150,7 +151,7 @@ int main(int argc, char* argv[]) {
       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();
+            full(i,j) = uniform(rng);
       gttoc_(fullTime);
       tictoc_getNode(fullTimeNode, fullTime);
       fullTime = fullTimeNode->secs();
@@ -161,7 +162,7 @@ int main(int argc, char* argv[]) {
       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();
+            top(i,j) = uniform(rng);
       gttoc_(topTime);
       tictoc_getNode(topTimeNode, topTime);
       topTime = topTimeNode->secs();
@@ -172,7 +173,7 @@ int main(int argc, char* argv[]) {
       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();
+            block(i,j) = uniform(rng);
       gttoc_(blockTime);
       tictoc_getNode(blockTimeNode, blockTime);
       blockTime = blockTimeNode->secs();
@@ -190,14 +191,14 @@ int main(int argc, char* argv[]) {
       cout << "Row-major matrix, random assignment:" << endl;
 
       // Do a few initial assignments to let any cache effects stabilize
-      for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj()));
+      for_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng),uniform_j(rng)));
       for(size_t rep=0; rep<1000; ++rep)
-        for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rng(); }
+        for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = uniform(rng); }
 
       gttic_(basicTime);
-      for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj()));
+      for_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng),uniform_j(rng)));
       for(size_t rep=0; rep<1000; ++rep)
-        for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rng(); }
+        for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = uniform(rng); }
       gttoc_(basicTime);
       tictoc_getNode(basicTimeNode, basicTime);
       basicTime = basicTimeNode->secs();
@@ -205,9 +206,9 @@ int main(int argc, char* argv[]) {
       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_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng),uniform_j(rng)));
       for(size_t rep=0; rep<1000; ++rep)
-        for(const ij_t& ij: ijs) { full(ij.first, ij.second) = rng(); }
+        for(const ij_t& ij: ijs) { full(ij.first, ij.second) = uniform(rng); }
       gttoc_(fullTime);
       tictoc_getNode(fullTimeNode, fullTime);
       fullTime = fullTimeNode->secs();
@@ -215,9 +216,9 @@ int main(int argc, char* argv[]) {
       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_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng)%top.rows(),uniform_j(rng)));
       for(size_t rep=0; rep<1000; ++rep)
-        for(const ij_t& ij: ijs) { top(ij.first, ij.second) = rng(); }
+        for(const ij_t& ij: ijs) { top(ij.first, ij.second) = uniform(rng); }
       gttoc_(topTime);
       tictoc_getNode(topTimeNode, topTime);
       topTime = topTimeNode->secs();
@@ -225,9 +226,9 @@ int main(int argc, char* argv[]) {
       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_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng)%block.rows(),uniform_j(rng)%block.cols()));
       for(size_t rep=0; rep<1000; ++rep)
-        for(const ij_t& ij: ijs) { block(ij.first, ij.second) = rng(); }
+        for(const ij_t& ij: ijs) { block(ij.first, ij.second) = uniform(rng); }
       gttoc_(blockTime);
       tictoc_getNode(blockTimeNode, blockTime);
       blockTime = blockTimeNode->secs();
@@ -250,7 +251,7 @@ int main(int argc, char* argv[]) {
 //    matrix mat(5,5);
 //    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();
+//        mat(i,j) = uniform(rng);
 //
 //    tri = ublas::triangular_adaptor<matrix, ublas::upper>(mat);
 //    cout << "  Assigned from triangular adapter: " << tri << endl;
@@ -259,13 +260,13 @@ int main(int argc, char* argv[]) {
 //
 //    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();
+//        mat(i,j) = uniform(rng);
 //    mat = tri;
 //    cout << "  Assign matrix from triangular: " << mat << endl;
 //
 //    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();
+//        mat(i,j) = uniform(rng);
 //    (ublas::triangular_adaptor<matrix, ublas::upper>(mat)) = tri;
 //    cout << "  Assign triangular adaptor from triangular: " << mat << endl;
 //  }
@@ -281,7 +282,7 @@ int main(int argc, char* argv[]) {
 //    matrix mat(5,7);
 //    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();
+//        mat(i,j) = uniform(rng);
 //
 //    tri = ublas::triangular_adaptor<matrix, ublas::upper>(mat);
 //    cout << "  Assigned from triangular adapter: " << tri << endl;
@@ -290,13 +291,13 @@ int main(int argc, char* argv[]) {
 //
 //    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();
+//        mat(i,j) = uniform(rng);
 //    mat = tri;
 //    cout << "  Assign matrix from triangular: " << mat << endl;
 //
 //    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();
+//        mat(i,j) = uniform(rng);
 //    mat = ublas::triangular_adaptor<matrix, ublas::upper>(mat);
 //    cout << "  Assign matrix from triangular adaptor of self: " << mat << endl;
 //  }
diff --git a/wrap/Method.h b/wrap/Method.h
index 88a73cd80..4d3c8d909 100644
--- a/wrap/Method.h
+++ b/wrap/Method.h
@@ -37,7 +37,7 @@ public:
       boost::optional<const Qualified> instName = boost::none, bool verbose =
           false);
 
-  virtual bool isStatic() const {
+  bool isStatic() const override {
     return false;
   }
 
@@ -64,10 +64,10 @@ public:
 private:
 
   // Emit method header
-  void proxy_header(FileWriter& proxyFile) const;
+  void proxy_header(FileWriter& proxyFile) const override;
 
-  virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
-      Str matlabUniqueName, const ArgumentList& args) const;
+  std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
+      Str matlabUniqueName, const ArgumentList& args) const override;
 };
 
 } // \namespace wrap
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
index ec02dc412..780c6f8da 100644
--- a/wrap/Module.cpp
+++ b/wrap/Module.cpp
@@ -248,7 +248,7 @@ void Module::parseMarkup(const std::string& data) {
   // Create type attributes table and check validity
   typeAttributes.addClasses(expandedClasses);
   typeAttributes.addForwardDeclarations(forward_declarations);
-  for (const TypedefPair p: typedefs)
+  for (const TypedefPair& p: typedefs)
     typeAttributes.addType(p.newType);
   // add Eigen types as template arguments are also checked ?
   vector<ForwardDeclaration> eigen;
diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h
index ba5086507..8d78bb48f 100644
--- a/wrap/ReturnType.h
+++ b/wrap/ReturnType.h
@@ -30,7 +30,7 @@ struct ReturnType : public Qualified {
   ReturnType(const std::string& name, Category c = CLASS, bool ptr = false)
       : Qualified(name, c), isPtr(ptr) {}
 
-  virtual void clear() {
+  void clear() override {
     Qualified::clear();
     isPtr = false;
   }
diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h
index 8392a1cc5..dbb918596 100644
--- a/wrap/StaticMethod.h
+++ b/wrap/StaticMethod.h
@@ -41,10 +41,10 @@ struct StaticMethod: public MethodBase {
 
 protected:
 
-  virtual void proxy_header(FileWriter& proxyFile) const;
+  void proxy_header(FileWriter& proxyFile) const override;
 
-  virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
-      Str matlabUniqueName, const ArgumentList& args) const;
+  std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
+      Str matlabUniqueName, const ArgumentList& args) const override;
 };
 
 } // \namespace wrap
diff --git a/wrap/utilities.h b/wrap/utilities.h
index 17c5cba12..8c6f9a0b2 100644
--- a/wrap/utilities.h
+++ b/wrap/utilities.h
@@ -40,7 +40,7 @@ class CantOpenFile : public std::exception {
  public:
  CantOpenFile(const std::string& filename) : what_("Can't open file " + filename) {}
   ~CantOpenFile() throw() {}
-  virtual const char* what() const throw() { return what_.c_str(); }
+  const char* what() const noexcept override { return what_.c_str(); }
 };
 
 class OutputError : public std::exception {
@@ -49,7 +49,7 @@ private:
 public:
   OutputError(const std::string& what) : what_(what) {}
   ~OutputError() throw() {}
-  virtual const char* what() const throw() { return what_.c_str(); }
+  const char* what() const noexcept override { return what_.c_str(); }
 };
 
 class ParseFailed : public std::exception {
@@ -58,7 +58,7 @@ class ParseFailed : public std::exception {
  public:
    ParseFailed(int length) : what_((boost::format("Parse failed at character [%d]")%(length-1)).str()) {}
    ~ParseFailed() throw() {}
-   virtual const char* what() const throw() { return what_.c_str(); }
+   const char* what() const noexcept override { return what_.c_str(); }
 };
 
 class DependencyMissing : public std::exception {
@@ -68,7 +68,7 @@ public:
   DependencyMissing(const std::string& dep, const std::string& loc) :
     what_("Missing dependency '" + dep + "' in " + loc) {}
   ~DependencyMissing() throw() {}
-  virtual const char* what() const throw() { return what_.c_str(); }
+  const char* what() const noexcept override { return what_.c_str(); }
 };
 
 class DuplicateDefinition : public std::exception {
@@ -78,7 +78,7 @@ public:
   DuplicateDefinition(const std::string& name) :
     what_("Duplicate definition of " + name) {}
   ~DuplicateDefinition() throw() {}
-  virtual const char* what() const throw() { return what_.c_str(); }
+  const char* what() const noexcept override { return what_.c_str(); }
 };
 
 class AttributeError : public std::exception {
@@ -88,7 +88,7 @@ public:
   AttributeError(const std::string& name, const std::string& problem) :
     what_("Class " + name + ": " + problem) {}
   ~AttributeError() throw() {}
-  virtual const char* what() const throw() { return what_.c_str(); }
+  const char* what() const noexcept override { return what_.c_str(); }
 };
 
 // "Unique" key to signal calling the matlab object constructor with a raw pointer