diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh
index 5cc6ada24..38359e87d 100644
--- a/.github/scripts/python.sh
+++ b/.github/scripts/python.sh
@@ -47,8 +47,6 @@ case $WRAPPER in
     ;;
 esac
 
-git submodule update --init --recursive
-
 mkdir $GITHUB_WORKSPACE/build
 cd $GITHUB_WORKSPACE/build
 
@@ -82,9 +80,9 @@ case $WRAPPER in
     $PYTHON -m unittest discover
     ;;
 "pybind")
-    cd $GITHUB_WORKSPACE/python
+    cd $GITHUB_WORKSPACE/build/python
     $PYTHON setup.py install --user --prefix=
-    cd $GITHUB_WORKSPACE/wrap/python/gtsam_py/tests
+    cd $GITHUB_WORKSPACE/python/gtsam/tests
     $PYTHON -m unittest discover
     ;;
 *)
diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh
index aa6e49650..f04100d0a 100644
--- a/.github/scripts/unix.sh
+++ b/.github/scripts/unix.sh
@@ -47,7 +47,6 @@ function configure()
   BUILD_DIR=$GITHUB_WORKSPACE/build
 
   #env
-  git submodule update --init --recursive
   rm -fr $BUILD_DIR || true
   mkdir $BUILD_DIR && cd $BUILD_DIR
 
@@ -64,11 +63,11 @@ function configure()
       -DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
       -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
       -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
-      -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
       -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
-      -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \
+      -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \
+      -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
+      -DGTSAM_TYPEDEF_POINTS_TO_VECTOR=${GTSAM_TYPEDEF_POINTS_TO_VECTOR:-OFF} \
       -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-      -DCMAKE_VERBOSE_MAKEFILE=ON \
       -DBOOST_ROOT=$BOOST_ROOT \
       -DBoost_NO_SYSTEM_PATHS=ON \
       -DBoost_ARCHITECTURE=-x64
diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml
index 911bec59c..afe328c3b 100644
--- a/.github/workflows/build-linux.yml
+++ b/.github/workflows/build-linux.yml
@@ -72,7 +72,7 @@ jobs:
         if: runner.os == 'Linux'
         run: |
           echo "BOOST_ROOT = $BOOST_ROOT"
-      - name: Build (Linux)
+      - name: Build and Test (Linux)
         if: runner.os == 'Linux'
         run: |
           bash .github/scripts/unix.sh -t
\ No newline at end of file
diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml
index 55d9071ef..363cd690f 100644
--- a/.github/workflows/build-macos.yml
+++ b/.github/workflows/build-macos.yml
@@ -45,7 +45,7 @@ jobs:
             echo "::set-env name=CC::clang"
             echo "::set-env name=CXX::clang++"
           fi
-      - name: Build (macOS)
+      - name: Build and Test (macOS)
         if: runner.os == 'macOS'
         run: |
           bash .github/scripts/unix.sh -t
\ No newline at end of file
diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml
index 0b4a7f12f..7a09c13d1 100644
--- a/.github/workflows/build-python.yml
+++ b/.github/workflows/build-python.yml
@@ -20,24 +20,25 @@ jobs:
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
           ubuntu-18.04-gcc-5,
-          ubuntu-18.04-gcc-9,
+          # ubuntu-18.04-gcc-9,  # TODO Disabled for now because of timeouts
           ubuntu-18.04-clang-9,
           macOS-10.15-xcode-11.3.1,
         ]
 
         build_type: [Debug, Release]
         python_version: [3]
-        wrapper: [cython]
+        wrapper: [pybind]
         include:
           - name: ubuntu-18.04-gcc-5
             os: ubuntu-18.04
             compiler: gcc
             version: "5"
 
-          - name: ubuntu-18.04-gcc-9
-            os: ubuntu-18.04
-            compiler: gcc
-            version: "9"
+          # TODO Disabled for now because of timeouts
+          # - name: ubuntu-18.04-gcc-9
+          #   os: ubuntu-18.04
+          #   compiler: gcc
+          #   version: "9"
 
           - name: ubuntu-18.04-clang-9
             os: ubuntu-18.04
diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml
new file mode 100644
index 000000000..21984311b
--- /dev/null
+++ b/.github/workflows/build-special.yml
@@ -0,0 +1,112 @@
+name: Special Cases CI
+
+on: [pull_request]
+
+jobs:
+  build:
+    name: ${{ matrix.name }} ${{ matrix.build_type }}
+    runs-on: ${{ matrix.os }}
+
+    env:
+      CTEST_OUTPUT_ON_FAILURE: ON
+      CTEST_PARALLEL_LEVEL: 2
+      CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
+      GTSAM_BUILD_UNSTABLE: ON
+
+    strategy:
+      fail-fast: false
+
+      matrix:
+        # Github Actions requires a single row to be added to the build matrix.
+        # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
+        name:
+          [
+            ubuntu-gcc-deprecated,
+            ubuntu-gcc-quaternions,
+            ubuntu-gcc-points-vector,
+          ]
+
+        build_type: [Debug, Release]
+
+        include:
+          - name: ubuntu-gcc-deprecated
+            os: ubuntu-18.04
+            compiler: gcc
+            version: "9"
+            flag: deprecated
+
+          - name: ubuntu-gcc-quaternions
+            os: ubuntu-18.04
+            compiler: gcc
+            version: "9"
+            flag: quaternions
+
+          - name: ubuntu-gcc-points-vector
+            os: ubuntu-18.04
+            compiler: gcc
+            version: "9"
+            flag: points-vector
+
+    steps:
+      - name: Checkout
+        uses: actions/checkout@master
+
+      - name: Install (Linux)
+        if: runner.os == 'Linux'
+        run: |
+          # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository.
+          if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
+            sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
+          fi
+          sudo apt-get -y update
+
+          sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
+
+          echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)"
+          echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)"
+
+          if [ "${{ matrix.compiler }}" = "gcc" ]; then
+            sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
+            echo "::set-env name=CC::gcc-${{ matrix.version }}"
+            echo "::set-env name=CXX::g++-${{ matrix.version }}"
+          else
+            sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
+            echo "::set-env name=CC::clang-${{ matrix.version }}"
+            echo "::set-env name=CXX::clang++-${{ matrix.version }}"
+          fi
+
+      - name: Install (macOS)
+        if: runner.os == 'macOS'
+        run: |
+          brew install cmake ninja boost
+          if [ "${{ matrix.compiler }}" = "gcc" ]; then
+            brew install gcc@${{ matrix.version }}
+            echo "::set-env name=CC::gcc-${{ matrix.version }}"
+            echo "::set-env name=CXX::g++-${{ matrix.version }}"
+          else
+            sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
+            echo "::set-env name=CC::clang"
+            echo "::set-env name=CXX::clang++"
+            fi
+
+      - name: Set Allow Deprecated Flag
+        if: matrix.flag == 'deprecated'
+        env:
+          GTSAM_ALLOW_DEPRECATED_SINCE_V41: ON
+        run: echo "Allow deprecated since version 4.1"
+
+      - name: Set Use Quaternions Flag
+        if: matrix.flag == 'quaternions'
+        env:
+          GTSAM_USE_QUATERNIONS: ON
+        run: echo "Use Quaternions for rotations"
+
+      - name: Set Typedef Points to Vector Flag
+        if: matrix.flag == 'points-vector'
+        env:
+          GTSAM_TYPEDEF_POINTS_TO_VECTOR: ON
+        run: echo "Typedef Points to Vector"
+
+      - name: Build & Test
+        run: |
+          bash .github/scripts/unix.sh -t
diff --git a/.gitignore b/.gitignore
index 1d89cac25..c2d6ce60f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -21,3 +21,4 @@ cython/gtsam_wrapper.pxd
 /CMakeSettings.json
 # for QtCreator:
 CMakeLists.txt.user*
+xcode/
diff --git a/CMakeLists.txt b/CMakeLists.txt
index d2b9bc75e..c2918f1cb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -65,18 +65,20 @@ add_custom_target(uninstall
 # Configurable Options
 if(GTSAM_UNSTABLE_AVAILABLE)
     option(GTSAM_BUILD_UNSTABLE              "Enable/Disable libgtsam_unstable"          ON)
+    option(GTSAM_UNSTABLE_BUILD_PYTHON       "Enable/Disable Python wrapper for libgtsam_unstable" ON)
+    option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF)
 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." 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_POSE3_EXPMAP                "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
+option(GTSAM_ROT3_EXPMAP                 "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
 option(GTSAM_ENABLE_CONSISTENCY_CHECKS   "Enable/Disable expensive consistency checks"       OFF)
 option(GTSAM_WITH_TBB                    "Use Intel Threaded Building Blocks (TBB) if available" ON)
 option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available" OFF)
 option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
 option(GTSAM_THROW_CHEIRALITY_EXCEPTION  "Throw exception when a triangulated point is behind a camera" ON)
+option(GTSAM_BUILD_PYTHON                "Enable/Disable building & installation of Python module with pybind11" OFF)
 option(GTSAM_ALLOW_DEPRECATED_SINCE_V41  "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
-option(GTSAM_TYPEDEF_POINTS_TO_VECTORS   "Typedef Point2 and Point3 to Eigen::Vector equivalents" OFF)
 option(GTSAM_SUPPORT_NESTED_DISSECTION   "Support Metis-based nested dissection" ON)
 option(GTSAM_TANGENT_PREINTEGRATION      "Use new ImuFactor with integration on tangent space" ON)
 if(NOT MSVC AND NOT XCODE_VERSION)
@@ -99,37 +101,40 @@ endif()
 # Options relating to MATLAB wrapper
 # TODO: Check for matlab mex binary before handling building of binaries
 option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab toolbox"  OFF)
-option(GTSAM_INSTALL_CYTHON_TOOLBOX      "Enable/Disable installation of Cython toolbox"  OFF)
-option(GTSAM_BUILD_WRAP                  "Enable/Disable building of matlab/cython wrap utility (necessary for matlab/cython interface)" ON)
-set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of python to build the cython wrapper for (or Default)")
+set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.")
 
 # Check / set dependent variables for MATLAB wrapper
-if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND NOT GTSAM_BUILD_WRAP)
-	message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX or GTSAM_INSTALL_CYTHON_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP")
-endif()
-if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND GTSAM_BUILD_TYPE_POSTFIXES)
-		set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
-endif()
-if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
-	message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
+if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES)
+    set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
 endif()
 
 if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
-	message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries.  If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
+    message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries.  If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
 endif()
 
-if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS)
-    message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the MATLAB toolbox cannot deal with this yet.  Please turn one of the two options off.")
-endif()
+if(GTSAM_BUILD_PYTHON)
+    if(GTSAM_UNSTABLE_BUILD_PYTHON)
+        if (NOT GTSAM_BUILD_UNSTABLE)
+            message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.")
+            set(GTSAM_UNSTABLE_BUILD_PYTHON OFF)
+        endif()
+    endif()
 
-if(GTSAM_INSTALL_CYTHON_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS)
-    message(FATAL_ERROR "GTSAM_INSTALL_CYTHON_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the CYTHON toolbox cannot deal with this yet.  Please turn one of the two options off.")
+    set(GTSAM_PY_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/python")
 endif()
 
 # Flags for choosing default packaging tools
 set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
 set(CPACK_GENERATOR        "TGZ" CACHE STRING "CPack Default Binary Generator")
 
+if (CMAKE_GENERATOR STREQUAL "Ninja" AND
+    ((CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.9) OR
+     (CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 3.5)))
+    # Force colored warnings in Ninja's output, if the compiler has -fdiagnostics-color support.
+    # Rationale in https://github.com/ninja-build/ninja/issues/814
+    add_compile_options(-fdiagnostics-color=always)
+endif()
+
 ###############################################################################
 # Find boost
 
@@ -138,18 +143,18 @@ set(CPACK_GENERATOR        "TGZ" CACHE STRING "CPack Default Binary Generator")
 # Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT
 
 if(MSVC)
-	# By default, boost only builds static libraries on windows
-	set(Boost_USE_STATIC_LIBS ON)  # only find static libs
-	# If we ever reset above on windows and, ...
-	# If we use Boost shared libs, disable auto linking.
-	# Some libraries, at least Boost Program Options, rely on this to export DLL symbols.
-	if(NOT Boost_USE_STATIC_LIBS)
-		list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK)
-	endif()
-	# Virtual memory range for PCH exceeded on VS2015
-	if(MSVC_VERSION LESS 1910) # older than VS2017
-	  list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295)
-	endif()
+    # By default, boost only builds static libraries on windows
+    set(Boost_USE_STATIC_LIBS ON)  # only find static libs
+    # If we ever reset above on windows and, ...
+    # If we use Boost shared libs, disable auto linking.
+    # Some libraries, at least Boost Program Options, rely on this to export DLL symbols.
+    if(NOT Boost_USE_STATIC_LIBS)
+        list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK)
+    endif()
+    # Virtual memory range for PCH exceeded on VS2015
+    if(MSVC_VERSION LESS 1910) # older than VS2017
+      list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295)
+    endif()
 endif()
 
 # If building DLLs in MSVC, we need to avoid EIGEN_STATIC_ASSERT()
@@ -157,7 +162,7 @@ endif()
 # See: https://bitbucket.org/gtborg/gtsam/issues/417/fail-to-build-on-msvc-2017
 #
 if(MSVC AND BUILD_SHARED_LIBS)
-	list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
+    list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
 endif()
 
 # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
@@ -227,16 +232,16 @@ find_package(GooglePerfTools)
 ###############################################################################
 # Support ccache, if installed
 if(NOT MSVC AND NOT XCODE_VERSION)
-	find_program(CCACHE_FOUND ccache)
-	if(CCACHE_FOUND)
-		if(GTSAM_BUILD_WITH_CCACHE)
-			set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache)
-			set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache)
-		else()
-			set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "")
-			set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK "")
-		endif()
-	endif(CCACHE_FOUND)
+    find_program(CCACHE_FOUND ccache)
+    if(CCACHE_FOUND)
+        if(GTSAM_BUILD_WITH_CCACHE)
+            set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache)
+            set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache)
+        else()
+            set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "")
+            set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK "")
+        endif()
+    endif(CCACHE_FOUND)
 endif()
 
 ###############################################################################
@@ -280,74 +285,74 @@ option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" OFF)
 
 # Switch for using system Eigen or GTSAM-bundled Eigen
 if(GTSAM_USE_SYSTEM_EIGEN)
-	find_package(Eigen3 REQUIRED)
+    find_package(Eigen3 REQUIRED)
 
-	# Use generic Eigen include paths e.g. <Eigen/Core>
-	set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
+    # Use generic Eigen include paths e.g. <Eigen/Core>
+    set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
 
-	# check if MKL is also enabled - can have one or the other, but not both!
-	# Note: Eigen >= v3.2.5 includes our patches
-	if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5))
-	  message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL")
-	endif()
+    # check if MKL is also enabled - can have one or the other, but not both!
+    # Note: Eigen >= v3.2.5 includes our patches
+    if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5))
+      message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL")
+    endif()
 
-	# Check for Eigen version which doesn't work with MKL
-	# See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527 for details.
-	if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4))
-		message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.")
-	endif()
+    # Check for Eigen version which doesn't work with MKL
+    # See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527 for details.
+    if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4))
+        message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.")
+    endif()
 
-	# The actual include directory (for BUILD cmake target interface):
-	set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}")
+    # The actual include directory (for BUILD cmake target interface):
+    set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}")
 else()
-	# Use bundled Eigen include path.
-	# Clear any variables set by FindEigen3
-	if(EIGEN3_INCLUDE_DIR)
-		set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
-	endif()
+    # Use bundled Eigen include path.
+    # Clear any variables set by FindEigen3
+    if(EIGEN3_INCLUDE_DIR)
+        set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
+    endif()
 
-	# set full path to be used by external projects
-	# this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in
-	set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/")
+    # set full path to be used by external projects
+    # this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in
+    set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/")
 
-	# The actual include directory (for BUILD cmake target interface):
-	set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${CMAKE_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
+    # The actual include directory (for BUILD cmake target interface):
+    set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${CMAKE_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
 endif()
 
 # Detect Eigen version:
 set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h")
 if (EXISTS ${EIGEN_VER_H})
-	file(READ "${EIGEN_VER_H}" STR_EIGEN_VERSION)
+    file(READ "${EIGEN_VER_H}" STR_EIGEN_VERSION)
 
-	# Extract the Eigen version from the Macros.h file, lines "#define EIGEN_WORLD_VERSION  XX", etc...
+    # Extract the Eigen version from the Macros.h file, lines "#define EIGEN_WORLD_VERSION  XX", etc...
 
-	string(REGEX MATCH "EIGEN_WORLD_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${STR_EIGEN_VERSION}")
-	string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${GTSAM_EIGEN_VERSION_WORLD}")
+    string(REGEX MATCH "EIGEN_WORLD_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${STR_EIGEN_VERSION}")
+    string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${GTSAM_EIGEN_VERSION_WORLD}")
 
-	string(REGEX MATCH "EIGEN_MAJOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${STR_EIGEN_VERSION}")
-	string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${GTSAM_EIGEN_VERSION_MAJOR}")
+    string(REGEX MATCH "EIGEN_MAJOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${STR_EIGEN_VERSION}")
+    string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${GTSAM_EIGEN_VERSION_MAJOR}")
 
-	string(REGEX MATCH "EIGEN_MINOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${STR_EIGEN_VERSION}")
-	string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${GTSAM_EIGEN_VERSION_MINOR}")
+    string(REGEX MATCH "EIGEN_MINOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${STR_EIGEN_VERSION}")
+    string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${GTSAM_EIGEN_VERSION_MINOR}")
 
-	set(GTSAM_EIGEN_VERSION "${GTSAM_EIGEN_VERSION_WORLD}.${GTSAM_EIGEN_VERSION_MAJOR}.${GTSAM_EIGEN_VERSION_MINOR}")
+    set(GTSAM_EIGEN_VERSION "${GTSAM_EIGEN_VERSION_WORLD}.${GTSAM_EIGEN_VERSION_MAJOR}.${GTSAM_EIGEN_VERSION_MINOR}")
 
-	message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}")
+    message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}")
 else()
-	message(WARNING "Cannot determine Eigen version, missing file: `${EIGEN_VER_H}`")
+    message(WARNING "Cannot determine Eigen version, missing file: `${EIGEN_VER_H}`")
 endif ()
 
 if (MSVC)
-	if (BUILD_SHARED_LIBS)
-		# mute eigen static assert to avoid errors in shared lib
-		list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
-	endif()
-	list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen
+    if (BUILD_SHARED_LIBS)
+        # mute eigen static assert to avoid errors in shared lib
+        list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
+    endif()
+    list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen
 endif()
 
 if (APPLE AND BUILD_SHARED_LIBS)
-	# Set the default install directory on macOS
-	set(CMAKE_INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib")
+    # Set the default install directory on macOS
+    set(CMAKE_INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib")
 endif()
 
 ###############################################################################
@@ -356,42 +361,42 @@ endif()
 # Build list of possible allocators
 set(possible_allocators "")
 if(GTSAM_USE_TBB)
-	list(APPEND possible_allocators TBB)
-	set(preferred_allocator TBB)
+    list(APPEND possible_allocators TBB)
+    set(preferred_allocator TBB)
 else()
-	list(APPEND possible_allocators BoostPool STL)
-	set(preferred_allocator STL)
+    list(APPEND possible_allocators BoostPool STL)
+    set(preferred_allocator STL)
 endif()
 if(GOOGLE_PERFTOOLS_FOUND)
-	list(APPEND possible_allocators tcmalloc)
+    list(APPEND possible_allocators tcmalloc)
 endif()
 
 # Check if current allocator choice is valid and set cache option
 list(FIND possible_allocators "${GTSAM_DEFAULT_ALLOCATOR}" allocator_valid)
 if(allocator_valid EQUAL -1)
-	set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator" FORCE)
+    set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator" FORCE)
 else()
-	set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator")
+    set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator")
 endif()
 set_property(CACHE GTSAM_DEFAULT_ALLOCATOR PROPERTY STRINGS ${possible_allocators})
 mark_as_advanced(GTSAM_DEFAULT_ALLOCATOR)
 
 # Define compile flags depending on allocator
 if("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "BoostPool")
-	set(GTSAM_ALLOCATOR_BOOSTPOOL 1)
+    set(GTSAM_ALLOCATOR_BOOSTPOOL 1)
 elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "STL")
-	set(GTSAM_ALLOCATOR_STL 1)
+    set(GTSAM_ALLOCATOR_STL 1)
 elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "TBB")
-	set(GTSAM_ALLOCATOR_TBB 1)
+    set(GTSAM_ALLOCATOR_TBB 1)
 elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc")
-	set(GTSAM_ALLOCATOR_STL 1) # tcmalloc replaces malloc, so to use it we use the STL allocator
-	list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc")
+    set(GTSAM_ALLOCATOR_STL 1) # tcmalloc replaces malloc, so to use it we use the STL allocator
+    list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc")
 endif()
 
 if(MSVC)
-	list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS)
-	list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
-	list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /bigobj) # Allow large object files for template-based code
+    list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS)
+    list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
+    list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /bigobj) # Allow large object files for template-based code
 endif()
 
 # GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
@@ -419,14 +424,11 @@ endif()
 # Build CppUnitLite
 add_subdirectory(CppUnitLite)
 
-# Build wrap
-if (GTSAM_BUILD_WRAP)
-    add_subdirectory(wrap)
-	# suppress warning of cython line being too long
-	if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
-		set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation")
-	endif()
-endif(GTSAM_BUILD_WRAP)
+# This is the new wrapper
+if(GTSAM_BUILD_PYTHON)
+    list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")
+    add_subdirectory(python)
+endif()
 
 # Build GTSAM library
 add_subdirectory(gtsam)
@@ -447,23 +449,9 @@ endif()
 
 # Matlab toolbox
 if (GTSAM_INSTALL_MATLAB_TOOLBOX)
-	add_subdirectory(matlab)
+    add_subdirectory(matlab)
 endif()
 
-# Cython wrap
-if (GTSAM_INSTALL_CYTHON_TOOLBOX)
-  set(GTSAM_INSTALL_CYTHON_TOOLBOX 1)
-  # Set up cache options
-  # 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 ${GTSAM_CYTHON_INSTALL_PATH})
-else()
-  set(GTSAM_INSTALL_CYTHON_TOOLBOX 0) # This will go into config.h
-endif()
-
-
 # Install config and export files
 GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in")
 export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake)
@@ -524,7 +512,9 @@ endif()
 print_enabled_config(${BUILD_SHARED_LIBS}                 "Build shared GTSAM libraries")
 print_enabled_config(${GTSAM_BUILD_TYPE_POSTFIXES}        "Put build type in library name")
 if(GTSAM_UNSTABLE_AVAILABLE)
-    print_enabled_config(${GTSAM_BUILD_UNSTABLE}          "Build libgtsam_unstable")
+    print_enabled_config(${GTSAM_BUILD_UNSTABLE}          "Build libgtsam_unstable        ")
+    print_enabled_config(${GTSAM_UNSTABLE_BUILD_PYTHON}   "Build GTSAM unstable Python    ")
+    print_enabled_config(${GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX} "Build MATLAB Toolbox for unstable")
 endif()
 
 if(NOT MSVC AND NOT XCODE_VERSION)
@@ -572,13 +562,13 @@ else()
 endif()
 
 if(NOT MSVC AND NOT XCODE_VERSION)
-	if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
+    if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
         print_config("Build with ccache" "Yes")
-	elseif(CCACHE_FOUND)
+    elseif(CCACHE_FOUND)
         print_config("Build with ccache" "ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
-	else()
+    else()
         print_config("Build with ccache" "No")
-	endif()
+    endif()
 endif()
 
 message(STATUS "Packaging flags")
@@ -591,10 +581,8 @@ print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS}   "Runtime consistency c
 print_enabled_config(${GTSAM_ROT3_EXPMAP}                 "Rot3 retract is full ExpMap     ")
 print_enabled_config(${GTSAM_POSE3_EXPMAP}                "Pose3 retract is full ExpMap    ")
 print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V41}  "Allow features deprecated in GTSAM 4.1")
-print_enabled_config(${GTSAM_TYPEDEF_POINTS_TO_VECTORS}   "Point3 is typedef to Vector3    ")
 print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION}   "Metis-based Nested Dissection   ")
 print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION}      "Use tangent-space preintegration")
-print_enabled_config(${GTSAM_BUILD_WRAP}                  "Build Wrap                      ")
 
 message(STATUS "MATLAB toolbox flags")
 print_enabled_config(${GTSAM_INSTALL_MATLAB_TOOLBOX}      "Install MATLAB toolbox          ")
@@ -602,23 +590,23 @@ if (${GTSAM_INSTALL_MATLAB_TOOLBOX})
     print_config("MATLAB root" "${MATLAB_ROOT}")
     print_config("MEX binary" "${MEX_COMMAND}")
 endif()
-
-message(STATUS "Cython toolbox flags                                      ")
-print_enabled_config(${GTSAM_INSTALL_CYTHON_TOOLBOX}      "Install Cython toolbox          ")
-if(GTSAM_INSTALL_CYTHON_TOOLBOX)
-    print_config("Python version" "${GTSAM_PYTHON_VERSION}")
+message(STATUS "Python toolbox flags                                      ")
+print_enabled_config(${GTSAM_BUILD_PYTHON}                "Build Python module with pybind ")
+if(GTSAM_BUILD_PYTHON)
+    print_config("Python version" ${GTSAM_PYTHON_VERSION})
 endif()
+
 message(STATUS "===============================================================")
 
 # Print warnings at the end
 if(GTSAM_WITH_TBB AND NOT TBB_FOUND)
-	message(WARNING "TBB 4.4 or newer was not found - this is ok, but note that GTSAM parallelization will be disabled.  Set GTSAM_WITH_TBB to 'Off' to avoid this warning.")
+    message(WARNING "TBB 4.4 or newer was not found - this is ok, but note that GTSAM parallelization will be disabled.  Set GTSAM_WITH_TBB to 'Off' to avoid this warning.")
 endif()
 if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND)
-	message(WARNING "MKL was not found - this is ok, but note that MKL will be disabled.  Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning.  See INSTALL.md for notes on performance.")
+    message(WARNING "MKL was not found - this is ok, but note that MKL will be disabled.  Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning.  See INSTALL.md for notes on performance.")
 endif()
 if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND)
-	message(WARNING "Your compiler does not support OpenMP.  Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.")
+    message(WARNING "Your compiler does not support OpenMP.  Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.")
 endif()
 
 # Include CPack *after* all flags
diff --git a/README.md b/README.md
index 015d65e3d..99903f0b9 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,10 @@
 # README - Georgia Tech Smoothing and Mapping Library
 
-**As of August 1, develop is officially in "Pre 4.1" mode, and features deprecated in 4.0 were removed. Use the last 4.0.3 release if you need those features. However, most are easily converted and can be tracked down (in 4.0.3) by disabling the cmake flag GTSAM_ALLOW_DEPRECATED_SINCE_V4**
+**Important Note**
+
+As of August 1 2020, the `develop` branch is officially in "Pre 4.1" mode, and features deprecated in 4.0 have been removed. Please use the last [4.0.3 release](https://github.com/borglab/gtsam/releases/tag/4.0.3) if you need those features. 
+
+However, most are easily converted and can be tracked down (in 4.0.3) by disabling the cmake flag `GTSAM_ALLOW_DEPRECATED_SINCE_V4`.
 
 ## What is GTSAM?
 
@@ -9,13 +13,14 @@ mapping (SAM) in robotics and vision, using Factor Graphs and Bayes
 Networks as the underlying computing paradigm rather than sparse
 matrices.
 
-| Platform  | Build Status  |
-|:---------:|:-------------:|
-| gcc/clang | [![Build Status](https://travis-ci.com/borglab/gtsam.svg?branch=develop)](https://travis-ci.com/borglab/gtsam/) |
-| MSVC      | [![Build status](https://ci.appveyor.com/api/projects/status/3enllitj52jsxwfg/branch/develop?svg=true)](https://ci.appveyor.com/project/dellaert/gtsam) |
+| Platform     | Compiler  | Build Status  |
+|:------------:|:---------:|:-------------:|
+| Ubuntu 18.04 | gcc/clang | ![Linux CI](https://github.com/borglab/gtsam/workflows/Linux%20CI/badge.svg) |
+| macOS        | clang     | ![macOS CI](https://github.com/borglab/gtsam/workflows/macOS%20CI/badge.svg) |
+| Windows      | MSVC      | ![Windows CI](https://github.com/borglab/gtsam/workflows/Windows%20CI/badge.svg) |
 
 
-On top of the C++ library, GTSAM includes [wrappers for MATLAB & Python](##Wrappers).
+On top of the C++ library, GTSAM includes [wrappers for MATLAB & Python](#wrappers).
 
 
 ## Quickstart
diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt
index d612e2fae..9d9ecd48b 100644
--- a/cmake/CMakeLists.txt
+++ b/cmake/CMakeLists.txt
@@ -17,8 +17,6 @@ install(FILES
   GtsamBuildTypes.cmake
   GtsamMakeConfigFile.cmake
   GtsamMatlabWrap.cmake
-  GtsamPythonWrap.cmake
-  GtsamCythonWrap.cmake
   GtsamTesting.cmake
   GtsamPrinting.cmake
   FindCython.cmake
diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake
deleted file mode 100644
index c8f876895..000000000
--- a/cmake/GtsamCythonWrap.cmake
+++ /dev/null
@@ -1,204 +0,0 @@
-# Check Cython version, need to be >=0.25.2
-# Unset these cached variables to avoid surprises when the python/cython
-# in the current environment are different from the cached!
-unset(PYTHON_EXECUTABLE CACHE)
-unset(CYTHON_EXECUTABLE CACHE)
-unset(PYTHON_INCLUDE_DIR CACHE)
-unset(PYTHON_MAJOR_VERSION CACHE)
-unset(PYTHON_LIBRARY CACHE)
-
-# Allow override from command line
-if(NOT DEFINED GTSAM_USE_CUSTOM_PYTHON_LIBRARY)
-  if(GTSAM_PYTHON_VERSION STREQUAL "Default")
-    find_package(PythonInterp REQUIRED)
-    find_package(PythonLibs REQUIRED)
-  else()
-    find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
-    find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
-  endif()
-endif()
-find_package(Cython 0.25.2 REQUIRED)
-
-execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c"
-    "from __future__ import print_function;import sys;print(sys.version[0], end='')"
-    OUTPUT_VARIABLE PYTHON_MAJOR_VERSION
-)
-
-# User-friendly Cython wrapping and installing function.
-# Builds a Cython module from the provided interface_header.
-# For example, for the interface header gtsam.h,
-# this will build the wrap module 'gtsam'.
-#
-# Arguments:
-#
-# interface_header:  The relative path to the wrapper interface definition file.
-# extra_imports: extra header to import in the Cython pxd file.
-#                For example, to use Cython gtsam.pxd in your own module,
-#        use "from gtsam cimport *"
-# install_path: destination to install the library
-# libs: libraries to link with
-# dependencies: Dependencies which need to be built before the wrapper
-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 "${install_path}")
-  wrap_library_cython("${interface_header}" "${generated_files_path}" "${extra_imports}" "${libs}" "${dependencies}")
-endfunction()
-
-function(set_up_required_cython_packages)
-  # Set up building of cython module
-  include_directories(${PYTHON_INCLUDE_DIRS})
-  find_package(NumPy REQUIRED)
-  include_directories(${NUMPY_INCLUDE_DIRS})
-endfunction()
-
-
-# Convert pyx to cpp by executing cython
-# This is the first step to compile cython from the command line
-# as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html
-#
-# Arguments:
-#    - target:  The specified target for this step
-#    - pyx_file:   The input pyx_file in full *absolute* path
-#    - generated_cpp:   The output cpp file in full absolute path
-#    - include_dirs:   Directories to include when executing cython
-function(pyx_to_cpp target pyx_file generated_cpp include_dirs)
-  foreach(dir ${include_dirs})
-    set(includes_for_cython ${includes_for_cython}  -I ${dir})
-  endforeach()
-
-  add_custom_command(
-    OUTPUT ${generated_cpp}
-    COMMAND
-    ${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus -${PYTHON_MAJOR_VERSION} ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
-    VERBATIM)
-  add_custom_target(${target} ALL DEPENDS ${generated_cpp})
-endfunction()
-
-# Build the cpp file generated by converting pyx using cython
-# This is the second step to compile cython from the command line
-# as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html
-#
-# Arguments:
-#    - target:  The specified target for this step
-#    - cpp_file:   The input cpp_file in full *absolute* path
-#    - output_lib_we:   The output lib filename only (without extension)
-#    - output_dir:   The output directory
-function(build_cythonized_cpp target cpp_file output_lib_we output_dir)
-  add_library(${target} MODULE ${cpp_file})
-  
-  if(WIN32)
-    # Use .pyd extension instead of .dll on Windows
-    set_target_properties(${target} PROPERTIES SUFFIX ".pyd")
-
-    # Add full path to the Python library
-    target_link_libraries(${target} ${PYTHON_LIBRARIES})
-  endif()
-  
-  if(APPLE)
-    set(link_flags "-undefined dynamic_lookup")
-  endif()
-  set_target_properties(${target}
-      PROPERTIES COMPILE_FLAGS "-w"
-      LINK_FLAGS "${link_flags}"
-      OUTPUT_NAME ${output_lib_we}
-      PREFIX ""
-      ${CMAKE_BUILD_TYPE_UPPER}_POSTFIX ""
-      LIBRARY_OUTPUT_DIRECTORY ${output_dir})
-endfunction()
-
-# Cythonize a pyx from the command line as described at
-# http://cython.readthedocs.io/en/latest/src/reference/compilation.html
-# Arguments:
-#    - target:        The specified target
-#    - pyx_file:      The input pyx_file in full *absolute* path
-#    - output_lib_we: The output lib filename only (without extension)
-#    - output_dir:    The output directory
-#    - include_dirs:  Directories to include when executing cython
-#    - libs:          Libraries to link with
-#    - interface_header: For dependency. Any update in interface header will re-trigger cythonize
-function(cythonize target pyx_file output_lib_we output_dir include_dirs libs interface_header dependencies)
-  get_filename_component(pyx_path "${pyx_file}" DIRECTORY)
-  get_filename_component(pyx_name "${pyx_file}" NAME_WE)
-  set(generated_cpp "${output_dir}/${pyx_name}.cpp")
-
-  set_up_required_cython_packages()
-  pyx_to_cpp(${target}_pyx2cpp ${pyx_file} ${generated_cpp} "${include_dirs}")
-
-  # Late dependency injection, to make sure this gets called whenever the interface header is updated
-  # See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes
-  add_custom_command(OUTPUT ${generated_cpp} DEPENDS ${interface_header} ${pyx_file} APPEND)
-  if (NOT "${dependencies}" STREQUAL "")
-    add_dependencies(${target}_pyx2cpp "${dependencies}")
-  endif()
-
-  build_cythonized_cpp(${target} ${generated_cpp} ${output_lib_we} ${output_dir})
-  if (NOT "${libs}" STREQUAL "")
-    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
-function(wrap_library_cython interface_header generated_files_path extra_imports libs dependencies)
-  # Wrap codegen interface
-  # Extract module path and name from interface header file name
-  # wrap requires interfacePath to be *absolute*
-  get_filename_component(interface_header "${interface_header}" ABSOLUTE)
-  get_filename_component(module_path "${interface_header}" PATH)
-  get_filename_component(module_name "${interface_header}" NAME_WE)
-
-  # Wrap module to Cython pyx
-  message(STATUS "Cython wrapper generating ${generated_files_path}/${module_name}.pyx")
-  set(generated_pyx "${generated_files_path}/${module_name}.pyx")
-  if(NOT EXISTS ${generated_files_path})
-    file(MAKE_DIRECTORY "${generated_files_path}")
-  endif()
-
-  add_custom_command(
-    OUTPUT ${generated_pyx}
-    DEPENDS ${interface_header} wrap
-    COMMAND
-        wrap --cython ${module_path} ${module_name} ${generated_files_path} "${extra_imports}"
-    VERBATIM
-    WORKING_DIRECTORY ${generated_files_path}/../)
-  add_custom_target(cython_wrap_${module_name}_pyx ALL DEPENDS ${generated_pyx})
-  if(NOT "${dependencies}" STREQUAL "")
-    add_dependencies(cython_wrap_${module_name}_pyx ${dependencies})
-  endif()
-
-  message(STATUS "Cythonize and build ${module_name}.pyx")
-  get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES)
-  cythonize(cythonize_${module_name} ${generated_pyx} ${module_name}
-    ${generated_files_path} "${include_dirs}" "${libs}" ${interface_header} cython_wrap_${module_name}_pyx)
-
-  # distclean
-  add_custom_target(wrap_${module_name}_cython_distclean
-      COMMAND cmake -E remove_directory ${generated_files_path})
-endfunction()
-
-# Helper function to install Cython scripts and handle multiple build types where the scripts
-# should be installed to all build type toolboxes
-#
-# Arguments:
-#  source_directory: The source directory to be installed. "The last component of each directory
-#                    name is appended to the destination directory but a trailing slash may be
-#                    used to avoid this because it leaves the last component empty."
-#                    (https://cmake.org/cmake/help/v3.3/command/install.html?highlight=install#installing-directories)
-#  dest_directory: The destination directory to install to.
-#  patterns: list of file patterns to install
-function(install_cython_scripts source_directory dest_directory patterns)
-  set(patterns_args "")
-  set(exclude_patterns "")
-
-  foreach(pattern ${patterns})
-    list(APPEND patterns_args PATTERN "${pattern}")
-  endforeach()
-
-  file(COPY "${source_directory}" DESTINATION "${dest_directory}"
-            FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
-endfunction()
diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake
index 5fc829bf2..111114a7b 100644
--- a/cmake/GtsamMatlabWrap.cmake
+++ b/cmake/GtsamMatlabWrap.cmake
@@ -23,6 +23,11 @@ else()
 	file(GLOB matlab_bin_directories "/usr/local/MATLAB/*/bin")
 	set(mex_program_name "mex")
 endif()
+
+if(GTSAM_CUSTOM_MATLAB_PATH)
+	set(matlab_bin_directories ${GTSAM_CUSTOM_MATLAB_PATH})
+endif()
+
 # Run find_program explicitly putting $PATH after our predefined program
 # directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents
 # finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is
@@ -209,15 +214,34 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
 
 	# Set up generation of module source file
 	file(MAKE_DIRECTORY "${generated_files_path}")
+
+	if(GTSAM_PYTHON_VERSION STREQUAL "Default")
+		find_package(PythonInterp REQUIRED)
+		find_package(PythonLibs REQUIRED)
+	else()
+		find_package(PythonInterp
+				${GTSAM_PYTHON_VERSION}
+				EXACT
+				REQUIRED)
+		find_package(PythonLibs
+				${GTSAM_PYTHON_VERSION}
+				EXACT
+				REQUIRED)
+	endif()
+
+	set(_ignore gtsam::Point2
+			gtsam::Point3)
 	add_custom_command(
 		OUTPUT ${generated_cpp_file}
-		DEPENDS ${interfaceHeader} wrap ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects}
-        COMMAND 
-            wrap --matlab
-            ${modulePath}
-            ${moduleName} 
-            ${generated_files_path} 
-            ${matlab_h_path} 
+		DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects}
+        COMMAND
+			${PYTHON_EXECUTABLE}
+			${CMAKE_SOURCE_DIR}/wrap/matlab_wrapper.py
+            --src ${interfaceHeader}
+			--module_name ${moduleName}
+            --out ${generated_files_path}
+			--top_module_namespaces ${moduleName}
+			--ignore ${_ignore}
 		VERBATIM
 		WORKING_DIRECTORY ${generated_files_path})
 		
diff --git a/cmake/GtsamPythonWrap.cmake b/cmake/GtsamPythonWrap.cmake
deleted file mode 100644
index 714e37488..000000000
--- a/cmake/GtsamPythonWrap.cmake
+++ /dev/null
@@ -1,102 +0,0 @@
-#Setup cache options
-set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version")
-set(GTSAM_BUILD_PYTHON_FLAGS "" CACHE STRING "Extra flags for running Matlab PYTHON compilation")
-set(GTSAM_PYTHON_INSTALL_PATH "" CACHE PATH "Python toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/borg/python")
-if(NOT GTSAM_PYTHON_INSTALL_PATH)
-  set(GTSAM_PYTHON_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/borg/python")
-endif() 
-
-#Author: Paul Furgale Modified by Andrew Melim
-function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY)
-  # # Boost
-  # find_package(Boost COMPONENTS python filesystem system REQUIRED)
-  # include_directories(${Boost_INCLUDE_DIRS})
-
-  # # Find Python
-  # FIND_PACKAGE(PythonLibs 2.7 REQUIRED)
-  # INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
-
-  IF(APPLE)
-    # The apple framework headers don't include the numpy headers for some reason.
-    GET_FILENAME_COMPONENT(REAL_PYTHON_INCLUDE ${PYTHON_INCLUDE_DIRS} REALPATH)
-    IF( ${REAL_PYTHON_INCLUDE} MATCHES Python.framework)
-      message("Trying to find extra headers for numpy from ${REAL_PYTHON_INCLUDE}.")
-      message("Looking in ${REAL_PYTHON_INCLUDE}/../../Extras/lib/python/numpy/core/include/numpy")
-      FIND_PATH(NUMPY_INCLUDE_DIR arrayobject.h
-  ${REAL_PYTHON_INCLUDE}/../../Extras/lib/python/numpy/core/include/numpy
-  ${REAL_PYTHON_INCLUDE}/numpy
-  )
-      IF(${NUMPY_INCLUDE_DIR} MATCHES NOTFOUND)
-  message("Unable to find numpy include directories: ${NUMPY_INCLUDE_DIR}")
-      ELSE()
-  message("Found headers at ${NUMPY_INCLUDE_DIR}")
-  INCLUDE_DIRECTORIES(${NUMPY_INCLUDE_DIR})
-  INCLUDE_DIRECTORIES(${NUMPY_INCLUDE_DIR}/..)
-      ENDIF()
-    ENDIF()
-  ENDIF(APPLE)
-
-  if(MSVC)
-    add_library(${moduleName}_python MODULE ${ARGN})
-    set_target_properties(${moduleName}_python PROPERTIES
-        OUTPUT_NAME         ${moduleName}_python
-        CLEAN_DIRECT_OUTPUT 1
-        VERSION             1
-        SOVERSION           0
-        SUFFIX              ".pyd")
-      target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp
-
-    set(PYLIB_OUTPUT_FILE $<TARGET_FILE:${moduleName}_python>)
-    message(${PYLIB_OUTPUT_FILE})
-    get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
-    set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.pyd)
-
-  ELSE()
-    # Create a shared library
-    add_library(${moduleName}_python SHARED ${generated_cpp_file})
-
-    set_target_properties(${moduleName}_python PROPERTIES
-        OUTPUT_NAME         ${moduleName}_python
-        CLEAN_DIRECT_OUTPUT 1)
-      target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp
-    # On OSX and Linux, the python library must end in the extension .so. Build this
-    # filename here.
-    get_property(PYLIB_OUTPUT_FILE TARGET ${moduleName}_python PROPERTY LOCATION)
-    set(PYLIB_OUTPUT_FILE $<TARGET_FILE:${moduleName}_python>)
-    message(${PYLIB_OUTPUT_FILE})
-    get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
-    set(PYLIB_SO_NAME lib${moduleName}_python.so)
-  ENDIF(MSVC)
-
-  # Installs the library in the gtsam folder, which is used by setup.py to create the gtsam package
-  set(PYTHON_MODULE_DIRECTORY ${CMAKE_SOURCE_DIR}/python/gtsam)
-  # Cause the library to be output in the correct directory.
-  add_custom_command(TARGET ${moduleName}_python
-    POST_BUILD
-    COMMAND cp -v ${PYLIB_OUTPUT_FILE} ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME}
-    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
-    COMMENT "Copying library files to python directory" )
-
-  # Cause the library to be output in the correct directory.
-  add_custom_command(TARGET ${TARGET_NAME}
-    POST_BUILD
-    COMMAND cp -v ${PYLIB_OUTPUT_FILE} ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME}
-    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
-    COMMENT "Copying library files to python directory" )
-
-  get_directory_property(AMCF ADDITIONAL_MAKE_CLEAN_FILES)
-  list(APPEND AMCF ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME})
-  set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${AMCF}") 
-endfunction(wrap_python)
-
-# Macro to get list of subdirectories
-macro(SUBDIRLIST result curdir)
-  file(GLOB children RELATIVE ${curdir} ${curdir}/*)
-  set(dirlist "")
-  foreach(child ${children})
-    if(IS_DIRECTORY ${curdir}/${child})
-        list(APPEND dirlist ${child})
-    endif()
-  endforeach()
-  set(${result} ${dirlist})
-endmacro()
diff --git a/cmake/dllexport.h.in b/cmake/dllexport.h.in
index 9a0a344b7..7d757edea 100644
--- a/cmake/dllexport.h.in
+++ b/cmake/dllexport.h.in
@@ -47,9 +47,14 @@
 #    endif
 #  endif
 #else
+#ifdef __APPLE__
+#  define @library_name@_EXPORT __attribute__((visibility("default")))
+#  define @library_name@_EXTERN_EXPORT extern
+#else
 #  define @library_name@_EXPORT
 #  define @library_name@_EXTERN_EXPORT extern
 #endif
+#endif
 
 #undef BUILD_SHARED_LIBS
 
diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
deleted file mode 100644
index 221025575..000000000
--- a/cython/CMakeLists.txt
+++ /dev/null
@@ -1,57 +0,0 @@
-# Install cython components
-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(${GTSAM_EIGENCY_INSTALL_PATH})
-
-  # Fix for error "C1128: number of sections exceeded object file format limit"
-  if(MSVC)
-    add_compile_options(/bigobj)
-  endif()
-
-  # First set up all the package related files.
-  # This also ensures the below wrap operations work correctly.
-  set(CYTHON_INSTALL_REQUIREMENTS_FILE "${PROJECT_SOURCE_DIR}/cython/requirements.txt")
-
-  # Install the custom-generated __init__.py
-  # This makes the cython (sub-)directories into python packages, so gtsam can be found while wrapping gtsam_unstable
-  configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py ${GTSAM_CYTHON_INSTALL_PATH}/gtsam/__init__.py COPYONLY)
-  configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable/__init__.py COPYONLY)
-  configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${GTSAM_CYTHON_INSTALL_PATH}/setup.py)
-
-  # Wrap gtsam
-  add_custom_target(gtsam_header DEPENDS "../gtsam.h")
-  wrap_and_install_library_cython("../gtsam.h" # interface_header
-                                  ""                  # extra imports
-                                  "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path
-                                  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
-  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
-                                    "from gtsam.gtsam cimport *"                  # extra imports
-                                    "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" # install path
-                                    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()
-
-  # 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")
-
-endif ()
diff --git a/cython/README.md b/cython/README.md
deleted file mode 100644
index f69b7a5a6..000000000
--- a/cython/README.md
+++ /dev/null
@@ -1,147 +0,0 @@
-# Python Wrapper
-
-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
-
-- 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.
-
-- Build GTSAM and the wrapper with `make`.
-
-- 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.
-
-- 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 <GTSAM_CYTHON_INSTALL_PATH>
-  python -m unittest discover
-  ```
-
-## Utils
-
-TODO
-
-## Examples
-
-TODO
-
-## Writing Your Own Scripts
-
-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
-    the flag order='F'. And you always get column-major matrices back.
-    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`
-
-- 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 Custom GTSAM-based Project
-
-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?
-
-### 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?)
-- [ ] 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] 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] 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.
-- [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)
-- [ ] [REFACTOR] better name for uninstantiateClass: very vague!! @cancelled (25-11-16 02:30) -- lazy
-- [ ] forward declaration? @cancelled (23-11-16 13:00) - nothing to do, seem to work?
-- [x] wrap VariableIndex: why is it in inference? If need to, shouldn't have constructors to specific FactorGraphs @done (23-11-16 13:00)
-- [x] Global functions @done (22-11-16 21:00)
-- [x] [REFACTOR] typesEqual --> isSameSignature @done (22-11-16 21:00)
-- [x] Proper overloads (constructors, static methods, methods) @done (20-11-16 21:00)
-- [x] Allow overloading methods. The current solution is annoying!!! @done (20-11-16 21:00)
-- [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] 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)
-- [x] ctypedef at correct places @done (16-09-12 18:34)
-- [x] expand template variable type in constructor/static methods? @done (16-09-12 18:34)
-- [x] NonlinearOptimizer: copy constructor deleted!!! @done (16-09-13 17:20)
-- [x] Value: no default constructor @done (16-09-13 17:20)
-- [x] ctypedef PriorFactor[Vector] PriorFactorVector @done (16-09-19 12:25)
-- [x] Delete duplicate methods in derived class @done (16-09-12 13:38)
-- [x] Fix return properly @done (16-09-11 17:14)
-- [x] handle pair @done (16-09-11 17:14)
-- [x] Eigency: ambiguous call: A(const T&) A(const Vector& v) and Eigency A(Map[Vector]& v) @done (16-09-11 07:59)
-- [x] Eigency: Constructor: ambiguous construct from Vector/Matrix @done (16-09-11 07:59)
-- [x] Eigency: Fix method template of Vector/Matrix: template argument is [Vector] while arugment is Map[Vector] @done (16-09-11 08:22)
-- [x] Robust noise: copy assignment operator is deleted because of shared_ptr of the abstract Base class @done (16-09-10 09:05)
-- [ ] Cython: Constructor: generate default constructor? (hack: if it's serializable?) @cancelled (16-09-13 17:20)
-- [ ] Eigency: Map[] to Block @created(16-09-10 07:59) @cancelled (16-09-11 08:28)
-
-- inference before symbolic/linear
-- what's the purpose of "virtual" ??
diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py
deleted file mode 100644
index d40ee4502..000000000
--- a/cython/gtsam/__init__.py
+++ /dev/null
@@ -1,26 +0,0 @@
-from .gtsam import *
-
-try:
-    import gtsam_unstable
-
-
-    def _deprecated_wrapper(item, name):
-        def wrapper(*args, **kwargs):
-            from warnings import warn
-            message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) +
-                       'Please import it from gtsam_unstable.')
-            warn(message)
-            return item(*args, **kwargs)
-        return wrapper
-
-
-    for name in dir(gtsam_unstable):
-        if not name.startswith('__'):
-            item = getattr(gtsam_unstable, name)
-            if callable(item):
-                item = _deprecated_wrapper(item, name)
-            globals()[name] = item
-
-except ImportError:
-    pass
-
diff --git a/cython/gtsam_eigency/CMakeLists.txt b/cython/gtsam_eigency/CMakeLists.txt
deleted file mode 100644
index a0cf0fbde..000000000
--- a/cython/gtsam_eigency/CMakeLists.txt
+++ /dev/null
@@ -1,42 +0,0 @@
-include(GtsamCythonWrap)
-
-# Copy eigency's sources to the build folder
-# 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 "${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 ${OUTPUT_DIR}/__init__.py)
-
-# include eigency headers
-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_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"
-  ${OUTPUT_DIR} "${EIGENCY_INCLUDE_DIR}" "" "" "")
-
-# Include Eigen headers:
-target_include_directories(cythonize_eigency_conversions PUBLIC
-  $<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
-  $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
-)
-target_include_directories(cythonize_eigency_core PUBLIC
-  $<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
-  $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
-)
-
-add_dependencies(cythonize_eigency_core cythonize_eigency_conversions)
-add_custom_target(cythonize_eigency)
-add_dependencies(cythonize_eigency cythonize_eigency_conversions cythonize_eigency_core)
-
-if(TARGET ${python_install_target})
-  add_dependencies(${python_install_target} cythonize_eigency)
-endif()
diff --git a/cython/gtsam_eigency/LICENSE.txt b/cython/gtsam_eigency/LICENSE.txt
deleted file mode 100644
index 71743c864..000000000
--- a/cython/gtsam_eigency/LICENSE.txt
+++ /dev/null
@@ -1,20 +0,0 @@
-Copyright (c) 2016 Wouter Boomsma
-
-Permission is hereby granted, free of charge, to any person obtaining
-a copy of this software and associated documentation files (the
-"Software"), to deal in the Software without restriction, including
-without limitation the rights to use, copy, modify, merge, publish,
-distribute, sublicense, and/or sell copies of the Software, and to
-permit persons to whom the Software is furnished to do so, subject to
-the following conditions:
-
-The above copyright notice and this permission notice shall be
-included in all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
-EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
-MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
-NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
-LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
-OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
-WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
diff --git a/cython/gtsam_eigency/__init__.py.in b/cython/gtsam_eigency/__init__.py.in
deleted file mode 100644
index a59d51eab..000000000
--- a/cython/gtsam_eigency/__init__.py.in
+++ /dev/null
@@ -1,13 +0,0 @@
-import os
-import numpy as np
-
-__eigen_dir__ = "${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}"
-
-def get_includes(include_eigen=True):
-    root = os.path.dirname(__file__)
-    parent = os.path.join(root, "..")
-    path = [root, parent, np.get_include()]
-    if include_eigen:
-        path.append(os.path.join(root, __eigen_dir__))
-    return path
-
diff --git a/cython/gtsam_eigency/conversions.pxd b/cython/gtsam_eigency/conversions.pxd
deleted file mode 100644
index f4445e585..000000000
--- a/cython/gtsam_eigency/conversions.pxd
+++ /dev/null
@@ -1,62 +0,0 @@
-cimport numpy as np
-
-cdef api np.ndarray[double, ndim=2] ndarray_double_C(double *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[double, ndim=2] ndarray_double_F(double *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[double, ndim=2] ndarray_copy_double_C(const double *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[double, ndim=2] ndarray_copy_double_F(const double *data, long rows, long cols, long outer_stride, long inner_stride)
-
-cdef api np.ndarray[float, ndim=2] ndarray_float_C(float *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[float, ndim=2] ndarray_float_F(float *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[float, ndim=2] ndarray_copy_float_C(const float *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[float, ndim=2] ndarray_copy_float_F(const float *data, long rows, long cols, long outer_stride, long inner_stride)
-
-cdef api np.ndarray[long, ndim=2] ndarray_long_C(long *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[long, ndim=2] ndarray_long_F(long *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[long, ndim=2] ndarray_copy_long_C(const long *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[long, ndim=2] ndarray_copy_long_F(const long *data, long rows, long cols, long outer_stride, long inner_stride)
-
-cdef api np.ndarray[unsigned long, ndim=2] ndarray_ulong_C(unsigned long *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[unsigned long, ndim=2] ndarray_ulong_F(unsigned long *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_C(const unsigned long *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_F(const unsigned long *data, long rows, long cols, long outer_stride, long inner_stride)
-
-cdef api np.ndarray[int, ndim=2] ndarray_int_C(int *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[int, ndim=2] ndarray_int_F(int *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[int, ndim=2] ndarray_copy_int_C(const int *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[int, ndim=2] ndarray_copy_int_F(const int *data, long rows, long cols, long outer_stride, long inner_stride)
-
-cdef api np.ndarray[unsigned int, ndim=2] ndarray_uint_C(unsigned int *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[unsigned int, ndim=2] ndarray_uint_F(unsigned int *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_C(const unsigned int *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_F(const unsigned int *data, long rows, long cols, long outer_stride, long inner_stride)
-
-cdef api np.ndarray[short, ndim=2] ndarray_short_C(short *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[short, ndim=2] ndarray_short_F(short *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[short, ndim=2] ndarray_copy_short_C(const short *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[short, ndim=2] ndarray_copy_short_F(const short *data, long rows, long cols, long outer_stride, long inner_stride)
-
-cdef api np.ndarray[unsigned short, ndim=2] ndarray_ushort_C(unsigned short *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[unsigned short, ndim=2] ndarray_ushort_F(unsigned short *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_C(const unsigned short *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_F(const unsigned short *data, long rows, long cols, long outer_stride, long inner_stride)
-
-cdef api np.ndarray[signed char, ndim=2] ndarray_schar_C(signed char *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[signed char, ndim=2] ndarray_schar_F(signed char *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[signed char, ndim=2] ndarray_copy_schar_C(const signed char *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[signed char, ndim=2] ndarray_copy_schar_F(const signed char *data, long rows, long cols, long outer_stride, long inner_stride)
-
-cdef api np.ndarray[unsigned char, ndim=2] ndarray_uchar_C(unsigned char *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[unsigned char, ndim=2] ndarray_uchar_F(unsigned char *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_C(const unsigned char *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_F(const unsigned char *data, long rows, long cols, long outer_stride, long inner_stride)
-
-cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_C(np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_F(np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_C(const np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_F(const np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride)
-
-cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_C(np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_F(np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_C(const np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride)
-cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_F(const np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride)
-
diff --git a/cython/gtsam_eigency/conversions.pyx b/cython/gtsam_eigency/conversions.pyx
deleted file mode 100644
index 55c9ae0cd..000000000
--- a/cython/gtsam_eigency/conversions.pyx
+++ /dev/null
@@ -1,327 +0,0 @@
-cimport cython
-import numpy as np
-from numpy.lib.stride_tricks import as_strided
-
-@cython.boundscheck(False)
-cdef np.ndarray[double, ndim=2] ndarray_double_C(double *data, long rows, long cols, long row_stride, long col_stride):
-    cdef double[:,:] mem_view = <double[:rows,:cols]>data
-    dtype = 'double'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
-@cython.boundscheck(False)
-cdef np.ndarray[double, ndim=2] ndarray_double_F(double *data, long rows, long cols, long row_stride, long col_stride):
-    cdef double[::1,:] mem_view = <double[:rows:1,:cols]>data
-    dtype = 'double'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
-
-@cython.boundscheck(False)
-cdef np.ndarray[double, ndim=2] ndarray_copy_double_C(const double *data, long rows, long cols, long row_stride, long col_stride):
-    cdef double[:,:] mem_view = <double[:rows,:cols]>data
-    dtype = 'double'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
-@cython.boundscheck(False)
-cdef np.ndarray[double, ndim=2] ndarray_copy_double_F(const double *data, long rows, long cols, long row_stride, long col_stride):
-    cdef double[::1,:] mem_view = <double[:rows:1,:cols]>data
-    dtype = 'double'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
-
-
-@cython.boundscheck(False)
-cdef np.ndarray[float, ndim=2] ndarray_float_C(float *data, long rows, long cols, long row_stride, long col_stride):
-    cdef float[:,:] mem_view = <float[:rows,:cols]>data
-    dtype = 'float'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
-@cython.boundscheck(False)
-cdef np.ndarray[float, ndim=2] ndarray_float_F(float *data, long rows, long cols, long row_stride, long col_stride):
-    cdef float[::1,:] mem_view = <float[:rows:1,:cols]>data
-    dtype = 'float'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
-
-@cython.boundscheck(False)
-cdef np.ndarray[float, ndim=2] ndarray_copy_float_C(const float *data, long rows, long cols, long row_stride, long col_stride):
-    cdef float[:,:] mem_view = <float[:rows,:cols]>data
-    dtype = 'float'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
-@cython.boundscheck(False)
-cdef np.ndarray[float, ndim=2] ndarray_copy_float_F(const float *data, long rows, long cols, long row_stride, long col_stride):
-    cdef float[::1,:] mem_view = <float[:rows:1,:cols]>data
-    dtype = 'float'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
-
-
-@cython.boundscheck(False)
-cdef np.ndarray[long, ndim=2] ndarray_long_C(long *data, long rows, long cols, long row_stride, long col_stride):
-    cdef long[:,:] mem_view = <long[:rows,:cols]>data
-    dtype = 'int_'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
-@cython.boundscheck(False)
-cdef np.ndarray[long, ndim=2] ndarray_long_F(long *data, long rows, long cols, long row_stride, long col_stride):
-    cdef long[::1,:] mem_view = <long[:rows:1,:cols]>data
-    dtype = 'int_'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
-
-@cython.boundscheck(False)
-cdef np.ndarray[long, ndim=2] ndarray_copy_long_C(const long *data, long rows, long cols, long row_stride, long col_stride):
-    cdef long[:,:] mem_view = <long[:rows,:cols]>data
-    dtype = 'int_'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
-@cython.boundscheck(False)
-cdef np.ndarray[long, ndim=2] ndarray_copy_long_F(const long *data, long rows, long cols, long row_stride, long col_stride):
-    cdef long[::1,:] mem_view = <long[:rows:1,:cols]>data
-    dtype = 'int_'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
-
-
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned long, ndim=2] ndarray_ulong_C(unsigned long *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned long[:,:] mem_view = <unsigned long[:rows,:cols]>data
-    dtype = 'uint'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned long, ndim=2] ndarray_ulong_F(unsigned long *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned long[::1,:] mem_view = <unsigned long[:rows:1,:cols]>data
-    dtype = 'uint'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
-
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_C(const unsigned long *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned long[:,:] mem_view = <unsigned long[:rows,:cols]>data
-    dtype = 'uint'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_F(const unsigned long *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned long[::1,:] mem_view = <unsigned long[:rows:1,:cols]>data
-    dtype = 'uint'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
-
-
-@cython.boundscheck(False)
-cdef np.ndarray[int, ndim=2] ndarray_int_C(int *data, long rows, long cols, long row_stride, long col_stride):
-    cdef int[:,:] mem_view = <int[:rows,:cols]>data
-    dtype = 'int'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
-@cython.boundscheck(False)
-cdef np.ndarray[int, ndim=2] ndarray_int_F(int *data, long rows, long cols, long row_stride, long col_stride):
-    cdef int[::1,:] mem_view = <int[:rows:1,:cols]>data
-    dtype = 'int'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
-
-@cython.boundscheck(False)
-cdef np.ndarray[int, ndim=2] ndarray_copy_int_C(const int *data, long rows, long cols, long row_stride, long col_stride):
-    cdef int[:,:] mem_view = <int[:rows,:cols]>data
-    dtype = 'int'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
-@cython.boundscheck(False)
-cdef np.ndarray[int, ndim=2] ndarray_copy_int_F(const int *data, long rows, long cols, long row_stride, long col_stride):
-    cdef int[::1,:] mem_view = <int[:rows:1,:cols]>data
-    dtype = 'int'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
-
-
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned int, ndim=2] ndarray_uint_C(unsigned int *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned int[:,:] mem_view = <unsigned int[:rows,:cols]>data
-    dtype = 'uint'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned int, ndim=2] ndarray_uint_F(unsigned int *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned int[::1,:] mem_view = <unsigned int[:rows:1,:cols]>data
-    dtype = 'uint'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
-
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_C(const unsigned int *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned int[:,:] mem_view = <unsigned int[:rows,:cols]>data
-    dtype = 'uint'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_F(const unsigned int *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned int[::1,:] mem_view = <unsigned int[:rows:1,:cols]>data
-    dtype = 'uint'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
-
-
-@cython.boundscheck(False)
-cdef np.ndarray[short, ndim=2] ndarray_short_C(short *data, long rows, long cols, long row_stride, long col_stride):
-    cdef short[:,:] mem_view = <short[:rows,:cols]>data
-    dtype = 'short'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
-@cython.boundscheck(False)
-cdef np.ndarray[short, ndim=2] ndarray_short_F(short *data, long rows, long cols, long row_stride, long col_stride):
-    cdef short[::1,:] mem_view = <short[:rows:1,:cols]>data
-    dtype = 'short'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
-
-@cython.boundscheck(False)
-cdef np.ndarray[short, ndim=2] ndarray_copy_short_C(const short *data, long rows, long cols, long row_stride, long col_stride):
-    cdef short[:,:] mem_view = <short[:rows,:cols]>data
-    dtype = 'short'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
-@cython.boundscheck(False)
-cdef np.ndarray[short, ndim=2] ndarray_copy_short_F(const short *data, long rows, long cols, long row_stride, long col_stride):
-    cdef short[::1,:] mem_view = <short[:rows:1,:cols]>data
-    dtype = 'short'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
-
-
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned short, ndim=2] ndarray_ushort_C(unsigned short *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned short[:,:] mem_view = <unsigned short[:rows,:cols]>data
-    dtype = 'ushort'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned short, ndim=2] ndarray_ushort_F(unsigned short *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned short[::1,:] mem_view = <unsigned short[:rows:1,:cols]>data
-    dtype = 'ushort'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
-
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_C(const unsigned short *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned short[:,:] mem_view = <unsigned short[:rows,:cols]>data
-    dtype = 'ushort'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_F(const unsigned short *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned short[::1,:] mem_view = <unsigned short[:rows:1,:cols]>data
-    dtype = 'ushort'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
-
-
-@cython.boundscheck(False)
-cdef np.ndarray[signed char, ndim=2] ndarray_schar_C(signed char *data, long rows, long cols, long row_stride, long col_stride):
-    cdef signed char[:,:] mem_view = <signed char[:rows,:cols]>data
-    dtype = 'int8'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
-@cython.boundscheck(False)
-cdef np.ndarray[signed char, ndim=2] ndarray_schar_F(signed char *data, long rows, long cols, long row_stride, long col_stride):
-    cdef signed char[::1,:] mem_view = <signed char[:rows:1,:cols]>data
-    dtype = 'int8'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
-
-@cython.boundscheck(False)
-cdef np.ndarray[signed char, ndim=2] ndarray_copy_schar_C(const signed char *data, long rows, long cols, long row_stride, long col_stride):
-    cdef signed char[:,:] mem_view = <signed char[:rows,:cols]>data
-    dtype = 'int8'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
-@cython.boundscheck(False)
-cdef np.ndarray[signed char, ndim=2] ndarray_copy_schar_F(const signed char *data, long rows, long cols, long row_stride, long col_stride):
-    cdef signed char[::1,:] mem_view = <signed char[:rows:1,:cols]>data
-    dtype = 'int8'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
-
-
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned char, ndim=2] ndarray_uchar_C(unsigned char *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned char[:,:] mem_view = <unsigned char[:rows,:cols]>data
-    dtype = 'uint8'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned char, ndim=2] ndarray_uchar_F(unsigned char *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned char[::1,:] mem_view = <unsigned char[:rows:1,:cols]>data
-    dtype = 'uint8'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
-
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_C(const unsigned char *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned char[:,:] mem_view = <unsigned char[:rows,:cols]>data
-    dtype = 'uint8'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
-@cython.boundscheck(False)
-cdef np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_F(const unsigned char *data, long rows, long cols, long row_stride, long col_stride):
-    cdef unsigned char[::1,:] mem_view = <unsigned char[:rows:1,:cols]>data
-    dtype = 'uint8'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
-
-
-@cython.boundscheck(False)
-cdef np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_C(np.complex128_t *data, long rows, long cols, long row_stride, long col_stride):
-    cdef np.complex128_t[:,:] mem_view = <np.complex128_t[:rows,:cols]>data
-    dtype = 'complex128'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
-@cython.boundscheck(False)
-cdef np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_F(np.complex128_t *data, long rows, long cols, long row_stride, long col_stride):
-    cdef np.complex128_t[::1,:] mem_view = <np.complex128_t[:rows:1,:cols]>data
-    dtype = 'complex128'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
-
-@cython.boundscheck(False)
-cdef np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_C(const np.complex128_t *data, long rows, long cols, long row_stride, long col_stride):
-    cdef np.complex128_t[:,:] mem_view = <np.complex128_t[:rows,:cols]>data
-    dtype = 'complex128'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
-@cython.boundscheck(False)
-cdef np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_F(const np.complex128_t *data, long rows, long cols, long row_stride, long col_stride):
-    cdef np.complex128_t[::1,:] mem_view = <np.complex128_t[:rows:1,:cols]>data
-    dtype = 'complex128'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
-
-
-@cython.boundscheck(False)
-cdef np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_C(np.complex64_t *data, long rows, long cols, long row_stride, long col_stride):
-    cdef np.complex64_t[:,:] mem_view = <np.complex64_t[:rows,:cols]>data
-    dtype = 'complex64'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])
-@cython.boundscheck(False)
-cdef np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_F(np.complex64_t *data, long rows, long cols, long row_stride, long col_stride):
-    cdef np.complex64_t[::1,:] mem_view = <np.complex64_t[:rows:1,:cols]>data
-    dtype = 'complex64'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])
-
-@cython.boundscheck(False)
-cdef np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_C(const np.complex64_t *data, long rows, long cols, long row_stride, long col_stride):
-    cdef np.complex64_t[:,:] mem_view = <np.complex64_t[:rows,:cols]>data
-    dtype = 'complex64'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]))
-@cython.boundscheck(False)
-cdef np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_F(const np.complex64_t *data, long rows, long cols, long row_stride, long col_stride):
-    cdef np.complex64_t[::1,:] mem_view = <np.complex64_t[:rows:1,:cols]>data
-    dtype = 'complex64'
-    cdef int itemsize = np.dtype(dtype).itemsize
-    return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]))
-
diff --git a/cython/gtsam_eigency/core.pxd b/cython/gtsam_eigency/core.pxd
deleted file mode 100644
index 9a84c3c16..000000000
--- a/cython/gtsam_eigency/core.pxd
+++ /dev/null
@@ -1,917 +0,0 @@
-cimport cython
-cimport numpy as np
-
-ctypedef signed char schar;
-ctypedef unsigned char uchar;
-
-ctypedef fused dtype:
-    uchar
-    schar
-    short
-    int
-    long
-    float
-    double
-
-ctypedef fused DenseType:
-    Matrix
-    Array
-
-ctypedef fused Rows:
-    _1
-    _2
-    _3
-    _4
-    _5
-    _6
-    _7
-    _8
-    _9
-    _10
-    _11
-    _12
-    _13
-    _14
-    _15
-    _16
-    _17
-    _18
-    _19
-    _20
-    _21
-    _22
-    _23
-    _24
-    _25
-    _26
-    _27
-    _28
-    _29
-    _30
-    _31
-    _32
-    Dynamic
-
-ctypedef Rows Cols
-ctypedef Rows StrideOuter
-ctypedef Rows StrideInner
-
-ctypedef fused DenseTypeShort:
-    Vector1i
-    Vector2i
-    Vector3i
-    Vector4i
-    VectorXi
-    RowVector1i
-    RowVector2i
-    RowVector3i
-    RowVector4i
-    RowVectorXi
-    Matrix1i
-    Matrix2i
-    Matrix3i
-    Matrix4i
-    MatrixXi
-    Vector1f
-    Vector2f
-    Vector3f
-    Vector4f
-    VectorXf
-    RowVector1f
-    RowVector2f
-    RowVector3f
-    RowVector4f
-    RowVectorXf
-    Matrix1f
-    Matrix2f
-    Matrix3f
-    Matrix4f
-    MatrixXf
-    Vector1d
-    Vector2d
-    Vector3d
-    Vector4d
-    VectorXd
-    RowVector1d
-    RowVector2d
-    RowVector3d
-    RowVector4d
-    RowVectorXd
-    Matrix1d
-    Matrix2d
-    Matrix3d
-    Matrix4d
-    MatrixXd
-    Vector1cf
-    Vector2cf
-    Vector3cf
-    Vector4cf
-    VectorXcf
-    RowVector1cf
-    RowVector2cf
-    RowVector3cf
-    RowVector4cf
-    RowVectorXcf
-    Matrix1cf
-    Matrix2cf
-    Matrix3cf
-    Matrix4cf
-    MatrixXcf
-    Vector1cd
-    Vector2cd
-    Vector3cd
-    Vector4cd
-    VectorXcd
-    RowVector1cd
-    RowVector2cd
-    RowVector3cd
-    RowVector4cd
-    RowVectorXcd
-    Matrix1cd
-    Matrix2cd
-    Matrix3cd
-    Matrix4cd
-    MatrixXcd
-    Array22i
-    Array23i
-    Array24i
-    Array2Xi
-    Array32i
-    Array33i
-    Array34i
-    Array3Xi
-    Array42i
-    Array43i
-    Array44i
-    Array4Xi
-    ArrayX2i
-    ArrayX3i
-    ArrayX4i
-    ArrayXXi
-    Array2i
-    Array3i
-    Array4i
-    ArrayXi
-    Array22f
-    Array23f
-    Array24f
-    Array2Xf
-    Array32f
-    Array33f
-    Array34f
-    Array3Xf
-    Array42f
-    Array43f
-    Array44f
-    Array4Xf
-    ArrayX2f
-    ArrayX3f
-    ArrayX4f
-    ArrayXXf
-    Array2f
-    Array3f
-    Array4f
-    ArrayXf
-    Array22d
-    Array23d
-    Array24d
-    Array2Xd
-    Array32d
-    Array33d
-    Array34d
-    Array3Xd
-    Array42d
-    Array43d
-    Array44d
-    Array4Xd
-    ArrayX2d
-    ArrayX3d
-    ArrayX4d
-    ArrayXXd
-    Array2d
-    Array3d
-    Array4d
-    ArrayXd
-    Array22cf
-    Array23cf
-    Array24cf
-    Array2Xcf
-    Array32cf
-    Array33cf
-    Array34cf
-    Array3Xcf
-    Array42cf
-    Array43cf
-    Array44cf
-    Array4Xcf
-    ArrayX2cf
-    ArrayX3cf
-    ArrayX4cf
-    ArrayXXcf
-    Array2cf
-    Array3cf
-    Array4cf
-    ArrayXcf
-    Array22cd
-    Array23cd
-    Array24cd
-    Array2Xcd
-    Array32cd
-    Array33cd
-    Array34cd
-    Array3Xcd
-    Array42cd
-    Array43cd
-    Array44cd
-    Array4Xcd
-    ArrayX2cd
-    ArrayX3cd
-    ArrayX4cd
-    ArrayXXcd
-    Array2cd
-    Array3cd
-    Array4cd
-    ArrayXcd
-
-ctypedef fused StorageOrder:
-    RowMajor
-    ColMajor
-
-ctypedef fused MapOptions:
-    Aligned
-    Unaligned
-
-cdef extern from "eigency_cpp.h" namespace "eigency":
-
-     cdef cppclass _1 "1":
-          pass
-          
-     cdef cppclass _2 "2":
-          pass
-
-     cdef cppclass _3 "3":
-          pass
-          
-     cdef cppclass _4 "4":
-          pass
-
-     cdef cppclass _5 "5":
-          pass
-          
-     cdef cppclass _6 "6":
-          pass
-
-     cdef cppclass _7 "7":
-          pass
-          
-     cdef cppclass _8 "8":
-          pass
-
-     cdef cppclass _9 "9":
-          pass
-          
-     cdef cppclass _10 "10":
-          pass
-
-     cdef cppclass _11 "11":
-          pass
-          
-     cdef cppclass _12 "12":
-          pass
-
-     cdef cppclass _13 "13":
-          pass
-          
-     cdef cppclass _14 "14":
-          pass
-
-     cdef cppclass _15 "15":
-          pass
-          
-     cdef cppclass _16 "16":
-          pass
-
-     cdef cppclass _17 "17":
-          pass
-          
-     cdef cppclass _18 "18":
-          pass
-
-     cdef cppclass _19 "19":
-          pass
-          
-     cdef cppclass _20 "20":
-          pass
-
-     cdef cppclass _21 "21":
-          pass
-          
-     cdef cppclass _22 "22":
-          pass
-
-     cdef cppclass _23 "23":
-          pass
-          
-     cdef cppclass _24 "24":
-          pass
-
-     cdef cppclass _25 "25":
-          pass
-          
-     cdef cppclass _26 "26":
-          pass
-
-     cdef cppclass _27 "27":
-          pass
-          
-     cdef cppclass _28 "28":
-          pass
-
-     cdef cppclass _29 "29":
-          pass
-          
-     cdef cppclass _30 "30":
-          pass
-
-     cdef cppclass _31 "31":
-          pass
-          
-     cdef cppclass _32 "32":
-          pass
-
-     cdef cppclass PlainObjectBase:
-          pass
-
-     cdef cppclass Map[DenseTypeShort](PlainObjectBase):
-         Map() except +
-         Map(np.ndarray array) except +
-
-     cdef cppclass FlattenedMap[DenseType, dtype, Rows, Cols]:
-         FlattenedMap() except +
-         FlattenedMap(np.ndarray array) except +
-
-     cdef cppclass FlattenedMapWithOrder "eigency::FlattenedMap" [DenseType, dtype, Rows, Cols, StorageOrder]:
-         FlattenedMapWithOrder() except +
-         FlattenedMapWithOrder(np.ndarray array) except +
-
-     cdef cppclass FlattenedMapWithStride "eigency::FlattenedMap" [DenseType, dtype, Rows, Cols, StorageOrder, MapOptions, StrideOuter, StrideInner]:
-         FlattenedMapWithStride() except +
-         FlattenedMapWithStride(np.ndarray array) except +
-
-     cdef np.ndarray ndarray_view(PlainObjectBase &)
-     cdef np.ndarray ndarray_copy(PlainObjectBase &)
-     cdef np.ndarray ndarray(PlainObjectBase &)
-
-
-cdef extern from "eigency_cpp.h" namespace "Eigen":
-
-     cdef cppclass Dynamic:
-          pass
-
-     cdef cppclass RowMajor:
-          pass
-
-     cdef cppclass ColMajor:
-          pass
-          
-     cdef cppclass Aligned:
-          pass
-
-     cdef cppclass Unaligned:
-          pass
-
-     cdef cppclass Matrix(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array(PlainObjectBase):
-          pass
-          
-     cdef cppclass VectorXd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Vector1i(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector2i(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector3i(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector4i(PlainObjectBase):
-          pass
-
-     cdef cppclass VectorXi(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector1i(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector2i(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector3i(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector4i(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVectorXi(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix1i(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix2i(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix3i(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix4i(PlainObjectBase):
-          pass
-
-     cdef cppclass MatrixXi(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector1f(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector2f(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector3f(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector4f(PlainObjectBase):
-          pass
-
-     cdef cppclass VectorXf(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector1f(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector2f(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector3f(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector4f(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVectorXf(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix1f(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix2f(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix3f(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix4f(PlainObjectBase):
-          pass
-
-     cdef cppclass MatrixXf(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector1d(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector2d(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector3d(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector4d(PlainObjectBase):
-          pass
-
-     cdef cppclass VectorXd(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector1d(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector2d(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector3d(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector4d(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVectorXd(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix1d(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix2d(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix3d(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix4d(PlainObjectBase):
-          pass
-
-     cdef cppclass MatrixXd(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector1cf(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector2cf(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector3cf(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector4cf(PlainObjectBase):
-          pass
-
-     cdef cppclass VectorXcf(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector1cf(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector2cf(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector3cf(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector4cf(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVectorXcf(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix1cf(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix2cf(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix3cf(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix4cf(PlainObjectBase):
-          pass
-
-     cdef cppclass MatrixXcf(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector1cd(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector2cd(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector3cd(PlainObjectBase):
-          pass
-
-     cdef cppclass Vector4cd(PlainObjectBase):
-          pass
-
-     cdef cppclass VectorXcd(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector1cd(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector2cd(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector3cd(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVector4cd(PlainObjectBase):
-          pass
-
-     cdef cppclass RowVectorXcd(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix1cd(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix2cd(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix3cd(PlainObjectBase):
-          pass
-
-     cdef cppclass Matrix4cd(PlainObjectBase):
-          pass
-
-     cdef cppclass MatrixXcd(PlainObjectBase):
-          pass
-
-     cdef cppclass Array22i(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array23i(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array24i(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array2Xi(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array32i(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array33i(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array34i(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array3Xi(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array42i(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array43i(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array44i(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array4Xi(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX2i(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX3i(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX4i(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayXXi(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array2i(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array3i(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array4i(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayXi(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array22f(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array23f(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array24f(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array2Xf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array32f(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array33f(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array34f(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array3Xf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array42f(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array43f(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array44f(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array4Xf(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX2f(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX3f(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX4f(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayXXf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array2f(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array3f(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array4f(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayXf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array22d(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array23d(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array24d(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array2Xd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array32d(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array33d(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array34d(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array3Xd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array42d(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array43d(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array44d(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array4Xd(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX2d(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX3d(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX4d(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayXXd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array2d(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array3d(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array4d(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayXd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array22cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array23cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array24cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array2Xcf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array32cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array33cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array34cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array3Xcf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array42cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array43cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array44cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array4Xcf(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX2cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX3cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX4cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayXXcf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array2cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array3cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array4cf(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayXcf(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array22cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array23cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array24cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array2Xcd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array32cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array33cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array34cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array3Xcd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array42cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array43cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array44cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array4Xcd(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX2cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX3cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayX4cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayXXcd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array2cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array3cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass Array4cd(PlainObjectBase):
-          pass
-          
-     cdef cppclass ArrayXcd(PlainObjectBase):
-          pass
-          
-
diff --git a/cython/gtsam_eigency/core.pyx b/cython/gtsam_eigency/core.pyx
deleted file mode 100644
index 8b1378917..000000000
--- a/cython/gtsam_eigency/core.pyx
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/cython/gtsam_eigency/eigency_cpp.h b/cython/gtsam_eigency/eigency_cpp.h
deleted file mode 100644
index ce303182e..000000000
--- a/cython/gtsam_eigency/eigency_cpp.h
+++ /dev/null
@@ -1,504 +0,0 @@
-#include <Eigen/Dense>
-
-#include <iostream>
-#include <stdexcept>
-#include <complex>
-
-typedef ::std::complex< double > __pyx_t_double_complex;
-typedef ::std::complex< float > __pyx_t_float_complex;
-
-#include "conversions_api.h"
-
-#ifndef EIGENCY_CPP
-#define EIGENCY_CPP
-
-namespace eigency {
-
-template<typename Scalar>
-inline PyArrayObject *_ndarray_view(Scalar *, long rows, long cols, bool is_row_major, long outer_stride=0, long inner_stride=0);
-template<typename Scalar>
-inline PyArrayObject *_ndarray_copy(const Scalar *, long rows, long cols, bool is_row_major, long outer_stride=0, long inner_stride=0);
-
-// Strides:
-// Eigen and numpy differ in their way of dealing with strides. Eigen has the concept of outer and
-// inner strides, which are dependent on whether the array/matrix is row-major of column-major:
-//     Inner stride: denotes the offset between succeeding elements in each row (row-major) or column (column-major).
-//     Outer stride: denotes the offset between succeeding rows (row-major) or succeeding columns (column-major).
-// In contrast, numpy's stride is simply a measure of how fast each dimension should be incremented.
-// Consequently, a switch in numpy storage order from row-major to column-major involves a switch
-// in strides, while it does not affect the stride in Eigen.
-template<>
-inline PyArrayObject *_ndarray_view<double>(double *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major) {
-        // Eigen row-major mode: row_stride=outer_stride, and col_stride=inner_stride
-        // If no stride is given, the row_stride is set to the number of columns.
-        return ndarray_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    } else {
-        // Eigen column-major mode: row_stride=outer_stride, and col_stride=inner_stride
-        // If no stride is given, the cow_stride is set to the number of rows.
-        return ndarray_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-    }
-}
-template<>
-inline PyArrayObject *_ndarray_copy<double>(const double *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_copy_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_copy_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-
-template<>
-inline PyArrayObject *_ndarray_view<float>(float *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-template<>
-inline PyArrayObject *_ndarray_copy<float>(const float *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_copy_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_copy_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-
-template<>
-inline PyArrayObject *_ndarray_view<long>(long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_long_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_long_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-template<>
-inline PyArrayObject *_ndarray_copy<long>(const long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_copy_long_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_copy_long_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-
-template<>
-inline PyArrayObject *_ndarray_view<unsigned long>(unsigned long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_ulong_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_ulong_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-template<>
-inline PyArrayObject *_ndarray_copy<unsigned long>(const unsigned long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_copy_ulong_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_copy_ulong_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-
-template<>
-inline PyArrayObject *_ndarray_view<int>(int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_int_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_int_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-template<>
-inline PyArrayObject *_ndarray_copy<int>(const int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_copy_int_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_copy_int_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-
-template<>
-inline PyArrayObject *_ndarray_view<unsigned int>(unsigned int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_uint_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_uint_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-template<>
-inline PyArrayObject *_ndarray_copy<unsigned int>(const unsigned int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_copy_uint_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_copy_uint_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-
-template<>
-inline PyArrayObject *_ndarray_view<short>(short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_short_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_short_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-template<>
-inline PyArrayObject *_ndarray_copy<short>(const short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_copy_short_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_copy_short_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-
-template<>
-inline PyArrayObject *_ndarray_view<unsigned short>(unsigned short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_ushort_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_ushort_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-template<>
-inline PyArrayObject *_ndarray_copy<unsigned short>(const unsigned short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_copy_ushort_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_copy_ushort_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-
-template<>
-inline PyArrayObject *_ndarray_view<signed char>(signed char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_schar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_schar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-template<>
-inline PyArrayObject *_ndarray_copy<signed char>(const signed char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_copy_schar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_copy_schar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-
-template<>
-inline PyArrayObject *_ndarray_view<unsigned char>(unsigned char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_uchar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_uchar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-template<>
-inline PyArrayObject *_ndarray_copy<unsigned char>(const unsigned char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_copy_uchar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_copy_uchar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-
-template<>
-inline PyArrayObject *_ndarray_view<std::complex<double> >(std::complex<double> *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_complex_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_complex_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-template<>
-inline PyArrayObject *_ndarray_copy<std::complex<double> >(const std::complex<double> *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_copy_complex_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_copy_complex_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-
-template<>
-inline PyArrayObject *_ndarray_view<std::complex<float> >(std::complex<float> *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_complex_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_complex_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-template<>
-inline PyArrayObject *_ndarray_copy<std::complex<float> >(const std::complex<float> *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) {
-    if (is_row_major)
-        return ndarray_copy_complex_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
-    else
-        return ndarray_copy_complex_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
-}
-
-
-template <typename Derived>
-inline PyArrayObject *ndarray(Eigen::PlainObjectBase<Derived> &m) {
-    import_gtsam_eigency__conversions();
-    return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor);
-}
-// If C++11 is available, check if m is an r-value reference, in
-// which case a copy should always be made
-#if __cplusplus >= 201103L
-template <typename Derived>
-inline PyArrayObject *ndarray(Eigen::PlainObjectBase<Derived> &&m) {
-    import_gtsam_eigency__conversions();
-    return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor);
-}
-#endif
-template <typename Derived>
-inline PyArrayObject *ndarray(const Eigen::PlainObjectBase<Derived> &m) {
-    import_gtsam_eigency__conversions();
-    return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor);
-}
-template <typename Derived>
-inline PyArrayObject *ndarray_view(Eigen::PlainObjectBase<Derived> &m) {
-    import_gtsam_eigency__conversions();
-    return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor);
-}
-template <typename Derived>
-inline PyArrayObject *ndarray_view(const Eigen::PlainObjectBase<Derived> &m) {
-    import_gtsam_eigency__conversions();
-    return _ndarray_view(const_cast<typename Derived::Scalar*>(m.data()), m.rows(), m.cols(), m.IsRowMajor);
-}
-template <typename Derived>
-inline PyArrayObject *ndarray_copy(const Eigen::PlainObjectBase<Derived> &m) {
-    import_gtsam_eigency__conversions();
-    return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor);
-}
-
-template <typename Derived, int MapOptions, typename Stride>
-inline PyArrayObject *ndarray(Eigen::Map<Derived, MapOptions, Stride> &m) {
-    import_gtsam_eigency__conversions();
-    return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
-}
-template <typename Derived, int MapOptions, typename Stride>
-inline PyArrayObject *ndarray(const Eigen::Map<Derived, MapOptions, Stride> &m) {
-    import_gtsam_eigency__conversions();
-    // Since this is a map, we assume that ownership is correctly taken care
-    // of, and we avoid taking a copy
-    return _ndarray_view(const_cast<typename Derived::Scalar*>(m.data()), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
-}
-template <typename Derived, int MapOptions, typename Stride>
-inline PyArrayObject *ndarray_view(Eigen::Map<Derived, MapOptions, Stride> &m) {
-    import_gtsam_eigency__conversions();
-    return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
-}
-template <typename Derived, int MapOptions, typename Stride>
-inline PyArrayObject *ndarray_view(const Eigen::Map<Derived, MapOptions, Stride> &m) {
-    import_gtsam_eigency__conversions();
-    return _ndarray_view(const_cast<typename Derived::Scalar*>(m.data()), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
-}
-template <typename Derived, int MapOptions, typename Stride>
-inline PyArrayObject *ndarray_copy(const Eigen::Map<Derived, MapOptions, Stride> &m) {
-    import_gtsam_eigency__conversions();
-    return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
-}
-
-
-template <typename MatrixType,
-          int _MapOptions = Eigen::Unaligned,
-          typename _StrideType=Eigen::Stride<0,0> >
-class MapBase: public Eigen::Map<MatrixType, _MapOptions, _StrideType> {
-public:
-    typedef Eigen::Map<MatrixType, _MapOptions, _StrideType> Base;
-    typedef typename Base::Scalar Scalar;
-
-    MapBase(Scalar* data,
-            long rows,
-            long cols,
-            _StrideType stride=_StrideType())
-        : Base(data,
-               // If both dimensions are dynamic or dimensions match, accept dimensions as they are
-               ((Base::RowsAtCompileTime==Eigen::Dynamic && Base::ColsAtCompileTime==Eigen::Dynamic) ||
-                (Base::RowsAtCompileTime==rows && Base::ColsAtCompileTime==cols))
-               ? rows
-               // otherwise, test if swapping them makes them fit
-               : ((Base::RowsAtCompileTime==cols || Base::ColsAtCompileTime==rows)
-                  ? cols
-                  : rows),
-               ((Base::RowsAtCompileTime==Eigen::Dynamic && Base::ColsAtCompileTime==Eigen::Dynamic) ||
-                (Base::RowsAtCompileTime==rows && Base::ColsAtCompileTime==cols))
-               ? cols
-               : ((Base::RowsAtCompileTime==cols || Base::ColsAtCompileTime==rows)
-                  ? rows
-                  : cols),
-               stride
-            )  {}
-
-    MapBase &operator=(const MatrixType &other) {
-        Base::operator=(other);
-        return *this;
-    }
-
-    virtual ~MapBase() { }
-};
-
-
-template <template<class,int,int,int,int,int> class EigencyDenseBase,
-          typename Scalar,
-          int _Rows, int _Cols,
-          int _Options = Eigen::AutoAlign |
-#if defined(__GNUC__) && __GNUC__==3 && __GNUC_MINOR__==4
-    // workaround a bug in at least gcc 3.4.6
-    // the innermost ?: ternary operator is misparsed. We write it slightly
-    // differently and this makes gcc 3.4.6 happy, but it's ugly.
-    // The error would only show up with EIGEN_DEFAULT_TO_ROW_MAJOR is defined
-    // (when EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION is RowMajor)
-                          ( (_Rows==1 && _Cols!=1) ? Eigen::RowMajor
-// EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION contains explicit namespace since Eigen 3.1.19
-#if EIGEN_VERSION_AT_LEAST(3,2,90)
-                          : !(_Cols==1 && _Rows!=1) ? EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION
-#else
-                          : !(_Cols==1 && _Rows!=1) ? Eigen::EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION
-#endif
-                          : ColMajor ),
-#else
-                          ( (_Rows==1 && _Cols!=1) ? Eigen::RowMajor
-                          : (_Cols==1 && _Rows!=1) ? Eigen::ColMajor
-// EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION contains explicit namespace since Eigen 3.1.19
-#if EIGEN_VERSION_AT_LEAST(3,2,90)
-                          : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ),
-#else
-                          : Eigen::EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ),
-#endif
-#endif
-          int _MapOptions = Eigen::Unaligned,
-          int _StrideOuter=0, int _StrideInner=0,
-          int _MaxRows = _Rows,
-          int _MaxCols = _Cols>
-class FlattenedMap: public MapBase<EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> >  {
-public:
-    typedef MapBase<EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > Base;
-
-    FlattenedMap()
-        : Base(NULL, 0, 0),
-          object_(NULL) {}
-
-    FlattenedMap(Scalar *data, long rows, long cols, long outer_stride=0, long inner_stride=0)
-        : Base(data, rows, cols,
-               Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)),
-          object_(NULL) {
-    }
-
-    FlattenedMap(PyArrayObject *object)
-        : Base((Scalar *)((PyArrayObject*)object)->data,
-        // : Base(_from_numpy<Scalar>((PyArrayObject*)object),
-               (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1,
-               (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0],
-               Eigen::Stride<_StrideOuter, _StrideInner>(_StrideOuter != Eigen::Dynamic ? _StrideOuter : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1,
-                                                         _StrideInner != Eigen::Dynamic ? _StrideInner : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0])),
-          object_(object) {
-
-        if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object))
-            throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map.");
-
-        Py_XINCREF(object_);
-    }
-    FlattenedMap &operator=(const FlattenedMap &other) {
-        if (other.object_) {
-            new (this) FlattenedMap(other.object_);
-        } else {
-            // Replace the memory that we point to (not a memory allocation)
-            new (this) FlattenedMap(const_cast<Scalar*>(other.data()),
-                                    other.rows(),
-                                    other.cols(),
-                                    other.outerStride(),
-                                    other.innerStride());
-        }
-
-        return *this;
-    }
-
-    operator Base() const {
-        return static_cast<Base>(*this);
-    }
-
-    operator Base&() const {
-        return static_cast<Base&>(*this);
-    }
-
-    operator EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>() const {
-        return EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>(static_cast<Base>(*this));
-    }
-
-    virtual ~FlattenedMap() {
-        Py_XDECREF(object_);
-    }
-
-private:
-    PyArrayObject * const object_;
-};
-
-
-template <typename MatrixType>
-class Map: public MapBase<MatrixType> {
-public:
-    typedef MapBase<MatrixType> Base;
-    typedef typename MatrixType::Scalar Scalar;
-
-    enum {
-        RowsAtCompileTime = Base::Base::RowsAtCompileTime,
-        ColsAtCompileTime = Base::Base::ColsAtCompileTime
-    };
-
-    Map()
-        : Base(NULL,
-               (RowsAtCompileTime == Eigen::Dynamic) ? 0 : RowsAtCompileTime,
-               (ColsAtCompileTime == Eigen::Dynamic) ? 0 : ColsAtCompileTime),
-          object_(NULL) {
-    }
-
-    Map(Scalar *data, long rows, long cols)
-        : Base(data, rows, cols),
-          object_(NULL) {}
-
-    Map(PyArrayObject *object)
-        : Base((PyObject*)object == Py_None? NULL: (Scalar *)object->data,
-               // ROW: If array is in row-major order, transpose (see README)
-               (PyObject*)object == Py_None? 0 :
-               (!PyArray_IS_F_CONTIGUOUS(object)
-                ? ((object->nd == 1)
-                   ? 1  // ROW: If 1D row-major numpy array, set to 1 (row vector)
-                   : object->dimensions[1])
-                : object->dimensions[0]),
-               // COLUMN: If array is in row-major order: transpose (see README)
-               (PyObject*)object == Py_None? 0 :
-               (!PyArray_IS_F_CONTIGUOUS(object)
-                ? object->dimensions[0]
-                : ((object->nd == 1)
-                   ? 1  // COLUMN: If 1D col-major numpy array, set to length (column vector)
-                   : object->dimensions[1]))),
-          object_(object) {
-
-        if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object))
-            throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map.");
-        Py_XINCREF(object_);
-    }
-
-    Map &operator=(const Map &other) {
-        if (other.object_) {
-            new (this) Map(other.object_);
-        } else {
-            // Replace the memory that we point to (not a memory allocation)
-            new (this) Map(const_cast<Scalar*>(other.data()),
-                          other.rows(),
-                          other.cols());
-        }
-
-        return *this;
-    }
-
-    Map &operator=(const MatrixType &other) {
-        MapBase<MatrixType>::operator=(other);
-        return *this;
-    }
-
-    operator Base() const {
-        return static_cast<Base>(*this);
-    }
-
-    operator Base&() const {
-        return static_cast<Base&>(*this);
-    }
-
-    operator MatrixType() const {
-        return MatrixType(static_cast<Base>(*this));
-    }
-
-    virtual ~Map() {
-        Py_XDECREF(object_);
-    }
-
-private:
-    PyArrayObject * const object_;
-};
-
-
-}
-
-#endif
-
-
-
diff --git a/cython/requirements.txt b/cython/requirements.txt
deleted file mode 100644
index 8d3c7aeb4..000000000
--- a/cython/requirements.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-Cython>=0.25.2
-backports_abc>=0.5
-numpy>=1.11.0
diff --git a/examples/Data/Klaus3.g2o b/examples/Data/Klaus3.g2o
index 4c7233fa7..83a6e6fd2 100644
--- a/examples/Data/Klaus3.g2o
+++ b/examples/Data/Klaus3.g2o
@@ -1,6 +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
+VERTEX_SE3:QUAT 0 -1.6618596980158338 -0.5736497760548741 -3.3319774096611026 -0.02676080288219576 -0.024497002638379624 -0.015064701622500615 0.9992281076190063
+VERTEX_SE3:QUAT 1 -1.431820463019384 -0.549139761976065 -3.160677992237872 -0.049543805396343954 -0.03232420352077356 -0.004386230477751116 0.998239108728862
+VERTEX_SE3:QUAT 2 -1.0394840214436651 -0.5268841046291037 -2.972143862665523 -0.07993768981394891 0.0825062894866454 -0.04088089479075661 0.9925378735259738
+EDGE_SE3:QUAT 0 1 0.23003923499644974 0.02451001407880915 0.17129941742323052 -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.6223756765721686 0.04676567142577037 0.35983354699557957 -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.3923364415757189 0.022255657346961222 0.18853412957234905 -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/IMUKittiExampleGPS.cpp b/examples/IMUKittiExampleGPS.cpp
index f1d89b47a..e2ca49647 100644
--- a/examples/IMUKittiExampleGPS.cpp
+++ b/examples/IMUKittiExampleGPS.cpp
@@ -287,7 +287,7 @@ int main(int argc, char* argv[]) {
                 new_values.insert(current_pose_key, gps_pose);
 
                 printf("################ POSE INCLUDED AT TIME %lf ################\n", t);
-                gps_pose.translation().print();
+                cout << gps_pose.translation();
                 printf("\n\n");
             } else {
                 new_values.insert(current_pose_key, current_pose_global);
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 8736a5954..37c4a1f88 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -133,6 +133,18 @@ endif()
 # paths so that the compiler uses GTSAM headers in our source directory instead
 # of any previously installed GTSAM headers.
 target_include_directories(gtsam BEFORE PUBLIC
+  # main gtsam includes:
+  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
+  $<INSTALL_INTERFACE:include/>
+  # config.h
+  $<BUILD_INTERFACE:${CMAKE_BINARY_DIR}>
+  # unit tests:
+  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/CppUnitLite>
+)
+# 3rdparty libraries: use the "system" flag so they are included via "-isystem"
+# and warnings (and warnings-considered-errors) in those headers are not 
+# reported as warnings/errors in our targets:
+target_include_directories(gtsam SYSTEM BEFORE PUBLIC
   # SuiteSparse_config
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/SuiteSparse_config>
   $<INSTALL_INTERFACE:include/gtsam/3rdparty/SuiteSparse_config>
@@ -141,13 +153,6 @@ target_include_directories(gtsam BEFORE PUBLIC
   # CCOLAMD
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Include>
   $<INSTALL_INTERFACE:include/gtsam/3rdparty/CCOLAMD>
-  # main gtsam includes:
-  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
-  $<INSTALL_INTERFACE:include/>
-  # config.h
-  $<BUILD_INTERFACE:${CMAKE_BINARY_DIR}>
-  # unit tests:
-  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/CppUnitLite>
 )
 if(GTSAM_SUPPORT_NESTED_DISSECTION)
   target_include_directories(gtsam BEFORE PUBLIC
@@ -212,5 +217,5 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
         endif()
 
     # Wrap
-    wrap_and_install_library(../gtsam.h "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
+    wrap_and_install_library(gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
 endif ()
diff --git a/gtsam/config.h.in b/gtsam/config.h.in
index 9dc10c36a..9d1bd4ebd 100644
--- a/gtsam/config.h.in
+++ b/gtsam/config.h.in
@@ -72,9 +72,6 @@
 // Make sure dependent projects that want it can see deprecated functions
 #cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V41
 
-// Publish flag about Eigen typedef
-#cmakedefine GTSAM_TYPEDEF_POINTS_TO_VECTORS
-
 // Support Metis-based nested dissection
 #cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION
 
diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp
index 4cead869f..d8060cfcf 100644
--- a/gtsam/geometry/Point2.cpp
+++ b/gtsam/geometry/Point2.cpp
@@ -50,37 +50,6 @@ double distance2(const Point2& p, const Point2& q, OptionalJacobian<1, 2> H1,
   }
 }
 
-#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
-
-/* ************************************************************************* */
-void Point2::print(const string& s) const {
-  cout << s << *this << endl;
-}
-
-/* ************************************************************************* */
-bool Point2::equals(const Point2& q, double tol) const {
-  return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol);
-}
-
-/* ************************************************************************* */
-double Point2::norm(OptionalJacobian<1,2> H) const {
-  return gtsam::norm2(*this, H);
-}
-
-/* ************************************************************************* */
-double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
-    OptionalJacobian<1,2> H2) const {
-  return gtsam::distance2(*this, point, H1, H2);
-}
-
-/* ************************************************************************* */
-ostream &operator<<(ostream &os, const Point2& p) {
-  os << '(' << p.x() << ", " << p.y() << ')';
-  return os;
-}
-
-#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
-
 /* ************************************************************************* */
 // Math inspired by http://paulbourke.net/geometry/circlesphere/
 boost::optional<Point2> circleCircleIntersection(double R_d, double r_d,
diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index e186f7b67..e6574fe41 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -22,111 +22,9 @@
 
 namespace gtsam {
 
-#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
-
-  /// As of GTSAM 4, in order to make GTSAM more lean,
-  /// it is now possible to just typedef Point2 to Vector2
-  typedef Vector2 Point2;
-
-#else
-
-/**
- * A 2D point
- * Complies with the Testable Concept
- * Functional, so no set functions: once created, a point is constant.
- * @addtogroup geometry
- * \nosubgrouping
- */
-class Point2 : public Vector2 {
-private:
-
-public:
-  enum { dimension = 2 };
-  /// @name Standard Constructors
-  /// @{
-
-  /// default constructor
-  Point2() {}
-
-  using Vector2::Vector2;
-
-  /// @}
-  /// @name Advanced Constructors
-  /// @{
-
-  /// construct from 2D vector
-  explicit Point2(const Vector2& v):Vector2(v) {}
-  /// @}
-  /// @name Testable
-  /// @{
-
-  /// print with optional string
-  GTSAM_EXPORT void print(const std::string& s = "") const;
-
-  /// equals with an tolerance, prints out message if unequal
-  GTSAM_EXPORT bool equals(const Point2& q, double tol = 1e-9) const;
-
-  /// @}
-  /// @name Group
-  /// @{
-
-  /// identity
-  inline static Point2 identity() {return Point2(0,0);}
-
-  /// @}
-  /// @name Vector Space
-  /// @{
-
-  /** creates a unit vector */
-  Point2 unit() const { return *this/norm(); }
-
-  /** norm of point, with derivative */
-  GTSAM_EXPORT double norm(OptionalJacobian<1,2> H = boost::none) const;
-
-  /** distance between two points */
-  GTSAM_EXPORT double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none,
-      OptionalJacobian<1,2> H2 = boost::none) const;
-
-  /// @}
-  /// @name Standard Interface
-  /// @{
-
-  /// equality
-  inline bool operator ==(const Point2& q) const {return x()==q.x() && y()==q.y();}
-
-  /// get x
-  inline double x() const {return (*this)[0];}
-
-  /// get y
-  inline double y() const {return (*this)[1];}
-
-  /// return vectorized form (column-wise).
-  const Vector2& vector() const { return *this; }
-
-  /// @}
-
-  /// Streaming
-  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p);
-
- private:
-  /// @name Advanced Interface
-  /// @{
-
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/)
-  {
-    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector2);}
-
- /// @}
-};
-
-template<>
-struct traits<Point2> : public internal::VectorSpace<Point2> {
-};
-
-#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
+/// As of GTSAM 4, in order to make GTSAM more lean,
+/// it is now possible to just typedef Point2 to Vector2
+typedef Vector2 Point2;
 
 /// Distance of the point from the origin, with Jacobian
 GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp
index 25fb9b92d..7a46f5988 100644
--- a/gtsam/geometry/Point3.cpp
+++ b/gtsam/geometry/Point3.cpp
@@ -22,47 +22,6 @@ using namespace std;
 
 namespace gtsam {
 
-#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
-bool Point3::equals(const Point3 &q, double tol) const {
-  return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol &&
-          std::abs(z() - q.z()) < tol);
-}
-
-void Point3::print(const string& s) const {
-  cout << s << *this << endl;
-}
-
-/* ************************************************************************* */
-double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1,
-                        OptionalJacobian<1, 3> H2) const {
-  return gtsam::distance3(*this,q,H1,H2);
-}
-
-double Point3::norm(OptionalJacobian<1,3> H) const {
-  return gtsam::norm3(*this, H);
-}
-
-Point3 Point3::normalized(OptionalJacobian<3,3> H) const {
-  return gtsam::normalize(*this, H);
-}
-
-Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H1,
-                     OptionalJacobian<3, 3> H2) const {
-  return gtsam::cross(*this, q, H1, H2);
-}
-
-double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H1,
-                   OptionalJacobian<1, 3> H2) const {
-  return gtsam::dot(*this, q, H1, H2);
-}
-
-/* ************************************************************************* */
-ostream &operator<<(ostream &os, const Point3& p) {
-  os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]'";
-  return os;
-}
-
-#endif
 /* ************************************************************************* */
 double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
                  OptionalJacobian<1, 3> H2) {
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 1ab5c313e..19e328022 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -29,106 +29,9 @@
 
 namespace gtsam {
 
-#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
-
-  /// As of GTSAM 4, in order to make GTSAM more lean,
-  /// it is now possible to just typedef Point3 to Vector3
-  typedef Vector3 Point3;
-
-#else
-
-/**
-   * A 3D point is just a Vector3 with some additional methods
-   * @addtogroup geometry
-   * \nosubgrouping
-   */
-class Point3 : public Vector3 {
-
-  public:
-
-    enum { dimension = 3 };
-
-    /// @name Standard Constructors
-    /// @{
-
-    using Vector3::Vector3;
-
-    /// @}
-    /// @name Testable
-    /// @{
-
-    /** print with optional string */
-	GTSAM_EXPORT void print(const std::string& s = "") const;
-
-    /** equals with an tolerance */
-	GTSAM_EXPORT bool equals(const Point3& p, double tol = 1e-9) const;
-
-    /// @}
-    /// @name Group
-    /// @{
-
-    /// identity for group operation
-    inline static Point3 identity() { return Point3(0.0, 0.0, 0.0); }
-
-    /// @}
-    /// @name Vector Space
-    /// @{
-
-    /** distance between two points */
-	GTSAM_EXPORT double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
-                    OptionalJacobian<1, 3> H2 = boost::none) const;
-
-    /** Distance of the point from the origin, with Jacobian */
-	GTSAM_EXPORT double norm(OptionalJacobian<1,3> H = boost::none) const;
-
-    /** normalize, with optional Jacobian */
-	GTSAM_EXPORT Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
-
-    /** cross product @return this x q */
-	GTSAM_EXPORT Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
-                                  OptionalJacobian<3, 3> H_q = boost::none) const;
-
-    /** dot product @return this * q*/
-	GTSAM_EXPORT double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, //
-                                OptionalJacobian<1, 3> H_q = boost::none) const;
-
-    /// @}
-    /// @name Standard Interface
-    /// @{
-
-    /// return as Vector3
-    const Vector3& vector() const { return *this; }
-
-    /// get x
-    inline double x() const {return (*this)[0];}
-
-    /// get y
-    inline double y() const {return (*this)[1];}
-
-    /// get z
-    inline double z() const {return (*this)[2];}
-
-    /// @}
-
-    /// Output stream operator
-    GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
-
-   private:
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3);
-    }
-  };
-
-template<>
-struct traits<Point3> : public internal::VectorSpace<Point3> {};
-
-template<>
-struct traits<const Point3> : public internal::VectorSpace<Point3> {};
-
-#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
+/// As of GTSAM 4, in order to make GTSAM more lean,
+/// it is now possible to just typedef Point3 to Vector3
+typedef Vector3 Point3;
 
 // Convenience typedef
 typedef std::pair<Point3, Point3> Point3Pair;
diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp
index 04ed16774..7502c4ccf 100644
--- a/gtsam/geometry/Rot2.cpp
+++ b/gtsam/geometry/Rot2.cpp
@@ -39,6 +39,13 @@ Rot2 Rot2::atan2(double y, double x) {
   return R.normalize();
 }
 
+/* ************************************************************************* */
+Rot2 Rot2::Random(std::mt19937& rng) {
+  uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
+  double angle = randomAngle(rng);
+  return fromAngle(angle);
+}
+
 /* ************************************************************************* */
 void Rot2::print(const string& s) const {
   cout << s << ": " << theta() << endl;
diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index f46f12540..8a361f558 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -22,6 +22,8 @@
 #include <gtsam/base/Lie.h>
 #include <boost/optional.hpp>
 
+#include <random>
+
 namespace gtsam {
 
   /**
@@ -79,6 +81,14 @@ namespace gtsam {
     /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
     static Rot2 atan2(double y, double x);
 
+    /**
+     * Random, generates random angle \in [-p,pi]
+     * Example:
+     *   std::mt19937 engine(42);
+     *   Unit3 unit = Unit3::Random(engine);
+     */
+    static Rot2 Random(std::mt19937 & rng);
+
     /// @}
     /// @name Testable
     /// @{
diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h
index 9edce8336..6180f4cc7 100644
--- a/gtsam/geometry/SOn-inl.h
+++ b/gtsam/geometry/SOn-inl.h
@@ -60,8 +60,9 @@ typename SO<N>::TangentVector SO<N>::ChartAtOrigin::Local(const SO& R,
 
 template <int N>
 typename SO<N>::MatrixDD SO<N>::AdjointMap() const {
+  if (N==2) return I_1x1; // SO(2) case
   throw std::runtime_error(
-      "SO<N>::AdjointMap only implemented for SO3 and SO4.");
+      "SO<N>::AdjointMap only implemented for SO2, SO3 and SO4.");
 }
 
 template <int N>
@@ -84,26 +85,22 @@ typename SO<N>::MatrixDD SO<N>::LogmapDerivative(const TangentVector& omega) {
   throw std::runtime_error("O<N>::LogmapDerivative only implemented for SO3.");
 }
 
+// Default fixed size version (but specialized elsewehere for N=2,3,4)
 template <int N>
 typename SO<N>::VectorN2 SO<N>::vec(
     OptionalJacobian<internal::NSquaredSO(N), dimension> H) const {
-  const size_t n = rows();
-  const size_t n2 = n * n;
-
   // Vectorize
-  VectorN2 X(n2);
-  X << Eigen::Map<const Matrix>(matrix_.data(), n2, 1);
+  VectorN2 X = Eigen::Map<const VectorN2>(matrix_.data());
 
   // If requested, calculate H as (I \oplus Q) * P,
   // where Q is the N*N rotation matrix, and P is calculated below.
   if (H) {
     // Calculate P matrix of vectorized generators
     // TODO(duy): Should we refactor this as the jacobian of Hat?
-    Matrix P = VectorizedGenerators(n);
-    const size_t d = dim();
-    H->resize(n2, d);
-    for (size_t i = 0; i < n; i++) {
-      H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);
+    Matrix P = SO<N>::VectorizedGenerators();
+    for (size_t i = 0; i < N; i++) {
+      H->block(i * N, 0, N, dimension) =
+          matrix_ * P.block(i * N, 0, N, dimension);
     }
   }
   return X;
diff --git a/gtsam/geometry/SOn.cpp b/gtsam/geometry/SOn.cpp
index 37b6c1784..c6cff4214 100644
--- a/gtsam/geometry/SOn.cpp
+++ b/gtsam/geometry/SOn.cpp
@@ -22,21 +22,18 @@
 namespace gtsam {
 
 template <>
-GTSAM_EXPORT
-Matrix SOn::Hat(const Vector& xi) {
+GTSAM_EXPORT void SOn::Hat(const Vector &xi, Eigen::Ref<Matrix> X) {
   size_t n = AmbientDim(xi.size());
-  if (n < 2) throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
-
-  Matrix X(n, n);  // allocate space for n*n skew-symmetric matrix
-  X.setZero();
-  if (n == 2) {
+  if (n < 2)
+    throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
+  else if (n == 2) {
     // Handle SO(2) case as recursion bottom
     assert(xi.size() == 1);
     X << 0, -xi(0), xi(0), 0;
   } else {
     // Recursively call SO(n-1) call for top-left block
     const size_t dmin = (n - 1) * (n - 2) / 2;
-    X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin));
+    Hat(xi.tail(dmin), X.topLeftCorner(n - 1, n - 1));
 
     // determine sign of last element (signs alternate)
     double sign = pow(-1.0, xi.size());
@@ -47,7 +44,14 @@ Matrix SOn::Hat(const Vector& xi) {
       X(j, n - 1) = -X(n - 1, j);
       sign = -sign;
     }
+    X(n - 1, n - 1) = 0; // bottom-right
   }
+}
+
+template <> GTSAM_EXPORT Matrix SOn::Hat(const Vector &xi) {
+  size_t n = AmbientDim(xi.size());
+  Matrix X(n, n); // allocate space for n*n skew-symmetric matrix
+  SOn::Hat(xi, X);
   return X;
 }
 
@@ -99,4 +103,27 @@ SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
   return result;
 }
 
+// Dynamic version of vec
+template <> typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const {
+  const size_t n = rows(), n2 = n * n;
+
+  // Vectorize
+  VectorN2 X(n2);
+  X << Eigen::Map<const Matrix>(matrix_.data(), n2, 1);
+
+  // If requested, calculate H as (I \oplus Q) * P,
+  // where Q is the N*N rotation matrix, and P is calculated below.
+  if (H) {
+    // Calculate P matrix of vectorized generators
+    // TODO(duy): Should we refactor this as the jacobian of Hat?
+    Matrix P = SOn::VectorizedGenerators(n);
+    const size_t d = dim();
+    H->resize(n2, d);
+    for (size_t i = 0; i < n; i++) {
+      H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);
+    }
+  }
+  return X;
+}
+
 }  // namespace gtsam
diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h
index 004569416..86b6019e1 100644
--- a/gtsam/geometry/SOn.h
+++ b/gtsam/geometry/SOn.h
@@ -98,8 +98,8 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
   static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
     Matrix Q = Matrix::Identity(n, n);
-    size_t p = R.rows();
-    assert(p < n && R.cols() == p);
+    const int p = R.rows();
+    assert(p >= 0 && p <= static_cast<int>(n) && R.cols() == p);
     Q.topLeftCorner(p, p) = R;
     return SO(Q);
   }
@@ -208,7 +208,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
 
   // Calculate run-time dimensionality of manifold.
   // Available as dimension or Dim() for fixed N.
-  size_t dim() const { return Dimension(matrix_.rows()); }
+  size_t dim() const { return Dimension(static_cast<size_t>(matrix_.rows())); }
 
   /**
    * Hat operator creates Lie algebra element corresponding to d-vector, where d
@@ -227,9 +227,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
    */
   static MatrixNN Hat(const TangentVector& xi);
 
-  /**
-   * Inverse of Hat. See note about xi element order in Hat.
-   */
+  /// In-place version of Hat (see details there), implements recursion.
+  static void Hat(const Vector &xi, Eigen::Ref<MatrixNN> X);
+
+  /// Inverse of Hat. See note about xi element order in Hat.
   static TangentVector Vee(const MatrixNN& X);
 
   // Chart at origin
@@ -295,10 +296,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   template <int N_ = N, typename = IsFixed<N_>>
   static Matrix VectorizedGenerators() {
     constexpr size_t N2 = static_cast<size_t>(N * N);
-    Matrix G(N2, dimension);
+    Eigen::Matrix<double, N2, dimension> G;
     for (size_t j = 0; j < dimension; j++) {
       const auto X = Hat(Vector::Unit(dimension, j));
-      G.col(j) = Eigen::Map<const Matrix>(X.data(), N2, 1);
+      G.col(j) = Eigen::Map<const VectorN2>(X.data());
     }
     return G;
   }
@@ -362,6 +363,11 @@ template <>
 SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
                                            DynamicJacobian H2) const;
 
+/*
+ * Specialize dynamic vec.
+ */
+template <> typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const;
+
 /** Serialization function */
 template<class Archive>
 void serialize(
diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp
index 8b9e8a7e6..6e4d408c7 100644
--- a/gtsam/geometry/tests/testPoint2.cpp
+++ b/gtsam/geometry/tests/testPoint2.cpp
@@ -237,16 +237,6 @@ TEST( Point2, circleCircleIntersection) {
 
 }
 
-/* ************************************************************************* */
-#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
-TEST( Point2, stream) {
-  Point2 p(1, 2);
-  std::ostringstream os;
-  os << p;
-  EXPECT(os.str() == "(1, 2)");
-}
-#endif
-
 /* ************************************************************************* */
 int main () {
   TestResult tr;
diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index e2396f7e9..a7c2ac50c 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -153,16 +153,6 @@ TEST( Point3, cross2) {
   }
 }
 
-/* ************************************************************************* */
-#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
-TEST( Point3, stream) {
-  Point3 p(1, 2, -3);
-  std::ostringstream os;
-  os << p;
-  EXPECT(os.str() == "[1, 2, -3]'");
-}
-#endif
-
 //*************************************************************************
 TEST (Point3, normalize) {
   Matrix actualH;
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 596fa8957..c94e21ba5 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -864,11 +864,7 @@ TEST( Pose3, stream)
   os << T;
 
   string expected;
-#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
   expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 0\n0\n0";;
-#else
-  expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: [0, 0, 0]'";
-#endif
 
   EXPECT(os.str() == expected);
 }
@@ -1043,13 +1039,9 @@ TEST(Pose3, print) {
   // Add expected rotation
   expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
 
-#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
   expected << "t: 1\n"
               "2\n"
               "3\n";
-#else
-  expected << "t: [" << translation.x() << ", " << translation.y() << ", " << translation.z() << "]'\n";
-#endif
 
   // reset cout to the original stream
   std::cout.rdbuf(oldbuf);
diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp
index 1cf8caed2..4d0ed98b3 100644
--- a/gtsam/geometry/tests/testSOn.cpp
+++ b/gtsam/geometry/tests/testSOn.cpp
@@ -39,8 +39,8 @@ using namespace std;
 using namespace gtsam;
 
 //******************************************************************************
-// Test dhynamic with n=0
-TEST(SOn, SO0) {
+// Test dynamic with n=0
+TEST(SOn, SOn0) {
   const auto R = SOn(0);
   EXPECT_LONGS_EQUAL(0, R.rows());
   EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
@@ -50,7 +50,8 @@ TEST(SOn, SO0) {
 }
 
 //******************************************************************************
-TEST(SOn, SO5) {
+// Test dynamic with n=5
+TEST(SOn, SOn5) {
   const auto R = SOn(5);
   EXPECT_LONGS_EQUAL(5, R.rows());
   EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
@@ -59,6 +60,28 @@ TEST(SOn, SO5) {
   EXPECT_LONGS_EQUAL(10, traits<SOn>::GetDimension(R));
 }
 
+//******************************************************************************
+// Test fixed with n=2
+TEST(SOn, SO0) {
+  const auto R = SO<2>();
+  EXPECT_LONGS_EQUAL(2, R.rows());
+  EXPECT_LONGS_EQUAL(1, SO<2>::dimension);
+  EXPECT_LONGS_EQUAL(1, SO<2>::Dim());
+  EXPECT_LONGS_EQUAL(1, R.dim());
+  EXPECT_LONGS_EQUAL(1, traits<SO<2>>::GetDimension(R));
+}
+
+//******************************************************************************
+// Test fixed with n=5
+TEST(SOn, SO5) {
+  const auto R = SO<5>();
+  EXPECT_LONGS_EQUAL(5, R.rows());
+  EXPECT_LONGS_EQUAL(10, SO<5>::dimension);
+  EXPECT_LONGS_EQUAL(10, SO<5>::Dim());
+  EXPECT_LONGS_EQUAL(10, R.dim());
+  EXPECT_LONGS_EQUAL(10, traits<SO<5>>::GetDimension(R));
+}
+
 //******************************************************************************
 TEST(SOn, Concept) {
   BOOST_CONCEPT_ASSERT((IsGroup<SOn>));
@@ -105,29 +128,29 @@ TEST(SOn, HatVee) {
   EXPECT(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2)));
 
   Matrix expected3(3, 3);
-  expected3 << 0, -3,  2, //
-               3,  0, -1, //
-              -2,  1,  0;
+  expected3 << 0, -3, 2, //
+      3, 0, -1,          //
+      -2, 1, 0;
   const auto actual3 = SOn::Hat(v.head<3>());
   EXPECT(assert_equal(expected3, actual3));
   EXPECT(assert_equal(skewSymmetric(1, 2, 3), actual3));
   EXPECT(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3)));
 
   Matrix expected4(4, 4);
-  expected4 << 0, -6,  5,  3, //
-               6,  0, -4, -2, //
-              -5,  4,  0,  1, //
-              -3,  2, -1,  0;
+  expected4 << 0, -6, 5, 3, //
+      6, 0, -4, -2,         //
+      -5, 4, 0, 1,          //
+      -3, 2, -1, 0;
   const auto actual4 = SOn::Hat(v.head<6>());
   EXPECT(assert_equal(expected4, actual4));
   EXPECT(assert_equal((Vector)v.head<6>(), SOn::Vee(actual4)));
 
   Matrix expected5(5, 5);
-  expected5 << 0,-10,  9,  7, -4, //
-              10,  0, -8, -6,  3, //
-              -9,  8,  0,  5, -2, //
-              -7,  6, -5,  0,  1, //
-               4, -3,  2, -1,  0;
+  expected5 << 0, -10, 9, 7, -4, //
+      10, 0, -8, -6, 3,          //
+      -9, 8, 0, 5, -2,           //
+      -7, 6, -5, 0, 1,           //
+      4, -3, 2, -1, 0;
   const auto actual5 = SOn::Hat(v);
   EXPECT(assert_equal(expected5, actual5));
   EXPECT(assert_equal((Vector)v, SOn::Vee(actual5)));
@@ -159,6 +182,22 @@ TEST(SOn, RetractLocal) {
   CHECK(assert_equal(v1, SOn::ChartAtOrigin::Local(Q1), 1e-7));
 }
 
+//******************************************************************************
+
+Matrix RetractJacobian(size_t n) { return SOn::VectorizedGenerators(n); }
+
+/// Test Jacobian of Retract at origin
+TEST(SOn, RetractJacobian) {
+  Matrix actualH = RetractJacobian(3);
+  boost::function<Matrix(const Vector &)> h = [](const Vector &v) {
+    return SOn::ChartAtOrigin::Retract(v).matrix();
+  };
+  Vector3 v;
+  v.setZero();
+  const Matrix expectedH = numericalDerivative11<Matrix, Vector, 3>(h, v, 1e-5);
+  CHECK(assert_equal(expectedH, actualH));
+}
+
 //******************************************************************************
 TEST(SOn, vec) {
   Vector10 v;
@@ -166,11 +205,28 @@ TEST(SOn, vec) {
   SOn Q = SOn::ChartAtOrigin::Retract(v);
   Matrix actualH;
   const Vector actual = Q.vec(actualH);
-  boost::function<Vector(const SOn&)> h = [](const SOn& Q) { return Q.vec(); };
+  boost::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
   const Matrix H = numericalDerivative11<Vector, SOn, 10>(h, Q, 1e-5);
   CHECK(assert_equal(H, actualH));
 }
 
+//******************************************************************************
+TEST(SOn, VectorizedGenerators) {
+  // Default fixed
+  auto actual2 = SO<2>::VectorizedGenerators();
+  CHECK(actual2.rows()==4 && actual2.cols()==1)
+
+  // Specialized
+  auto actual3 = SO<3>::VectorizedGenerators();
+  CHECK(actual3.rows()==9 && actual3.cols()==3)
+  auto actual4 = SO<4>::VectorizedGenerators();
+  CHECK(actual4.rows()==16 && actual4.cols()==6)
+  
+  // Dynamic
+  auto actual5 = SOn::VectorizedGenerators(5);
+  CHECK(actual5.rows()==25 && actual5.cols()==10)
+}
+
 //******************************************************************************
 int main() {
   TestResult tr;
diff --git a/gtsam.h b/gtsam/gtsam.i
similarity index 95%
rename from gtsam.h
rename to gtsam/gtsam.i
index 86bfa05b7..d67b74812 100644
--- a/gtsam.h
+++ b/gtsam/gtsam.i
@@ -618,6 +618,9 @@ class SOn {
   // Other methods
   Vector vec() const;
   Matrix matrix() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/geometry/Rot3.h>
@@ -2171,7 +2174,11 @@ class Values {
   // void insert(size_t j, const gtsam::Value& value);
   // void update(size_t j, const gtsam::Value& val);
   // gtsam::Value at(size_t j) const;
-
+  
+  // The order is important: Vector has to precede Point2/Point3 so `atVector`
+  // can work for those fixed-size vectors.
+  void insert(size_t j, Vector vector);
+  void insert(size_t j, Matrix matrix);
   void insert(size_t j, const gtsam::Point2& point2);
   void insert(size_t j, const gtsam::Point3& point3);
   void insert(size_t j, const gtsam::Rot2& rot2);
@@ -2188,8 +2195,6 @@ class Values {
   void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(size_t j, const gtsam::NavState& nav_state);
-  void insert(size_t j, Vector vector);
-  void insert(size_t j, Matrix matrix);
 
   void update(size_t j, const gtsam::Point2& point2);
   void update(size_t j, const gtsam::Point3& point3);
@@ -2797,16 +2802,16 @@ class SfmData {
 
 string findExampleDataFile(string name);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
-    gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
+    gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
-    gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise);
+    gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
-    gtsam::noiseModel::Diagonal* model, int maxID);
+    gtsam::noiseModel::Diagonal* model, int maxIndex);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
     gtsam::noiseModel::Diagonal* model);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
-    gtsam::noiseModel::Base* model);
+    gtsam::noiseModel::Base* model, int maxIndex);
 void save2D(const gtsam::NonlinearFactorGraph& graph,
     const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
     string filename);
@@ -2816,8 +2821,8 @@ class BetweenFactorPose3s
 {
   BetweenFactorPose3s();
   size_t size() const;
-  gtsam::BetweenFactorPose3* at(size_t i) const;
-  void push_back(const gtsam::BetweenFactorPose3* factor);
+  gtsam::BetweenFactor<gtsam::Pose3>* at(size_t i) const;
+  void push_back(const gtsam::BetweenFactor<gtsam::Pose3>* factor);
 };
 
 #include <gtsam/slam/InitializePose3.h>
@@ -2855,7 +2860,7 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
 };
 
 #include <gtsam/slam/FrobeniusFactor.h>
-gtsam::noiseModel::Isotropic* ConvertPose3NoiseModel(
+gtsam::noiseModel::Isotropic* ConvertNoiseModel(
     gtsam::noiseModel::Base* model, size_t d);
 
 template<T = {gtsam::SO3, gtsam::SO4}>
@@ -2874,12 +2879,14 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
   Vector evaluateError(const T& R1, const T& R2);
 };
 
-virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor {
-  FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
+#include <gtsam/sfm/ShonanFactor.h>
+
+virtual class ShonanFactor3 : gtsam::NoiseModelFactor {
+  ShonanFactor3(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);
+  ShonanFactor3(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);
 };
 
 #include <gtsam/sfm/BinaryMeasurement.h>
@@ -2895,6 +2902,125 @@ class BinaryMeasurement {
 typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
 typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
 
+#include <gtsam/sfm/ShonanAveraging.h>
+
+// TODO(frank): copy/pasta below until we have integer template paremeters in wrap!
+
+class ShonanAveragingParameters2 {
+  ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm);
+  ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm, string method);
+  gtsam::LevenbergMarquardtParams getLMParams() const;  
+  void setOptimalityThreshold(double value);
+  double getOptimalityThreshold() const;
+  void setAnchor(size_t index, const gtsam::Rot2& value);
+  void setAnchorWeight(double value);
+  double getAnchorWeight() const;
+  void setKarcherWeight(double value);
+  double getKarcherWeight();
+  void setGaugesWeight(double value);
+  double getGaugesWeight();
+};
+
+class ShonanAveragingParameters3 {
+  ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm);
+  ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm, string method);
+  gtsam::LevenbergMarquardtParams getLMParams() const;  
+  void setOptimalityThreshold(double value);
+  double getOptimalityThreshold() const;
+  void setAnchor(size_t index, const gtsam::Rot3& value);
+  void setAnchorWeight(double value);
+  double getAnchorWeight() const;
+  void setKarcherWeight(double value);
+  double getKarcherWeight();
+  void setGaugesWeight(double value);
+  double getGaugesWeight();
+};
+
+class ShonanAveraging2 {
+  ShonanAveraging2(string g2oFile);
+  ShonanAveraging2(string g2oFile,
+                   const gtsam::ShonanAveragingParameters2 &parameters);
+
+  // Query properties
+  size_t nrUnknowns() const;
+  size_t nrMeasurements() const;
+  gtsam::Rot2 measured(size_t i);
+  gtsam::KeyVector keys(size_t i);
+
+  // Matrix API (advanced use, debugging)
+  Matrix denseD() const;
+  Matrix denseQ() const;
+  Matrix denseL() const;
+  // Matrix computeLambda_(Matrix S) const;
+  Matrix computeLambda_(const gtsam::Values& values) const;
+  Matrix computeA_(const gtsam::Values& values) const;
+  double computeMinEigenValue(const gtsam::Values& values) const;
+  gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values,
+                               const Vector& minEigenVector, double minEigenValue) const;
+
+  // Advanced API
+  gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
+  gtsam::Values initializeRandomlyAt(size_t p) const;
+  double costAt(size_t p, const gtsam::Values& values) const;
+  pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
+  bool checkOptimality(const gtsam::Values& values) const;
+  gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial);
+  // gtsam::Values tryOptimizingAt(size_t p) const;
+  gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const;
+  gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const;
+  gtsam::Values roundSolution(const gtsam::Values& values) const;
+
+  // Basic API
+  double cost(const gtsam::Values& values) const;
+  gtsam::Values initializeRandomly() const;
+  pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
+};
+
+class ShonanAveraging3 {
+  ShonanAveraging3(string g2oFile);
+  ShonanAveraging3(string g2oFile,
+                   const gtsam::ShonanAveragingParameters3 &parameters);
+  
+  // TODO(frank): deprecate once we land pybind wrapper
+  ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors);
+  ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors,
+                   const gtsam::ShonanAveragingParameters3 &parameters);
+
+  // Query properties
+  size_t nrUnknowns() const;
+  size_t nrMeasurements() const;
+  gtsam::Rot3 measured(size_t i);
+  gtsam::KeyVector keys(size_t i);
+
+  // Matrix API (advanced use, debugging)
+  Matrix denseD() const;
+  Matrix denseQ() const;
+  Matrix denseL() const;
+  // Matrix computeLambda_(Matrix S) const;
+  Matrix computeLambda_(const gtsam::Values& values) const;
+  Matrix computeA_(const gtsam::Values& values) const;
+  double computeMinEigenValue(const gtsam::Values& values) const;
+  gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values,
+                               const Vector& minEigenVector, double minEigenValue) const;
+
+  // Advanced API
+  gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
+  gtsam::Values initializeRandomlyAt(size_t p) const;
+  double costAt(size_t p, const gtsam::Values& values) const;
+  pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
+  bool checkOptimality(const gtsam::Values& values) const;
+  gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial);
+  // gtsam::Values tryOptimizingAt(size_t p) const;
+  gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const;
+  gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const;
+  gtsam::Values roundSolution(const gtsam::Values& values) const;
+
+  // Basic API
+  double cost(const gtsam::Values& values) const;
+  gtsam::Values initializeRandomly() const;
+  pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
+};
+
 //*************************************************************************
 // Navigation
 //*************************************************************************
diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp
index c07240752..d38b76255 100644
--- a/gtsam/navigation/tests/testNavState.cpp
+++ b/gtsam/navigation/tests/testNavState.cpp
@@ -215,11 +215,7 @@ TEST(NavState, Stream)
   os << state;
 
   string expected;
-#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
   expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: 0\n0\n0\nv: 0\n0\n0";
-#else
-  expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: [0, 0, 0]'\nv: [0, 0, 0]'";
-#endif
 
   EXPECT(os.str() == expected);
 }
diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp
index 328a3facf..9a9c487b6 100644
--- a/gtsam/nonlinear/NonlinearOptimizer.cpp
+++ b/gtsam/nonlinear/NonlinearOptimizer.cpp
@@ -90,7 +90,7 @@ void NonlinearOptimizer::defaultOptimize() {
   // Iterative loop
   do {
     // Do next iteration
-    currentError = error();
+    currentError = error(); // TODO(frank): don't do this twice at first !? Computed above!
     iterate();
     tictoc_finishedIteration();
 
diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
new file mode 100644
index 000000000..2485418cf
--- /dev/null
+++ b/gtsam/sfm/ShonanAveraging.cpp
@@ -0,0 +1,854 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   ShonanAveraging.cpp
+ * @date   March 2019 - August 2020
+ * @author Frank Dellaert, David Rosen, and Jing Wu
+ * @brief  Shonan Averaging algorithm
+ */
+
+#include <gtsam/sfm/ShonanAveraging.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/sfm/ShonanGaugeFactor.h>
+#include <gtsam/slam/FrobeniusFactor.h>
+#include <gtsam/slam/KarcherMeanFactor-inl.h>
+#include <gtsam/sfm/ShonanFactor.h>
+
+#include <Eigen/Eigenvalues>
+#include <SymEigsSolver.h>
+
+#include <algorithm>
+#include <complex>
+#include <iostream>
+#include <map>
+#include <random>
+#include <vector>
+
+namespace gtsam {
+
+// In Wrappers we have no access to this so have a default ready
+static std::mt19937 kRandomNumberGenerator(42);
+
+using Sparse = Eigen::SparseMatrix<double>;
+
+/* ************************************************************************* */
+template <size_t d>
+ShonanAveragingParameters<d>::ShonanAveragingParameters(
+    const LevenbergMarquardtParams &_lm, const std::string &method,
+    double optimalityThreshold, double alpha, double beta, double gamma)
+    : lm(_lm), optimalityThreshold(optimalityThreshold), alpha(alpha),
+      beta(beta), gamma(gamma) {
+  // By default, we will do conjugate gradient
+  lm.linearSolverType = LevenbergMarquardtParams::Iterative;
+
+  // Create subgraph builder parameters
+  SubgraphBuilderParameters builderParameters;
+  builderParameters.skeletonType = SubgraphBuilderParameters::KRUSKAL;
+  builderParameters.skeletonWeight = SubgraphBuilderParameters::EQUAL;
+  builderParameters.augmentationWeight = SubgraphBuilderParameters::SKELETON;
+  builderParameters.augmentationFactor = 0.0;
+
+  auto pcg = boost::make_shared<PCGSolverParameters>();
+
+  // Choose optimization method
+  if (method == "SUBGRAPH") {
+    lm.iterativeParams =
+        boost::make_shared<SubgraphSolverParameters>(builderParameters);
+  } else if (method == "SGPC") {
+    pcg->preconditioner_ =
+        boost::make_shared<SubgraphPreconditionerParameters>(builderParameters);
+    lm.iterativeParams = pcg;
+  } else if (method == "JACOBI") {
+    pcg->preconditioner_ =
+        boost::make_shared<BlockJacobiPreconditionerParameters>();
+    lm.iterativeParams = pcg;
+  } else if (method == "QR") {
+    lm.setLinearSolverType("MULTIFRONTAL_QR");
+  } else if (method == "CHOLESKY") {
+    lm.setLinearSolverType("MULTIFRONTAL_CHOLESKY");
+  } else {
+    throw std::invalid_argument("ShonanAveragingParameters: unknown method");
+  }
+}
+
+/* ************************************************************************* */
+// Explicit instantiation for d=2 and d=3
+template struct ShonanAveragingParameters<2>;
+template struct ShonanAveragingParameters<3>;
+
+/* ************************************************************************* */
+// Calculate number of unknown rotations referenced by measurements
+template <size_t d>
+static size_t
+NrUnknowns(const typename ShonanAveraging<d>::Measurements &measurements) {
+  std::set<Key> keys;
+  for (const auto &measurement : measurements) {
+    keys.insert(measurement.key1());
+    keys.insert(measurement.key2());
+  }
+  return keys.size();
+}
+
+/* ************************************************************************* */
+template <size_t d>
+ShonanAveraging<d>::ShonanAveraging(const Measurements &measurements,
+                                    const Parameters &parameters)
+    : parameters_(parameters), measurements_(measurements),
+      nrUnknowns_(NrUnknowns<d>(measurements)) {
+  for (const auto &measurement : measurements_) {
+    const auto &model = measurement.noiseModel();
+    if (model && model->dim() != SO<d>::dimension) {
+      measurement.print("Factor with incorrect noise model:\n");
+      throw std::invalid_argument("ShonanAveraging: measurements passed to "
+                                  "constructor have incorrect dimension.");
+    }
+  }
+  Q_ = buildQ();
+  D_ = buildD();
+  L_ = D_ - Q_;
+}
+
+/* ************************************************************************* */
+template <size_t d>
+NonlinearFactorGraph ShonanAveraging<d>::buildGraphAt(size_t p) const {
+  NonlinearFactorGraph graph;
+  auto G = boost::make_shared<Matrix>(SO<-1>::VectorizedGenerators(p));
+  for (const auto &measurement : measurements_) {
+    const auto &keys = measurement.keys();
+    const auto &Rij = measurement.measured();
+    const auto &model = measurement.noiseModel();
+    graph.emplace_shared<ShonanFactor<d>>(keys[0], keys[1], Rij, p, model, G);
+  }
+
+  // Possibly add Karcher prior
+  if (parameters_.beta > 0) {
+    const size_t dim = SOn::Dimension(p);
+    graph.emplace_shared<KarcherMeanFactor<SOn>>(graph.keys(), dim);
+  }
+
+  // Possibly add gauge factors - they are probably useless as gradient is zero
+  if (parameters_.gamma > 0 && p > d + 1) {
+    for (auto key : graph.keys())
+      graph.emplace_shared<ShonanGaugeFactor>(key, p, d, parameters_.gamma);
+  }
+
+  return graph;
+}
+
+/* ************************************************************************* */
+template <size_t d>
+double ShonanAveraging<d>::costAt(size_t p, const Values &values) const {
+  const NonlinearFactorGraph graph = buildGraphAt(p);
+  return graph.error(values);
+}
+
+/* ************************************************************************* */
+template <size_t d>
+boost::shared_ptr<LevenbergMarquardtOptimizer>
+ShonanAveraging<d>::createOptimizerAt(size_t p, const Values &initial) const {
+  // Build graph
+  NonlinearFactorGraph graph = buildGraphAt(p);
+
+  // Anchor prior is added here as depends on initial value (and cost is zero)
+  if (parameters_.alpha > 0) {
+    size_t i;
+    Rot value;
+    const size_t dim = SOn::Dimension(p);
+    std::tie(i, value) = parameters_.anchor;
+    auto model = noiseModel::Isotropic::Precision(dim, parameters_.alpha);
+    graph.emplace_shared<PriorFactor<SOn>>(i, SOn::Lift(p, value.matrix()),
+                                           model);
+  }
+
+  // Optimize
+  return boost::make_shared<LevenbergMarquardtOptimizer>(graph, initial,
+                                                         parameters_.lm);
+}
+
+/* ************************************************************************* */
+template <size_t d>
+Values ShonanAveraging<d>::tryOptimizingAt(size_t p,
+                                           const Values &initial) const {
+  auto lm = createOptimizerAt(p, initial);
+  return lm->optimize();
+}
+
+/* ************************************************************************* */
+// Project to pxdN Stiefel manifold
+template <size_t d>
+Matrix ShonanAveraging<d>::StiefelElementMatrix(const Values &values) {
+  const size_t N = values.size();
+  const size_t p = values.at<SOn>(0).rows();
+  Matrix S(p, N * d);
+  for (const auto it : values.filter<SOn>()) {
+    S.middleCols<d>(it.key * d) =
+        it.value.matrix().leftCols<d>(); // project Qj to Stiefel
+  }
+  return S;
+}
+
+/* ************************************************************************* */
+template <>
+Values ShonanAveraging<2>::projectFrom(size_t p, const Values &values) const {
+  Values result;
+  for (const auto it : values.filter<SOn>()) {
+    assert(it.value.rows() == p);
+    const auto &M = it.value.matrix();
+    const Rot2 R = Rot2::atan2(M(1, 0), M(0, 0));
+    result.insert(it.key, R);
+  }
+  return result;
+}
+
+template <>
+Values ShonanAveraging<3>::projectFrom(size_t p, const Values &values) const {
+  Values result;
+  for (const auto it : values.filter<SOn>()) {
+    assert(it.value.rows() == p);
+    const auto &M = it.value.matrix();
+    const Rot3 R = Rot3::ClosestTo(M.topLeftCorner<3, 3>());
+    result.insert(it.key, R);
+  }
+  return result;
+}
+
+/* ************************************************************************* */
+template <size_t d> static Matrix RoundSolutionS(const Matrix &S) {
+  const size_t N = S.cols() / d;
+  // First, compute a thin SVD of S
+  Eigen::JacobiSVD<Matrix> svd(S, Eigen::ComputeThinV);
+  const Vector sigmas = svd.singularValues();
+
+  // Construct a diagonal matrix comprised of the first d singular values
+  using DiagonalMatrix = Eigen::DiagonalMatrix<double, d>;
+  DiagonalMatrix Sigma_d;
+  Sigma_d.diagonal() = sigmas.head<d>();
+
+  // Now, construct a rank-d truncated singular value decomposition for S
+  Matrix R = Sigma_d * svd.matrixV().leftCols<d>().transpose();
+
+  // Count the number of blocks whose determinants have positive sign
+  size_t numPositiveBlocks = 0;
+  for (size_t i = 0; i < N; ++i) {
+    // Compute the determinant of the ith dxd block of R
+    double determinant = R.middleCols<d>(d * i).determinant();
+    if (determinant > 0)
+      ++numPositiveBlocks;
+  }
+
+  if (numPositiveBlocks < N / 2) {
+    // Less than half of the total number of blocks have the correct sign.
+    // To reverse their orientations, multiply with a reflection matrix.
+    DiagonalMatrix reflector;
+    reflector.setIdentity();
+    reflector.diagonal()(d - 1) = -1;
+    R = reflector * R;
+  }
+
+  return R;
+}
+
+/* ************************************************************************* */
+template <> Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const {
+  // Round to a 2*2N matrix
+  Matrix R = RoundSolutionS<2>(S);
+
+  // Finally, project each dxd rotation block to SO(2)
+  Values values;
+  for (size_t j = 0; j < nrUnknowns(); ++j) {
+    const Rot2 Ri = Rot2::atan2(R(1, 2 * j), R(0, 2 * j));
+    values.insert(j, Ri);
+  }
+  return values;
+}
+
+template <> Values ShonanAveraging<3>::roundSolutionS(const Matrix &S) const {
+  // Round to a 3*3N matrix
+  Matrix R = RoundSolutionS<3>(S);
+
+  // Finally, project each dxd rotation block to SO(3)
+  Values values;
+  for (size_t j = 0; j < nrUnknowns(); ++j) {
+    const Rot3 Ri = Rot3::ClosestTo(R.middleCols<3>(3 * j));
+    values.insert(j, Ri);
+  }
+  return values;
+}
+
+/* ************************************************************************* */
+template <size_t d>
+Values ShonanAveraging<d>::roundSolution(const Values &values) const {
+  // Project to pxdN Stiefel manifold...
+  Matrix S = StiefelElementMatrix(values);
+  // ...and call version above.
+  return roundSolutionS(S);
+}
+
+/* ************************************************************************* */
+template <size_t d>
+double ShonanAveraging<d>::cost(const Values &values) const {
+  NonlinearFactorGraph graph;
+  for (const auto &measurement : measurements_) {
+    const auto &keys = measurement.keys();
+    const auto &Rij = measurement.measured();
+    const auto &model = measurement.noiseModel();
+    graph.emplace_shared<FrobeniusBetweenFactor<SO<d>>>(
+        keys[0], keys[1], SO<d>(Rij.matrix()), model);
+  }
+  // Finally, project each dxd rotation block to SO(d)
+  Values result;
+  for (const auto it : values.filter<Rot>()) {
+    result.insert(it.key, SO<d>(it.value.matrix()));
+  }
+  return graph.error(result);
+}
+
+/* ************************************************************************* */
+// Get kappa from noise model
+template <typename T>
+static double Kappa(const BinaryMeasurement<T> &measurement) {
+  const auto &isotropic = boost::dynamic_pointer_cast<noiseModel::Isotropic>(
+      measurement.noiseModel());
+  if (!isotropic) {
+    throw std::invalid_argument(
+        "Shonan averaging noise models must be isotropic.");
+  }
+  const double sigma = isotropic->sigma();
+  return 1.0 / (sigma * sigma);
+}
+
+/* ************************************************************************* */
+template <size_t d> Sparse ShonanAveraging<d>::buildD() const {
+  // Each measurement contributes 2*d elements along the diagonal of the
+  // degree matrix.
+  static constexpr size_t stride = 2 * d;
+
+  // Reserve space for triplets
+  std::vector<Eigen::Triplet<double>> triplets;
+  triplets.reserve(stride * measurements_.size());
+
+  for (const auto &measurement : measurements_) {
+    // Get pose keys
+    const auto &keys = measurement.keys();
+
+    // Get kappa from noise model
+    double kappa = Kappa<Rot>(measurement);
+
+    const size_t di = d * keys[0], dj = d * keys[1];
+    for (size_t k = 0; k < d; k++) {
+      // Elements of ith block-diagonal
+      triplets.emplace_back(di + k, di + k, kappa);
+      // Elements of jth block-diagonal
+      triplets.emplace_back(dj + k, dj + k, kappa);
+    }
+  }
+
+  // Construct and return a sparse matrix from these triplets
+  const size_t dN = d * nrUnknowns();
+  Sparse D(dN, dN);
+  D.setFromTriplets(triplets.begin(), triplets.end());
+
+  return D;
+}
+
+/* ************************************************************************* */
+template <size_t d> Sparse ShonanAveraging<d>::buildQ() const {
+  // Each measurement contributes 2*d^2 elements on a pair of symmetric
+  // off-diagonal blocks
+  static constexpr size_t stride = 2 * d * d;
+
+  // Reserve space for triplets
+  std::vector<Eigen::Triplet<double>> triplets;
+  triplets.reserve(stride * measurements_.size());
+
+  for (const auto &measurement : measurements_) {
+    // Get pose keys
+    const auto &keys = measurement.keys();
+
+    // Extract rotation measurement
+    const auto Rij = measurement.measured().matrix();
+
+    // Get kappa from noise model
+    double kappa = Kappa<Rot>(measurement);
+
+    const size_t di = d * keys[0], dj = d * keys[1];
+    for (size_t r = 0; r < d; r++) {
+      for (size_t c = 0; c < d; c++) {
+        // Elements of ij block
+        triplets.emplace_back(di + r, dj + c, kappa * Rij(r, c));
+        // Elements of ji block
+        triplets.emplace_back(dj + r, di + c, kappa * Rij(c, r));
+      }
+    }
+  }
+
+  // Construct and return a sparse matrix from these triplets
+  const size_t dN = d * nrUnknowns();
+  Sparse Q(dN, dN);
+  Q.setFromTriplets(triplets.begin(), triplets.end());
+
+  return Q;
+}
+
+/* ************************************************************************* */
+template <size_t d>
+Sparse ShonanAveraging<d>::computeLambda(const Matrix &S) const {
+  // Each pose contributes 2*d elements along the diagonal of Lambda
+  static constexpr size_t stride = d * d;
+
+  // Reserve space for triplets
+  const size_t N = nrUnknowns();
+  std::vector<Eigen::Triplet<double>> triplets;
+  triplets.reserve(stride * N);
+
+  // Do sparse-dense multiply to get Q*S'
+  auto QSt = Q_ * S.transpose();
+
+  for (size_t j = 0; j < N; j++) {
+    // Compute B, the building block for the j^th diagonal block of Lambda
+    const size_t dj = d * j;
+    Matrix B = QSt.middleRows(dj, d) * S.middleCols<d>(dj);
+
+    // Elements of jth block-diagonal
+    for (size_t r = 0; r < d; r++)
+      for (size_t c = 0; c < d; c++)
+        triplets.emplace_back(dj + r, dj + c, 0.5 * (B(r, c) + B(c, r)));
+  }
+
+  // Construct and return a sparse matrix from these triplets
+  Sparse Lambda(d * N, d * N);
+  Lambda.setFromTriplets(triplets.begin(), triplets.end());
+  return Lambda;
+}
+
+/* ************************************************************************* */
+template <size_t d>
+Sparse ShonanAveraging<d>::computeLambda(const Values &values) const {
+  // Project to pxdN Stiefel manifold...
+  Matrix S = StiefelElementMatrix(values);
+  // ...and call version above.
+  return computeLambda(S);
+}
+
+/* ************************************************************************* */
+template <size_t d>
+Sparse ShonanAveraging<d>::computeA(const Values &values) const {
+  assert(values.size() == nrUnknowns());
+  const Matrix S = StiefelElementMatrix(values);
+  auto Lambda = computeLambda(S);
+  return Lambda - Q_;
+}
+
+/* ************************************************************************* */
+/// MINIMUM EIGENVALUE COMPUTATIONS
+
+/** This is a lightweight struct used in conjunction with Spectra to compute
+ * the minimum eigenvalue and eigenvector of a sparse matrix A; it has a single
+ * nontrivial function, perform_op(x,y), that computes and returns the product
+ * y = (A + sigma*I) x */
+struct MatrixProdFunctor {
+  // Const reference to an externally-held matrix whose minimum-eigenvalue we
+  // want to compute
+  const Sparse &A_;
+
+  // Spectral shift
+  double sigma_;
+
+  // Constructor
+  explicit MatrixProdFunctor(const Sparse &A, double sigma = 0)
+      : A_(A), sigma_(sigma) {}
+
+  int rows() const { return A_.rows(); }
+  int cols() const { return A_.cols(); }
+
+  // Matrix-vector multiplication operation
+  void perform_op(const double *x, double *y) const {
+    // Wrap the raw arrays as Eigen Vector types
+    Eigen::Map<const Vector> X(x, rows());
+    Eigen::Map<Vector> Y(y, rows());
+
+    // Do the multiplication using wrapped Eigen vectors
+    Y = A_ * X + sigma_ * X;
+  }
+};
+
+/// Function to compute the minimum eigenvalue of A using Lanczos in Spectra.
+/// This does 2 things:
+///
+/// (1)  Quick (coarse) eigenvalue computation to estimate the largest-magnitude
+/// eigenvalue (2)  A second eigenvalue computation applied to A-sigma*I, where
+/// sigma is chosen to make the minimum eigenvalue of A the extremal eigenvalue
+/// of A-sigma*I
+///
+/// Upon completion, this returns a boolean value indicating whether the minimum
+/// eigenvalue was computed to the required precision -- if so, its sets the
+/// values of minEigenValue and minEigenVector appropriately
+
+/// Note that in the following function signature, S is supposed to be the
+/// block-row-matrix that is a critical point for the optimization algorithm;
+/// either S (Stiefel manifold) or R (block rotations).  We use this to
+/// construct a starting vector v for the Lanczos process that will be close to
+/// the minimum eigenvector we're looking for whenever the relaxation is exact
+/// -- this is a key feature that helps to make this method fast.  Note that
+/// instead of passing in all of S, it would be enough to pass in one of S's
+/// *rows*, if that's more convenient.
+
+// For the defaults, David Rosen says:
+//   - maxIterations refers to the max number of Lanczos iterations to run;
+//   ~1000 should be sufficiently large
+//   - We've been using 10^-4 for the nonnegativity tolerance
+//   - for numLanczosVectors, 20 is a good default value
+
+static bool
+SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue,
+                        Vector *minEigenVector = 0, size_t *numIterations = 0,
+                        size_t maxIterations = 1000,
+                        double minEigenvalueNonnegativityTolerance = 10e-4,
+                        Eigen::Index numLanczosVectors = 20) {
+  // a. Estimate the largest-magnitude eigenvalue of this matrix using Lanczos
+  MatrixProdFunctor lmOperator(A);
+  Spectra::SymEigsSolver<double, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN,
+                         MatrixProdFunctor>
+      lmEigenValueSolver(&lmOperator, 1, std::min(numLanczosVectors, A.rows()));
+  lmEigenValueSolver.init();
+
+  const int lmConverged = lmEigenValueSolver.compute(
+      maxIterations, 1e-4, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
+
+  // Check convergence and bail out if necessary
+  if (lmConverged != 1)
+    return false;
+
+  const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0);
+
+  if (lmEigenValue < 0) {
+    // The largest-magnitude eigenvalue is negative, and therefore also the
+    // minimum eigenvalue, so just return this solution
+    *minEigenValue = lmEigenValue;
+    if (minEigenVector) {
+      *minEigenVector = lmEigenValueSolver.eigenvectors(1).col(0);
+      minEigenVector->normalize(); // Ensure that this is a unit vector
+    }
+    return true;
+  }
+
+  // The largest-magnitude eigenvalue is positive, and is therefore the
+  // maximum  eigenvalue.  Therefore, after shifting the spectrum of A
+  // by -2*lmEigenValue (by forming A - 2*lambda_max*I), the  shifted
+  // spectrum will lie in the interval [minEigenValue(A) - 2* lambda_max(A),
+  // -lambda_max*A]; in particular, the largest-magnitude eigenvalue of
+  //  A - 2*lambda_max*I is minEigenValue - 2*lambda_max, with corresponding
+  // eigenvector v_min
+
+  MatrixProdFunctor minShiftedOperator(A, -2 * lmEigenValue);
+
+  Spectra::SymEigsSolver<double, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN,
+                         MatrixProdFunctor>
+      minEigenValueSolver(&minShiftedOperator, 1,
+                          std::min(numLanczosVectors, A.rows()));
+
+  // If S is a critical point of F, then S^T is also in the null space of S -
+  // Lambda(S) (cf. Lemma 6 of the tech report), and therefore its rows are
+  // eigenvectors corresponding to the eigenvalue 0.  In the case  that the
+  // relaxation is exact, this is the *minimum* eigenvalue, and therefore the
+  // rows of S are exactly the eigenvectors that we're looking for.  On the
+  // other hand, if the relaxation is *not* exact, then S - Lambda(S) has at
+  // least one strictly negative eigenvalue, and the rows of S are *unstable
+  // fixed points* for the Lanczos iterations.  Thus, we will take a slightly
+  // "fuzzed" version of the first row of S as an initialization for the
+  // Lanczos iterations; this allows for rapid convergence in the case that
+  // the relaxation is exact (since are starting close to a solution), while
+  // simultaneously allowing the iterations to escape from this fixed point in
+  // the case that the relaxation is not exact.
+  Vector v0 = S.row(0).transpose();
+  Vector perturbation(v0.size());
+  perturbation.setRandom();
+  perturbation.normalize();
+  Vector xinit = v0 + (.03 * v0.norm()) * perturbation; // Perturb v0 by ~3%
+
+  // Use this to initialize the eigensolver
+  minEigenValueSolver.init(xinit.data());
+
+  // Now determine the relative precision required in the Lanczos method in
+  // order to be able to estimate the smallest eigenvalue within an *absolute*
+  // tolerance of 'minEigenvalueNonnegativityTolerance'
+  const int minConverged = minEigenValueSolver.compute(
+      maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue,
+      Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
+
+  if (minConverged != 1)
+    return false;
+
+  *minEigenValue = minEigenValueSolver.eigenvalues()(0) + 2 * lmEigenValue;
+  if (minEigenVector) {
+    *minEigenVector = minEigenValueSolver.eigenvectors(1).col(0);
+    minEigenVector->normalize(); // Ensure that this is a unit vector
+  }
+  if (numIterations)
+    *numIterations = minEigenValueSolver.num_iterations();
+  return true;
+}
+
+/* ************************************************************************* */
+template <size_t d> Sparse ShonanAveraging<d>::computeA(const Matrix &S) const {
+  auto Lambda = computeLambda(S);
+  return Lambda - Q_;
+}
+
+/* ************************************************************************* */
+template <size_t d>
+double ShonanAveraging<d>::computeMinEigenValue(const Values &values,
+                                                Vector *minEigenVector) const {
+  assert(values.size() == nrUnknowns());
+  const Matrix S = StiefelElementMatrix(values);
+  auto A = computeA(S);
+
+  double minEigenValue;
+  bool success = SparseMinimumEigenValue(A, S, &minEigenValue, minEigenVector);
+  if (!success) {
+    throw std::runtime_error(
+        "SparseMinimumEigenValue failed to compute minimum eigenvalue.");
+  }
+  return minEigenValue;
+}
+
+/* ************************************************************************* */
+template <size_t d>
+std::pair<double, Vector>
+ShonanAveraging<d>::computeMinEigenVector(const Values &values) const {
+  Vector minEigenVector;
+  double minEigenValue = computeMinEigenValue(values, &minEigenVector);
+  return std::make_pair(minEigenValue, minEigenVector);
+}
+
+/* ************************************************************************* */
+template <size_t d>
+bool ShonanAveraging<d>::checkOptimality(const Values &values) const {
+  double minEigenValue = computeMinEigenValue(values);
+  return minEigenValue > parameters_.optimalityThreshold;
+}
+
+/* ************************************************************************* */
+/// Create a tangent direction xi with eigenvector segment v_i
+template <size_t d>
+Vector ShonanAveraging<d>::MakeATangentVector(size_t p, const Vector &v,
+                                              size_t i) {
+  // Create a tangent direction xi with eigenvector segment v_i
+  const size_t dimension = SOn::Dimension(p);
+  const auto v_i = v.segment<d>(d * i);
+  Vector xi = Vector::Zero(dimension);
+  double sign = pow(-1.0, round((p + 1) / 2) + 1);
+  for (size_t j = 0; j < d; j++) {
+    xi(j + p - d - 1) = sign * v_i(d - j - 1);
+    sign = -sign;
+  }
+  return xi;
+}
+
+/* ************************************************************************* */
+template <size_t d>
+Matrix ShonanAveraging<d>::riemannianGradient(size_t p,
+                                              const Values &values) const {
+  Matrix S_dot = StiefelElementMatrix(values);
+  // calculate the gradient of F(Q_dot) at Q_dot
+  Matrix euclideanGradient = 2 * (L_ * (S_dot.transpose())).transpose();
+  // cout << "euclidean gradient rows and cols" << euclideanGradient.rows() <<
+  // "\t" << euclideanGradient.cols() << endl;
+
+  // project the gradient onto the entire euclidean space
+  Matrix symBlockDiagProduct(p, d * nrUnknowns());
+  for (size_t i = 0; i < nrUnknowns(); i++) {
+    // Compute block product
+    const size_t di = d * i;
+    const Matrix P = S_dot.middleCols<d>(di).transpose() *
+                     euclideanGradient.middleCols<d>(di);
+    // Symmetrize this block
+    Matrix S = .5 * (P + P.transpose());
+    // Compute S_dot * S and set corresponding block
+    symBlockDiagProduct.middleCols<d>(di) = S_dot.middleCols<d>(di) * S;
+  }
+  Matrix riemannianGradient = euclideanGradient - symBlockDiagProduct;
+  return riemannianGradient;
+}
+
+/* ************************************************************************* */
+template <size_t d>
+Values ShonanAveraging<d>::LiftwithDescent(size_t p, const Values &values,
+                                           const Vector &minEigenVector) {
+  Values lifted = LiftTo<SOn>(p, values);
+  for (auto it : lifted.filter<SOn>()) {
+    // Create a tangent direction xi with eigenvector segment v_i
+    // Assumes key is 0-based integer
+    const Vector xi = MakeATangentVector(p, minEigenVector, it.key);
+    // Move the old value in the descent direction
+    it.value = it.value.retract(xi);
+  }
+  return lifted;
+}
+
+/* ************************************************************************* */
+template <size_t d>
+Values ShonanAveraging<d>::initializeWithDescent(
+    size_t p, const Values &values, const Vector &minEigenVector,
+    double minEigenValue, double gradienTolerance,
+    double preconditionedGradNormTolerance) const {
+  double funcVal = costAt(p - 1, values);
+  double alphaMin = 1e-2;
+  double alpha =
+      std::max(1024 * alphaMin, 10 * gradienTolerance / fabs(minEigenValue));
+  vector<double> alphas;
+  vector<double> fvals;
+  // line search
+  while ((alpha >= alphaMin)) {
+    Values Qplus = LiftwithDescent(p, values, alpha * minEigenVector);
+    double funcValTest = costAt(p, Qplus);
+    Matrix gradTest = riemannianGradient(p, Qplus);
+    double gradTestNorm = gradTest.norm();
+    // Record alpha and funcVal
+    alphas.push_back(alpha);
+    fvals.push_back(funcValTest);
+    if ((funcVal > funcValTest) && (gradTestNorm > gradienTolerance)) {
+      return Qplus;
+    }
+    alpha /= 2;
+  }
+
+  auto fminIter = min_element(fvals.begin(), fvals.end());
+  auto minIdx = distance(fvals.begin(), fminIter);
+  double fMin = fvals[minIdx];
+  double aMin = alphas[minIdx];
+  if (fMin < funcVal) {
+    Values Qplus = LiftwithDescent(p, values, aMin * minEigenVector);
+    return Qplus;
+  }
+
+  return LiftwithDescent(p, values, alpha * minEigenVector);
+}
+
+/* ************************************************************************* */
+template <size_t d>
+Values ShonanAveraging<d>::initializeRandomly(std::mt19937 &rng) const {
+  Values initial;
+  for (size_t j = 0; j < nrUnknowns(); j++) {
+    initial.insert(j, Rot::Random(rng));
+  }
+  return initial;
+}
+
+/* ************************************************************************* */
+template <size_t d> Values ShonanAveraging<d>::initializeRandomly() const {
+  return initializeRandomly(kRandomNumberGenerator);
+}
+
+/* ************************************************************************* */
+template <size_t d>
+Values ShonanAveraging<d>::initializeRandomlyAt(size_t p,
+                                                std::mt19937 &rng) const {
+  const Values randomRotations = initializeRandomly(rng);
+  return LiftTo<Rot3>(p, randomRotations); // lift to p!
+}
+
+/* ************************************************************************* */
+template <size_t d>
+Values ShonanAveraging<d>::initializeRandomlyAt(size_t p) const {
+  return initializeRandomlyAt(p, kRandomNumberGenerator);
+}
+
+/* ************************************************************************* */
+template <size_t d>
+std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
+                                                  size_t pMin,
+                                                  size_t pMax) const {
+  Values Qstar;
+  Values initialSOp = LiftTo<Rot>(pMin, initialEstimate);  // lift to pMin!
+  for (size_t p = pMin; p <= pMax; p++) {
+    // Optimize until convergence at this level
+    Qstar = tryOptimizingAt(p, initialSOp);
+
+    // Check certificate of global optimzality
+    Vector minEigenVector;
+    double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector);
+    if (minEigenValue > parameters_.optimalityThreshold) {
+      // If at global optimum, round and return solution
+      const Values SO3Values = roundSolution(Qstar);
+      return std::make_pair(SO3Values, minEigenValue);
+    }
+
+    // Not at global optimimum yet, so check whether we will go to next level
+    if (p != pMax) {
+      // Calculate initial estimate for next level by following minEigenVector
+      initialSOp =
+          initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue);
+    }
+  }
+  throw std::runtime_error("Shonan::run did not converge for given pMax");
+}
+
+/* ************************************************************************* */
+// Explicit instantiation for d=2
+template class ShonanAveraging<2>;
+
+ShonanAveraging2::ShonanAveraging2(const Measurements &measurements,
+                                   const Parameters &parameters)
+    : ShonanAveraging<2>(measurements, parameters) {}
+ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
+    : ShonanAveraging<2>(parseMeasurements<Rot2>(g2oFile), parameters) {}
+
+/* ************************************************************************* */
+// Explicit instantiation for d=3
+template class ShonanAveraging<3>;
+
+ShonanAveraging3::ShonanAveraging3(const Measurements &measurements,
+                                   const Parameters &parameters)
+    : ShonanAveraging<3>(measurements, parameters) {}
+
+ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
+    : ShonanAveraging<3>(parseMeasurements<Rot3>(g2oFile), parameters) {}
+
+// TODO(frank): Deprecate after we land pybind wrapper
+
+// Extract Rot3 measurement from Pose3 betweenfactors
+// Modeled after similar function in dataset.cpp
+static BinaryMeasurement<Rot3>
+convert(const BetweenFactor<Pose3>::shared_ptr &f) {
+  auto gaussian =
+      boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
+  if (!gaussian)
+    throw std::invalid_argument(
+        "parseMeasurements<Rot3> can only convert Pose3 measurements "
+        "with Gaussian noise models.");
+  const Matrix6 M = gaussian->covariance();
+  return BinaryMeasurement<Rot3>(
+      f->key1(), f->key2(), f->measured().rotation(),
+      noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true));
+}
+
+static ShonanAveraging3::Measurements
+extractRot3Measurements(const BetweenFactorPose3s &factors) {
+  ShonanAveraging3::Measurements result;
+  result.reserve(factors.size());
+  for (auto f : factors)
+    result.push_back(convert(f));
+  return result;
+}
+
+ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors,
+                                   const Parameters &parameters)
+    : ShonanAveraging<3>(extractRot3Measurements(factors), parameters) {}
+
+/* ************************************************************************* */
+} // namespace gtsam
diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h
new file mode 100644
index 000000000..ed94329a2
--- /dev/null
+++ b/gtsam/sfm/ShonanAveraging.h
@@ -0,0 +1,365 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   ShonanAveraging.h
+ * @date   March 2019 - August 2020
+ * @author Frank Dellaert, David Rosen, and Jing Wu
+ * @brief  Shonan Averaging algorithm
+ */
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Rot2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
+#include <gtsam/sfm/BinaryMeasurement.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/dllexport.h>
+
+#include <Eigen/Sparse>
+#include <map>
+#include <string>
+#include <type_traits>
+#include <utility>
+
+namespace gtsam {
+class NonlinearFactorGraph;
+class LevenbergMarquardtOptimizer;
+
+/// Parameters governing optimization etc.
+template <size_t d> struct GTSAM_EXPORT ShonanAveragingParameters {
+  // Select Rot2 or Rot3 interface based template parameter d
+  using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
+  using Anchor = std::pair<size_t, Rot>;
+
+  // Paremeters themselves:
+  LevenbergMarquardtParams lm; // LM parameters
+  double optimalityThreshold;  // threshold used in checkOptimality
+  Anchor anchor;               // pose to use as anchor if not Karcher
+  double alpha;                // weight of anchor-based prior (default 0)
+  double beta;                 // weight of Karcher-based prior (default 1)
+  double gamma;                // weight of gauge-fixing factors (default 0)
+
+  ShonanAveragingParameters(const LevenbergMarquardtParams &lm =
+                                LevenbergMarquardtParams::CeresDefaults(),
+                            const std::string &method = "JACOBI",
+                            double optimalityThreshold = -1e-4,
+                            double alpha = 0.0, double beta = 1.0,
+                            double gamma = 0.0);
+
+  LevenbergMarquardtParams getLMParams() const { return lm; }
+
+  void setOptimalityThreshold(double value) { optimalityThreshold = value; }
+  double getOptimalityThreshold() const { return optimalityThreshold; }
+
+  void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; }
+
+  void setAnchorWeight(double value) { alpha = value; }
+  double getAnchorWeight() { return alpha; }
+
+  void setKarcherWeight(double value) { beta = value; }
+  double getKarcherWeight() { return beta; }
+
+  void setGaugesWeight(double value) { gamma = value; }
+  double getGaugesWeight() { return gamma; }
+};
+
+using ShonanAveragingParameters2 = ShonanAveragingParameters<2>;
+using ShonanAveragingParameters3 = ShonanAveragingParameters<3>;
+
+/**
+ * Class that implements Shonan Averaging from our ECCV'20 paper.
+ * Note: The "basic" API uses all Rot values (Rot2 or Rot3, depending on value
+ * of d), whereas the different levels and "advanced" API at SO(p) needs Values
+ * of type SOn<Dynamic>.
+ *
+ * The template parameter d can be 2 or 3.
+ * Both are specialized in the .cpp file.
+ *
+ * If you use this code in your work, please consider citing our paper:
+ *    Shonan Rotation Averaging, Global Optimality by Surfing SO(p)^n
+ *    Frank Dellaert, David M. Rosen, Jing Wu, Robert Mahony, and Luca Carlone,
+ *    European Computer Vision Conference, 2020.
+ * You can view our ECCV spotlight video at https://youtu.be/5ppaqMyHtE0
+ */
+template <size_t d> class GTSAM_EXPORT ShonanAveraging {
+public:
+  using Sparse = Eigen::SparseMatrix<double>;
+
+  // Define the Parameters type and use its typedef of the rotation type:
+  using Parameters = ShonanAveragingParameters<d>;
+  using Rot = typename Parameters::Rot;
+
+  // We store SO(d) BetweenFactors to get noise model
+  // TODO(frank): use BinaryMeasurement?
+  using Measurements = std::vector<BinaryMeasurement<Rot>>;
+
+private:
+  Parameters parameters_;
+  Measurements measurements_;
+  size_t nrUnknowns_;
+  Sparse D_; // Sparse (diagonal) degree matrix
+  Sparse Q_; // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr
+  Sparse L_; // connection Laplacian L = D - Q, needed for optimality check
+
+  /**
+   * Build 3Nx3N sparse matrix consisting of rotation measurements, arranged as
+   * (i,j) and (j,i) blocks within a sparse matrix.
+   */
+  Sparse buildQ() const;
+
+  /// Build 3Nx3N sparse degree matrix D
+  Sparse buildD() const;
+
+public:
+  /// @name Standard Constructors
+  /// @{
+
+  /// Construct from set of relative measurements (given as BetweenFactor<Rot3>
+  /// for now) NoiseModel *must* be isotropic.
+  ShonanAveraging(const Measurements &measurements,
+                  const Parameters &parameters = Parameters());
+
+  /// @}
+  /// @name Query properties
+  /// @{
+
+  /// Return number of unknowns
+  size_t nrUnknowns() const { return nrUnknowns_; }
+
+  /// Return number of measurements
+  size_t nrMeasurements() const { return measurements_.size(); }
+
+  /// k^th binary measurement
+  const BinaryMeasurement<Rot> &measurement(size_t k) const {
+    return measurements_[k];
+  }
+
+  /// k^th measurement, as a Rot.
+  const Rot &measured(size_t k) const { return measurements_[k].measured(); }
+
+  /// Keys for k^th measurement, as a vector of Key values.
+  const KeyVector &keys(size_t k) const { return measurements_[k].keys(); }
+
+  /// @}
+  /// @name Matrix API (advanced use, debugging)
+  /// @{
+
+  Sparse D() const { return D_; }              ///< Sparse version of D
+  Matrix denseD() const { return Matrix(D_); } ///< Dense version of D
+  Sparse Q() const { return Q_; }              ///< Sparse version of Q
+  Matrix denseQ() const { return Matrix(Q_); } ///< Dense version of Q
+  Sparse L() const { return L_; }              ///< Sparse version of L
+  Matrix denseL() const { return Matrix(L_); } ///< Dense version of L
+
+  /// Version that takes pxdN Stiefel manifold elements
+  Sparse computeLambda(const Matrix &S) const;
+
+  /// Dense versions of computeLambda for wrapper/testing
+  Matrix computeLambda_(const Values &values) const {
+    return Matrix(computeLambda(values));
+  }
+
+  /// Dense versions of computeLambda for wrapper/testing
+  Matrix computeLambda_(const Matrix &S) const {
+    return Matrix(computeLambda(S));
+  }
+
+  /// Compute A matrix whose Eigenvalues we will examine
+  Sparse computeA(const Values &values) const;
+
+  /// Version that takes pxdN Stiefel manifold elements
+  Sparse computeA(const Matrix &S) const;
+
+  /// Dense version of computeA for wrapper/testing
+  Matrix computeA_(const Values &values) const {
+    return Matrix(computeA(values));
+  }
+
+  /// Project to pxdN Stiefel manifold
+  static Matrix StiefelElementMatrix(const Values &values);
+
+  /**
+   * Compute minimum eigenvalue for optimality check.
+   * @param values: should be of type SOn
+   */
+  double computeMinEigenValue(const Values &values,
+                              Vector *minEigenVector = nullptr) const;
+
+  /// Project pxdN Stiefel manifold matrix S to Rot3^N
+  Values roundSolutionS(const Matrix &S) const;
+
+  /// Create a tangent direction xi with eigenvector segment v_i
+  static Vector MakeATangentVector(size_t p, const Vector &v, size_t i);
+
+  /// Calculate the riemannian gradient of F(values) at values
+  Matrix riemannianGradient(size_t p, const Values &values) const;
+
+  /**
+   * Lift up the dimension of values in type SO(p-1) with descent direction
+   * provided by minEigenVector and return new values in type SO(p)
+   */
+  static Values LiftwithDescent(size_t p, const Values &values,
+                                const Vector &minEigenVector);
+
+  /**
+   * Given some values at p-1, return new values at p, by doing a line search
+   * along the descent direction, computed from the minimum eigenvector at p-1.
+   * @param values should be of type SO(p-1)
+   * @param minEigenVector corresponding to minEigenValue at level p-1
+   * @return values of type SO(p)
+   */
+  Values
+  initializeWithDescent(size_t p, const Values &values,
+                        const Vector &minEigenVector, double minEigenValue,
+                        double gradienTolerance = 1e-2,
+                        double preconditionedGradNormTolerance = 1e-4) const;
+  /// @}
+  /// @name Advanced API
+  /// @{
+
+  /**
+   * Build graph for SO(p)
+   * @param p the dimensionality of the rotation manifold to optimize over
+   */
+  NonlinearFactorGraph buildGraphAt(size_t p) const;
+
+  /**
+   * Create initial Values of type SO(p)
+   * @param p the dimensionality of the rotation manifold 
+   */
+  Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const;
+
+  /// Version of initializeRandomlyAt with fixed random seed. 
+  Values initializeRandomlyAt(size_t p) const;
+
+  /**
+   * Calculate cost for SO(p)
+   * Values should be of type SO(p)
+   */
+  double costAt(size_t p, const Values &values) const;
+
+  /**
+   * Given an estimated local minimum Yopt for the (possibly lifted)
+   * relaxation, this function computes and returns the block-diagonal elements
+   * of the corresponding Lagrange multiplier.
+   */
+  Sparse computeLambda(const Values &values) const;
+
+  /**
+   * Compute minimum eigenvalue for optimality check.
+   * @param values: should be of type SOn
+   * @return minEigenVector and minEigenValue
+   */
+  std::pair<double, Vector> computeMinEigenVector(const Values &values) const;
+
+  /**
+   * Check optimality
+   * @param values: should be of type SOn
+   */
+  bool checkOptimality(const Values &values) const;
+
+  /**
+   * Try to create optimizer at SO(p)
+   * @param p the dimensionality of the rotation manifold to optimize over
+   * @param initial initial SO(p) values
+   * @return lm optimizer
+   */
+  boost::shared_ptr<LevenbergMarquardtOptimizer> createOptimizerAt(
+      size_t p, const Values &initial) const;
+
+  /**
+   * Try to optimize at SO(p)
+   * @param p the dimensionality of the rotation manifold to optimize over
+   * @param initial initial SO(p) values
+   * @return SO(p) values
+   */
+  Values tryOptimizingAt(size_t p, const Values &initial) const;
+
+  /**
+   * Project from SO(p) to Rot2 or Rot3
+   * Values should be of type SO(p)
+   */
+  Values projectFrom(size_t p, const Values &values) const;
+
+  /**
+   * Project from SO(p)^N to Rot2^N or Rot3^N
+   * Values should be of type SO(p)
+   */
+  Values roundSolution(const Values &values) const;
+
+  /// Lift Values of type T to SO(p)
+  template <class T> static Values LiftTo(size_t p, const Values &values) {
+    Values result;
+    for (const auto it : values.filter<T>()) {
+      result.insert(it.key, SOn::Lift(p, it.value.matrix()));
+    }
+    return result;
+  }
+
+  /// @}
+  /// @name Basic API
+  /// @{
+
+  /**
+   * Calculate cost for SO(3)
+   * Values should be of type Rot3
+   */
+  double cost(const Values &values) const;
+
+  /**
+   * Initialize randomly at SO(d)
+   * @param rng random number generator
+   * Example:
+   *   std::mt19937 rng(42);
+   *   Values initial = initializeRandomly(rng, p);
+   */
+  Values initializeRandomly(std::mt19937 &rng) const;
+
+  /// Random initialization for wrapper, fixed random seed. 
+  Values initializeRandomly() const;
+
+  /**
+   * Optimize at different values of p until convergence.
+   * @param initial initial Rot3 values
+   * @param pMin value of p to start Riemanian staircase at (default: d).
+   * @param pMax maximum value of p to try (default: 10)
+   * @return (Rot3 values, minimum eigenvalue)
+   */
+  std::pair<Values, double> run(const Values &initialEstimate, size_t pMin = d,
+                                size_t pMax = 10) const;
+  /// @}
+};
+
+// Subclasses for d=2 and d=3 that explicitly instantiate, as well as provide a
+// convenience interface with file access.
+
+class ShonanAveraging2 : public ShonanAveraging<2> {
+public:
+  ShonanAveraging2(const Measurements &measurements,
+                   const Parameters &parameters = Parameters());
+  ShonanAveraging2(string g2oFile, const Parameters &parameters = Parameters());
+};
+
+class ShonanAveraging3 : public ShonanAveraging<3> {
+public:
+  ShonanAveraging3(const Measurements &measurements,
+                   const Parameters &parameters = Parameters());
+  ShonanAveraging3(string g2oFile, const Parameters &parameters = Parameters());
+
+  // TODO(frank): Deprecate after we land pybind wrapper
+  ShonanAveraging3(const BetweenFactorPose3s &factors,
+                   const Parameters &parameters = Parameters());
+};
+} // namespace gtsam
diff --git a/gtsam/sfm/ShonanFactor.cpp b/gtsam/sfm/ShonanFactor.cpp
new file mode 100644
index 000000000..b911fb5a4
--- /dev/null
+++ b/gtsam/sfm/ShonanFactor.cpp
@@ -0,0 +1,141 @@
+/* ----------------------------------------------------------------------------
+
+ * 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/sfm/ShonanFactor.h>
+
+#include <gtsam/base/timing.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/FrobeniusFactor.h>
+
+#include <cmath>
+#include <iostream>
+#include <vector>
+
+using namespace std;
+
+namespace gtsam {
+
+//******************************************************************************
+template <size_t d>
+ShonanFactor<d>::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
+                              const SharedNoiseModel &model,
+                              const boost::shared_ptr<Matrix> &G)
+    : NoiseModelFactor2<SOn, SOn>(ConvertNoiseModel(model, p * d), j1, j2),
+      M_(R12.matrix()), // d*d in all cases
+      p_(p),            // 4 for SO(4)
+      pp_(p * p),       // 16 for SO(4)
+      G_(G) {
+  if (noiseModel()->dim() != d * p_)
+    throw std::invalid_argument(
+        "ShonanFactor: model with incorrect dimension.");
+  if (!G) {
+    G_ = boost::make_shared<Matrix>();
+    *G_ = SOn::VectorizedGenerators(p); // expensive!
+  }
+  if (static_cast<size_t>(G_->rows()) != pp_ ||
+      static_cast<size_t>(G_->cols()) != SOn::Dimension(p))
+    throw std::invalid_argument("ShonanFactor: passed in generators "
+                                "of incorrect dimension.");
+}
+
+//******************************************************************************
+template <size_t d>
+void ShonanFactor<d>::print(const std::string &s,
+                            const KeyFormatter &keyFormatter) const {
+  std::cout << s << "ShonanFactor<" << p_ << ">(" << keyFormatter(key1()) << ","
+            << keyFormatter(key2()) << ")\n";
+  traits<Matrix>::Print(M_, "  M: ");
+  noiseModel_->print("  noise model: ");
+}
+
+//******************************************************************************
+template <size_t d>
+bool ShonanFactor<d>::equals(const NonlinearFactor &expected,
+                             double tol) const {
+  auto e = dynamic_cast<const ShonanFactor *>(&expected);
+  return e != nullptr && NoiseModelFactor2<SOn, SOn>::equals(*e, tol) &&
+         p_ == e->p_ && M_ == e->M_;
+}
+
+//******************************************************************************
+template <size_t d>
+void ShonanFactor<d>::fillJacobians(const Matrix &M1, const Matrix &M2,
+                                    boost::optional<Matrix &> H1,
+                                    boost::optional<Matrix &> H2) const {
+  gttic(ShonanFactor_Jacobians);
+  const size_t dim = p_ * d; // Stiefel manifold dimension
+
+  if (H1) {
+    // If asked, calculate Jacobian H1 as as (M' \otimes M1) * G
+    // M' = dxd, M1 = pxp, G = (p*p)xDim(p), result should be dim x Dim(p)
+    // (M' \otimes M1) is dim*dim, but last pp-dim columns are zero
+    *H1 = Matrix::Zero(dim, G_->cols());
+    for (size_t j = 0; j < d; j++) {
+      auto MG_j = M1 * G_->middleRows(j * p_, p_); // p_ * Dim(p)
+      for (size_t i = 0; i < d; i++) {
+        H1->middleRows(i * p_, p_) -= M_(j, i) * MG_j;
+      }
+    }
+  }
+  if (H2) {
+    // If asked, calculate Jacobian H2 as as (I_d \otimes M2) * G
+    // I_d = dxd, M2 = pxp, G = (p*p)xDim(p), result should be dim x Dim(p)
+    // (I_d \otimes M2) is dim*dim, but again last pp-dim columns are zero
+    H2->resize(dim, G_->cols());
+    for (size_t i = 0; i < d; i++) {
+      H2->middleRows(i * p_, p_) = M2 * G_->middleRows(i * p_, p_);
+    }
+  }
+}
+
+//******************************************************************************
+template <size_t d>
+Vector ShonanFactor<d>::evaluateError(const SOn &Q1, const SOn &Q2,
+                                      boost::optional<Matrix &> H1,
+                                      boost::optional<Matrix &> H2) const {
+  gttic(ShonanFactor_evaluateError);
+
+  const Matrix &M1 = Q1.matrix();
+  const Matrix &M2 = Q2.matrix();
+  if (M1.rows() != static_cast<int>(p_) || M2.rows() != static_cast<int>(p_))
+    throw std::invalid_argument("Invalid dimension SOn values passed to "
+                                "ShonanFactor<d>::evaluateError");
+
+  const size_t dim = p_ * d; // 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<d>() * M_;
+  hQ1 << Eigen::Map<const Matrix>(Q1PR12.data(), dim, 1);
+
+  this->fillJacobians(M1, M2, H1, H2);
+
+  return fQ2 - hQ1;
+}
+
+/* ************************************************************************* */
+// Explicit instantiation for d=2 and d=3
+template class ShonanFactor<2>;
+template class ShonanFactor<3>;
+
+//******************************************************************************
+
+} // namespace gtsam
diff --git a/gtsam/sfm/ShonanFactor.h b/gtsam/sfm/ShonanFactor.h
new file mode 100644
index 000000000..3c43c2c52
--- /dev/null
+++ b/gtsam/sfm/ShonanFactor.h
@@ -0,0 +1,91 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   ShonanFactor.h
+ * @date   March 2019
+ * @author Frank Dellaert
+ * @brief  Main factor type in Shonan averaging, on SO(n) pairs
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Rot2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/SOn.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+#include <type_traits>
+
+namespace gtsam {
+
+/**
+ * ShonanFactor is a BetweenFactor that moves in SO(p), but will
+ * land on the SO(d) sub-manifold of SO(p) at the global minimum. It projects
+ * the SO(p) matrices down to a Stiefel manifold of p*d matrices.
+ */
+template <size_t d>
+class GTSAM_EXPORT ShonanFactor : public NoiseModelFactor2<SOn, SOn> {
+  Matrix M_;                    ///< measured rotation between R1 and R2
+  size_t p_, pp_;               ///< dimensionality constants
+  boost::shared_ptr<Matrix> G_; ///< matrix of vectorized generators
+
+  // Select Rot2 or Rot3 interface based template parameter d
+  using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
+
+public:
+  /// @name Constructor
+  /// @{
+
+  /// Constructor. Note we convert to d*p-dimensional noise model.
+  /// To save memory and mallocs, pass in the vectorized Lie algebra generators:
+  ///    G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(p));
+  ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
+               const SharedNoiseModel &model = nullptr,
+               const boost::shared_ptr<Matrix> &G = nullptr);
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// print with optional string
+  void
+  print(const std::string &s,
+        const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
+
+  /// assert equality up to a tolerance
+  bool equals(const NonlinearFactor &expected,
+              double tol = 1e-9) const override;
+
+  /// @}
+  /// @name NoiseModelFactor2 methods
+  /// @{
+
+  /// 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;
+  /// @}
+
+private:
+  /// Calculate Jacobians if asked, Only implemented for d=2 and 3 in .cpp
+  void fillJacobians(const Matrix &M1, const Matrix &M2,
+                     boost::optional<Matrix &> H1,
+                     boost::optional<Matrix &> H2) const;
+};
+
+// Explicit instantiation for d=2 and d=3 in .cpp file:
+using ShonanFactor2 = ShonanFactor<2>;
+using ShonanFactor3 = ShonanFactor<3>;
+
+} // namespace gtsam
diff --git a/gtsam/sfm/ShonanGaugeFactor.h b/gtsam/sfm/ShonanGaugeFactor.h
new file mode 100644
index 000000000..4847c5d58
--- /dev/null
+++ b/gtsam/sfm/ShonanGaugeFactor.h
@@ -0,0 +1,108 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   ShonanGaugeFactor.h
+ * @date   March 2019
+ * @author Frank Dellaert
+ * @brief  Factor used in Shonan Averaging to clamp down gauge freedom
+ */
+
+#pragma once
+
+#include <gtsam/geometry/SOn.h>
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+#include <boost/assign/list_of.hpp>
+
+namespace gtsam {
+/**
+ * The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving
+ * in the stabilizer.
+ *
+ * Details: SO(p) contains the p*3 Stiefel matrices of orthogonal frames: we
+ * take those to be the 3 columns on the left.
+ * The P*P skew-symmetric matrices associated with so(p) can be partitioned as
+ * (Appendix B in the ECCV paper):
+ * | [w] -K' |
+ * |  K  [g] |
+ * where w is the SO(3) space, K are the extra Stiefel diemnsions (wormhole!)
+ * and (g)amma are extra dimensions in SO(p) that do not modify the cost
+ * function. The latter corresponds to rotations SO(p-d), and so the first few
+ * values of p-d for d==3 with their corresponding dimensionality are {0:0, 1:0,
+ * 2:1, 3:3, ...}
+ *
+ * The ShonanGaugeFactor adds a unit Jacobian to these extra dimensions,
+ * essentially restricting the optimization to the Stiefel manifold.
+ */
+class GTSAM_EXPORT ShonanGaugeFactor : public NonlinearFactor {
+  // Row dimension, equal to the dimensionality of SO(p-d)
+  size_t rows_;
+
+  /// Constant Jacobian
+  boost::shared_ptr<JacobianFactor> whitenedJacobian_;
+
+public:
+  /**
+   * Construct from key for an SO(p) matrix, for base dimension d (2 or 3)
+   * If parameter gamma is given, it acts as a precision = 1/sigma^2, and
+   * the Jacobian will be multiplied with 1/sigma = sqrt(gamma).
+   */
+  ShonanGaugeFactor(Key key, size_t p, size_t d = 3,
+                    boost::optional<double> gamma = boost::none)
+      : NonlinearFactor(boost::assign::cref_list_of<1>(key)) {
+    if (p < d) {
+      throw std::invalid_argument("ShonanGaugeFactor must have p>=d.");
+    }
+    // Calculate dimensions
+    size_t q = p - d;
+    size_t P = SOn::Dimension(p); // dimensionality of SO(p)
+    rows_ = SOn::Dimension(q);    // dimensionality of SO(q), the gauge
+
+    // Create constant Jacobian as a rows_*P matrix: there are rows_ penalized
+    // dimensions, but it is a bit tricky to find them among the P columns.
+    // The key is to look at how skew-symmetric matrices are laid out in SOn.h:
+    // the first tangent dimension will always be included, but beyond that we
+    // have to be careful. We always need to skip the d top-rows of the skew-
+    // symmetric matrix as they below to K, part of the Stiefel manifold.
+    Matrix A(rows_, P);
+    A.setZero();
+    double invSigma = gamma ? std::sqrt(*gamma) : 1.0;
+    size_t i = 0, j = 0, n = p - 1 - d;
+    while (i < rows_) {
+      A.block(i, j, n, n) = invSigma * Matrix::Identity(n, n);
+      i += n;
+      j += n + d; // skip d columns
+      n -= 1;
+    }
+    // TODO(frank): assign the right one in the right columns
+    whitenedJacobian_ =
+        boost::make_shared<JacobianFactor>(key, A, Vector::Zero(rows_));
+  }
+
+  /// Destructor
+  virtual ~ShonanGaugeFactor() {}
+
+  /// Calculate the error of the factor: always zero
+  double error(const Values &c) const override { return 0; }
+
+  /// get the dimension of the factor (number of rows on linearization)
+  size_t dim() const override { return rows_; }
+
+  /// linearize to a GaussianFactor
+  boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
+    return whitenedJacobian_;
+  }
+};
+// \ShonanGaugeFactor
+
+} // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp
new file mode 100644
index 000000000..cc4319e15
--- /dev/null
+++ b/gtsam/sfm/tests/testShonanAveraging.cpp
@@ -0,0 +1,360 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testShonanAveraging.cpp
+ * @date   March 2019
+ * @author Frank Dellaert
+ * @brief  Unit tests for Shonan Averaging algorithm
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/sfm/ShonanAveraging.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/FrobeniusFactor.h>
+
+#include <algorithm>
+#include <iostream>
+#include <map>
+#include <random>
+
+using namespace std;
+using namespace gtsam;
+
+template <size_t d>
+using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
+
+template <size_t d>
+using Pose = typename std::conditional<d == 2, Pose2, Pose3>::type;
+
+ShonanAveraging3 fromExampleName(
+    const std::string &name,
+    ShonanAveraging3::Parameters parameters = ShonanAveraging3::Parameters()) {
+  string g2oFile = findExampleDataFile(name);
+  return ShonanAveraging3(g2oFile, parameters);
+}
+
+static const ShonanAveraging3 kShonan = fromExampleName("toyExample.g2o");
+
+static std::mt19937 kRandomNumberGenerator(42);
+
+/* ************************************************************************* */
+TEST(ShonanAveraging3, checkConstructor) {
+  EXPECT_LONGS_EQUAL(5, kShonan.nrUnknowns());
+
+  EXPECT_LONGS_EQUAL(15, kShonan.D().rows());
+  EXPECT_LONGS_EQUAL(15, kShonan.D().cols());
+  auto D = kShonan.denseD();
+  EXPECT_LONGS_EQUAL(15, D.rows());
+  EXPECT_LONGS_EQUAL(15, D.cols());
+
+  EXPECT_LONGS_EQUAL(15, kShonan.Q().rows());
+  EXPECT_LONGS_EQUAL(15, kShonan.Q().cols());
+  auto Q = kShonan.denseQ();
+  EXPECT_LONGS_EQUAL(15, Q.rows());
+  EXPECT_LONGS_EQUAL(15, Q.cols());
+
+  EXPECT_LONGS_EQUAL(15, kShonan.L().rows());
+  EXPECT_LONGS_EQUAL(15, kShonan.L().cols());
+  auto L = kShonan.denseL();
+  EXPECT_LONGS_EQUAL(15, L.rows());
+  EXPECT_LONGS_EQUAL(15, L.cols());
+}
+
+/* ************************************************************************* */
+TEST(ShonanAveraging3, buildGraphAt) {
+  auto graph = kShonan.buildGraphAt(5);
+  EXPECT_LONGS_EQUAL(7, graph.size());
+}
+
+/* ************************************************************************* */
+TEST(ShonanAveraging3, checkOptimality) {
+  const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
+  Values random = ShonanAveraging3::LiftTo<Rot3>(4, randomRotations);  // lift to 4!
+  auto Lambda = kShonan.computeLambda(random);
+  EXPECT_LONGS_EQUAL(15, Lambda.rows());
+  EXPECT_LONGS_EQUAL(15, Lambda.cols());
+  EXPECT_LONGS_EQUAL(45, Lambda.nonZeros());
+  auto lambdaMin = kShonan.computeMinEigenValue(random);
+  // EXPECT_DOUBLES_EQUAL(-5.2964625490657866, lambdaMin,
+  //                      1e-4);  // Regression test
+  EXPECT_DOUBLES_EQUAL(-414.87376657555996, lambdaMin,
+                       1e-4); // Regression test
+  EXPECT(!kShonan.checkOptimality(random));
+}
+
+/* ************************************************************************* */
+TEST(ShonanAveraging3, tryOptimizingAt3) {
+  const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
+  Values initial = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);  // convert to SOn
+  EXPECT(!kShonan.checkOptimality(initial));
+  const Values result = kShonan.tryOptimizingAt(3, initial);
+  EXPECT(kShonan.checkOptimality(result));
+  auto lambdaMin = kShonan.computeMinEigenValue(result);
+  EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, lambdaMin,
+                       1e-4); // Regression test
+  EXPECT_DOUBLES_EQUAL(0, kShonan.costAt(3, result), 1e-4);
+  const Values SO3Values = kShonan.roundSolution(result);
+  EXPECT_DOUBLES_EQUAL(0, kShonan.cost(SO3Values), 1e-4);
+}
+
+/* ************************************************************************* */
+TEST(ShonanAveraging3, tryOptimizingAt4) {
+  const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
+  Values random = ShonanAveraging3::LiftTo<Rot3>(4, randomRotations);  // lift to 4!
+  const Values result = kShonan.tryOptimizingAt(4, random);
+  EXPECT(kShonan.checkOptimality(result));
+  EXPECT_DOUBLES_EQUAL(0, kShonan.costAt(4, result), 1e-3);
+  auto lambdaMin = kShonan.computeMinEigenValue(result);
+  EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, lambdaMin,
+                       1e-4); // Regression test
+  const Values SO3Values = kShonan.roundSolution(result);
+  EXPECT_DOUBLES_EQUAL(0, kShonan.cost(SO3Values), 1e-4);
+}
+
+/* ************************************************************************* */
+TEST(ShonanAveraging3, MakeATangentVector) {
+  Vector9 v;
+  v << 1, 2, 3, 4, 5, 6, 7, 8, 9;
+  Matrix expected(5, 5);
+  expected << 0, 0, 0, 0, -4, //
+      0, 0, 0, 0, -5,         //
+      0, 0, 0, 0, -6,         //
+      0, 0, 0, 0, 0,          //
+      4, 5, 6, 0, 0;
+  const Vector xi_1 = ShonanAveraging3::MakeATangentVector(5, v, 1);
+  const auto actual = SOn::Hat(xi_1);
+  CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST(ShonanAveraging3, LiftTo) {
+  auto I = genericValue(Rot3());
+  Values initial{{0, I}, {1, I}, {2, I}};
+  Values lifted = ShonanAveraging3::LiftTo<Rot3>(5, initial);
+  EXPECT(assert_equal(SOn(5), lifted.at<SOn>(0)));
+}
+
+/* ************************************************************************* */
+TEST(ShonanAveraging3, CheckWithEigen) {
+  // control randomness
+  static std::mt19937 rng(0);
+  Vector descentDirection = Vector::Random(15); // for use below
+  const Values randomRotations = kShonan.initializeRandomly(rng);
+  Values random = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);
+
+  // Optimize
+  const Values Qstar3 = kShonan.tryOptimizingAt(3, random);
+
+  // Compute Eigenvalue with Spectra solver
+  double lambda = kShonan.computeMinEigenValue(Qstar3);
+
+  // Check Eigenvalue with slow Eigen version, converts matrix A to dense matrix!
+  const Matrix S = ShonanAveraging3::StiefelElementMatrix(Qstar3);
+  auto A = kShonan.computeA(S);
+  bool computeEigenvectors = false;
+  Eigen::EigenSolver<Matrix> eigenSolver(Matrix(A), computeEigenvectors);
+  auto lambdas = eigenSolver.eigenvalues().real();
+  double minEigenValue = lambdas(0);
+  for (int i = 1; i < lambdas.size(); i++)
+      minEigenValue = min(lambdas(i), minEigenValue);
+
+  // Actual check
+  EXPECT_DOUBLES_EQUAL(minEigenValue, lambda, 1e-12);
+
+  // Construct test descent direction (as minEigenVector is not predictable
+  // across platforms, being one from a basically flat 3d- subspace)
+
+  // Check descent
+  Values initialQ4 =
+      ShonanAveraging3::LiftwithDescent(4, Qstar3, descentDirection);
+  EXPECT_LONGS_EQUAL(5, initialQ4.size());
+
+  // TODO(frank): uncomment this regression test: currently not repeatable
+  // across platforms.
+  // Matrix expected(4, 4);
+  // expected << 0.0459224, -0.688689, -0.216922, 0.690321, //
+  //     0.92381, 0.191931, 0.255854, 0.21042,              //
+  //     -0.376669, 0.301589, 0.687953, 0.542111,           //
+  //     -0.0508588, 0.630804, -0.643587, 0.43046;
+  // EXPECT(assert_equal(SOn(expected), initialQ4.at<SOn>(0), 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(ShonanAveraging3, initializeWithDescent) {
+  const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
+  Values random = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);
+  const Values Qstar3 = kShonan.tryOptimizingAt(3, random);
+  Vector minEigenVector;
+  double lambdaMin = kShonan.computeMinEigenValue(Qstar3, &minEigenVector);
+  Values initialQ4 =
+      kShonan.initializeWithDescent(4, Qstar3, minEigenVector, lambdaMin);
+  EXPECT_LONGS_EQUAL(5, initialQ4.size());
+}
+
+/* ************************************************************************* */
+TEST(ShonanAveraging3, run) {
+  auto initial = kShonan.initializeRandomly(kRandomNumberGenerator); 
+  auto result = kShonan.run(initial, 5);
+  EXPECT_DOUBLES_EQUAL(0, kShonan.cost(result.first), 1e-3);
+  EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, result.second,
+                       1e-4); // Regression test
+}
+
+/* ************************************************************************* */
+namespace klaus {
+// The data in the file is the Colmap solution
+const Rot3 wR0(0.9992281076190063, -0.02676080288219576, -0.024497002638379624,
+               -0.015064701622500615);
+const Rot3 wR1(0.998239108728862, -0.049543805396343954, -0.03232420352077356,
+               -0.004386230477751116);
+const Rot3 wR2(0.9925378735259738, -0.07993768981394891, 0.0825062894866454,
+               -0.04088089479075661);
+} // namespace klaus
+
+TEST(ShonanAveraging3, runKlaus) {
+  using namespace klaus;
+
+  // Initialize a Shonan instance without the Karcher mean
+  ShonanAveraging3::Parameters parameters;
+  parameters.setKarcherWeight(0);
+
+  // Load 3 pose example taken in Klaus by Shicong
+  static const ShonanAveraging3 shonan =
+      fromExampleName("Klaus3.g2o", parameters);
+
+  // Check nr poses
+  EXPECT_LONGS_EQUAL(3, shonan.nrUnknowns());
+
+  // Colmap uses the Y-down vision frame, and the first 3 rotations are close to
+  // identity. We check that below. Note tolerance is quite high.
+  static const Rot3 identity;
+  EXPECT(assert_equal(identity, wR0, 0.2));
+  EXPECT(assert_equal(identity, wR1, 0.2));
+  EXPECT(assert_equal(identity, wR2, 0.2));
+
+  // Get measurements
+  const Rot3 R01 = shonan.measured(0);
+  const Rot3 R12 = shonan.measured(1);
+  const Rot3 R02 = shonan.measured(2);
+
+  // Regression test to make sure data did not change.
+  EXPECT(assert_equal(Rot3(0.9995433591728293, -0.022048798853273946,
+                           -0.01796327847857683, 0.010210006313668573),
+                      R01));
+
+  // Check Colmap solution agrees OK with relative rotation measurements.
+  EXPECT(assert_equal(R01, wR0.between(wR1), 0.1));
+  EXPECT(assert_equal(R12, wR1.between(wR2), 0.1));
+  EXPECT(assert_equal(R02, wR0.between(wR2), 0.1));
+
+  // Run Shonan (with prior on first rotation)
+  auto initial = shonan.initializeRandomly(kRandomNumberGenerator); 
+  auto result = shonan.run(initial, 5);
+  EXPECT_DOUBLES_EQUAL(0, shonan.cost(result.first), 1e-2);
+  EXPECT_DOUBLES_EQUAL(-9.2259161494467889e-05, result.second,
+                       1e-4); // Regression
+
+  // Get Shonan solution in new frame R (R for result)
+  const Rot3 rR0 = result.first.at<Rot3>(0);
+  const Rot3 rR1 = result.first.at<Rot3>(1);
+  const Rot3 rR2 = result.first.at<Rot3>(2);
+
+  // rR0 = rRw * wR0 => rRw = rR0 * wR0.inverse()
+  // rR1 = rRw * wR1
+  // rR2 = rRw * wR2
+
+  const Rot3 rRw = rR0 * wR0.inverse();
+  EXPECT(assert_equal(rRw * wR1, rR1, 0.1))
+  EXPECT(assert_equal(rRw * wR2, rR2, 0.1))
+}
+
+/* ************************************************************************* */
+TEST(ShonanAveraging3, runKlausKarcher) {
+  using namespace klaus;
+
+  // Load 3 pose example taken in Klaus by Shicong
+  static const ShonanAveraging3 shonan = fromExampleName("Klaus3.g2o");
+
+  // Run Shonan (with Karcher mean prior)
+  auto initial = shonan.initializeRandomly(kRandomNumberGenerator); 
+  auto result = shonan.run(initial, 5);
+  EXPECT_DOUBLES_EQUAL(0, shonan.cost(result.first), 1e-2);
+  EXPECT_DOUBLES_EQUAL(-1.361402670507772e-05, result.second,
+                       1e-4); // Regression test
+
+  // Get Shonan solution in new frame R (R for result)
+  const Rot3 rR0 = result.first.at<Rot3>(0);
+  const Rot3 rR1 = result.first.at<Rot3>(1);
+  const Rot3 rR2 = result.first.at<Rot3>(2);
+
+  const Rot3 rRw = rR0 * wR0.inverse();
+  EXPECT(assert_equal(rRw * wR1, rR1, 0.1))
+  EXPECT(assert_equal(rRw * wR2, rR2, 0.1))
+}
+
+/* ************************************************************************* */
+TEST(ShonanAveraging2, runKlausKarcher) {
+  // Load 2D toy example
+  auto lmParams = LevenbergMarquardtParams::CeresDefaults();
+  // lmParams.setVerbosityLM("SUMMARY");
+  string g2oFile = findExampleDataFile("noisyToyGraph.txt");
+  ShonanAveraging2::Parameters parameters(lmParams);
+  auto measurements = parseMeasurements<Rot2>(g2oFile);
+  ShonanAveraging2 shonan(measurements, parameters);
+  EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns());
+
+  // Check graph building
+  NonlinearFactorGraph graph = shonan.buildGraphAt(2);
+  EXPECT_LONGS_EQUAL(6, graph.size());
+  auto initial = shonan.initializeRandomly(kRandomNumberGenerator); 
+  auto result = shonan.run(initial, 2);
+  EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6);
+  EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate!
+}
+
+/* ************************************************************************* */
+// Test alpha/beta/gamma prior weighting.
+TEST(ShonanAveraging3, PriorWeights) {
+  auto lmParams = LevenbergMarquardtParams::CeresDefaults();
+  ShonanAveraging3::Parameters params(lmParams);
+  EXPECT_DOUBLES_EQUAL(0, params.alpha, 1e-9);
+  EXPECT_DOUBLES_EQUAL(1, params.beta, 1e-9);
+  EXPECT_DOUBLES_EQUAL(0, params.gamma, 1e-9);
+  double alpha = 100.0, beta = 200.0, gamma = 300.0;
+  params.setAnchorWeight(alpha);
+  params.setKarcherWeight(beta);
+  params.setGaugesWeight(gamma);
+  EXPECT_DOUBLES_EQUAL(alpha, params.alpha, 1e-9);
+  EXPECT_DOUBLES_EQUAL(beta, params.beta, 1e-9);
+  EXPECT_DOUBLES_EQUAL(gamma, params.gamma, 1e-9);
+  params.setKarcherWeight(0);
+  static const ShonanAveraging3 shonan = fromExampleName("Klaus3.g2o", params);
+  for (auto i : {0,1,2}) {
+    const auto& m = shonan.measurement(i);
+    auto isotropic =
+        boost::static_pointer_cast<noiseModel::Isotropic>(m.noiseModel());
+    CHECK(isotropic != nullptr);
+    EXPECT_LONGS_EQUAL(3, isotropic->dim());
+    EXPECT_DOUBLES_EQUAL(0.2, isotropic->sigma(), 1e-9);
+  }
+  auto I = genericValue(Rot3());
+  Values initial{{0, I}, {1, I}, {2, I}};
+  EXPECT_DOUBLES_EQUAL(3.0756, shonan.cost(initial), 1e-4);
+  auto result = shonan.run(initial, 3, 3);
+  EXPECT_DOUBLES_EQUAL(0.0015, shonan.cost(result.first), 1e-4);
+}
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
diff --git a/gtsam/sfm/tests/testShonanFactor.cpp b/gtsam/sfm/tests/testShonanFactor.cpp
new file mode 100644
index 000000000..ef94c5cf4
--- /dev/null
+++ b/gtsam/sfm/tests/testShonanFactor.cpp
@@ -0,0 +1,121 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * 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/sfm/ShonanFactor.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
+
+//******************************************************************************
+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(ShonanFactor3, evaluateError) {
+  auto model = noiseModel::Isotropic::Sigma(3, 1.2);
+  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 = ShonanFactor3(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(ShonanFactor3, 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 = ShonanFactor3(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));
+}
+
+/* ************************************************************************* */
+TEST(ShonanFactor2, evaluateError) {
+  auto model = noiseModel::Isotropic::Sigma(1, 1.2);
+  const Rot2 R1(0.3), R2(0.5), R12(0.2);
+  for (const size_t p : {5, 4, 3, 2}) {
+    Matrix M = Matrix::Identity(p, p);
+    M.topLeftCorner(2, 2) = R1.matrix();
+    SOn Q1(M);
+    M.topLeftCorner(2, 2) = R2.matrix();
+    SOn Q2(M);
+    auto factor = ShonanFactor2(1, 2, R12, 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);
+  }
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
diff --git a/gtsam/sfm/tests/testShonanGaugeFactor.cpp b/gtsam/sfm/tests/testShonanGaugeFactor.cpp
new file mode 100644
index 000000000..344394b9c
--- /dev/null
+++ b/gtsam/sfm/tests/testShonanGaugeFactor.cpp
@@ -0,0 +1,106 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testShonanGaugeFactor.cpp
+ * @date   March 2019
+ * @author Frank Dellaert
+ * @brief  Unit tests for ShonanGaugeFactor class
+ */
+
+#include <gtsam/sfm/ShonanGaugeFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <iostream>
+#include <map>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+// Check dimensions of all low-dim GaugeFactors
+TEST(ShonanAveraging, GaugeFactorLows) {
+  constexpr Key key(123);
+  EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 2, 2).dim());
+  EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 3, 2).dim());
+  EXPECT_LONGS_EQUAL(1, ShonanGaugeFactor(key, 4, 2).dim()); // SO(4-2) -> 1
+  EXPECT_LONGS_EQUAL(3, ShonanGaugeFactor(key, 5, 2).dim()); // SO(5-2) -> 3
+
+  EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 3, 3).dim());
+  EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 4, 3).dim());
+  EXPECT_LONGS_EQUAL(1, ShonanGaugeFactor(key, 5, 3).dim()); // SO(5-3) -> 1
+}
+
+/* ************************************************************************* */
+// Check ShonanGaugeFactor for SO(6)
+TEST(ShonanAveraging, GaugeFactorSO6) {
+  constexpr Key key(666);
+  ShonanGaugeFactor factor(key, 6); // For SO(6)
+  Matrix A = Matrix::Zero(3, 15);   // SO(6-3) = SO(3) == 3-dimensional gauge
+  A(0, 0) = 1; // first 2 of 6^th skew column, which has 5 non-zero entries
+  A(1, 1) = 1; // then we skip 3 tangent dimensions
+  A(2, 5) = 1; // first of 5th skew colum, which has 4 non-zero entries above
+               // diagonal.
+  JacobianFactor linearized(key, A, Vector::Zero(3));
+  Values values;
+  EXPECT_LONGS_EQUAL(3, factor.dim());
+  EXPECT(assert_equal(linearized, *boost::dynamic_pointer_cast<JacobianFactor>(
+                                      factor.linearize(values))));
+}
+
+/* ************************************************************************* */
+// Check ShonanGaugeFactor for SO(7)
+TEST(ShonanAveraging, GaugeFactorSO7) {
+  constexpr Key key(777);
+  ShonanGaugeFactor factor(key, 7); // For SO(7)
+  Matrix A = Matrix::Zero(6, 21);   // SO(7-3) = SO(4) == 6-dimensional gauge
+  A(0, 0) = 1; // first 3 of 7^th skew column, which has 6 non-zero entries
+  A(1, 1) = 1;
+  A(2, 2) = 1;  // then we skip 3 tangent dimensions
+  A(3, 6) = 1;  // first 2 of 6^th skew column, which has 5 non-zero entries
+  A(4, 7) = 1;  // then we skip 3 tangent dimensions
+  A(5, 11) = 1; // first of 5th skew colum, which has 4 non-zero entries above
+                // diagonal.
+  JacobianFactor linearized(key, A, Vector::Zero(6));
+  Values values;
+  EXPECT_LONGS_EQUAL(6, factor.dim());
+  EXPECT(assert_equal(linearized, *boost::dynamic_pointer_cast<JacobianFactor>(
+                                      factor.linearize(values))));
+}
+
+/* ************************************************************************* */
+// Check ShonanGaugeFactor for SO(6), with base SO(2)
+TEST(ShonanAveraging, GaugeFactorSO6over2) {
+  constexpr Key key(602);
+  double gamma = 4;
+  ShonanGaugeFactor factor(key, 6, 2, gamma); // For SO(6), base SO(2)
+  Matrix A = Matrix::Zero(6, 15); // SO(6-2) = SO(4) == 6-dimensional gauge
+  A(0, 0) = 2; // first 3 of 6^th skew column, which has 5 non-zero entries
+  A(1, 1) = 2;
+  A(2, 2) = 2; // then we skip only 2 tangent dimensions
+  A(3, 5) = 2; // first 2 of 5^th skew column, which has 4 non-zero entries
+  A(4, 6) = 2; // then we skip only 2 tangent dimensions
+  A(5, 9) = 2; // first of 4th skew colum, which has 3 non-zero entries above
+               // diagonal.
+  JacobianFactor linearized(key, A, Vector::Zero(6));
+  Values values;
+  EXPECT_LONGS_EQUAL(6, factor.dim());
+  EXPECT(assert_equal(linearized, *boost::dynamic_pointer_cast<JacobianFactor>(
+                                      factor.linearize(values))));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp
index 80aeea947..5697a0cd6 100644
--- a/gtsam/slam/FrobeniusFactor.cpp
+++ b/gtsam/slam/FrobeniusFactor.cpp
@@ -18,118 +18,38 @@
 
 #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) {
+boost::shared_ptr<noiseModel::Isotropic>
+ConvertNoiseModel(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)
+    auto sigmas = model->sigmas();
+    size_t n = sigmas.size();
+    if (n == 1) {
+      sigma = sigmas(0); // Rot2
+      goto exit;
+    }
+    if (n == 3 || n == 6) {
+      sigma = sigmas(2); // Pose2, Rot3, or Pose3
+      if (sigmas(0) != sigma || sigmas(1) != sigma) {
+        if (!defaultToUnit) {
           throw std::runtime_error("Can only convert isotropic rotation noise");
-      } else {
-        sigma = sigmas(0);
+        }
       }
+      goto exit;
+    }
+    if (!defaultToUnit) {
+      throw std::runtime_error("Can only convert Pose2/Pose3 noise models");
     }
   }
+exit:
   return noiseModel::Isotropic::Sigma(d, sigma);
 }
 
 //******************************************************************************
-FrobeniusWormholeFactor::FrobeniusWormholeFactor(
-    Key j1, Key j2, const Rot3 &R12, size_t p, const SharedNoiseModel &model,
-    const boost::shared_ptr<Matrix> &G)
-    : 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)
-      G_(G) {
-  if (noiseModel()->dim() != 3 * p_)
-    throw std::invalid_argument(
-        "FrobeniusWormholeFactor: model with incorrect dimension.");
-  if (!G) {
-    G_ = boost::make_shared<Matrix>();
-    *G_ = SOn::VectorizedGenerators(p); // expensive!
-  }
-  if (static_cast<size_t>(G_->rows()) != pp_ ||
-      static_cast<size_t>(G_->cols()) != SOn::Dimension(p))
-    throw std::invalid_argument("FrobeniusWormholeFactor: passed in generators "
-                                "of incorrect dimension.");
-}
 
-//******************************************************************************
-void FrobeniusWormholeFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const {
-  std::cout << s << "FrobeniusWormholeFactor<" << p_ << ">(" << keyFormatter(key1()) << ","
-            << keyFormatter(key2()) << ")\n";
-  traits<Matrix>::Print(M_, "  M: ");
-  noiseModel_->print("  noise model: ");
-}
-
-//******************************************************************************
-bool FrobeniusWormholeFactor::equals(const NonlinearFactor &expected,
-                                     double tol) const {
-  auto e = dynamic_cast<const FrobeniusWormholeFactor *>(&expected);
-  return e != nullptr && NoiseModelFactor2<SOn, SOn>::equals(*e, tol) &&
-         p_ == e->p_ && M_ == e->M_;
-}
-
-//******************************************************************************
-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
+} // namespace gtsam
diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h
index 474cf6143..1fc37c785 100644
--- a/gtsam/slam/FrobeniusFactor.h
+++ b/gtsam/slam/FrobeniusFactor.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/geometry/Rot2.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/SOn.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
@@ -25,23 +26,24 @@
 namespace gtsam {
 
 /**
- * When creating (any) FrobeniusFactor we convert a 6-dimensional Pose3
- * BetweenFactor noise model into an 9 or 16-dimensional isotropic noise
+ * When creating (any) FrobeniusFactor we can convert a Rot/Pose
+ * BetweenFactor noise model into a n-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
+ * null we return a n-dimensional isotropic noise model with sigma=1.0. If
+ * not, we we check if the d-dimensional noise model on rotations is
+ * isotropic. If it is, we extend to 'n' 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);
+GTSAM_EXPORT boost::shared_ptr<noiseModel::Isotropic>
+ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
+                  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> {
+class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1<Rot> {
   enum { Dim = Rot::VectorN2::RowsAtCompileTime };
   using MatrixNN = typename Rot::MatrixNN;
   Eigen::Matrix<double, Dim, 1> vecM_;  ///< vectorized matrix to approximate
@@ -50,7 +52,7 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
   /// Constructor
   FrobeniusPrior(Key j, const MatrixNN& M,
                  const SharedNoiseModel& model = nullptr)
-      : NoiseModelFactor1<Rot>(ConvertPose3NoiseModel(model, Dim), j) {
+      : NoiseModelFactor1<Rot>(ConvertNoiseModel(model, Dim), j) {
     vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
   }
 
@@ -66,13 +68,13 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
  * The template argument can be any fixed-size SO<N>.
  */
 template <class Rot>
-class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
+class GTSAM_EXPORT 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,
+      : NoiseModelFactor2<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
                                     j2) {}
 
   /// Error is just Frobenius norm between rotation matrices.
@@ -106,7 +108,7 @@ class GTSAM_EXPORT FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
   FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
                          const SharedNoiseModel& model = nullptr)
       : NoiseModelFactor2<Rot, Rot>(
-            ConvertPose3NoiseModel(model, Dim), j1, j2),
+            ConvertNoiseModel(model, Dim), j1, j2),
         R12_(R12),
         R2hat_H_R1_(R12.inverse().AdjointMap()) {}
 
@@ -150,52 +152,4 @@ class GTSAM_EXPORT FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
   /// @}
 };
 
-/**
- * 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_;               ///< dimensionality constants
-  boost::shared_ptr<Matrix> G_; ///< matrix of vectorized generators
-
-public:
-  /// @name Constructor
-  /// @{
-
-  /// Constructor. Note we convert to 3*p-dimensional noise model.
-  /// To save memory and mallocs, pass in the vectorized Lie algebra generators:
-  ///    G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(p));
-  FrobeniusWormholeFactor(Key j1, Key j2, const Rot3 &R12, size_t p = 4,
-                          const SharedNoiseModel &model = nullptr,
-                          const boost::shared_ptr<Matrix> &G = nullptr);
-
-  /// @}
-  /// @name Testable
-  /// @{
-
-  /// print with optional string
-  void
-  print(const std::string &s,
-        const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
-
-  /// assert equality up to a tolerance
-  bool equals(const NonlinearFactor &expected,
-              double tol = 1e-9) const override;
-
-  /// @}
-  /// @name NoiseModelFactor2 methods 
-  /// @{
-
-  /// 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/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h
index f10cc7e42..c81a9adc5 100644
--- a/gtsam/slam/KarcherMeanFactor-inl.h
+++ b/gtsam/slam/KarcherMeanFactor-inl.h
@@ -58,20 +58,22 @@ T FindKarcherMean(std::initializer_list<T>&& rotations) {
 
 template <class T>
 template <typename CONTAINER>
-KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER& keys, int d)
-    : NonlinearFactor(keys) {
+KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER &keys, int d,
+                                        boost::optional<double> beta)
+    : NonlinearFactor(keys), d_(static_cast<size_t>(d)) {
   if (d <= 0) {
     throw std::invalid_argument(
         "KarcherMeanFactor needs dimension for dynamic types.");
   }
-  // Create the constant Jacobian made of D*D identity matrices,
-  // where D is the dimensionality of the manifold.
-  const auto I = Eigen::MatrixXd::Identity(d, d);
+  // Create the constant Jacobian made of d*d identity matrices,
+  // where d is the dimensionality of the manifold.
+  Matrix A = Matrix::Identity(d, d);
+  if (beta) A *= std::sqrt(*beta);
   std::map<Key, Matrix> terms;
   for (Key j : keys) {
-    terms[j] = I;
+    terms[j] = A;
   }
-  jacobian_ =
-      boost::make_shared<JacobianFactor>(terms, Eigen::VectorXd::Zero(d));
+  whitenedJacobian_ =
+      boost::make_shared<JacobianFactor>(terms, Vector::Zero(d));
 }
 }  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h
index 54b3930d4..b7cd3b11a 100644
--- a/gtsam/slam/KarcherMeanFactor.h
+++ b/gtsam/slam/KarcherMeanFactor.h
@@ -30,44 +30,51 @@ namespace gtsam {
  * PriorFactors.
  */
 template <class T>
-T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations);
+T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &rotations);
 
-template <class T>
-T FindKarcherMean(std::initializer_list<T>&& rotations);
+template <class T> T FindKarcherMean(std::initializer_list<T> &&rotations);
 
 /**
  * The KarcherMeanFactor creates a constraint on all SO(n) variables with
  * given keys that the Karcher mean (see above) will stay the same. Note the
  * mean itself is irrelevant to the constraint and is not a parameter: the
  * constraint is implemented as enforcing that the sum of local updates is
- * equal to zero, hence creating a rank-dim constraint. Note it is implemented as
- * a soft constraint, as typically it is used to fix a gauge freedom.
+ * equal to zero, hence creating a rank-dim constraint. Note it is implemented
+ * as a soft constraint, as typically it is used to fix a gauge freedom.
  * */
-template <class T>
-class KarcherMeanFactor : public NonlinearFactor {
+template <class T> class KarcherMeanFactor : public NonlinearFactor {
+  // Compile time dimension: can be -1
+  enum { D = traits<T>::dimension };
+
+  // Runtime dimension: always >=0
+  size_t d_;
+
   /// Constant Jacobian made of d*d identity matrices
-  boost::shared_ptr<JacobianFactor> jacobian_;
+  boost::shared_ptr<JacobianFactor> whitenedJacobian_;
 
-  enum {D = traits<T>::dimension};
-
- public:
-  /// Construct from given keys.
+public:
+  /**
+   * Construct from given keys.
+   * If parameter beta is given, it acts as a precision = 1/sigma^2, and
+   * the Jacobian will be multiplied with 1/sigma = sqrt(beta).
+   */
   template <typename CONTAINER>
-  KarcherMeanFactor(const CONTAINER& keys, int d=D);
+  KarcherMeanFactor(const CONTAINER &keys, int d = D,
+                    boost::optional<double> beta = boost::none);
 
   /// Destructor
   virtual ~KarcherMeanFactor() {}
 
   /// Calculate the error of the factor: always zero
-  double error(const Values& c) const override { return 0; }
+  double error(const Values &c) const override { return 0; }
 
   /// get the dimension of the factor (number of rows on linearization)
-  size_t dim() const override { return D; }
+  size_t dim() const override { return d_; }
 
   /// linearize to a GaussianFactor
-  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
-    return jacobian_;
+  boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
+    return whitenedJacobian_;
   }
 };
 // \KarcherMeanFactor
-}  // namespace gtsam
+} // namespace gtsam
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index d7b067d70..797e778a3 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -570,7 +570,7 @@ GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, size_t maxIndex,
 
 /* ************************************************************************* */
 GraphAndValues load2D_robust(const string &filename,
-                             noiseModel::Base::shared_ptr &model,
+                             const noiseModel::Base::shared_ptr &model,
                              size_t maxIndex) {
   return load2D(filename, model, maxIndex);
 }
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index 53abe55ba..f6752eb34 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -172,7 +172,7 @@ GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
 
 /// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel
 GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
-    noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
+    const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
 
 /** save 2d graph */
 GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h
index d60923d8e..680f2d175 100644
--- a/gtsam/slam/expressions.h
+++ b/gtsam/slam/expressions.h
@@ -75,30 +75,6 @@ inline Unit3_ unrotate(const Rot3_& x, const Unit3_& p) {
   return Unit3_(x, &Rot3::unrotate, p);
 }
 
-#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
-namespace internal {
-// define a rotate and unrotate for Vector3
-inline Vector3 rotate(const Rot3& R, const Vector3& v,
-               OptionalJacobian<3, 3> H1 = boost::none,
-               OptionalJacobian<3, 3> H2 = boost::none) {
-  return R.rotate(v, H1, H2);
-}
-inline Vector3 unrotate(const Rot3& R, const Vector3& v,
-                 OptionalJacobian<3, 3> H1 = boost::none,
-                 OptionalJacobian<3, 3> H2 = boost::none) {
-  return R.unrotate(v, H1, H2);
-}
-}  // namespace internal
-inline Expression<Vector3> rotate(const Rot3_& R,
-                                  const Expression<Vector3>& v) {
-  return Expression<Vector3>(internal::rotate, R, v);
-}
-inline Expression<Vector3> unrotate(const Rot3_& R,
-                                    const Expression<Vector3>& v) {
-  return Expression<Vector3>(internal::unrotate, R, v);
-}
-#endif
-
 // Projection
 
 typedef Expression<Cal3_S2> Cal3_S2_;
diff --git a/gtsam/slam/tests/testFrobeniusFactor.cpp b/gtsam/slam/tests/testFrobeniusFactor.cpp
index 9cb0c19fa..321b54c86 100644
--- a/gtsam/slam/tests/testFrobeniusFactor.cpp
+++ b/gtsam/slam/tests/testFrobeniusFactor.cpp
@@ -188,54 +188,6 @@ TEST(FrobeniusBetweenFactorSO4, evaluateError) {
   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;
diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt
index 010b32710..ec161baa8 100644
--- a/gtsam_unstable/CMakeLists.txt
+++ b/gtsam_unstable/CMakeLists.txt
@@ -108,7 +108,7 @@ list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
 
 # Wrap version for gtsam_unstable
-if (GTSAM_INSTALL_MATLAB_TOOLBOX)
+if (GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
     # Set up codegen
     include(GtsamMatlabWrap)
 
@@ -119,8 +119,8 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
         endif()
 
     # Wrap
-    wrap_and_install_library(gtsam_unstable.h "gtsam" "" "${mexFlags}")
-endif(GTSAM_INSTALL_MATLAB_TOOLBOX)
+    wrap_and_install_library(gtsam_unstable.i "gtsam" "" "${mexFlags}")
+endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
 
 
 # Build examples
diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.i
similarity index 99%
rename from gtsam_unstable/gtsam_unstable.h
rename to gtsam_unstable/gtsam_unstable.i
index ef2d16bf0..05b30bb0b 100644
--- a/gtsam_unstable/gtsam_unstable.h
+++ b/gtsam_unstable/gtsam_unstable.i
@@ -12,10 +12,14 @@ class gtsam::Point2Vector;
 class gtsam::Rot2;
 class gtsam::Pose2;
 class gtsam::Point3;
+class gtsam::SO3;
+class gtsam::SO4;
+class gtsam::SOn;
 class gtsam::Rot3;
 class gtsam::Pose3;
 virtual class gtsam::noiseModel::Base;
 virtual class gtsam::noiseModel::Gaussian;
+virtual class gtsam::noiseModel::Isotropic;
 virtual class gtsam::imuBias::ConstantBias;
 virtual class gtsam::NonlinearFactor;
 virtual class gtsam::NoiseModelFactor;
@@ -39,6 +43,7 @@ class gtsam::KeyVector;
 class gtsam::LevenbergMarquardtParams;
 class gtsam::ISAM2Params;
 class gtsam::GaussianDensity;
+class gtsam::LevenbergMarquardtOptimizer;
 
 namespace gtsam {
 
@@ -282,7 +287,6 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
   void serializable() const; // enabling serialization functionality
 };
 
-
 #include <gtsam/slam/BetweenFactor.h>
 template<T = {gtsam::PoseRTV}>
 virtual class BetweenFactor : gtsam::NoiseModelFactor {
diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h
index d551209c9..43607ac41 100644
--- a/gtsam_unstable/slam/BetweenFactorEM.h
+++ b/gtsam_unstable/slam/BetweenFactorEM.h
@@ -114,7 +114,7 @@ public:
   /** implement functions needed to derive from Factor */
 
   /* ************************************************************************* */
-  virtual double error(const Values& x) const {
+  double error(const Values &x) const override {
     return whitenedError(x).squaredNorm();
   }
 
@@ -125,8 +125,7 @@ public:
    * 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>();
diff --git a/gtsam_unstable/timing/process_shonan_timing_results.py b/gtsam_unstable/timing/process_shonan_timing_results.py
new file mode 100644
index 000000000..9cf934dba
--- /dev/null
+++ b/gtsam_unstable/timing/process_shonan_timing_results.py
@@ -0,0 +1,215 @@
+"""
+Process timing results from timeShonanAveraging
+"""
+
+import xlrd
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.ticker import FuncFormatter
+import heapq
+from collections import Counter
+
+def make_combined_plot(name, p_values, times, costs, min_cost_range=10):
+    """ Make a plot that combines timing and SO(3) cost.
+        Arguments:
+            name: string of the plot title
+            p_values: list of p-values (int)
+            times: list of timings (seconds)
+            costs: list of costs (double)
+        Will calculate the range of the costs, default minimum range = 10.0
+    """
+    min_cost = min(costs)
+    cost_range = max(max(costs)-min_cost,min_cost_range)
+    fig = plt.figure()
+    ax1 = fig.add_subplot(111)
+    ax1.plot(p_values, times, label="time")
+    ax1.set_ylabel('Time used to optimize \ seconds')
+    ax1.set_xlabel('p_value')
+    ax2 = ax1.twinx()
+    ax2.plot(p_values, costs, 'r', label="cost")
+    ax2.set_ylabel('Cost at SO(3) form')
+    ax2.set_xlabel('p_value')
+    ax2.set_xticks(p_values)
+    ax2.set_ylim(min_cost, min_cost + cost_range)
+    plt.title(name, fontsize=12)
+    ax1.legend(loc="upper left")
+    ax2.legend(loc="upper right")
+    plt.interactive(False)
+    plt.show()
+
+def make_convergence_plot(name, p_values, times, costs, iter=10):
+    """ Make a bar that show the success rate for each p_value according to whether the SO(3) cost converges
+        Arguments:
+            name: string of the plot title
+            p_values: list of p-values (int)
+            times: list of timings (seconds)
+            costs: list of costs (double)
+            iter: int of iteration number for each p_value
+    """
+    
+    max_cost = np.mean(np.array(heapq.nlargest(iter, costs)))
+    # calculate mean costs for each p value
+    p_values = list(dict(Counter(p_values)).keys())
+    # make sure the iter number 
+    iter = int(len(times)/len(p_values))
+    p_mean_cost = [np.mean(np.array(costs[i*iter:(i+1)*iter])) for i in range(len(p_values))]
+    p_max = p_values[p_mean_cost.index(max(p_mean_cost))]
+    # print(p_mean_cost)
+    # print(p_max)
+
+    #take mean and make the combined plot
+    mean_times = []
+    mean_costs = []
+    for p in p_values:
+        costs_tmp = costs[p_values.index(p)*iter:(p_values.index(p)+1)*iter]
+        mean_cost = sum(costs_tmp)/len(costs_tmp)
+        mean_costs.append(mean_cost)
+        times_tmp = times[p_values.index(p)*iter:(p_values.index(p)+1)*iter]
+        mean_time = sum(times_tmp)/len(times_tmp)
+        mean_times.append(mean_time)
+    make_combined_plot(name, p_values,mean_times, mean_costs)
+
+    # calculate the convergence rate for each p_value
+    p_success_rates = []
+    if p_mean_cost[0] >= 0.95*np.mean(np.array(costs)) and p_mean_cost[0] <= 1.05*np.mean(np.array(costs)):
+        p_success_rates = [ 1.0 for p in p_values]
+    else:
+        for p in p_values:
+            if p > p_max:
+                p_costs = costs[p_values.index(p)*iter:(p_values.index(p)+1)*iter]
+                # print(p_costs)
+                converged = [ int(p_cost < 0.3*max_cost) for p_cost in p_costs]
+                success_rate = sum(converged)/len(converged)    
+                p_success_rates.append(success_rate)
+            else:
+                p_success_rates.append(0)
+
+    plt.bar(p_values, p_success_rates, align='center', alpha=0.5)
+    plt.xticks(p_values)
+    plt.yticks(np.arange(0, 1.2, 0.2), ['0%', '20%', '40%', '60%', '80%', '100%'])
+    plt.xlabel("p_value")
+    plt.ylabel("success rate")
+    plt.title(name, fontsize=12)
+    plt.show()
+
+def make_eigen_and_bound_plot(name, p_values, times1, costPs, cost3s, times2, min_eigens, subounds):
+    """ Make a plot that combines time for optimizing, time for optimizing and compute min_eigen,
+        min_eigen, subound (subound = (f_R - f_SDP) / f_SDP).
+        Arguments:
+            name: string of the plot title
+            p_values: list of p-values (int)
+            times1: list of timings (seconds)
+            costPs: f_SDP
+            cost3s: f_R
+            times2: list of timings (seconds)
+            min_eigens: list of min_eigen (double)
+            subounds: list of subound (double)
+    """
+    
+    if dict(Counter(p_values))[5] != 1:
+        p_values = list(dict(Counter(p_values)).keys())
+        iter = int(len(times1)/len(p_values))
+        p_mean_times1 = [np.mean(np.array(times1[i*iter:(i+1)*iter])) for i in range(len(p_values))]
+        p_mean_times2 = [np.mean(np.array(times2[i*iter:(i+1)*iter])) for i in range(len(p_values))]
+        print("p_values \n", p_values)
+        print("p_mean_times_opti \n", p_mean_times1)
+        print("p_mean_times_eig \n", p_mean_times2)
+
+        p_mean_costPs = [np.mean(np.array(costPs[i*iter:(i+1)*iter])) for i in range(len(p_values))]
+        p_mean_cost3s = [np.mean(np.array(cost3s[i*iter:(i+1)*iter])) for i in range(len(p_values))]
+        p_mean_lambdas = [np.mean(np.array(min_eigens[i*iter:(i+1)*iter])) for i in range(len(p_values))]
+
+        print("p_mean_costPs \n", p_mean_costPs)
+        print("p_mean_cost3s \n", p_mean_cost3s)
+        print("p_mean_lambdas \n", p_mean_lambdas)
+        print("*******************************************************************************************************************")
+
+
+    else:
+        plt.figure(1)
+        plt.ylabel('Time used (seconds)')
+        plt.xlabel('p_value')
+        plt.plot(p_values, times1, 'r', label="time for optimizing")
+        plt.plot(p_values, times2, 'blue', label="time for optimizing and check")
+        plt.title(name, fontsize=12)
+        plt.legend(loc="best")
+        plt.interactive(False)
+        plt.show()
+
+        plt.figure(2)
+        plt.ylabel('Min eigen_value')
+        plt.xlabel('p_value')
+        plt.plot(p_values, min_eigens, 'r', label="min_eigen values")
+        plt.title(name, fontsize=12)
+        plt.legend(loc="best")
+        plt.interactive(False)
+        plt.show()
+
+        plt.figure(3)
+        plt.ylabel('sub_bounds')
+        plt.xlabel('p_value')
+        plt.plot(p_values, subounds, 'blue', label="sub_bounds")
+        plt.title(name, fontsize=12)
+        plt.legend(loc="best")
+        plt.show()
+
+# Process arguments and call plot function
+import argparse
+import csv
+import os
+
+parser = argparse.ArgumentParser()
+parser.add_argument("path")
+args = parser.parse_args()
+
+
+file_path = []
+domain = os.path.abspath(args.path)
+for info in os.listdir(args.path):
+    file_path.append(os.path.join(domain, info))
+file_path.sort()
+print(file_path)
+
+
+# name of all the plots
+names = {}
+names[0] = 'tinyGrid3D vertex = 9, edge = 11'
+names[1] = 'smallGrid3D vertex = 125, edge = 297'
+names[2] = 'parking-garage vertex = 1661, edge = 6275'
+names[3] = 'sphere2500 vertex = 2500, edge = 4949'
+# names[4] = 'sphere_bignoise vertex = 2200, edge = 8647'
+names[5] = 'torus3D vertex = 5000, edge = 9048'
+names[6] = 'cubicle vertex = 5750, edge = 16869'
+names[7] = 'rim vertex = 10195, edge = 29743'
+
+# Parse CSV file
+for key, name in names.items():
+    print(key, name)
+
+    # find  according file to process
+    name_file = None
+    for path in file_path:
+        if name[0:3] in path:
+            name_file = path
+    if name_file == None:
+        print("The file %s is not in the path" % name)
+        continue
+
+    p_values, times1, costPs, cost3s, times2, min_eigens, subounds = [],[],[],[],[],[],[]
+    with open(name_file) as csvfile:
+        reader = csv.reader(csvfile, delimiter='\t')
+        for row in reader:
+            print(row)
+            p_values.append(int(row[0]))
+            times1.append(float(row[1]))
+            costPs.append(float(row[2]))
+            cost3s.append(float(row[3]))
+            if len(row) > 4:
+                times2.append(float(row[4]))
+                min_eigens.append(float(row[5]))
+                subounds.append(float(row[6]))
+
+    #plot
+    # make_combined_plot(name, p_values, times1, cost3s)
+    # make_convergence_plot(name, p_values, times1, cost3s)
+    make_eigen_and_bound_plot(name, p_values, times1, costPs, cost3s, times2, min_eigens, subounds)
diff --git a/gtsam_unstable/timing/timeShonanAveraging.cpp b/gtsam_unstable/timing/timeShonanAveraging.cpp
new file mode 100644
index 000000000..795961aef
--- /dev/null
+++ b/gtsam_unstable/timing/timeShonanAveraging.cpp
@@ -0,0 +1,182 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testShonanAveraging.cpp
+ * @date   September 2019
+ * @author Jing Wu
+ * @author Frank Dellaert
+ * @brief  Timing script for Shonan Averaging algorithm
+ */
+
+#include "gtsam/base/Matrix.h"
+#include "gtsam/base/Vector.h"
+#include "gtsam/geometry/Point3.h"
+#include "gtsam/geometry/Rot3.h"
+#include <gtsam/base/timing.h>
+#include <gtsam/sfm/ShonanAveraging.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <map>
+
+using namespace std;
+using namespace gtsam;
+
+// string g2oFile = findExampleDataFile("toyExample.g2o");
+
+// save a single line of timing info to an output stream
+void saveData(size_t p, double time1, double costP, double cost3, double time2,
+              double min_eigenvalue, double suBound, std::ostream* os) {
+    *os << (int)p << "\t" << time1 << "\t" << costP << "\t" << cost3 << "\t"
+        << time2 << "\t" << min_eigenvalue << "\t" << suBound << endl;
+}
+
+void checkR(const Matrix& R) {
+    Matrix R2 = R.transpose();
+    Matrix actual_R = R2 * R;
+    assert_equal(Rot3(),Rot3(actual_R));
+}
+
+void saveResult(string name, const Values& values) {
+    ofstream myfile;
+    myfile.open("shonan_result_of_" + name + ".dat");
+    size_t nrSO3 = values.count<SO3>();
+    myfile << "#Type SO3 Number " << nrSO3 << "\n";
+    for (int i = 0; i < nrSO3; ++i) {
+        Matrix R = values.at<SO3>(i).matrix();
+        // Check if the result of R.Transpose*R satisfy orthogonal constraint
+        checkR(R);
+        myfile << i;
+        for (int m = 0; m < 3; ++m) {
+            for (int n = 0; n < 3; ++n) {
+                myfile << " " << R(m, n);
+            }
+        }
+        myfile << "\n";
+    }
+    myfile.close();
+    cout << "Saved shonan_result.dat file" << endl;
+}
+
+void saveG2oResult(string name, const Values& values, std::map<Key, Pose3> poses) {
+    ofstream myfile;
+    myfile.open("shonan_result_of_" + name + ".g2o");
+    size_t nrSO3 = values.count<SO3>();
+    for (int i = 0; i < nrSO3; ++i) {
+        Matrix R = values.at<SO3>(i).matrix();
+        // Check if the result of R.Transpose*R satisfy orthogonal constraint
+        checkR(R);
+        myfile << "VERTEX_SE3:QUAT" << " ";
+        myfile << i << " ";
+        myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " ";
+        Vector quaternion = Rot3(R).quaternion();
+        myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1) << " " << quaternion(0);
+        myfile << "\n";
+    }
+    myfile.close();
+    cout << "Saved shonan_result.g2o file" << endl;
+}
+
+void saveResultQuat(const Values& values) {
+    ofstream myfile;
+    myfile.open("shonan_result.dat");
+    size_t nrSOn = values.count<SOn>();
+    for (int i = 0; i < nrSOn; ++i) {
+        GTSAM_PRINT(values.at<SOn>(i));
+        Rot3 R = Rot3(values.at<SOn>(i).matrix());
+        float x = R.toQuaternion().x();
+        float y = R.toQuaternion().y();
+        float z = R.toQuaternion().z();
+        float w = R.toQuaternion().w();
+        myfile << "QuatSO3 " << i;
+        myfile << "QuatSO3 " << i << " " << w << " " << x << " " << y << " " << z << "\n";
+        myfile.close();
+    }
+}
+
+int main(int argc, char* argv[]) {
+    // primitive argument parsing:
+    if (argc > 3) {
+        throw runtime_error("Usage: timeShonanAveraging  [g2oFile]");
+    }
+
+    string g2oFile;
+    try {
+        if (argc > 1)
+            g2oFile = argv[argc - 1];
+        else
+            g2oFile = string(
+                "/home/jingwu/git/SESync/data/sphere2500.g2o");
+
+    } catch (const exception& e) {
+        cerr << e.what() << '\n';
+        exit(1);
+    }
+
+    // Create a csv output file
+    size_t pos1 = g2oFile.find("data/");
+    size_t pos2 = g2oFile.find(".g2o");
+    string name = g2oFile.substr(pos1 + 5, pos2 - pos1 - 5);
+    cout << name << endl;
+    ofstream csvFile("shonan_timing_of_" + name + ".csv");
+
+    // Create Shonan averaging instance from the file.
+    // ShonanAveragingParameters parameters;
+    // double sigmaNoiseInRadians = 0 * M_PI / 180;
+    // parameters.setNoiseSigma(sigmaNoiseInRadians);
+    static const ShonanAveraging3 kShonan(g2oFile);
+
+    // increase p value and try optimize using Shonan Algorithm. use chrono for
+    // timing
+    size_t pMin = 3;
+    Values Qstar;
+    Vector minEigenVector;
+    double CostP = 0, Cost3 = 0, lambdaMin = 0, suBound = 0;
+    cout << "(int)p" << "\t" << "time1" << "\t" << "costP" << "\t" << "cost3" << "\t"
+        << "time2" << "\t" << "MinEigenvalue" << "\t" << "SuBound" << endl;
+
+    const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
+
+    for (size_t p = pMin; p < 6; p++) {
+        // Randomly initialize at lowest level, initialize by line search after that
+        const Values initial =
+            (p > pMin) ? kShonan.initializeWithDescent(p, Qstar, minEigenVector,
+                                                       lambdaMin)
+                       : ShonanAveraging::LiftTo<Rot3>(pMin, randomRotations);
+        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
+        // optimizing
+        const Values result = kShonan.tryOptimizingAt(p, initial);
+        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
+        chrono::duration<double> timeUsed1 =
+            chrono::duration_cast<chrono::duration<double>>(t2 - t1);
+        lambdaMin = kShonan.computeMinEigenValue(result, &minEigenVector);
+        chrono::steady_clock::time_point t3 = chrono::steady_clock::now();
+        chrono::duration<double> timeUsed2 =
+            chrono::duration_cast<chrono::duration<double>>(t3 - t1);
+        Qstar = result;
+        CostP = kShonan.costAt(p, result);
+        const Values SO3Values = kShonan.roundSolution(result);
+        Cost3 = kShonan.cost(SO3Values);
+        suBound = (Cost3 - CostP) / CostP;
+
+        saveData(p, timeUsed1.count(), CostP, Cost3, timeUsed2.count(),
+                 lambdaMin, suBound, &cout);
+        saveData(p, timeUsed1.count(), CostP, Cost3, timeUsed2.count(),
+                 lambdaMin, suBound, &csvFile);
+    }
+    saveResult(name, kShonan.roundSolution(Qstar));
+    // saveG2oResult(name, kShonan.roundSolution(Qstar), kShonan.Poses());
+    return 0;
+}
diff --git a/matlab/+gtsam/Point2.m b/matlab/+gtsam/Point2.m
new file mode 100644
index 000000000..3ea65c33e
--- /dev/null
+++ b/matlab/+gtsam/Point2.m
@@ -0,0 +1,12 @@
+function pt = Point2(varargin)
+    % Point2 shim
+    if nargin == 2 && isa(varargin{1}, 'double')
+        pt = [varargin{1} varargin{2}]';
+    elseif nargin == 1
+        pt = varargin{1};
+    elseif nargin == 0
+        pt = [0 0]';
+    else
+        error('Arguments do not match any overload of Point2 shim');
+    end
+end
\ No newline at end of file
diff --git a/matlab/+gtsam/Point3.m b/matlab/+gtsam/Point3.m
new file mode 100644
index 000000000..5f66b4517
--- /dev/null
+++ b/matlab/+gtsam/Point3.m
@@ -0,0 +1,12 @@
+function pt = Point3(varargin)
+    % Point3 shim
+    if nargin == 3 && isa(varargin{1}, 'double')
+        pt = [varargin{1} varargin{2} varargin{3}]';
+    elseif nargin == 1
+        pt = varargin{1};
+    elseif nargin == 0
+        pt = [0 0 0]';
+    else
+        error('Arguments do not match any overload of Point3 shim');
+    end
+end
\ No newline at end of file
diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m
index 2b913b52d..697a57faa 100644
--- a/matlab/+gtsam/cylinderSampleProjection.m
+++ b/matlab/+gtsam/cylinderSampleProjection.m
@@ -50,9 +50,9 @@ for i = 1:cylinderNum
         visible = true;
         for k = 1:cylinderNum
 
-            rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
-            rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
-            rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
+            rayCameraToPoint = pose.translation().between(sampledPoint3);
+            rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid);
+            rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3);
 
             % Condition 1: all points in front of the cylinders'
             % surfaces are visible
diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m
index 10409ac6d..58b4140fd 100644
--- a/matlab/+gtsam/cylinderSampleProjectionStereo.m
+++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m
@@ -25,20 +25,20 @@ for i = 1:cylinderNum
         % For Cheirality Exception
         sampledPoint3 = cylinders{i}.Points{j};
         sampledPoint3local = pose.transformTo(sampledPoint3);
-        if sampledPoint3local.z < 0
+        if sampledPoint3local(3) < 0
             continue; 
         end
         
         % measurements 
-        Z.du = K.fx() * K.baseline() / sampledPoint3local.z;  
-        Z.uL = K.fx() * sampledPoint3local.x / sampledPoint3local.z + K.px();
+        Z.du = K.fx() * K.baseline() / sampledPoint3local(3);  
+        Z.uL = K.fx() * sampledPoint3local(1) / sampledPoint3local(3) + K.px();
         Z.uR = Z.uL + Z.du;
-        Z.v = K.fy() / sampledPoint3local.z + K.py();
+        Z.v = K.fy() / sampledPoint3local(3) + K.py();
 
         % ignore points not visible in the scene
-        if Z.uL < 0 || Z.uL >= imageSize.x || ...
-                Z.uR < 0 || Z.uR >= imageSize.x || ...
-                Z.v < 0 || Z.v >= imageSize.y 
+        if Z.uL < 0 || Z.uL >= imageSize(1) || ...
+                Z.uR < 0 || Z.uR >= imageSize(1) || ...
+                Z.v < 0 || Z.v >= imageSize(2) 
             continue;       
         end            
 
@@ -54,9 +54,9 @@ for i = 1:cylinderNum
         visible = true;
         for k = 1:cylinderNum
 
-            rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
-            rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
-            rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
+            rayCameraToPoint = sampledPoint3 - pose.translation();
+            rayCameraToCylinder = cylinders{k}.centroid - pose.translation();
+            rayCylinderToPoint = sampledPoint3 - cylinders{k}.centroid;
 
             % Condition 1: all points in front of the cylinders'
             % surfaces are visible
diff --git a/matlab/+gtsam/cylinderSampling.m b/matlab/+gtsam/cylinderSampling.m
index dcaa9c721..dc76295fa 100644
--- a/matlab/+gtsam/cylinderSampling.m
+++ b/matlab/+gtsam/cylinderSampling.m
@@ -12,8 +12,8 @@ function [cylinder] = cylinderSampling(baseCentroid, radius, height, density)
     % sample the points
     for i = 1:pointsNum
         theta = 2 * pi * rand;
-        x = radius * cos(theta) + baseCentroid.x;
-        y = radius * sin(theta) + baseCentroid.y;
+        x = radius * cos(theta) + baseCentroid(1);
+        y = radius * sin(theta) + baseCentroid(2);
         z = height * rand;
         points3{i,1} = Point3([x,y,z]');
     end
@@ -22,5 +22,5 @@ function [cylinder] = cylinderSampling(baseCentroid, radius, height, density)
     cylinder.radius = radius;
     cylinder.height = height;
     cylinder.Points = points3;
-    cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2);
+    cylinder.centroid = Point3(baseCentroid(1), baseCentroid(2), height/2);
 end
\ No newline at end of file
diff --git a/matlab/+gtsam/plotCamera.m b/matlab/+gtsam/plotCamera.m
index d0d1f45b7..986cd9a68 100644
--- a/matlab/+gtsam/plotCamera.m
+++ b/matlab/+gtsam/plotCamera.m
@@ -1,7 +1,7 @@
 function plotCamera(pose, axisLength)
 	hold on
 
-    C = pose.translation().vector();
+    C = pose.translation();
     R = pose.rotation().matrix();
     
     xAxis = C+R(:,1)*axisLength;
diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m
index 5d4a287c4..202f2409b 100644
--- a/matlab/+gtsam/plotFlyingResults.m
+++ b/matlab/+gtsam/plotFlyingResults.m
@@ -13,7 +13,7 @@ set(gcf, 'Position', [80,1,1800,1000]);
 %% plot all the cylinders and sampled points
 
 axis equal
-axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
+axis([0, options.fieldSize(1), 0, options.fieldSize(2), 0, options.height + 30]);
 xlabel('X (m)');
 ylabel('Y (m)');
 zlabel('Height (m)');
@@ -50,8 +50,8 @@ for i = 1:cylinderNum
     
     [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
 
-    X = X + cylinders{i}.centroid.x;
-    Y = Y + cylinders{i}.centroid.y;
+    X = X + cylinders{i}.centroid(1);
+    Y = Y + cylinders{i}.centroid(2);
     Z = Z * cylinders{i}.height;
 
     h_cylinder{i} = surf(X,Y,Z);
@@ -76,7 +76,7 @@ for i = 1:posesSize
     %plotCamera(poses{i}, 3);
     
     gRp = poses{i}.rotation().matrix();  % rotation from pose to global
-    C = poses{i}.translation().vector();
+    C = poses{i}.translation();
     axisLength = 3;
     
     xAxis = C+gRp(:,1)*axisLength;
@@ -111,14 +111,14 @@ for i = 1:posesSize
     for j = 1:pointSize
         if ~isempty(pts3d{j}.cov{i})
             hold on
-            h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z);
-            h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], ...
+            h_point{j} = plot3(pts3d{j}.data(1), pts3d{j}.data(2), pts3d{j}.data(3));
+            h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data(1); pts3d{j}.data(2); pts3d{j}.data(3)], ...
                 pts3d{j}.cov{i}, options.plot.covarianceScale);
         end
     end 
     
     axis equal
-    axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
+    axis([0, options.fieldSize(1), 0, options.fieldSize(2), 0, options.height + 30]);
 
     drawnow
     
@@ -158,7 +158,7 @@ for i = 1 : posesSize
     hold on
     
     campos([poses{i}.x, poses{i}.y, poses{i}.z]);
-    camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]);
+    camtarget([options.fieldSize(1)/2, options.fieldSize(2)/2, 0]);
     camlight(hlight, 'headlight');
     
     if options.writeVideo
diff --git a/matlab/+gtsam/plotPoint2.m b/matlab/+gtsam/plotPoint2.m
index cd066951d..8d10ecab6 100644
--- a/matlab/+gtsam/plotPoint2.m
+++ b/matlab/+gtsam/plotPoint2.m
@@ -1,10 +1,10 @@
 function plotPoint2(p,color,P)
 % plotPoint2 shows a Point2, possibly with covariance matrix
 if size(color,2)==1
-    plot(p.x,p.y,[color '*']);
+    plot(p(1),p(2),[color '*']);
 else
-    plot(p.x,p.y,color);
+    plot(p(1),p(2),color);
 end
 if exist('P', 'var') && (~isempty(P))
-    gtsam.covarianceEllipse([p.x;p.y],P,color(1));
+    gtsam.covarianceEllipse([p(1);p(2)],P,color(1));
 end
\ No newline at end of file
diff --git a/matlab/+gtsam/plotPoint3.m b/matlab/+gtsam/plotPoint3.m
index 390b44939..85c84a210 100644
--- a/matlab/+gtsam/plotPoint3.m
+++ b/matlab/+gtsam/plotPoint3.m
@@ -1,12 +1,12 @@
 function plotPoint3(p, color, P)
 %PLOTPOINT3 Plot a Point3 with an optional covariance matrix
 if size(color,2)==1
-    plot3(p.x,p.y,p.z,[color '*']);
+    plot3(p(1),p(2),p(3),[color '*']);
 else
-    plot3(p.x,p.y,p.z,color);
+    plot3(p(1),p(2),p(3),color);
 end
 if exist('P', 'var')
-    gtsam.covarianceEllipse3D([p.x;p.y;p.z],P);
+    gtsam.covarianceEllipse3D([p(1);p(2);p(3)],P);
 end
 
 end
diff --git a/matlab/+gtsam/plotPose3.m b/matlab/+gtsam/plotPose3.m
index 8c3c7dd76..258e3f776 100644
--- a/matlab/+gtsam/plotPose3.m
+++ b/matlab/+gtsam/plotPose3.m
@@ -4,7 +4,7 @@ if nargin<3,axisLength=0.1;end
 
 % get rotation and translation (center)
 gRp = pose.rotation().matrix();  % rotation from pose to global
-C = pose.translation().vector();
+C = pose.translation();
 
 if ~isempty(axisLength)
     % draw the camera axes
diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m
index 60c9f1295..04cddb7f7 100644
--- a/matlab/+gtsam/points2DTrackStereo.m
+++ b/matlab/+gtsam/points2DTrackStereo.m
@@ -38,7 +38,7 @@ graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
 %% initialize graph and values
 initialEstimate = Values;
 for i = 1:pointsNum
-    point_j = points3d{i}.data.retract(0.05*randn(3,1));
+    point_j = points3d{i}.data + (0.05*randn(3,1));
     initialEstimate.insert(symbol('p', i), point_j);    
 end
 
diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m
index add2bc75a..a0dfef22a 100644
--- a/matlab/gtsam_examples/CameraFlyingExample.m
+++ b/matlab/gtsam_examples/CameraFlyingExample.m
@@ -46,7 +46,7 @@ options.camera.horizon = 60;
 % camera baseline
 options.camera.baseline = 0.05;
 % camera focal length
-options.camera.f = round(options.camera.resolution.x * options.camera.horizon / ...
+options.camera.f = round(options.camera.resolution(1) * options.camera.horizon / ...
     options.camera.fov);
 % camera focal baseline
 options.camera.fB = options.camera.f * options.camera.baseline;
@@ -54,15 +54,15 @@ options.camera.fB = options.camera.f * options.camera.baseline;
 options.camera.disparity = options.camera.fB / options.camera.horizon;
 % Monocular Camera Calibration
 options.camera.monoK = Cal3_S2(options.camera.f, options.camera.f, 0, ...
-    options.camera.resolution.x/2, options.camera.resolution.y/2);
+    options.camera.resolution(1)/2, options.camera.resolution(2)/2);
 % Stereo Camera Calibration
 options.camera.stereoK = Cal3_S2Stereo(options.camera.f, options.camera.f, 0, ...
-    options.camera.resolution.x/2, options.camera.resolution.y/2, options.camera.disparity);
+    options.camera.resolution(1)/2, options.camera.resolution(2)/2, options.camera.disparity);
 
 % write video output
 options.writeVideo = true;
 % the testing field size (unit: meter)
-options.fieldSize = Point2([100, 100]');
+options.fieldSize = Point2(100, 100);
 % camera flying speed (unit: meter / second)
 options.speed = 20;
 % camera flying height
@@ -113,14 +113,14 @@ theta = 0;
 i = 1;
 while i <= cylinderNum
     theta = theta + 2*pi/10;
-    x = 40 * rand * cos(theta) + options.fieldSize.x/2;
-    y = 20 * rand * sin(theta) + options.fieldSize.y/2;
-    baseCentroid{i} = Point2([x, y]');
+    x = 40 * rand * cos(theta) + options.fieldSize(1)/2;
+    y = 20 * rand * sin(theta) + options.fieldSize(2)/2;
+    baseCentroid{i} = Point2(x, y);
     
     % prevent two cylinders interact with each other
     regenerate = false;
     for j = 1:i-1
-        if i > 1 && baseCentroid{i}.dist(baseCentroid{j}) < options.cylinder.radius * 2
+        if i > 1 && norm(baseCentroid{i} - baseCentroid{j}) < options.cylinder.radius * 2
             regenerate = true;
             break;
         end
@@ -146,17 +146,17 @@ while 1
     angle = 0.1*pi*(rand-0.5);
     inc_t = Point3(distance * cos(angle), ...
         distance * sin(angle), 0);
-    t = t.compose(inc_t);
+    t = t + inc_t;
     
-    if t.x > options.fieldSize.x - 10 || t.y > options.fieldSize.y - 10;
+    if t(1) > options.fieldSize(1) - 10 || t(2) > options.fieldSize(2) - 10;
         break;
     end
     
     %t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ...
     %    15, 10]');
     camera = PinholeCameraCal3_S2.Lookat(t, ... 
-        Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
-        Point3([0,0,1]'), options.camera.monoK);    
+        Point3(options.fieldSize(1)/2, options.fieldSize(2)/2, 0), ...
+        Point3(0,0,1), options.camera.monoK);    
     cameraPoses{end+1} = camera.pose;
 end
 
diff --git a/matlab/gtsam_examples/MonocularVOExample.m b/matlab/gtsam_examples/MonocularVOExample.m
index 11c4253de..0d09a1487 100644
--- a/matlab/gtsam_examples/MonocularVOExample.m
+++ b/matlab/gtsam_examples/MonocularVOExample.m
@@ -15,14 +15,14 @@ import gtsam.*
 
 %% Create two cameras and corresponding essential matrix E
 aRb = Rot3.Yaw(pi/2);
-aTb = Point3 (0.1, 0, 0);
+aTb = [.1, 0, 0]';
 identity=Pose3;
 aPb = Pose3(aRb, aTb);
 cameraA = CalibratedCamera(identity);
 cameraB = CalibratedCamera(aPb);
 
 %% Create 5 points
-P = { Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5) };
+P = { [0, 0, 1]', [-0.1, 0, 1]', [0.1, 0, 1]', [0, 0.5, 0.5]', [0, -0.5, 0.5]' };
 
 %% Project points in both cameras
 for i=1:5
diff --git a/matlab/gtsam_examples/PlanarSLAMExample.m b/matlab/gtsam_examples/PlanarSLAMExample.m
index aec933d74..3febc23e6 100644
--- a/matlab/gtsam_examples/PlanarSLAMExample.m
+++ b/matlab/gtsam_examples/PlanarSLAMExample.m
@@ -71,9 +71,12 @@ marginals = Marginals(graph, result);
 plot2DTrajectory(result, [], marginals);
 plot2DPoints(result, 'b', marginals);
 
-plot([result.atPose2(i1).x; result.atPoint2(j1).x],[result.atPose2(i1).y; result.atPoint2(j1).y], 'c-');
-plot([result.atPose2(i2).x; result.atPoint2(j1).x],[result.atPose2(i2).y; result.atPoint2(j1).y], 'c-');
-plot([result.atPose2(i3).x; result.atPoint2(j2).x],[result.atPose2(i3).y; result.atPoint2(j2).y], 'c-');
+p_j1 = result.atPoint2(j1);
+p_j2 = result.atPoint2(j2);
+
+plot([result.atPose2(i1).x; p_j1(1)],[result.atPose2(i1).y; p_j1(2)], 'c-');
+plot([result.atPose2(i2).x; p_j1(1)],[result.atPose2(i2).y; p_j1(2)], 'c-');
+plot([result.atPose2(i3).x; p_j2(1)],[result.atPose2(i3).y; p_j2(2)], 'c-');
 axis([-0.6 4.8 -1 1])
 axis equal
 view(2)
diff --git a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m
index 375ed645c..93979a68a 100644
--- a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m
+++ b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m
@@ -60,15 +60,18 @@ for j=1:2
     S{j}=chol(Q{j}); % for sampling
 end
 
-plot([sample.atPose2(i1).x; sample.atPoint2(j1).x],[sample.atPose2(i1).y; sample.atPoint2(j1).y], 'c-');
-plot([sample.atPose2(i2).x; sample.atPoint2(j1).x],[sample.atPose2(i2).y; sample.atPoint2(j1).y], 'c-');
-plot([sample.atPose2(i3).x; sample.atPoint2(j2).x],[sample.atPose2(i3).y; sample.atPoint2(j2).y], 'c-');
+p_j1 = sample.atPoint2(j1);
+p_j2 = sample.atPoint2(j2);
+
+plot([sample.atPose2(i1).x; p_j1(1)],[sample.atPose2(i1).y; p_j1(2)], 'c-');
+plot([sample.atPose2(i2).x; p_j1(1)],[sample.atPose2(i2).y; p_j1(2)], 'c-');
+plot([sample.atPose2(i3).x; p_j2(1)],[sample.atPose2(i3).y; p_j2(2)], 'c-');
 view(2); axis auto; axis equal
 
 %% Do Sampling on point 2
 N=1000;
 for s=1:N
     delta = S{2}*randn(2,1);
-    proposedPoint = Point2(point{2}.x+delta(1),point{2}.y+delta(2));
+    proposedPoint = Point2(point{2} + delta);
     plotPoint2(proposedPoint,'k.')
 end
\ No newline at end of file
diff --git a/matlab/gtsam_examples/Pose2SLAMwSPCG.m b/matlab/gtsam_examples/Pose2SLAMwSPCG.m
index 67f22fe1d..2df7efe2f 100644
--- a/matlab/gtsam_examples/Pose2SLAMwSPCG.m
+++ b/matlab/gtsam_examples/Pose2SLAMwSPCG.m
@@ -54,7 +54,7 @@ initialEstimate.print(sprintf('\nInitial estimate:\n'));
 %% Optimize using Levenberg-Marquardt optimization with SubgraphSolver
 params = gtsam.LevenbergMarquardtParams;
 subgraphParams = gtsam.SubgraphSolverParameters;
-params.setLinearSolverType('CONJUGATE_GRADIENT');
+params.setLinearSolverType('ITERATIVE');
 params.setIterativeParams(subgraphParams);
 optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate);
 result = optimizer.optimize();
diff --git a/matlab/gtsam_examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m
index 584ace09a..f94a2ee4e 100644
--- a/matlab/gtsam_examples/SBAExample.m
+++ b/matlab/gtsam_examples/SBAExample.m
@@ -47,7 +47,7 @@ end
 %% Add Gaussian priors for a pose and a landmark to constrain the system
 cameraPriorNoise  = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas);
 firstCamera = PinholeCameraCal3_S2(truth.cameras{1}.pose, truth.K);
-graph.add(PriorFactorSimpleCamera(symbol('c',1), firstCamera, cameraPriorNoise));
+graph.add(PriorFactorPinholeCameraCal3_S2(symbol('c',1), firstCamera, cameraPriorNoise));
 
 pointPriorNoise  = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
 graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise));
@@ -64,7 +64,7 @@ for i=1:size(truth.cameras,2)
     initialEstimate.insert(symbol('c',i), camera_i);
 end
 for j=1:size(truth.points,2)
-    point_j = Point3(truth.points{j}.vector() + 0.1*randn(3,1));
+    point_j = Point3(truth.points{j} + 0.1*randn(3,1));
     initialEstimate.insert(symbol('p',j), point_j);
 end
 initialEstimate.print(sprintf('\nInitial estimate:\n  '));
diff --git a/matlab/gtsam_examples/SFMExample.m b/matlab/gtsam_examples/SFMExample.m
index 6700e90d2..a57da929c 100644
--- a/matlab/gtsam_examples/SFMExample.m
+++ b/matlab/gtsam_examples/SFMExample.m
@@ -58,7 +58,7 @@ for i=1:size(truth.cameras,2)
     initialEstimate.insert(symbol('x',i), pose_i);
 end
 for j=1:size(truth.points,2)
-    point_j = Point3(truth.points{j}.vector() + 0.1*randn(3,1));
+    point_j = Point3(truth.points{j} + 0.1*randn(3,1));
     initialEstimate.insert(symbol('p',j), point_j);
 end
 initialEstimate.print(sprintf('\nInitial estimate:\n  '));
diff --git a/matlab/gtsam_examples/StereoVOExample.m b/matlab/gtsam_examples/StereoVOExample.m
index b437ca80c..e7345fcf2 100644
--- a/matlab/gtsam_examples/StereoVOExample.m
+++ b/matlab/gtsam_examples/StereoVOExample.m
@@ -51,10 +51,10 @@ graph.add(GenericStereoFactor3D(StereoPoint2(320, 270, 115), stereo_model, x2, l
 initialEstimate = Values;
 initialEstimate.insert(x1, first_pose);
 % noisy estimate for pose 2
-initialEstimate.insert(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1)));
-initialEstimate.insert(l1, Point3( 1,  1, 5));
-initialEstimate.insert(l2, Point3(-1,  1, 5));
-initialEstimate.insert(l3, Point3( 0,-.5, 5));
+initialEstimate.insert(x2, Pose3(Rot3(), [0.1, -.1, 1.1]'));
+initialEstimate.insert(l1, [ 1,  1, 5]');
+initialEstimate.insert(l2, [-1,  1, 5]');
+initialEstimate.insert(l3, [ 0,-.5, 5]');
 
 %% optimize
 fprintf(1,'Optimizing\n'); tic
diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m
index 8a0485334..ce505df5d 100644
--- a/matlab/gtsam_examples/VisualizeMEstimators.m
+++ b/matlab/gtsam_examples/VisualizeMEstimators.m
@@ -48,7 +48,7 @@ function plot_m_estimator(x, model, plot_title, fig_id, filename)
     rho = zeros(size(x));
     for i = 1:size(x, 2)
         w(i) = model.weight(x(i));
-        rho(i) = model.residual(x(i));
+        rho(i) = model.loss(x(i));
     end
 
     psi = w .* x;
diff --git a/matlab/gtsam_tests/testPlanarSLAMExample.m b/matlab/gtsam_tests/testPlanarSLAMExample.m
index d3cab5d1f..e0b4dbfc8 100644
--- a/matlab/gtsam_tests/testPlanarSLAMExample.m
+++ b/matlab/gtsam_tests/testPlanarSLAMExample.m
@@ -66,4 +66,4 @@ CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
 
 point_1 = result.atPoint2(symbol('l',1));
 marginals.marginalCovariance(symbol('l',1));
-CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4));
+CHECK('point_1.equals(Point2(2,2),1e-4)',norm(point_1 - Point2(2,2)) < 1e-4);
diff --git a/matlab/gtsam_tests/testSFMExample.m b/matlab/gtsam_tests/testSFMExample.m
index 985cbdb2c..a1f63c3a7 100644
--- a/matlab/gtsam_tests/testSFMExample.m
+++ b/matlab/gtsam_tests/testSFMExample.m
@@ -69,5 +69,5 @@ end
 
 for j=1:size(truth.points,2)
     point_j = result.atPoint3(symbol('p',j));
-    CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
+    CHECK('point_j.equals(truth.points{j},1e-5)',norm(point_j - truth.points{j}) < 1e-5)
 end
diff --git a/matlab/gtsam_tests/testStereoVOExample.m b/matlab/gtsam_tests/testStereoVOExample.m
index 218d0ace1..c721244ba 100644
--- a/matlab/gtsam_tests/testStereoVOExample.m
+++ b/matlab/gtsam_tests/testStereoVOExample.m
@@ -65,4 +65,4 @@ pose_x1 = result.atPose3(x1);
 CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4));
 
 point_l1 = result.atPoint3(l1);
-CHECK('point_1.equals(expected_l1,1e-4)',point_l1.equals(expected_l1,1e-4));
\ No newline at end of file
+CHECK('point_1.equals(expected_l1,1e-4)',norm(point_l1 - expected_l1) < 1e-4);
\ No newline at end of file
diff --git a/matlab/gtsam_tests/testValues.m b/matlab/gtsam_tests/testValues.m
index fe2cd30fe..48bc83f2c 100644
--- a/matlab/gtsam_tests/testValues.m
+++ b/matlab/gtsam_tests/testValues.m
@@ -5,8 +5,8 @@ values = Values;
 E = EssentialMatrix(Rot3,Unit3);
 tol = 1e-9;
 
-values.insert(0, Point2);
-values.insert(1, Point3);
+values.insert(0, Point2(0, 0));
+values.insert(1, Point3(0, 0, 0));
 values.insert(2, Rot2);
 values.insert(3, Pose2);
 values.insert(4, Rot3);
@@ -21,8 +21,8 @@ values.insert(10, imuBias.ConstantBias);
 values.insert(11, [1;2;3]);
 values.insert(12, [1 2;3 4]);
 
-EXPECT('at',values.atPoint2(0).equals(Point2,tol));
-EXPECT('at',values.atPoint3(1).equals(Point3,tol));
+EXPECT('at',values.atPoint2(0) == Point2(0, 0));
+EXPECT('at',values.atPoint3(1) == Point3(0, 0, 0));
 EXPECT('at',values.atRot2(2).equals(Rot2,tol));
 EXPECT('at',values.atPose2(3).equals(Pose2,tol));
 EXPECT('at',values.atRot3(4).equals(Rot3,tol));
diff --git a/matlab/gtsam_tests/testVisualISAMExample.m b/matlab/gtsam_tests/testVisualISAMExample.m
index 223e823a6..f75942ea7 100644
--- a/matlab/gtsam_tests/testVisualISAMExample.m
+++ b/matlab/gtsam_tests/testVisualISAMExample.m
@@ -51,5 +51,5 @@ end
 
 for j=1:size(truth.points,2)
     point_j = result.atPoint3(symbol('l',j));
-    CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
+    CHECK('point_j.equals(truth.points{j},1e-5)',norm(point_j - truth.points{j}) < 1e-5)
 end
diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison.m b/matlab/unstable_examples/+imuSimulator/IMUComparison.m
index 68e20bb25..871f023ef 100644
--- a/matlab/unstable_examples/+imuSimulator/IMUComparison.m
+++ b/matlab/unstable_examples/+imuSimulator/IMUComparison.m
@@ -28,19 +28,19 @@ currentVelocityGlobalIMUbody = currentVelocityGlobal;
 %% Prepare data structures for actual trajectory and estimates
 % Actual trajectory
 positions = zeros(3, length(times)+1);
-positions(:,1) = currentPoseGlobal.translation.vector;
+positions(:,1) = currentPoseGlobal.translation;
 poses(1).p = positions(:,1);
 poses(1).R = currentPoseGlobal.rotation.matrix;
 
 % Trajectory estimate (integrated in the navigation frame)
 positionsIMUnav = zeros(3, length(times)+1);
-positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector;
+positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation;
 posesIMUnav(1).p = positionsIMUnav(:,1);
 posesIMUnav(1).R = poses(1).R;
 
 % Trajectory estimate (integrated in the body frame)
 positionsIMUbody = zeros(3, length(times)+1);
-positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector;
+positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation;
 posesIMUbody(1).p = positionsIMUbody(:,1);
 posesIMUbody(1).R = poses(1).R;
 
@@ -120,9 +120,9 @@ for t = times
     currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT);
   
   %% Store data in some structure for statistics and plots
-  positions(:,i) = currentPoseGlobal.translation.vector;
-  positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector;
-  positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector;  
+  positions(:,i) = currentPoseGlobal.translation;
+  positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation;
+  positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation;
   % - 
   poses(i).p = positions(:,i);
   posesIMUbody(i).p = positionsIMUbody(:,i);
diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m
index c589bea32..450697de0 100644
--- a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m
+++ b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m
@@ -28,7 +28,7 @@ currentVelocityGlobal = velocity;
 %% Prepare data structures for actual trajectory and estimates
 % Actual trajectory
 positions = zeros(3, length(times)+1);
-positions(:,1) = currentPoseGlobal.translation.vector;
+positions(:,1) = currentPoseGlobal.translation;
 poses(1).p = positions(:,1);
 poses(1).R = currentPoseGlobal.rotation.matrix;
 
@@ -112,7 +112,7 @@ for t = times
   end
   
   %% Store data in some structure for statistics and plots
-  positions(:,i) = currentPoseGlobal.translation.vector;
+  positions(:,i) = currentPoseGlobal.translation;
   i = i + 1;
 end
 
diff --git a/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m b/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m
index c86e40a21..0d8abad2c 100644
--- a/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m
+++ b/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m
@@ -7,7 +7,7 @@ measuredOmega = omega1Body;
 
 % Acceleration measurement (in this simple toy example no other forces 
 % act on the body and the only acceleration is the centripetal Coriolis acceleration)
-measuredAcc = Point3(cross(omega1Body, velocity1Body)).vector;
+measuredAcc = Point3(cross(omega1Body, velocity1Body));
 acc_omega = [ measuredAcc; measuredOmega ];
 
 end
diff --git a/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m b/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m
index 534b9365e..5ed1fc516 100644
--- a/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m
+++ b/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m
@@ -6,16 +6,16 @@ import gtsam.*;
 
 % Calculate gyro measured rotation rate by transforming body rotation rate
 % into the IMU frame.
-measuredOmega = imuFrame.rotation.unrotate(Point3(omega1Body)).vector;
+measuredOmega = imuFrame.rotation.unrotate(Point3(omega1Body));
 
 % Transform both velocities into IMU frame, accounting for the velocity
 % induced by rigid body rotation on a lever arm (Coriolis effect).
 velocity1inertial = imuFrame.rotation.unrotate( ...
-    Point3(velocity1Body + cross(omega1Body, imuFrame.translation.vector))).vector;
+    Point3(velocity1Body + cross(omega1Body, imuFrame.translation)));
 
 imu2in1 = Rot3.Expmap(measuredOmega * deltaT);
 velocity2inertial = imu2in1.rotate(imuFrame.rotation.unrotate( ...
-    Point3(velocity2Body + cross(omega2Body, imuFrame.translation.vector)))).vector;
+    Point3(velocity2Body + cross(omega2Body, imuFrame.translation))));
 
 % Acceleration in IMU frame
 measuredAcc = (velocity2inertial - velocity1inertial) / deltaT;
diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m
index 35d27aa73..ee4deb433 100644
--- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m
+++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m
@@ -190,13 +190,13 @@ for i = 1:length(times)
         newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b));
         
         % Store data
-        positionsInFixedGT(:,1) = currentPoseFixedGT.translation.vector;
+        positionsInFixedGT(:,1) = currentPoseFixedGT.translation;
         velocityInFixedGT(:,1) = currentVelocityFixedGT;
-        positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation.vector;
-        %velocityInRotatingGT(:,1) = currentPoseRotatingGT.velocity.vector;
-        positionsEstimates(:,i) = currentPoseEstimate.translation.vector;
-        velocitiesEstimates(:,i) = currentVelocityEstimate.vector;
-        currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation.vector;
+        positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation;
+        %velocityInRotatingGT(:,1) = currentPoseRotatingGT.velocity;
+        positionsEstimates(:,i) = currentPoseEstimate.translation;
+        velocitiesEstimates(:,i) = currentVelocityEstimate;
+        currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation;
         currentRotatingFrameForPlot(1).R = currentRotatingFrame.rotation.matrix;
     else
         
@@ -204,18 +204,18 @@ for i = 1:length(times)
         % Update the position and velocity
         % x_t = x_0 + v_0*dt + 1/2*a_0*dt^2
         % v_t = v_0 + a_0*dt
-        currentPositionFixedGT = Point3(currentPoseFixedGT.translation.vector ...
+        currentPositionFixedGT = Point3(currentPoseFixedGT.translation ...
             + currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT);
         currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT;
         
         currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); % constant orientation
         
         % Rotate pose in fixed frame to get pose in rotating frame
-        previousPositionRotatingGT = currentPoseRotatingGT.translation.vector;
+        previousPositionRotatingGT = currentPoseRotatingGT.translation;
         currentRotatingFrame = currentRotatingFrame.compose(changePoseRotatingFrame);
         inverseCurrentRotatingFrame = (currentRotatingFrame.inverse);
         currentPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentPoseFixedGT);
-        currentPositionRotatingGT = currentPoseRotatingGT.translation.vector;
+        currentPositionRotatingGT = currentPoseRotatingGT.translation;
         
         % Get velocity in rotating frame by treating it like a position and using compose 
         % Maybe Luca knows a better way to do this within GTSAM. 
@@ -230,11 +230,11 @@ for i = 1:length(times)
         %    - 0.5 * accelFixed * deltaT * deltaT) / deltaT + accelFixed * deltaT;
         
         % Store GT (ground truth) poses
-        positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector;
+        positionsInFixedGT(:,i) = currentPoseFixedGT.translation;
         velocityInFixedGT(:,i) = currentVelocityFixedGT;
-        positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation.vector;
+        positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation;
         velocityInRotatingGT(:,i) = currentVelocityRotatingGT;
-        currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation.vector;
+        currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation;
         currentRotatingFrameForPlot(i).R = currentRotatingFrame.rotation.matrix;
         
         %% Estimate trajectory in rotating frame using GTSAM (ground truth measurements)
@@ -303,9 +303,9 @@ for i = 1:length(times)
             currentVelocityEstimate = isam.calculateEstimate(currentVelKey);
             currentBias = isam.calculateEstimate(currentBiasKey);
             
-            positionsEstimates(:,i) = currentPoseEstimate.translation.vector;
-            velocitiesEstimates(:,i) = currentVelocityEstimate.vector;
-            biasEstimates(:,i) = currentBias.vector;
+            positionsEstimates(:,i) = currentPoseEstimate.translation;
+            velocitiesEstimates(:,i) = currentVelocityEstimate;
+            biasEstimates(:,i) = currentBias;
             
             % In matrix form: R_error = R_gt'*R_estimate
             % Perform Logmap on the rotation matrix to get a vector
diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m
index 2eddf75ee..64ba36d3b 100644
--- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m
+++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m
@@ -151,14 +151,14 @@ hold on;
 if options.includeCameraFactors
   b = [-1000 2000 -2000 2000 -30 30];
   for i = 1:size(metadata.camera.gtLandmarkPoints,2)
-      p = metadata.camera.gtLandmarkPoints(i).vector;
+      p = metadata.camera.gtLandmarkPoints(i);
       if(p(1) > b(1) && p(1) < b(2) && p(2) > b(3) && p(2) < b(4) && p(3) > b(5) && p(3) < b(6))
           plot3(p(1), p(2), p(3), 'k+');
       end
   end
   pointsToPlot = metadata.camera.gtLandmarkPoints(find(projectionFactorSeenBy > 0));
   for i = 1:length(pointsToPlot)
-      p = pointsToPlot(i).vector;
+      p = pointsToPlot(i);
       plot3(p(1), p(2), p(3), 'gs', 'MarkerSize', 10);
   end
 end
@@ -233,9 +233,9 @@ for k=1:numMonteCarloRuns
   for i=0:options.trajectoryLength
     % compute estimation errors
     currentPoseKey = symbol('x', i);
-    gtPosition  = gtValues.at(currentPoseKey).translation.vector;
-    estPosition = estimate.at(currentPoseKey).translation.vector;
-    estR = estimate.at(currentPoseKey).rotation.matrix;
+    gtPosition  = gtValues.atPose3(currentPoseKey).translation;
+    estPosition = estimate.atPose3(currentPoseKey).translation;
+    estR = estimate.atPose3(currentPoseKey).rotation.matrix;
     errPosition = estPosition - gtPosition;
     
     % compute covariances:
diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
index 00ae4b9c2..07f146dcb 100644
--- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
+++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
@@ -14,7 +14,7 @@ graph = NonlinearFactorGraph;
 for i=0:length(measurements)
   % Get the current pose
   currentPoseKey = symbol('x', i);
-  currentPose = values.at(currentPoseKey);
+  currentPose = values.atPose3(currentPoseKey);
   
   if i==0
     %% first time step, add priors
@@ -26,11 +26,11 @@ for i=0:length(measurements)
     % IMU velocity and bias priors
     if options.includeIMUFactors == 1
       currentVelKey = symbol('v', 0);
-      currentVel = values.at(currentVelKey).vector;
+      currentVel = values.atPoint3(currentVelKey);
       graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel));
       
       currentBiasKey = symbol('b', 0);
-      currentBias = values.at(currentBiasKey);
+      currentBias = values.atPoint3(currentBiasKey);
       graph.add(PriorFactorConstantBias(currentBiasKey, currentBias, noiseModels.noisePriorBias));
     end
     
@@ -155,7 +155,7 @@ for i=0:length(measurements)
     if options.includeCameraFactors == 1        
       for j = 1:length(measurements(i).landmarks)
         cameraMeasurmentNoise = measurementNoise.cameraNoiseVector .* randn(2,1);
-        cameraPixelMeasurement = measurements(i).landmarks(j).vector;
+        cameraPixelMeasurement = measurements(i).landmarks(j);
         % Only add the measurement if it is in the image frame (based on calibration)
         if(cameraPixelMeasurement(1) > 0 && cameraPixelMeasurement(2) > 0 ...
              && cameraPixelMeasurement(1) < 2*metadata.camera.calibration.px ...
diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
index 195b7ff69..3d8a9b5d2 100644
--- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
+++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
@@ -40,7 +40,7 @@ if options.useRealData == 1
     
     %% gt Between measurements
     if options.includeBetweenFactors == 1 && i > 0
-      prevPose = values.at(currentPoseKey - 1);
+      prevPose = values.atPose3(currentPoseKey - 1);
       deltaPose = prevPose.between(currentPose);
       measurements(i).deltaVector = Pose3.Logmap(deltaPose);
     end
@@ -65,7 +65,7 @@ if options.useRealData == 1
           IMUdeltaPose = IMUPose1.between(IMUPose2);
           IMUdeltaPoseVector     = Pose3.Logmap(IMUdeltaPose);
           IMUdeltaRotVector      = IMUdeltaPoseVector(1:3);
-          IMUdeltaPositionVector = IMUPose2.translation.vector - IMUPose1.translation.vector; % translation in absolute frame
+          IMUdeltaPositionVector = IMUPose2.translation - IMUPose1.translation; % translation in absolute frame
           
           measurements(i).imu(j).deltaT = deltaT;
           
@@ -88,7 +88,7 @@ if options.useRealData == 1
     
     %% gt GPS measurements
     if options.includeGPSFactors == 1 && i > 0
-      gpsPositionVector = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd).translation.vector;
+      gpsPositionVector = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd).translation;
       measurements(i).gpsPositionVector = gpsPositionVector;
     end
     
diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m
index 3f72f1215..2de1e1103 100644
--- a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m
+++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m
@@ -5,9 +5,9 @@ function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory( ...
 import gtsam.*;
 
 imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT);
-accelGlobal = initialPoseGlobal.rotation().rotate(Point3(acc_omegaIMU(1:3))).vector;
+accelGlobal = initialPoseGlobal.rotation().rotate(Point3(acc_omegaIMU(1:3)));
 
-finalPosition = Point3(initialPoseGlobal.translation.vector ...
+finalPosition = Point3(initialPoseGlobal.translation ...
     + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
 finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
 finalRotation = initialPoseGlobal.rotation.compose(imu2in1);
diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m
index 50b223060..bec2d760d 100644
--- a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m
+++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m
@@ -3,7 +3,7 @@ function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory_bodyFrame(
 
 % Before integrating in the body frame we need to compensate for the Coriolis 
 % effect
-acc_body =  acc_omegaIMU(1:3) - Point3(cross(acc_omegaIMU(4:6), velocity1Body)).vector;
+acc_body =  acc_omegaIMU(1:3) - Point3(cross(acc_omegaIMU(4:6), velocity1Body));
 % after compensating for coriolis this will be essentially zero
 % since we are moving at constant body velocity 
 
@@ -16,8 +16,8 @@ finalPositionBody = velocity1Body * deltaT + 0.5 * acc_body * deltaT * deltaT;
 finalVelocityBody = velocity1Body + acc_body * deltaT; 
 
 %% Express the integrated quantities in the global frame
-finalVelocityGlobal = initialVelocityGlobal + (initialPoseGlobal.rotation().rotate(Point3(finalVelocityBody)).vector() );
-finalPosition = initialPoseGlobal.translation().vector() + initialPoseGlobal.rotation().rotate( Point3(finalPositionBody)).vector() ;
+finalVelocityGlobal = initialVelocityGlobal + (initialPoseGlobal.rotation().rotate(Point3(finalVelocityBody)) );
+finalPosition = initialPoseGlobal.translation() + initialPoseGlobal.rotation().rotate( Point3(finalPositionBody)) ;
 finalRotation = initialPoseGlobal.rotation.compose(imu2in1);
 % Include position and rotation in a pose
 finalPose = Pose3(finalRotation, Point3(finalPosition) );
diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m
index b919520ac..ea851315f 100644
--- a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m
+++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m
@@ -9,8 +9,8 @@ finalRotation = initialPoseGlobal.rotation.compose(imu2in1);
 
 intermediateRotation = initialPoseGlobal.rotation.compose( Rot3.Expmap(acc_omegaIMU(4:6) * deltaT/2 ));
 % Integrate positions (equation (1) in Lupton)
-accelGlobal = intermediateRotation.rotate(Point3(acc_omegaIMU(1:3))).vector;
-finalPosition = Point3(initialPoseGlobal.translation.vector ...
+accelGlobal = intermediateRotation.rotate(Point3(acc_omegaIMU(1:3)));
+finalPosition = Point3(initialPoseGlobal.translation ...
     + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
 finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
 
diff --git a/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m b/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m
index e51b622b0..a704342ae 100644
--- a/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m
+++ b/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m
@@ -6,16 +6,16 @@ import gtsam.*;
 % Rotation: R^1_2
 body2in1 = Rot3.Expmap(omega1Body * deltaT);
 % Velocity 2 in frame 1: v^1_2 = R^1_2 v^2_2
-velocity2inertial = body2in1.rotate(Point3(velocity2Body)).vector;
+velocity2inertial = body2in1.rotate(Point3(velocity2Body));
 % Acceleration: a^1 = (v^1_2 - v^1_1)/dt
 accelBody1 = (velocity2inertial - velocity1Body) / deltaT;
 
 % Velocity 1 in frame W: v^W_1 = R^W_1 v^1_1
-initialVelocityGlobal = initialPose.rotation().rotate(Point3(velocity1Body)).vector;
+initialVelocityGlobal = initialPose.rotation().rotate(Point3(velocity1Body));
 % Acceleration in frame W: a^W = R^W_1 a^1
-accelGlobal = initialPose.rotation().rotate(Point3(accelBody1)).vector;
+accelGlobal = initialPose.rotation().rotate(Point3(accelBody1));
 
-finalPosition = Point3(initialPose.translation.vector + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
+finalPosition = Point3(initialPose.translation + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
 finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
 finalRotation = initialPose.rotation.compose(body2in1);
 finalPose = Pose3(finalRotation, finalPosition);
diff --git a/matlab/unstable_examples/+imuSimulator/test1onestep.m b/matlab/unstable_examples/+imuSimulator/test1onestep.m
index 883569849..cb66d23d6 100644
--- a/matlab/unstable_examples/+imuSimulator/test1onestep.m
+++ b/matlab/unstable_examples/+imuSimulator/test1onestep.m
@@ -10,7 +10,7 @@ omega = [0;0;0.1];
 velocity = [1;0;0];
 R = Rot3.Expmap(omega * deltaT);
 
-velocity2body = R.unrotate(Point3(velocity)).vector;
+velocity2body = R.unrotate(Point3(velocity));
 acc_omegaExpected = [-0.01; 0; 0; 0; 0; 0.1];
 acc_omegaActual = imuSimulator.calculateIMUMeasurement(omega, omega, velocity, velocity2body, deltaT, Pose3(Rot3, Point3([1;0;0])));
 disp('IMU measurement discrepancy:');
@@ -40,7 +40,7 @@ disp(acc_omegaActual - acc_omegaExpected);
 initialPose = Pose3;
 initialVelocity = velocity;
 finalPoseExpected = Pose3.Expmap([ omega; velocity ] * deltaT);
-finalVelocityExpected = finalPoseExpected.rotation.rotate(Point3(velocity)).vector;
+finalVelocityExpected = finalPoseExpected.rotation.rotate(Point3(velocity));
 [ finalPoseActual, finalVelocityActual ] = imuSimulator.integrateTrajectory(initialPose, omega, velocity, velocity, deltaT);
 disp('Final pose discrepancy:');
 disp(finalPoseExpected.between(finalPoseActual).matrix);
diff --git a/matlab/unstable_examples/+imuSimulator/test2constglobal.m b/matlab/unstable_examples/+imuSimulator/test2constglobal.m
index 19956d08a..6ab35d50b 100644
--- a/matlab/unstable_examples/+imuSimulator/test2constglobal.m
+++ b/matlab/unstable_examples/+imuSimulator/test2constglobal.m
@@ -21,12 +21,12 @@ positions = zeros(3, length(times)+1);
 
 i = 2;
 for t = times
-    velocity1body = currentPoseGlobal.rotation.unrotate(Point3(currentVelocityGlobal)).vector;
+    velocity1body = currentPoseGlobal.rotation.unrotate(Point3(currentVelocityGlobal));
     R = Rot3.Expmap(omega * deltaT);
-    velocity2body = currentPoseGlobal.rotation.compose(R).unrotate(Point3(currentVelocityGlobal)).vector;
+    velocity2body = currentPoseGlobal.rotation.compose(R).unrotate(Point3(currentVelocityGlobal));
     [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory(currentPoseGlobal, omega, velocity1body, velocity2body, deltaT);
     
-    positions(:,i) = currentPoseGlobal.translation.vector;
+    positions(:,i) = currentPoseGlobal.translation;
     i = i + 1;
 end
 
diff --git a/matlab/unstable_examples/+imuSimulator/test3constbody.m b/matlab/unstable_examples/+imuSimulator/test3constbody.m
index b3ee2edfc..8ee14ab78 100644
--- a/matlab/unstable_examples/+imuSimulator/test3constbody.m
+++ b/matlab/unstable_examples/+imuSimulator/test3constbody.m
@@ -26,27 +26,27 @@ currentPoseGlobal = Pose3;
 currentVelocityGlobal = velocity;
 % Initial state (IMU)
 currentPoseGlobalIMU = Pose3; %currentPoseGlobal.compose(IMUinBody);
-%currentVelocityGlobalIMU = IMUinBody.rotation.unrotate(Point3(velocity)).vector; % no Coriolis here? 
+%currentVelocityGlobalIMU = IMUinBody.rotation.unrotate(Point3(velocity)); % no Coriolis here?
 currentVelocityGlobalIMU = IMUinBody.rotation.unrotate( ...
-     Point3(velocity + cross(omega, IMUinBody.translation.vector))).vector;
+     Point3(velocity + cross(omega, IMUinBody.translation)));
    
 
 % Positions
 % body trajectory
 positions = zeros(3, length(times)+1);
-positions(:,1) = currentPoseGlobal.translation.vector;
+positions(:,1) = currentPoseGlobal.translation;
 poses(1).p = positions(:,1);
 poses(1).R = currentPoseGlobal.rotation.matrix;
 
 % Expected IMU trajectory (from body trajectory and lever arm)
 positionsIMUe = zeros(3, length(times)+1);
-positionsIMUe(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
+positionsIMUe(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation;
 posesIMUe(1).p = positionsIMUe(:,1);
 posesIMUe(1).R = poses(1).R * IMUinBody.rotation.matrix;
 
 % Integrated IMU trajectory (from IMU measurements)
 positionsIMU = zeros(3, length(times)+1);
-positionsIMU(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
+positionsIMU(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation;
 posesIMU(1).p = positionsIMU(:,1);
 posesIMU(1).R = IMUinBody.compose(currentPoseGlobalIMU).rotation.matrix;
 
@@ -62,9 +62,9 @@ for t = times
         currentPoseGlobalIMU, currentVelocityGlobalIMU, acc_omega, deltaT);
 
     % Store data in some structure for statistics and plots  
-    positions(:,i) = currentPoseGlobal.translation.vector;
-    positionsIMUe(:,i) = currentPoseGlobal.translation.vector + currentPoseGlobal.rotation.matrix * IMUinBody.translation.vector;
-    positionsIMU(:,i) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
+    positions(:,i) = currentPoseGlobal.translation;
+    positionsIMUe(:,i) = currentPoseGlobal.translation + currentPoseGlobal.rotation.matrix * IMUinBody.translation;
+    positionsIMU(:,i) = IMUinBody.compose(currentPoseGlobalIMU).translation;
     
     poses(i).p = positions(:,i);
     posesIMUe(i).p = positionsIMUe(:,i);
diff --git a/matlab/unstable_examples/+imuSimulator/test4circle.m b/matlab/unstable_examples/+imuSimulator/test4circle.m
index 22ee175dd..ab2c546db 100644
--- a/matlab/unstable_examples/+imuSimulator/test4circle.m
+++ b/matlab/unstable_examples/+imuSimulator/test4circle.m
@@ -34,19 +34,19 @@ currentVelocityGlobalIMUbody = currentVelocityGlobal;
 %% Prepare data structures for actual trajectory and estimates
 % Actual trajectory
 positions = zeros(3, length(times)+1);
-positions(:,1) = currentPoseGlobal.translation.vector;
+positions(:,1) = currentPoseGlobal.translation;
 poses(1).p = positions(:,1);
 poses(1).R = currentPoseGlobal.rotation.matrix;
 
 % Trajectory estimate (integrated in the navigation frame)
 positionsIMUnav = zeros(3, length(times)+1);
-positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector;
+positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation;
 posesIMUnav(1).p = positionsIMUnav(:,1);
 posesIMUnav(1).R = poses(1).R;
 
 % Trajectory estimate (integrated in the body frame)
 positionsIMUbody = zeros(3, length(times)+1);
-positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector;
+positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation;
 posesIMUbody(1).p = positionsIMUbody(:,1);
 posesIMUbody(1).R = poses(1).R;
 
@@ -72,9 +72,9 @@ for t = times
     currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT);
   
   %% Store data in some structure for statistics and plots
-  positions(:,i) = currentPoseGlobal.translation.vector;
-  positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector;
-  positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector;  
+  positions(:,i) = currentPoseGlobal.translation;
+  positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation;
+  positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation;
   % - 
   poses(i).p = positions(:,i);
   posesIMUbody(i).p = positionsIMUbody(:,i);
diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m
index 9a8a27344..d2f2bc34d 100644
--- a/matlab/unstable_examples/FlightCameraTransformIMU.m
+++ b/matlab/unstable_examples/FlightCameraTransformIMU.m
@@ -120,7 +120,7 @@ for i=1:size(trajectory)-1
     end
     
     % current ground-truth position indicator
-    h_cursor = plot3(a1, pose_t.x,pose_t.y,pose_t.z,'*');
+    h_cursor = plot3(a1, pose_t(1),pose_t(2),pose_t(3),'*');
  
     camera_pose = pose.compose(camera_transform);
     
@@ -198,7 +198,7 @@ for i=1:size(trajectory)-1
                 if ~result.exists(lKey)
                     p = landmarks.atPoint3(lKey);
                     n = normrnd(0,landmark_noise,3,1);
-                    noisy_landmark = Point3(p.x()+n(1),p.y()+n(2),p.z()+n(3));
+                    noisy_landmark = p + n;
                     initial.insert(lKey, noisy_landmark);
 
                     % and add a prior since its position is known
@@ -245,32 +245,33 @@ for i=1:size(trajectory)-1
         initial = Values;
         fg = NonlinearFactorGraph;
         
-        currentVelocityGlobal = result.at(currentVelKey);
-        currentBias = result.at(currentBiasKey);
+        currentVelocityGlobal = result.atPoint3(currentVelKey);
+        currentBias = result.atConstantBias(currentBiasKey);
         
         %% plot current pose result
-        isam_pose = result.at(xKey);
+        isam_pose = result.atPose3(xKey);
         pose_t = isam_pose.translation();
 
         if exist('h_result','var')
             delete(h_result);
         end
 
-        h_result = plot3(a1, pose_t.x,pose_t.y,pose_t.z,'^b', 'MarkerSize', 10);
+        h_result = plot3(a1, pose_t(1),pose_t(2),pose_t(3),'^b', 'MarkerSize', 10);
         title(a1, sprintf('Step %d', i));
         
         if exist('h_text1(1)', 'var')
             delete(h_text1(1));
 %             delete(h_text2(1));
         end
-        ty = result.at(transformKey).translation().y();
-        K_estimate = result.at(calibrationKey);
+        t = result.atPose3(transformKey).translation();
+        ty = t(2);
+        K_estimate = result.atCal3_S2(calibrationKey);
         K_errors = K.localCoordinates(K_estimate);
         
-        camera_transform_estimate = result.at(transformKey);
+        camera_transform_estimate = result.atPose3(transformKey);
         
-        fx = result.at(calibrationKey).fx();
-        fy = result.at(calibrationKey).fy();
+        fx = result.atCal3_S2(calibrationKey).fx();
+        fy = result.atCal3_S2(calibrationKey).fy();
 %         h_text1 = text(-600,0,0,sprintf('Y-Transform(0.0): %0.2f',ty));
         text(0,1300,0,sprintf('Calibration and IMU-cam transform errors:'));
         
@@ -304,7 +305,7 @@ for i=1:size(trajectory)-1
 end
 
 %% print out final camera transform and write video
-result.at(transformKey);
+result.atPose3(transformKey);
 if(write_video)
     close(videoObj);
 end
\ No newline at end of file
diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
index 4557d711f..9796a9737 100644
--- a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
+++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
@@ -53,7 +53,7 @@ y_shift = Point3(0,0.5,0);
 
 % insert shifted points
 for i=1:nrPoints
-   initial.insert(100+i,landmarks{i}.compose(y_shift)); 
+   initial.insert(100+i,landmarks{i} + y_shift); 
 end
 
 figure(1);
@@ -134,7 +134,7 @@ for i=1:steps
     end
     if i == 2
         fg.add(PriorFactorPose3(2, Pose3(Rot3(),Point3(1,0,0)),pose_cov));
-        fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
+        fg.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
         fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
     end
     if i > 1
@@ -144,7 +144,7 @@ for i=1:steps
             step = move_circle;
         end
         
-        initial.insert(i,result.at(i-1).compose(step));
+        initial.insert(i,result.atPose3(i-1).compose(step));
         fg.add(BetweenFactorPose3(i-1,i, step, covariance));
         
         deltaT = 1;
@@ -158,10 +158,13 @@ for i=1:steps
     
         [ currentIMUPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
     currentIMUPoseGlobal, omega, velocity, velocity, deltaT);
-
-        currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
-        currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
-        IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3));
+        
+        params = gtsam.PreintegrationParams.MakeSharedD(9.81);
+        params.setAccelerometerCovariance(IMU_metadata.AccelerometerSigma.^2 * eye(3));
+        params.setGyroscopeCovariance(IMU_metadata.GyroscopeSigma.^2 * eye(3));
+        params.setIntegrationCovariance(IMU_metadata.IntegrationSigma.^2 * eye(3));
+        currentSummarizedMeasurement = gtsam.PreintegratedImuMeasurements( ...
+        params, currentBias);
     
         accMeas = acc_omega(1:3)-g;
         omegaMeas = acc_omega(4:6);
@@ -171,7 +174,7 @@ for i=1:steps
         fg.add(ImuFactor( ...
         i-1, currentVelKey-1, ...
         i, currentVelKey, ...
-        currentBiasKey, currentSummarizedMeasurement, g, w_coriolis));
+        currentBiasKey, currentSummarizedMeasurement));
     
         % Bias evolution as given in the IMU metadata
         fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
@@ -204,8 +207,8 @@ for i=1:steps
         initial = Values;
         fg = NonlinearFactorGraph;
         
-        currentVelocityGlobal = isam.calculateEstimate(currentVelKey);
-        currentBias = isam.calculateEstimate(currentBiasKey);
+        currentVelocityGlobal = isam.calculateEstimate().atVector(currentVelKey);
+        currentBias = isam.calculateEstimate().atConstantBias(currentBiasKey);
         
         %% Compute some marginals
         marginal = isam.marginalCovariance(calibrationKey);
@@ -249,10 +252,10 @@ for i=1:steps
     gtsam.plotPose3(currentIMUPoseGlobal, [], 2);
     
     %% plot results
-    result_camera_transform = result.at(transformKey);
+    result_camera_transform = result.atPose3(transformKey);
     for j=1:i
-      gtsam.plotPose3(result.at(j),[],0.5);
-      gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5);
+      gtsam.plotPose3(result.atPose3(j),[],0.5);
+      gtsam.plotPose3(result.atPose3(j).compose(result_camera_transform),[],0.5);
     end
     
     xlabel('x (m)');
@@ -265,14 +268,15 @@ for i=1:steps
 %     axis equal
     
     for l=101:100+nrPoints
-        plotPoint3(result.at(l),'g');
+        plotPoint3(result.atPoint3(l),'g');
     end
     
-    ty = result.at(transformKey).translation().y();
-    fx = result.at(calibrationKey).fx();
-    fy = result.at(calibrationKey).fy();
-    px = result.at(calibrationKey).px();
-    py = result.at(calibrationKey).py();
+    t = result.atPose3(transformKey).translation();
+    ty = t(2);
+    fx = result.atCal3_S2(calibrationKey).fx();
+    fy = result.atCal3_S2(calibrationKey).fy();
+    px = result.atCal3_S2(calibrationKey).px();
+    py = result.atCal3_S2(calibrationKey).py();
     text(1,5,5,sprintf('Y-Transform(0.0): %0.2f',ty));
     text(1,5,3,sprintf('fx(900): %.0f',fx));
     text(1,5,1,sprintf('fy(900): %.0f',fy));
@@ -342,10 +346,10 @@ end
 fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
 
 disp('Transform after optimization');
-result.at(transformKey)
+result.atPose3(transformKey)
 
 disp('Calibration after optimization');
-result.at(calibrationKey)
+result.atCal3_S2(calibrationKey)
 
 disp('Bias after optimization');
 currentBias
diff --git a/matlab/unstable_examples/TransformProjectionFactorExample.m b/matlab/unstable_examples/TransformProjectionFactorExample.m
index 79a96209d..f6a896cff 100644
--- a/matlab/unstable_examples/TransformProjectionFactorExample.m
+++ b/matlab/unstable_examples/TransformProjectionFactorExample.m
@@ -33,7 +33,7 @@ y_shift = Point3(0,1,0);
 
 % insert shifted points
 for i=1:nrPoints
-   initial.insert(100+i,landmarks{i}.compose(y_shift)); 
+   initial.insert(100+i,landmarks{i} + y_shift); 
 end
 
 figure(1);
diff --git a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m
index ca5b70c62..88f6cc63c 100644
--- a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m
+++ b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m
@@ -47,7 +47,7 @@ y_shift = Point3(0,1,0);
 
 % insert shifted points
 for i=1:nrPoints
-   initial.insert(100+i,landmarks{i}.compose(y_shift)); 
+   initial.insert(100+i,landmarks{i} + y_shift); 
 end
 
 figure(1);
@@ -146,7 +146,8 @@ for i=1:20
         plotPoint3(result.atPoint3(l),'g');
     end
     
-    ty = result.atPose3(1000).translation().y();
+    t = result.atPose3(1000).translation();
+    ty = t(2);
     text(5,5,5,sprintf('Y-Transform: %0.2g',ty));
   
     if(write_video)
diff --git a/matlab/unstable_examples/plot_projected_landmarks.m b/matlab/unstable_examples/plot_projected_landmarks.m
index 6b8101844..30e222016 100644
--- a/matlab/unstable_examples/plot_projected_landmarks.m
+++ b/matlab/unstable_examples/plot_projected_landmarks.m
@@ -25,9 +25,9 @@ for i = 0:measurement_keys.size-1
     key_index = gtsam.symbolIndex(key);
     p = landmarks.atPoint3(gtsam.symbol('l',key_index));
     
-    x(i+1) = p.x;
-    y(i+1) = p.y;
-    z(i+1) = p.z;
+    x(i+1) = p(1);
+    y(i+1) = p(2);
+    z(i+1) = p(3);
     
 end
 
diff --git a/matlab/unstable_examples/project_landmarks.m b/matlab/unstable_examples/project_landmarks.m
index aaccc9248..3bccef94b 100644
--- a/matlab/unstable_examples/project_landmarks.m
+++ b/matlab/unstable_examples/project_landmarks.m
@@ -11,9 +11,9 @@ function [ measurements ] = project_landmarks( pose, landmarks, K )
         z = camera.project(landmarks.atPoint3(symbol('l',i)));
         
         % check bounding box
-        if z.x < 0 || z.x > 1280
+        if z(1) < 0 || z(1) > 1280
             continue
-        elseif z.y < 0 || z.y > 960
+        elseif z(2) < 0 || z(2) > 960
             continue
         end
            
diff --git a/matlab/unstable_examples/testTSAMFactors.m b/matlab/unstable_examples/testTSAMFactors.m
index 5cfd0aa80..2d1de5855 100644
--- a/matlab/unstable_examples/testTSAMFactors.m
+++ b/matlab/unstable_examples/testTSAMFactors.m
@@ -50,8 +50,8 @@ result = optimizer.optimize();
 % Check result
 CHECK('error',result.atPose2(100).equals(b1,1e-5))
 CHECK('error',result.atPose2(10).equals(origin,1e-5))
-CHECK('error',result.atPoint2(1).equals(Point2(0,1),1e-5))
-CHECK('error',result.atPoint2(2).equals(Point2(0,1),1e-5))
+CHECK('error',result.atPoint2(1) - Point2(0,1) < 1e-5)
+CHECK('error',result.atPoint2(2) - Point2(0,1) < 1e-5)
 CHECK('error',result.atPose2(20).equals(origin,1e-5))
 CHECK('error',result.atPose2(200).equals(b2,1e-5))
 
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
new file mode 100644
index 000000000..315f81f4e
--- /dev/null
+++ b/python/CMakeLists.txt
@@ -0,0 +1,107 @@
+set(GTSAM_PYTHON_BUILD_DIRECTORY ${PROJECT_BINARY_DIR}/python)
+
+if (NOT GTSAM_BUILD_PYTHON)
+    return()
+endif()
+
+# Generate setup.py.
+file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS)
+configure_file(${PROJECT_SOURCE_DIR}/python/setup.py.in
+                ${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py)
+
+set(WRAP_USE_CUSTOM_PYTHON_LIBRARY ${GTSAM_USE_CUSTOM_PYTHON_LIBRARY})
+set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION})
+
+include(PybindWrap)
+
+add_custom_target(gtsam_header DEPENDS "${PROJECT_SOURCE_DIR}/gtsam/gtsam.i")
+add_custom_target(gtsam_unstable_header DEPENDS "${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i")
+
+# ignoring the non-concrete types (type aliases)
+set(ignore
+    gtsam::Point2
+    gtsam::Point3
+    gtsam::LieVector
+    gtsam::LieMatrix
+    gtsam::ISAM2ThresholdMapValue
+    gtsam::FactorIndices
+    gtsam::FactorIndexSet
+    gtsam::BetweenFactorPose3s
+    gtsam::Point2Vector
+    gtsam::Pose3Vector
+    gtsam::KeyVector)
+
+pybind_wrap(gtsam_py # target
+            ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header
+            "gtsam.cpp" # generated_cpp
+            "gtsam" # module_name
+            "gtsam" # top_namespace
+            "${ignore}" # ignore_classes
+            ${PROJECT_SOURCE_DIR}/python/gtsam/gtsam.tpl
+            gtsam # libs
+            "gtsam;gtsam_header" # dependencies
+            ON # use_boost
+            )
+
+set_target_properties(gtsam_py PROPERTIES
+    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+    INSTALL_RPATH_USE_LINK_PATH TRUE
+    OUTPUT_NAME "gtsam"
+    LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam"
+    DEBUG_POSTFIX "" # Otherwise you will have a wrong name
+    RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name
+    )
+
+set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam)
+
+# Symlink all tests .py files to build folder.
+create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam"
+        "${GTSAM_MODULE_PATH}")
+
+if(GTSAM_UNSTABLE_BUILD_PYTHON)
+set(ignore
+        gtsam::Point2
+        gtsam::Point3
+        gtsam::LieVector
+        gtsam::LieMatrix
+        gtsam::ISAM2ThresholdMapValue
+        gtsam::FactorIndices
+        gtsam::FactorIndexSet
+        gtsam::BetweenFactorPose3s
+        gtsam::Point2Vector
+        gtsam::Pose3Vector
+        gtsam::KeyVector
+        gtsam::FixedLagSmootherKeyTimestampMapValue)
+pybind_wrap(gtsam_unstable_py # target
+        ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header
+        "gtsam_unstable.cpp" # generated_cpp
+        "gtsam_unstable" # module_name
+        "gtsam" # top_namespace
+        "${ignore}" # ignore_classes
+        ${PROJECT_SOURCE_DIR}/python/gtsam_unstable/gtsam_unstable.tpl
+        gtsam_unstable # libs
+        "gtsam_unstable;gtsam_unstable_header" # dependencies
+        ON # use_boost
+        )
+
+set_target_properties(gtsam_unstable_py PROPERTIES
+        INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+        INSTALL_RPATH_USE_LINK_PATH TRUE
+        OUTPUT_NAME "gtsam_unstable"
+        LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable"
+        DEBUG_POSTFIX "" # Otherwise you will have a wrong name
+        RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name
+        )
+
+set(GTSAM_UNSTABLE_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable)
+
+# Symlink all tests .py files to build folder.
+create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable"
+        "${GTSAM_UNSTABLE_MODULE_PATH}")
+endif()
+
+set(GTSAM_PYTHON_INSTALL_TARGET python-install)
+add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET}
+        COMMAND ${PYTHON_EXECUTABLE} ${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py install
+        DEPENDS gtsam_py gtsam_unstable_py
+        WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY})
diff --git a/python/README.md b/python/README.md
new file mode 100644
index 000000000..b1a3a865f
--- /dev/null
+++ b/python/README.md
@@ -0,0 +1,65 @@
+# README
+
+# Python Wrapper
+
+This is the Python wrapper around the GTSAM C++ library. We use our custom [wrap library](https://github.com/borglab/wrap) 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 `pyparsing(>=2.4.2)`, and `numpy(>=1.11.0)`. These can be installed as follows:
+
+  ```bash
+  pip install -r <gtsam_folder>/python/requirements.txt
+  ```
+
+## Install
+
+- Run cmake with the `GTSAM_BUILD_PYTHON` cmake flag enabled to configure building the wrapper. The wrapped module will be built and copied to the directory `<PROJECT_BINARY_DIR>/python`.
+
+- Build GTSAM and the wrapper with `make` (or `ninja` if you use `-GNinja`).
+
+- To install, simply run `make python-install` (`ninja 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.
+
+- You can also directly run `make python-install` without running `make`, and it will compile all the dependencies accordingly.
+
+## Unit Tests
+
+The Python toolbox also has a small set of unit tests located in the
+test directory. To run them:
+
+  ```bash
+  cd <GTSAM_SOURCE_DIRECTORY>/python/gtsam/tests
+  python -m unittest discover
+  ```
+
+## Utils
+
+TODO
+
+## Examples
+
+TODO
+
+## Writing Your Own Scripts
+
+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`, to avoid any conversion needed.
+  - Also, GTSAM expects _column-major_ matrices, unlike the default storage
+    scheme in numpy. But this is only performance-related as `pybind11` should translate them when needed. However, this will result a copy if your matrix is not in the expected type
+    and storage order.
+
+## Wrapping Custom GTSAM-based Project
+
+Please refer to the template project and the corresponding tutorial available [here](https://github.com/borglab/GTSAM-project-python).
diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py
new file mode 100644
index 000000000..e6fd8c9c8
--- /dev/null
+++ b/python/gtsam/__init__.py
@@ -0,0 +1,27 @@
+from .gtsam import *
+
+
+def _init():
+    """This function is to add shims for the long-gone Point2 and Point3 types"""
+
+    import numpy as np
+
+    global Point2  # export function
+
+    def Point2(x=0, y=0):
+        """Shim for the deleted Point2 type."""
+        return np.array([x, y], dtype=float)
+
+    global Point3  # export function
+
+    def Point3(x=0, y=0, z=0):
+        """Shim for the deleted Point3 type."""
+        return np.array([x, y, z], dtype=float)
+
+    # for interactive debugging
+    if __name__ == "__main__":
+        # we want all definitions accessible
+        globals().update(locals())
+
+
+_init()
diff --git a/cython/gtsam/examples/DogLegOptimizerExample.py b/python/gtsam/examples/DogLegOptimizerExample.py
similarity index 95%
rename from cython/gtsam/examples/DogLegOptimizerExample.py
rename to python/gtsam/examples/DogLegOptimizerExample.py
index 776ceedc4..26f4fef84 100644
--- a/cython/gtsam/examples/DogLegOptimizerExample.py
+++ b/python/gtsam/examples/DogLegOptimizerExample.py
@@ -35,17 +35,17 @@ def run(args):
     graph = gtsam.NonlinearFactorGraph()
 
     # Priors
-    prior = gtsam.noiseModel_Isotropic.Sigma(3, 1)
+    prior = gtsam.noiseModel.Isotropic.Sigma(3, 1)
     graph.add(gtsam.PriorFactorPose2(11, T11, prior))
     graph.add(gtsam.PriorFactorPose2(21, T21, prior))
 
     # Odometry
-    model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.01, 0.01, 0.3]))
+    model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01, 0.01, 0.3]))
     graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model))
     graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model))
 
     # Range
-    model_rho = gtsam.noiseModel_Isotropic.Sigma(1, 0.01)
+    model_rho = gtsam.noiseModel.Isotropic.Sigma(1, 0.01)
     graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho))
 
     params = gtsam.DoglegParams()
diff --git a/cython/gtsam/examples/GPSFactorExample.py b/python/gtsam/examples/GPSFactorExample.py
similarity index 93%
rename from cython/gtsam/examples/GPSFactorExample.py
rename to python/gtsam/examples/GPSFactorExample.py
index 493a07725..0bc0d1bf3 100644
--- a/cython/gtsam/examples/GPSFactorExample.py
+++ b/python/gtsam/examples/GPSFactorExample.py
@@ -26,8 +26,8 @@ lon0 = -84.30626
 h0 = 274
 
 # Create noise models
-GPS_NOISE = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
-PRIOR_NOISE = gtsam.noiseModel_Isotropic.Sigma(6, 0.25)
+GPS_NOISE = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
+PRIOR_NOISE = gtsam.noiseModel.Isotropic.Sigma(6, 0.25)
 
 # Create an empty nonlinear factor graph
 graph = gtsam.NonlinearFactorGraph()
diff --git a/cython/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py
similarity index 87%
rename from cython/gtsam/examples/ImuFactorExample.py
rename to python/gtsam/examples/ImuFactorExample.py
index 06742fcd1..eec7c5ebd 100644
--- a/cython/gtsam/examples/ImuFactorExample.py
+++ b/python/gtsam/examples/ImuFactorExample.py
@@ -18,9 +18,7 @@ import math
 import gtsam
 import matplotlib.pyplot as plt
 import numpy as np
-from gtsam import symbol_shorthand_B as B
-from gtsam import symbol_shorthand_V as V
-from gtsam import symbol_shorthand_X as X
+from gtsam.symbol_shorthand import B, V, X
 from gtsam.utils.plot import plot_pose3
 from mpl_toolkits.mplot3d import Axes3D
 
@@ -36,8 +34,8 @@ class ImuFactorExample(PreintegrationExample):
 
     def __init__(self, twist_scenario="sick_twist"):
         self.velocity = np.array([2, 0, 0])
-        self.priorNoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
-        self.velNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
+        self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
+        self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
 
         # Choose one of these twists to change scenario:
         twist_scenarios = dict(
@@ -50,7 +48,7 @@ class ImuFactorExample(PreintegrationExample):
 
         accBias = np.array([-0.3, 0.1, 0.2])
         gyroBias = np.array([0.1, 0.3, -0.1])
-        bias = gtsam.imuBias_ConstantBias(accBias, gyroBias)
+        bias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
 
         dt = 1e-2
         super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario],
@@ -69,6 +67,7 @@ class ImuFactorExample(PreintegrationExample):
         # initialize data structure for pre-integrated IMU measurements
         pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
 
+        T = 12
         num_poses = T  # assumes 1 factor per second
         initial = gtsam.Values()
         initial.insert(BIAS_KEY, self.actualBias)
@@ -110,11 +109,12 @@ class ImuFactorExample(PreintegrationExample):
                 pim.resetIntegration()
 
                 rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1)
-                translationNoise = gtsam.Point3(np.random.randn(3)*1)
+                translationNoise = gtsam.Point3(*np.random.randn(3)*1)
                 poseNoise = gtsam.Pose3(rotationNoise, translationNoise)
 
                 actual_state_i = self.scenario.navState(t + self.dt)
-                print("Actual state at {0}:\n{1}".format(t+self.dt, actual_state_i))
+                print("Actual state at {0}:\n{1}".format(
+                    t+self.dt, actual_state_i))
 
                 noisy_state_i = gtsam.NavState(
                     actual_state_i.pose().compose(poseNoise),
@@ -124,7 +124,7 @@ class ImuFactorExample(PreintegrationExample):
                 initial.insert(V(i+1), noisy_state_i.velocity())
                 i += 1
 
-        # add prior on end
+        # add priors on end
         # self.addPrior(num_poses - 1, graph)
 
         initial.print_("Initial values:")
@@ -140,7 +140,8 @@ class ImuFactorExample(PreintegrationExample):
         if compute_covariances:
             # Calculate and print marginal covariances
             marginals = gtsam.Marginals(graph, result)
-            print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY))
+            print("Covariance on bias:\n",
+                  marginals.marginalCovariance(BIAS_KEY))
             for i in range(num_poses):
                 print("Covariance on pose {}:\n{}\n".format(
                     i, marginals.marginalCovariance(X(i))))
@@ -157,7 +158,7 @@ class ImuFactorExample(PreintegrationExample):
 
         gtsam.utils.plot.set_axes_equal(POSES_FIG+1)
 
-        print("Bias Values", result.atimuBias_ConstantBias(BIAS_KEY))
+        print("Bias Values", result.atConstantBias(BIAS_KEY))
 
         plt.ioff()
         plt.show()
@@ -170,8 +171,10 @@ if __name__ == '__main__':
                         choices=("zero_twist", "forward_twist", "loop_twist", "sick_twist"))
     parser.add_argument("--time", "-T", default=12,
                         type=int, help="Total time in seconds")
-    parser.add_argument("--compute_covariances", default=False, action='store_true')
+    parser.add_argument("--compute_covariances",
+                        default=False, action='store_true')
     parser.add_argument("--verbose", default=False, action='store_true')
     args = parser.parse_args()
 
-    ImuFactorExample(args.twist_scenario).run(args.time, args.compute_covariances, args.verbose)
+    ImuFactorExample(args.twist_scenario).run(
+        args.time, args.compute_covariances, args.verbose)
diff --git a/cython/gtsam/examples/ImuFactorISAM2Example.py b/python/gtsam/examples/ImuFactorISAM2Example.py
similarity index 88%
rename from cython/gtsam/examples/ImuFactorISAM2Example.py
rename to python/gtsam/examples/ImuFactorISAM2Example.py
index 4187fe878..bb90b95bf 100644
--- a/cython/gtsam/examples/ImuFactorISAM2Example.py
+++ b/python/gtsam/examples/ImuFactorISAM2Example.py
@@ -18,9 +18,7 @@ from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
                    PinholeCameraCal3_S2, Point3, Pose3,
                    PriorFactorConstantBias, PriorFactorPose3,
                    PriorFactorVector, Rot3, Values)
-from gtsam import symbol_shorthand_B as B
-from gtsam import symbol_shorthand_V as V
-from gtsam import symbol_shorthand_X as X
+from gtsam.symbol_shorthand import B, V, X
 from gtsam.utils import plot
 
 
@@ -30,7 +28,7 @@ def vector3(x, y, z):
 
 
 g = 9.81
-kGravity = vector3(0, 0, -g)
+n_gravity = vector3(0, 0, -g)
 
 
 def preintegration_parameters():
@@ -44,7 +42,7 @@ def preintegration_parameters():
     PARAMS.setUse2ndOrderCoriolis(False)
     PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
 
-    BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
+    BIAS_COVARIANCE = gtsam.noiseModel.Isotropic.Variance(6, 0.1)
     DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
                   Point3(0.05, -0.10, 0.20))
 
@@ -61,8 +59,6 @@ def get_camera(radius):
 
 def get_scenario(radius, pose_0, angular_velocity, delta_t):
     """Create the set of ground-truth landmarks and poses"""
-    
-
     angular_velocity_vector = vector3(0, -angular_velocity, 0)
     linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
     scenario = ConstantTwistScenario(
@@ -97,18 +93,18 @@ def IMU_example():
 
     # Add a prior on pose x0. This indirectly specifies where the origin is.
     # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-    noise = gtsam.noiseModel_Diagonal.Sigmas(
+    noise = gtsam.noiseModel.Diagonal.Sigmas(
         np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
     graph.push_back(PriorFactorPose3(X(0), pose_0, noise))
 
     # Add imu priors
     biasKey = B(0)
-    biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
-    biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
+    biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
+    biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias.ConstantBias(),
                                         biasnoise)
     graph.push_back(biasprior)
-    initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
-    velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
+    initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
+    velnoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
 
     # Calculate with correct initial velocity
     n_velocity = vector3(0, angular_velocity * radius, 0)
@@ -134,14 +130,14 @@ def IMU_example():
             if i % 5 == 0:
                 biasKey += 1
                 factor = BetweenFactorConstantBias(
-                    biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
+                    biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(), BIAS_COVARIANCE)
                 graph.add(factor)
-                initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
+                initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
 
             # Predict acceleration and gyro measurements in (actual) body frame
             nRb = scenario.rotation(t).matrix()
             bRn = np.transpose(nRb)
-            measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, kGravity)
+            measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
             measuredOmega = scenario.omega_b(t)
             accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
 
diff --git a/cython/gtsam/examples/OdometryExample.py b/python/gtsam/examples/OdometryExample.py
similarity index 94%
rename from cython/gtsam/examples/OdometryExample.py
rename to python/gtsam/examples/OdometryExample.py
index e778e3f85..8b519ce9a 100644
--- a/cython/gtsam/examples/OdometryExample.py
+++ b/python/gtsam/examples/OdometryExample.py
@@ -21,8 +21,8 @@ import matplotlib.pyplot as plt
 import gtsam.utils.plot as gtsam_plot
 
 # Create noise models
-ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
-PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
+ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
+PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
 
 # Create an empty nonlinear factor graph
 graph = gtsam.NonlinearFactorGraph()
diff --git a/cython/gtsam/examples/PlanarManipulatorExample.py b/python/gtsam/examples/PlanarManipulatorExample.py
similarity index 98%
rename from cython/gtsam/examples/PlanarManipulatorExample.py
rename to python/gtsam/examples/PlanarManipulatorExample.py
index e42ae09d7..9af4f7fcc 100644
--- a/cython/gtsam/examples/PlanarManipulatorExample.py
+++ b/python/gtsam/examples/PlanarManipulatorExample.py
@@ -167,13 +167,11 @@ class ThreeLinkArm(object):
         axes = fig.gca()
 
         sXl1 = Pose2(0, 0, math.radians(90))
-        t = sXl1.translation()
-        p1 = np.array([t.x(), t.y()])
+        p1 = sXl1.translation()
         gtsam_plot.plot_pose2_on_axes(axes, sXl1)
 
         def plot_line(p, g, color):
-            t = g.translation()
-            q = np.array([t.x(), t.y()])
+            q = g.translation()
             line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
             axes.plot(line[:, 0], line[:, 1], color)
             return q
diff --git a/cython/gtsam/examples/PlanarSLAMExample.py b/python/gtsam/examples/PlanarSLAMExample.py
similarity index 91%
rename from cython/gtsam/examples/PlanarSLAMExample.py
rename to python/gtsam/examples/PlanarSLAMExample.py
index c84f0f834..5ffdf048d 100644
--- a/cython/gtsam/examples/PlanarSLAMExample.py
+++ b/python/gtsam/examples/PlanarSLAMExample.py
@@ -13,15 +13,15 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
 
 from __future__ import print_function
 
-import gtsam
 import numpy as np
-from gtsam import symbol_shorthand_L as L
-from gtsam import symbol_shorthand_X as X
+
+import gtsam
+from gtsam.symbol_shorthand import X, L
 
 # Create noise models
-PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
-ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
-MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
+PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
+ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
+MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2]))
 
 # Create an empty nonlinear factor graph
 graph = gtsam.NonlinearFactorGraph()
diff --git a/cython/gtsam/examples/Pose2SLAMExample.py b/python/gtsam/examples/Pose2SLAMExample.py
similarity index 96%
rename from cython/gtsam/examples/Pose2SLAMExample.py
rename to python/gtsam/examples/Pose2SLAMExample.py
index 680f2209f..2569f0953 100644
--- a/cython/gtsam/examples/Pose2SLAMExample.py
+++ b/python/gtsam/examples/Pose2SLAMExample.py
@@ -27,10 +27,9 @@ def vector3(x, y, z):
     """Create 3d double numpy array."""
     return np.array([x, y, z], dtype=np.float)
 
-
 # Create noise models
-PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.3, 0.3, 0.1))
-ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.2, 0.2, 0.1))
+PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.3, 0.3, 0.1))
+ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.2, 0.2, 0.1))
 
 # 1. Create a factor graph container and add factors to it
 graph = gtsam.NonlinearFactorGraph()
diff --git a/cython/gtsam/examples/Pose2SLAMExample_g2o.py b/python/gtsam/examples/Pose2SLAMExample_g2o.py
similarity index 96%
rename from cython/gtsam/examples/Pose2SLAMExample_g2o.py
rename to python/gtsam/examples/Pose2SLAMExample_g2o.py
index 09114370d..b2ba9c5bc 100644
--- a/cython/gtsam/examples/Pose2SLAMExample_g2o.py
+++ b/python/gtsam/examples/Pose2SLAMExample_g2o.py
@@ -53,7 +53,7 @@ graph, initial = gtsam.readG2o(g2oFile, is3D)
 assert args.kernel == "none", "Supplied kernel type is not yet implemented"
 
 # Add prior on the pose having index (key) = 0
-priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
+priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
 graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
 
 params = gtsam.GaussNewtonParams()
@@ -82,7 +82,7 @@ else:
     print ("Done!")
 
 if args.plot:
-    resultPoses = gtsam.utilities_extractPose2(result)
+    resultPoses = gtsam.utilities.extractPose2(result)
     for i in range(resultPoses.shape[0]):
         plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
     plt.show()
diff --git a/cython/gtsam/examples/Pose3SLAMExample_g2o.py b/python/gtsam/examples/Pose3SLAMExample_g2o.py
similarity index 93%
rename from cython/gtsam/examples/Pose3SLAMExample_g2o.py
rename to python/gtsam/examples/Pose3SLAMExample_g2o.py
index 3c1a54f7b..82b3bda98 100644
--- a/cython/gtsam/examples/Pose3SLAMExample_g2o.py
+++ b/python/gtsam/examples/Pose3SLAMExample_g2o.py
@@ -39,11 +39,11 @@ is3D = True
 graph, initial = gtsam.readG2o(g2oFile, is3D)
 
 # Add Prior on the first key
-priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
+priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
                                                          1e-4, 1e-4, 1e-4))
 
 print("Adding prior to g2o file ")
-firstKey = initial.keys().at(0)
+firstKey = initial.keys()[0]
 graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
 
 params = gtsam.GaussNewtonParams()
@@ -65,7 +65,7 @@ else:
     print ("Done!")
 
 if args.plot:
-    resultPoses = gtsam.utilities_allPose3s(result)
+    resultPoses = gtsam.utilities.allPose3s(result)
     for i in range(resultPoses.size()):
         plot.plot_pose3(1, resultPoses.atPose3(i))
     plt.show()
diff --git a/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py
similarity index 91%
rename from cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py
rename to python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py
index 02c696905..2b2c5f991 100644
--- a/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py
+++ b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py
@@ -24,9 +24,9 @@ is3D = True
 graph, initial = gtsam.readG2o(g2oFile, is3D)
 
 # Add prior on the first key. TODO: assumes first key ios z
-priorModel = gtsam.noiseModel_Diagonal.Variances(
+priorModel = gtsam.noiseModel.Diagonal.Variances(
     np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
-firstKey = initial.keys().at(0)
+firstKey = initial.keys()[0]
 graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
 
 # Initializing Pose3 - chordal relaxation"
diff --git a/cython/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py
similarity index 97%
rename from cython/gtsam/examples/PreintegrationExample.py
rename to python/gtsam/examples/PreintegrationExample.py
index 958221ac9..b54919bec 100644
--- a/cython/gtsam/examples/PreintegrationExample.py
+++ b/python/gtsam/examples/PreintegrationExample.py
@@ -10,12 +10,11 @@ A script validating the Preintegration of IMU measurements
 
 import math
 
+import gtsam
 import matplotlib.pyplot as plt
 import numpy as np
-from mpl_toolkits.mplot3d import Axes3D
-
-import gtsam
 from gtsam.utils.plot import plot_pose3
+from mpl_toolkits.mplot3d import Axes3D
 
 IMU_FIG = 1
 POSES_FIG = 2
@@ -68,7 +67,7 @@ class PreintegrationExample(object):
         else:
             accBias = np.array([0, 0.1, 0])
             gyroBias = np.array([0, 0, 0])
-            self.actualBias = gtsam.imuBias_ConstantBias(accBias, gyroBias)
+            self.actualBias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
 
         self.runner = gtsam.ScenarioRunner(
             self.scenario, self.params, self.dt, self.actualBias)
@@ -114,7 +113,7 @@ class PreintegrationExample(object):
         actualPose = self.scenario.pose(t)
         plot_pose3(POSES_FIG, actualPose, scale)
         t = actualPose.translation()
-        self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim])
+        self.maxDim = max([max(np.abs(t)), self.maxDim])
         ax = plt.gca()
         ax.set_xlim3d(-self.maxDim, self.maxDim)
         ax.set_ylim3d(-self.maxDim, self.maxDim)
diff --git a/cython/gtsam/examples/README.md b/python/gtsam/examples/README.md
similarity index 84%
rename from cython/gtsam/examples/README.md
rename to python/gtsam/examples/README.md
index 99bce00e2..e998e4dcd 100644
--- a/cython/gtsam/examples/README.md
+++ b/python/gtsam/examples/README.md
@@ -1,17 +1,12 @@
-These examples are almost identical to the old handwritten python wrapper
-examples. However, there are just some slight name changes, for example
-`noiseModel.Diagonal` becomes `noiseModel_Diagonal` etc...
-Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthand_B(0)` or `B(0)` if we use python aliasing.
-
 # Porting Progress
 
 | C++ Example Name                                      | Ported |
 |-------------------------------------------------------|--------|
 | CameraResectioning                                    |        |
 | CreateSFMExampleData                                  |        |
-| DiscreteBayesNet_FG                                   | none of the required discrete functionality is exposed through cython |
-| easyPoint2KalmanFilter                                | ExtendedKalmanFilter not exposed through cython |
-| elaboratePoint2KalmanFilter                           | GaussianSequentialSolver not exposed through cython |
+| DiscreteBayesNet_FG                                   | none of the required discrete functionality is exposed through Python |
+| easyPoint2KalmanFilter                                | ExtendedKalmanFilter not yet exposed through Python |
+| elaboratePoint2KalmanFilter                           | GaussianSequentialSolver not yet exposed through Python |
 | ImuFactorExample2                                     | X      |
 | ImuFactorsExample                                     |        |
 | ISAM2Example_SmartFactor                              |        |
@@ -25,7 +20,7 @@ Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthan
 | Pose2SLAMExample_g2o                                  | X      |
 | Pose2SLAMExample_graph                                |        |
 | Pose2SLAMExample_graphviz                             |        |
-| Pose2SLAMExample_lago                                 | lago not exposed through cython |
+| Pose2SLAMExample_lago                                 | lago not yet exposed through Python |
 | Pose2SLAMStressTest                                   |        |
 | Pose2SLAMwSPCG                                        |        |
 | Pose3SLAMExample_changeKeys                           |        |
@@ -47,11 +42,11 @@ Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthan
 | StereoVOExample                                       |        |
 | StereoVOExample_large                                 |        |
 | TimeTBB                                               |        |
-| UGM_chain                                             | discrete functionality not exposed |
-| UGM_small                                             | discrete functionality not exposed |
+| UGM_chain                                             | discrete functionality not yet exposed |
+| UGM_small                                             | discrete functionality not yet exposed |
 | VisualISAM2Example                                    | X      |
 | VisualISAMExample                                     | X      |
 
 Extra Examples (with no C++ equivalent)
 - PlanarManipulatorExample
-- SFMData
+- SFMData
\ No newline at end of file
diff --git a/cython/gtsam/examples/SFMExample.py b/python/gtsam/examples/SFMExample.py
similarity index 91%
rename from cython/gtsam/examples/SFMExample.py
rename to python/gtsam/examples/SFMExample.py
index e02def2f9..f0c4c82ba 100644
--- a/cython/gtsam/examples/SFMExample.py
+++ b/python/gtsam/examples/SFMExample.py
@@ -13,14 +13,15 @@ from __future__ import print_function
 import gtsam
 import matplotlib.pyplot as plt
 import numpy as np
-from gtsam import symbol_shorthand_L as L
-from gtsam import symbol_shorthand_X as X
+from gtsam import symbol_shorthand
+L = symbol_shorthand.L
+X = symbol_shorthand.X
+
 from gtsam.examples import SFMdata
-from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
+from gtsam import (Cal3_S2, DoglegOptimizer,
                          GenericProjectionFactorCal3_S2, Marginals,
                          NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
-                         Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3,
-                         SimpleCamera, Values)
+                         Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values)
 from gtsam.utils import plot
 
 
@@ -56,7 +57,7 @@ def main():
     K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
 
     # Define the camera observation noise model
-    measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, 1.0)  # one pixel in u and v
+    measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)  # one pixel in u and v
 
     # Create the set of ground-truth landmarks
     points = SFMdata.createPoints()
@@ -69,7 +70,7 @@ def main():
 
     # Add a prior on pose x1. This indirectly specifies where the origin is.
     # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
-    pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
+    pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
     factor = PriorFactorPose3(X(0), poses[0], pose_noise)
     graph.push_back(factor)
 
@@ -85,7 +86,7 @@ def main():
     # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
     # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
     # between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
-    point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
+    point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
     factor = PriorFactorPoint3(L(0), points[0], point_noise)
     graph.push_back(factor)
     graph.print_('Factor Graph:\n')
@@ -97,7 +98,7 @@ def main():
         transformed_pose = pose.retract(0.1*np.random.randn(6,1))
         initial_estimate.insert(X(i), transformed_pose)
     for j, point in enumerate(points):
-        transformed_point = Point3(point.vector() + 0.1*np.random.randn(3))
+        transformed_point = point + 0.1*np.random.randn(3)
         initial_estimate.insert(L(j), transformed_point)
     initial_estimate.print_('Initial Estimates:\n')
 
diff --git a/cython/gtsam/examples/SFMdata.py b/python/gtsam/examples/SFMdata.py
similarity index 92%
rename from cython/gtsam/examples/SFMdata.py
rename to python/gtsam/examples/SFMdata.py
index c586f7e52..6ac9c5726 100644
--- a/cython/gtsam/examples/SFMdata.py
+++ b/python/gtsam/examples/SFMdata.py
@@ -33,7 +33,8 @@ def createPoses(K):
     poses = []
     for theta in angles:
         position = gtsam.Point3(radius*np.cos(theta),
-                                radius*np.sin(theta), height)
+                                radius*np.sin(theta),
+                                height)
         camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
         poses.append(camera.pose())
     return poses
diff --git a/cython/gtsam/examples/SimpleRotation.py b/python/gtsam/examples/SimpleRotation.py
similarity index 96%
rename from cython/gtsam/examples/SimpleRotation.py
rename to python/gtsam/examples/SimpleRotation.py
index 4e82d3778..0fef261f8 100644
--- a/cython/gtsam/examples/SimpleRotation.py
+++ b/python/gtsam/examples/SimpleRotation.py
@@ -10,10 +10,9 @@ This example will perform a relatively trivial optimization on
 a single variable with a single factor.
 """
 
-import gtsam
 import numpy as np
-from gtsam import symbol_shorthand_X as X
-
+import gtsam
+from gtsam.symbol_shorthand import X
 
 def main():
     """
@@ -33,7 +32,7 @@ def main():
     """
     prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
     prior.print_('goal angle')
-    model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
+    model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
     key = X(1)
     factor = gtsam.PriorFactorRot2(key, prior, model)
 
diff --git a/cython/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py
similarity index 94%
rename from cython/gtsam/examples/VisualISAM2Example.py
rename to python/gtsam/examples/VisualISAM2Example.py
index 49e6ca95c..bacf510ec 100644
--- a/cython/gtsam/examples/VisualISAM2Example.py
+++ b/python/gtsam/examples/VisualISAM2Example.py
@@ -17,8 +17,7 @@ import gtsam
 import gtsam.utils.plot as gtsam_plot
 import matplotlib.pyplot as plt
 import numpy as np
-from gtsam import symbol_shorthand_L as L
-from gtsam import symbol_shorthand_X as X
+from gtsam.symbol_shorthand import L, X
 from gtsam.examples import SFMdata
 from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
 
@@ -64,7 +63,7 @@ def visual_ISAM2_example():
     K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
 
     # Define the camera observation noise model
-    measurement_noise = gtsam.noiseModel_Isotropic.Sigma(
+    measurement_noise = gtsam.noiseModel.Isotropic.Sigma(
         2, 1.0)  # one pixel in u and v
 
     # Create the set of ground-truth landmarks
@@ -110,12 +109,12 @@ def visual_ISAM2_example():
         # at least twice before adding it to iSAM.
         if i == 0:
             # Add a prior on pose x0
-            pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array(
+            pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array(
                 [0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))  # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
             graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise))
 
             # Add a prior on landmark l0
-            point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
+            point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
             graph.push_back(gtsam.PriorFactorPoint3(
                 L(0), points[0], point_noise))  # add directly to graph
 
@@ -123,7 +122,7 @@ def visual_ISAM2_example():
             # Intentionally initialize the variables off from the ground truth
             for j, point in enumerate(points):
                 initial_estimate.insert(L(j), gtsam.Point3(
-                    point.x()-0.25, point.y()+0.20, point.z()+0.15))
+                    point[0]-0.25, point[1]+0.20, point[2]+0.15))
         else:
             # Update iSAM with the new factors
             isam.update(graph, initial_estimate)
diff --git a/cython/gtsam/examples/VisualISAMExample.py b/python/gtsam/examples/VisualISAMExample.py
similarity index 77%
rename from cython/gtsam/examples/VisualISAMExample.py
rename to python/gtsam/examples/VisualISAMExample.py
index 5cc37867b..f99d3f3e6 100644
--- a/cython/gtsam/examples/VisualISAMExample.py
+++ b/python/gtsam/examples/VisualISAMExample.py
@@ -15,13 +15,11 @@ from __future__ import print_function
 import numpy as np
 import gtsam
 from gtsam.examples import SFMdata
-from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
-                         NonlinearFactorGraph, NonlinearISAM, Point3, Pose3,
-                         PriorFactorPoint3, PriorFactorPose3, Rot3,
-                         PinholeCameraCal3_S2, Values)
-from gtsam import symbol_shorthand_L as L
-from gtsam import symbol_shorthand_X as X
-
+from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
+                   NonlinearFactorGraph, NonlinearISAM, Pose3,
+                   PriorFactorPoint3, PriorFactorPose3, Rot3,
+                   PinholeCameraCal3_S2, Values, Point3)
+from gtsam.symbol_shorthand import X, L
 
 def main():
     """
@@ -34,7 +32,8 @@ def main():
     K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
 
     # Define the camera observation noise model
-    camera_noise = gtsam.noiseModel_Isotropic.Sigma(2, 1.0)  # one pixel in u and v
+    camera_noise = gtsam.noiseModel.Isotropic.Sigma(
+        2, 1.0)  # one pixel in u and v
 
     # Create the set of ground-truth landmarks
     points = SFMdata.createPoints()
@@ -55,11 +54,13 @@ def main():
         # Add factors for each landmark observation
         for j, point in enumerate(points):
             measurement = camera.project(point)
-            factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, X(i), L(j), K)
+            factor = GenericProjectionFactorCal3_S2(
+                measurement, camera_noise, X(i), L(j), K)
             graph.push_back(factor)
 
         # Intentionally initialize the variables off from the ground truth
-        noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), t=Point3(0.05, -0.10, 0.20))
+        noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25),
+                      t=Point3(0.05, -0.10, 0.20))
         initial_xi = pose.compose(noise)
 
         # Add an initial guess for the current pose
@@ -71,12 +72,13 @@ def main():
         # adding it to iSAM.
         if i == 0:
             # Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
-            pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
+            pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
+                np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
             factor = PriorFactorPose3(X(0), poses[0], pose_noise)
             graph.push_back(factor)
 
             # Add a prior on landmark l0
-            point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
+            point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
             factor = PriorFactorPoint3(L(0), points[0], point_noise)
             graph.push_back(factor)
 
@@ -84,8 +86,8 @@ def main():
             noise = np.array([-0.25, 0.20, 0.15])
             for j, point in enumerate(points):
                 # Intentionally initialize the variables off from the ground truth
-                initial_lj = points[j].vector() + noise
-                initial_estimate.insert(L(j), Point3(initial_lj))
+                initial_lj = points[j] + noise
+                initial_estimate.insert(L(j), initial_lj)
         else:
             # Update iSAM with the new factors
             isam.update(graph, initial_estimate)
diff --git a/cython/gtsam/examples/__init__.py b/python/gtsam/examples/__init__.py
similarity index 100%
rename from cython/gtsam/examples/__init__.py
rename to python/gtsam/examples/__init__.py
diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl
new file mode 100644
index 000000000..634a81e90
--- /dev/null
+++ b/python/gtsam/gtsam.tpl
@@ -0,0 +1,48 @@
+/**
+ * @file  gtsam.cpp
+ * @brief   The auto-generated wrapper C++ source code.
+ * @author  Duy-Nguyen Ta, Fan Jiang, Matthew Sklar
+ * @date  Aug. 18, 2020
+ *
+ * ** THIS FILE IS AUTO-GENERATED, DO NOT MODIFY! **
+ */
+
+// Include relevant boost libraries required by GTSAM
+{include_boost}
+
+#include <pybind11/eigen.h>
+#include <pybind11/stl_bind.h>
+#include <pybind11/pybind11.h>
+#include "gtsam/config.h"
+#include "gtsam/base/serialization.h"
+#include "gtsam/nonlinear/utilities.h"  // for RedirectCout.
+
+// These are the included headers listed in `gtsam.i`
+{includes}
+#include <boost/serialization/export.hpp>
+
+// Export classes for serialization
+{boost_class_export}
+
+// Holder type for pybind11
+{hoder_type}
+
+// Preamble for STL classes
+// TODO(fan): make this automatic
+#include "python/gtsam/preamble.h"
+
+using namespace std;
+
+namespace py = pybind11;
+
+PYBIND11_MODULE({module_name}, m_) {{
+    m_.doc() = "pybind11 wrapper of {module_name}";
+
+{wrapped_namespace}
+
+// Specializations for STL classes
+// TODO(fan): make this automatic
+#include "python/gtsam/specializations.h"
+
+}}
+
diff --git a/python/gtsam/imuBias.py b/python/gtsam/imuBias.py
new file mode 100644
index 000000000..399cefb98
--- /dev/null
+++ b/python/gtsam/imuBias.py
@@ -0,0 +1,4 @@
+# This trick is to allow direct import of sub-modules
+# without this, we can only do `from gtsam.gtsam.imuBias import X`
+# with this trick, we can do `from gtsam.imuBias import X`
+from .gtsam.imuBias import *
diff --git a/python/gtsam/noiseModel.py b/python/gtsam/noiseModel.py
new file mode 100644
index 000000000..6e1b43488
--- /dev/null
+++ b/python/gtsam/noiseModel.py
@@ -0,0 +1,4 @@
+# This trick is to allow direct import of sub-modules
+# without this, we can only do `from gtsam.gtsam.noiseModel import X`
+# with this trick, we can do `from gtsam.noiseModel import X`
+from .gtsam.noiseModel import *
\ No newline at end of file
diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h
new file mode 100644
index 000000000..0a1199122
--- /dev/null
+++ b/python/gtsam/preamble.h
@@ -0,0 +1,13 @@
+// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+// These are required to save one copy operation on Python calls
+#ifdef GTSAM_ALLOCATOR_TBB
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key>>);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> >);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::BetweenFactor<gtsam::Pose3>>);
+#else
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key>);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> >);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::BetweenFactor<gtsam::Pose3>>);
+#endif
diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h
new file mode 100644
index 000000000..2bd6a98a1
--- /dev/null
+++ b/python/gtsam/specializations.h
@@ -0,0 +1,13 @@
+// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+// These are required to save one copy operation on Python calls
+#ifdef GTSAM_ALLOCATOR_TBB
+py::bind_vector<std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >(m_, "KeyVector");
+py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
+py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
+py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
+#else
+py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
+py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
+py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
+py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
+#endif
diff --git a/python/gtsam/symbol_shorthand.py b/python/gtsam/symbol_shorthand.py
new file mode 100644
index 000000000..748d36558
--- /dev/null
+++ b/python/gtsam/symbol_shorthand.py
@@ -0,0 +1,4 @@
+# This trick is to allow direct import of sub-modules
+# without this, we can only do `from gtsam.gtsam.symbol_shorthand import X`
+# with this trick, we can do `from gtsam.symbol_shorthand import X`
+from .gtsam.symbol_shorthand import *
\ No newline at end of file
diff --git a/cython/gtsam/tests/testScenarioRunner.py b/python/gtsam/tests/testScenarioRunner.py
similarity index 96%
rename from cython/gtsam/tests/testScenarioRunner.py
rename to python/gtsam/tests/testScenarioRunner.py
index 97a97b0ec..2af16a794 100644
--- a/cython/gtsam/tests/testScenarioRunner.py
+++ b/python/gtsam/tests/testScenarioRunner.py
@@ -32,7 +32,7 @@ class TestScenarioRunner(GtsamTestCase):
 
         dt = 0.1
         params = gtsam.PreintegrationParams.MakeSharedU(self.g)
-        bias = gtsam.imuBias_ConstantBias()
+        bias = gtsam.imuBias.ConstantBias()
         runner = gtsam.ScenarioRunner(
             scenario, params, dt, bias)
 
diff --git a/cython/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py
similarity index 100%
rename from cython/gtsam/tests/test_Cal3Unified.py
rename to python/gtsam/tests/test_Cal3Unified.py
diff --git a/cython/gtsam/tests/test_FrobeniusFactor.py b/python/gtsam/tests/test_FrobeniusFactor.py
similarity index 92%
rename from cython/gtsam/tests/test_FrobeniusFactor.py
rename to python/gtsam/tests/test_FrobeniusFactor.py
index f3f5354bb..e808627f5 100644
--- a/cython/gtsam/tests/test_FrobeniusFactor.py
+++ b/python/gtsam/tests/test_FrobeniusFactor.py
@@ -13,7 +13,7 @@ import unittest
 
 import numpy as np
 from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
-                   FrobeniusWormholeFactor, SOn)
+                   ShonanFactor3, SOn)
 
 id = SO4()
 v1 = np.array([0, 0, 0, 0.1, 0, 0])
@@ -43,7 +43,7 @@ class TestFrobeniusFactorSO4(unittest.TestCase):
         """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)
+        factor = ShonanFactor3(1, 2, Rot3(R1.between(R2).matrix()), p=4)
         I4 = SOn(4)
         Q1 = I4.retract(v1)
         Q2 = I4.retract(v2)
diff --git a/cython/gtsam/tests/test_GaussianFactorGraph.py b/python/gtsam/tests/test_GaussianFactorGraph.py
similarity index 94%
rename from cython/gtsam/tests/test_GaussianFactorGraph.py
rename to python/gtsam/tests/test_GaussianFactorGraph.py
index 983825d8b..a29b0f263 100644
--- a/cython/gtsam/tests/test_GaussianFactorGraph.py
+++ b/python/gtsam/tests/test_GaussianFactorGraph.py
@@ -16,7 +16,7 @@ import unittest
 
 import gtsam
 import numpy as np
-from gtsam import symbol_shorthand_X as X
+from gtsam.symbol_shorthand import X
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -28,8 +28,8 @@ def create_graph():
     x1 = X(1)
     x2 = X(2)
     
-    BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
-    PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
+    BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
+    PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
 
     graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE)
     graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE)
diff --git a/cython/gtsam/tests/test_JacobianFactor.py b/python/gtsam/tests/test_JacobianFactor.py
similarity index 93%
rename from cython/gtsam/tests/test_JacobianFactor.py
rename to python/gtsam/tests/test_JacobianFactor.py
index 04433492b..6e049ed47 100644
--- a/cython/gtsam/tests/test_JacobianFactor.py
+++ b/python/gtsam/tests/test_JacobianFactor.py
@@ -48,7 +48,7 @@ class TestJacobianFactor(GtsamTestCase):
         # the RHS
         b2 = np.array([-1., 1.5, 2., -1.])
         sigmas = np.array([1., 1., 1., 1.])
-        model4 = gtsam.noiseModel_Diagonal.Sigmas(sigmas)
+        model4 = gtsam.noiseModel.Diagonal.Sigmas(sigmas)
         combined = gtsam.JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4)
 
         # eliminate the first variable (x2) in the combined factor, destructive
@@ -66,7 +66,7 @@ class TestJacobianFactor(GtsamTestCase):
                         [+0.00, -8.94427]])
         d = np.array([2.23607, -1.56525])
         expectedCG = gtsam.GaussianConditional(
-            x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2))
+            x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel.Unit.Create(2))
         # check if the result matches
         self.gtsamAssertEquals(actualCG, expectedCG, 1e-4)
 
@@ -82,7 +82,7 @@ class TestJacobianFactor(GtsamTestCase):
         # the RHS
         b1 = np.array([0.0, 0.894427])
 
-        model2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([1., 1.]))
+        model2 = gtsam.noiseModel.Diagonal.Sigmas(np.array([1., 1.]))
         expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2)
 
         # check if the result matches the combined (reduced) factor
diff --git a/cython/gtsam/tests/test_KalmanFilter.py b/python/gtsam/tests/test_KalmanFilter.py
similarity index 95%
rename from cython/gtsam/tests/test_KalmanFilter.py
rename to python/gtsam/tests/test_KalmanFilter.py
index 94c41df72..48a91b96c 100644
--- a/cython/gtsam/tests/test_KalmanFilter.py
+++ b/python/gtsam/tests/test_KalmanFilter.py
@@ -22,13 +22,13 @@ class TestKalmanFilter(GtsamTestCase):
         F = np.eye(2)
         B = np.eye(2)
         u = np.array([1.0, 0.0])
-        modelQ = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1]))
+        modelQ = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]))
         Q = 0.01 * np.eye(2)
         H = np.eye(2)
         z1 = np.array([1.0, 0.0])
         z2 = np.array([2.0, 0.0])
         z3 = np.array([3.0, 0.0])
-        modelR = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1]))
+        modelR = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]))
         R = 0.01 * np.eye(2)
 
         # Create the set of expected output TestValues
diff --git a/cython/gtsam/tests/test_KarcherMeanFactor.py b/python/gtsam/tests/test_KarcherMeanFactor.py
similarity index 96%
rename from cython/gtsam/tests/test_KarcherMeanFactor.py
rename to python/gtsam/tests/test_KarcherMeanFactor.py
index 6976decc1..a315a506c 100644
--- a/cython/gtsam/tests/test_KarcherMeanFactor.py
+++ b/python/gtsam/tests/test_KarcherMeanFactor.py
@@ -18,7 +18,7 @@ import numpy as np
 from gtsam.utils.test_case import GtsamTestCase
 
 KEY = 0
-MODEL = gtsam.noiseModel_Unit.Create(3)
+MODEL = gtsam.noiseModel.Unit.Create(3)
 
 
 def find_Karcher_mean_Rot3(rotations):
@@ -59,8 +59,8 @@ class TestKarcherMean(GtsamTestCase):
         R12 = R.compose(R.compose(R))
         graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
         keys = gtsam.KeyVector()
-        keys.push_back(1)
-        keys.push_back(2)
+        keys.append(1)
+        keys.append(2)
         graph.add(gtsam.KarcherMeanFactorRot3(keys))
 
         initial = gtsam.Values()
diff --git a/cython/gtsam/tests/test_LocalizationExample.py b/python/gtsam/tests/test_LocalizationExample.py
similarity index 94%
rename from cython/gtsam/tests/test_LocalizationExample.py
rename to python/gtsam/tests/test_LocalizationExample.py
index 6ce65f087..8ae3583f0 100644
--- a/cython/gtsam/tests/test_LocalizationExample.py
+++ b/python/gtsam/tests/test_LocalizationExample.py
@@ -26,7 +26,7 @@ class TestLocalizationExample(GtsamTestCase):
         # Add two odometry factors
         # create a measurement for both factors (the same in this case)
         odometry = gtsam.Pose2(2.0, 0.0, 0.0)
-        odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(
+        odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(
             np.array([0.2, 0.2, 0.1]))  # 20cm std on x,y, 0.1 rad on theta
         graph.add(gtsam.BetweenFactorPose2(0, 1, odometry, odometryNoise))
         graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
@@ -37,7 +37,7 @@ class TestLocalizationExample(GtsamTestCase):
         groundTruth.insert(0, gtsam.Pose2(0.0, 0.0, 0.0))
         groundTruth.insert(1, gtsam.Pose2(2.0, 0.0, 0.0))
         groundTruth.insert(2, gtsam.Pose2(4.0, 0.0, 0.0))
-        model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 10.]))
+        model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 10.]))
         for i in range(3):
             graph.add(gtsam.PriorFactorPose2(i, groundTruth.atPose2(i), model))
 
diff --git a/cython/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py
similarity index 88%
rename from cython/gtsam/tests/test_NonlinearOptimizer.py
rename to python/gtsam/tests/test_NonlinearOptimizer.py
index 985dc30a2..e9234a43b 100644
--- a/cython/gtsam/tests/test_NonlinearOptimizer.py
+++ b/python/gtsam/tests/test_NonlinearOptimizer.py
@@ -15,11 +15,11 @@ from __future__ import print_function
 import unittest
 
 import gtsam
-from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
+from gtsam import (DoglegOptimizer, DoglegParams,
+                   DummyPreconditionerParameters, GaussNewtonOptimizer,
                    GaussNewtonParams, LevenbergMarquardtOptimizer,
-                   LevenbergMarquardtParams, PCGSolverParameters,
-                   DummyPreconditionerParameters, NonlinearFactorGraph, Ordering,
-                   Point2, PriorFactorPoint2, Values)
+                   LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
+                   PCGSolverParameters, Point2, PriorFactorPoint2, Values)
 from gtsam.utils.test_case import GtsamTestCase
 
 KEY1 = 1
@@ -30,7 +30,7 @@ class TestScenario(GtsamTestCase):
     def test_optimize(self):
         """Do trivial test with three optimizer variants."""
         fg = NonlinearFactorGraph()
-        model = gtsam.noiseModel_Unit.Create(2)
+        model = gtsam.noiseModel.Unit.Create(2)
         fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
 
         # test error at minimum
diff --git a/cython/gtsam/tests/test_OdometryExample.py b/python/gtsam/tests/test_OdometryExample.py
similarity index 94%
rename from cython/gtsam/tests/test_OdometryExample.py
rename to python/gtsam/tests/test_OdometryExample.py
index c8ea95588..72e532f20 100644
--- a/cython/gtsam/tests/test_OdometryExample.py
+++ b/python/gtsam/tests/test_OdometryExample.py
@@ -25,7 +25,7 @@ class TestOdometryExample(GtsamTestCase):
 
         # Add a Gaussian prior on pose x_1
         priorMean = gtsam.Pose2(0.0, 0.0, 0.0)  # prior mean is at origin
-        priorNoise = gtsam.noiseModel_Diagonal.Sigmas(
+        priorNoise = gtsam.noiseModel.Diagonal.Sigmas(
             np.array([0.3, 0.3, 0.1]))  # 30cm std on x,y, 0.1 rad on theta
         # add directly to graph
         graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
@@ -33,7 +33,7 @@ class TestOdometryExample(GtsamTestCase):
         # Add two odometry factors
         # create a measurement for both factors (the same in this case)
         odometry = gtsam.Pose2(2.0, 0.0, 0.0)
-        odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(
+        odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(
             np.array([0.2, 0.2, 0.1]))  # 20cm std on x,y, 0.1 rad on theta
         graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
         graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))
diff --git a/cython/gtsam/tests/test_PlanarSLAMExample.py b/python/gtsam/tests/test_PlanarSLAMExample.py
similarity index 93%
rename from cython/gtsam/tests/test_PlanarSLAMExample.py
rename to python/gtsam/tests/test_PlanarSLAMExample.py
index ae813d35c..8cb3ad2ac 100644
--- a/cython/gtsam/tests/test_PlanarSLAMExample.py
+++ b/python/gtsam/tests/test_PlanarSLAMExample.py
@@ -32,13 +32,13 @@ class TestPlanarSLAM(GtsamTestCase):
         # Add prior
         # gaussian for prior
         priorMean = gtsam.Pose2(0.0, 0.0, 0.0)  # prior at origin
-        priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
+        priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
         # add directly to graph
         graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
 
         # Add odometry
         # general noisemodel for odometry
-        odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
+        odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
         graph.add(gtsam.BetweenFactorPose2(
             1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise))
         graph.add(gtsam.BetweenFactorPose2(
@@ -49,7 +49,7 @@ class TestPlanarSLAM(GtsamTestCase):
             4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
 
         # Add pose constraint
-        model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
+        model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
         graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model))
 
         # Initialize to noisy points
diff --git a/cython/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py
similarity index 100%
rename from cython/gtsam/tests/test_Pose2.py
rename to python/gtsam/tests/test_Pose2.py
diff --git a/cython/gtsam/tests/test_Pose2SLAMExample.py b/python/gtsam/tests/test_Pose2SLAMExample.py
similarity index 93%
rename from cython/gtsam/tests/test_Pose2SLAMExample.py
rename to python/gtsam/tests/test_Pose2SLAMExample.py
index a79b6b18c..e47b9fbff 100644
--- a/cython/gtsam/tests/test_Pose2SLAMExample.py
+++ b/python/gtsam/tests/test_Pose2SLAMExample.py
@@ -32,13 +32,13 @@ class TestPose2SLAMExample(GtsamTestCase):
         # Add prior
         # gaussian for prior
         priorMean = gtsam.Pose2(0.0, 0.0, 0.0)  # prior at origin
-        priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
+        priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
         # add directly to graph
         graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
 
         # Add odometry
         # general noisemodel for odometry
-        odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
+        odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
         graph.add(gtsam.BetweenFactorPose2(
             1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise))
         graph.add(gtsam.BetweenFactorPose2(
@@ -49,7 +49,7 @@ class TestPose2SLAMExample(GtsamTestCase):
             4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
 
         # Add pose constraint
-        model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
+        model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
         graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model))
 
         # Initialize to noisy points
diff --git a/cython/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py
similarity index 100%
rename from cython/gtsam/tests/test_Pose3.py
rename to python/gtsam/tests/test_Pose3.py
diff --git a/cython/gtsam/tests/test_Pose3SLAMExample.py b/python/gtsam/tests/test_Pose3SLAMExample.py
similarity index 97%
rename from cython/gtsam/tests/test_Pose3SLAMExample.py
rename to python/gtsam/tests/test_Pose3SLAMExample.py
index 1e9eaac67..fce171b55 100644
--- a/cython/gtsam/tests/test_Pose3SLAMExample.py
+++ b/python/gtsam/tests/test_Pose3SLAMExample.py
@@ -30,7 +30,7 @@ class TestPose3SLAMExample(GtsamTestCase):
         fg = gtsam.NonlinearFactorGraph()
         fg.add(gtsam.NonlinearEqualityPose3(0, p0))
         delta = p0.between(p1)
-        covariance = gtsam.noiseModel_Diagonal.Sigmas(
+        covariance = gtsam.noiseModel.Diagonal.Sigmas(
             np.array([0.05, 0.05, 0.05, 5. * pi / 180, 5. * pi / 180, 5. * pi / 180]))
         fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance))
         fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance))
diff --git a/cython/gtsam/tests/test_PriorFactor.py b/python/gtsam/tests/test_PriorFactor.py
similarity index 88%
rename from cython/gtsam/tests/test_PriorFactor.py
rename to python/gtsam/tests/test_PriorFactor.py
index 66207b800..0582cf5d7 100644
--- a/cython/gtsam/tests/test_PriorFactor.py
+++ b/python/gtsam/tests/test_PriorFactor.py
@@ -23,14 +23,14 @@ class TestPriorFactor(GtsamTestCase):
 
         key = 5
         priorPose3 = gtsam.Pose3()
-        model = gtsam.noiseModel_Unit.Create(6)
+        model = gtsam.noiseModel.Unit.Create(6)
         factor = gtsam.PriorFactorPose3(key, priorPose3, model)
         values.insert(key, priorPose3)
         self.assertEqual(factor.error(values), 0)
 
         key = 3
         priorVector = np.array([0., 0., 0.])
-        model = gtsam.noiseModel_Unit.Create(3)
+        model = gtsam.noiseModel.Unit.Create(3)
         factor = gtsam.PriorFactorVector(key, priorVector, model)
         values.insert(key, priorVector)
         self.assertEqual(factor.error(values), 0)
@@ -45,14 +45,14 @@ class TestPriorFactor(GtsamTestCase):
         # define and add Pose3 prior
         key = 5
         priorPose3 = gtsam.Pose3()
-        model = gtsam.noiseModel_Unit.Create(6)
+        model = gtsam.noiseModel.Unit.Create(6)
         graph.addPriorPose3(key, priorPose3, model)
         self.assertEqual(graph.size(), 1)
 
         # define and add Vector prior
         key = 3
         priorVector = np.array([0., 0., 0.])
-        model = gtsam.noiseModel_Unit.Create(3)
+        model = gtsam.noiseModel.Unit.Create(3)
         graph.addPriorVector(key, priorVector, model)
         self.assertEqual(graph.size(), 2)
 
diff --git a/cython/gtsam/tests/test_SFMExample.py b/python/gtsam/tests/test_SFMExample.py
similarity index 72%
rename from cython/gtsam/tests/test_SFMExample.py
rename to python/gtsam/tests/test_SFMExample.py
index e8fa46186..47a3cbe3e 100644
--- a/cython/gtsam/tests/test_SFMExample.py
+++ b/python/gtsam/tests/test_SFMExample.py
@@ -15,8 +15,9 @@ import numpy as np
 import gtsam
 import gtsam.utils.visual_data_generator as generator
 from gtsam import symbol
+from gtsam.noiseModel import Isotropic, Diagonal
 from gtsam.utils.test_case import GtsamTestCase
-
+from gtsam.symbol_shorthand import X, P
 
 class TestSFMExample(GtsamTestCase):
 
@@ -34,29 +35,29 @@ class TestSFMExample(GtsamTestCase):
         graph = gtsam.NonlinearFactorGraph()
 
         # Add factors for all measurements
-        measurementNoise = gtsam.noiseModel_Isotropic.Sigma(2, measurementNoiseSigma)
+        measurementNoise = Isotropic.Sigma(2, measurementNoiseSigma)
         for i in range(len(data.Z)):
             for k in range(len(data.Z[i])):
                 j = data.J[i][k]
                 graph.add(gtsam.GenericProjectionFactorCal3_S2(
                     data.Z[i][k], measurementNoise,
-                    symbol(ord('x'), i), symbol(ord('p'), j), data.K))
+                    X(i), P(j), data.K))
 
-        posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
-        graph.add(gtsam.PriorFactorPose3(symbol(ord('x'), 0),
+        posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas)
+        graph.add(gtsam.PriorFactorPose3(X(0),
                                    truth.cameras[0].pose(), posePriorNoise))
-        pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, pointNoiseSigma)
-        graph.add(gtsam.PriorFactorPoint3(symbol(ord('p'), 0),
+        pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma)
+        graph.add(gtsam.PriorFactorPoint3(P(0),
                                     truth.points[0], pointPriorNoise))
 
         # Initial estimate
         initialEstimate = gtsam.Values()
         for i in range(len(truth.cameras)):
             pose_i = truth.cameras[i].pose()
-            initialEstimate.insert(symbol(ord('x'), i), pose_i)
+            initialEstimate.insert(X(i), pose_i)
         for j in range(len(truth.points)):
             point_j = truth.points[j]
-            initialEstimate.insert(symbol(ord('p'), j), point_j)
+            initialEstimate.insert(P(j), point_j)
 
         # Optimization
         optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
@@ -66,16 +67,16 @@ class TestSFMExample(GtsamTestCase):
 
         # Marginalization
         marginals = gtsam.Marginals(graph, result)
-        marginals.marginalCovariance(symbol(ord('p'), 0))
-        marginals.marginalCovariance(symbol(ord('x'), 0))
+        marginals.marginalCovariance(P(0))
+        marginals.marginalCovariance(X(0))
 
         # Check optimized results, should be equal to ground truth
         for i in range(len(truth.cameras)):
-            pose_i = result.atPose3(symbol(ord('x'), i))
+            pose_i = result.atPose3(X(i))
             self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
 
         for j in range(len(truth.points)):
-            point_j = result.atPoint3(symbol(ord('p'), j))
+            point_j = result.atPoint3(P(j))
             self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
 
 if __name__ == "__main__":
diff --git a/cython/gtsam/tests/test_SO4.py b/python/gtsam/tests/test_SO4.py
similarity index 100%
rename from cython/gtsam/tests/test_SO4.py
rename to python/gtsam/tests/test_SO4.py
diff --git a/cython/gtsam/tests/test_SOn.py b/python/gtsam/tests/test_SOn.py
similarity index 100%
rename from cython/gtsam/tests/test_SOn.py
rename to python/gtsam/tests/test_SOn.py
diff --git a/cython/gtsam/tests/test_Scenario.py b/python/gtsam/tests/test_Scenario.py
similarity index 90%
rename from cython/gtsam/tests/test_Scenario.py
rename to python/gtsam/tests/test_Scenario.py
index 09601fba5..fc5965829 100644
--- a/cython/gtsam/tests/test_Scenario.py
+++ b/python/gtsam/tests/test_Scenario.py
@@ -43,8 +43,11 @@ class TestScenario(GtsamTestCase):
         # R = v/w, so test if loop crests at 2*R
         R = v / w
         T30 = scenario.pose(T)
+        xyz = T30.rotation().xyz()
+        if xyz[0] < 0:
+            xyz = -xyz
         np.testing.assert_almost_equal(
-            np.array([math.pi, 0, math.pi]), T30.rotation().xyz())
+            np.array([math.pi, 0, math.pi]), xyz)
         self.gtsamAssertEquals(gtsam.Point3(
             0, 0, 2.0 * R), T30.translation(), 1e-9)
 
diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py
new file mode 100644
index 000000000..4c423574d
--- /dev/null
+++ b/python/gtsam/tests/test_ShonanAveraging.py
@@ -0,0 +1,139 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for Shonan Rotation Averaging.
+Author: Frank Dellaert
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+import unittest
+
+import gtsam
+from gtsam import ShonanAveraging3, ShonanAveragingParameters3
+from gtsam.utils.test_case import GtsamTestCase
+
+DEFAULT_PARAMS = ShonanAveragingParameters3(
+    gtsam.LevenbergMarquardtParams.CeresDefaults())
+
+
+def fromExampleName(name: str, parameters=DEFAULT_PARAMS):
+    g2oFile = gtsam.findExampleDataFile(name)
+    return ShonanAveraging3(g2oFile, parameters)
+
+
+class TestShonanAveraging(GtsamTestCase):
+    """Tests for Shonan Rotation Averaging."""
+
+    def setUp(self):
+        """Set up common variables."""
+        self.shonan = fromExampleName("toyExample.g2o")
+
+    def test_checkConstructor(self):
+        self.assertEqual(5, self.shonan.nrUnknowns())
+
+        D = self.shonan.denseD()
+        self.assertEqual((15, 15), D.shape)
+
+        Q = self.shonan.denseQ()
+        self.assertEqual((15, 15), Q.shape)
+
+        L = self.shonan.denseL()
+        self.assertEqual((15, 15), L.shape)
+
+    def test_buildGraphAt(self):
+        graph = self.shonan.buildGraphAt(5)
+        self.assertEqual(7, graph.size())
+
+    def test_checkOptimality(self):
+        random = self.shonan.initializeRandomlyAt(4)
+        lambdaMin = self.shonan.computeMinEigenValue(random)
+        self.assertAlmostEqual(-414.87376657555996,
+                               lambdaMin, places=3)  # Regression test
+        self.assertFalse(self.shonan.checkOptimality(random))
+
+    def test_tryOptimizingAt3(self):
+        initial = self.shonan.initializeRandomlyAt(3)
+        self.assertFalse(self.shonan.checkOptimality(initial))
+        result = self.shonan.tryOptimizingAt(3, initial)
+        self.assertTrue(self.shonan.checkOptimality(result))
+        lambdaMin = self.shonan.computeMinEigenValue(result)
+        self.assertAlmostEqual(-5.427688831332745e-07,
+                               lambdaMin, places=3)  # Regression test
+        self.assertAlmostEqual(0, self.shonan.costAt(3, result), places=3)
+        SO3Values = self.shonan.roundSolution(result)
+        self.assertAlmostEqual(0, self.shonan.cost(SO3Values), places=3)
+
+    def test_tryOptimizingAt4(self):
+        random = self.shonan.initializeRandomlyAt(4)
+        result = self.shonan.tryOptimizingAt(4, random)
+        self.assertTrue(self.shonan.checkOptimality(result))
+        self.assertAlmostEqual(0, self.shonan.costAt(4, result), places=2)
+        lambdaMin = self.shonan.computeMinEigenValue(result)
+        self.assertAlmostEqual(-5.427688831332745e-07,
+                               lambdaMin, places=3)  # Regression test
+        SO3Values = self.shonan.roundSolution(result)
+        self.assertAlmostEqual(0, self.shonan.cost(SO3Values), places=3)
+
+    def test_initializeWithDescent(self):
+        random = self.shonan.initializeRandomlyAt(3)
+        Qstar3 = self.shonan.tryOptimizingAt(3, random)
+        lambdaMin, minEigenVector = self.shonan.computeMinEigenVector(Qstar3)
+        initialQ4 = self.shonan.initializeWithDescent(
+            4, Qstar3, minEigenVector, lambdaMin)
+        self.assertAlmostEqual(5, initialQ4.size())
+
+    def test_run(self):
+        initial = self.shonan.initializeRandomly()
+        result, lambdaMin = self.shonan.run(initial, 5, 10)
+        self.assertAlmostEqual(0, self.shonan.cost(result), places=2)
+        self.assertAlmostEqual(-5.427688831332745e-07,
+                               lambdaMin, places=3)  # Regression test
+
+    def test_runKlausKarcher(self):
+        # Load 2D toy example
+        lmParams = gtsam.LevenbergMarquardtParams.CeresDefaults()
+        # lmParams.setVerbosityLM("SUMMARY")
+        g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt")
+        parameters = gtsam.ShonanAveragingParameters2(lmParams)
+        shonan = gtsam.ShonanAveraging2(g2oFile, parameters)
+        self.assertAlmostEqual(4, shonan.nrUnknowns())
+
+        # Check graph building
+        graph = shonan.buildGraphAt(2)
+        self.assertAlmostEqual(6, graph.size())
+        initial = shonan.initializeRandomly()
+        result, lambdaMin = shonan.run(initial, 2, 10)
+        self.assertAlmostEqual(0.0008211, shonan.cost(result), places=5)
+        self.assertAlmostEqual(0, lambdaMin, places=9)  # certificate!
+
+    # Test alpha/beta/gamma prior weighting.
+    def test_PriorWeights(self):
+        lmParams = gtsam.LevenbergMarquardtParams.CeresDefaults()
+        params = ShonanAveragingParameters3(lmParams)
+        self.assertAlmostEqual(0, params.getAnchorWeight(), 1e-9)
+        self.assertAlmostEqual(1, params.getKarcherWeight(), 1e-9)
+        self.assertAlmostEqual(0, params.getGaugesWeight(), 1e-9)
+        alpha, beta, gamma = 100.0, 200.0, 300.0
+        params.setAnchorWeight(alpha)
+        params.setKarcherWeight(beta)
+        params.setGaugesWeight(gamma)
+        self.assertAlmostEqual(alpha, params.getAnchorWeight(), 1e-9)
+        self.assertAlmostEqual(beta, params.getKarcherWeight(), 1e-9)
+        self.assertAlmostEqual(gamma, params.getGaugesWeight(), 1e-9)
+        params.setKarcherWeight(0)
+        shonan = fromExampleName("Klaus3.g2o", params)
+
+        initial = gtsam.Values()
+        for i in range(3):
+            initial.insert(i, gtsam.Rot3())
+        self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3)
+        result, _lambdaMin = shonan.run(initial, 3, 3)
+        self.assertAlmostEqual(0.0015, shonan.cost(result), places=3)
+
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/python/gtsam/tests/test_SimpleCamera.py
similarity index 82%
rename from cython/gtsam/tests/test_SimpleCamera.py
rename to python/gtsam/tests/test_SimpleCamera.py
index a3654a5f1..efdfec561 100644
--- a/cython/gtsam/tests/test_SimpleCamera.py
+++ b/python/gtsam/tests/test_SimpleCamera.py
@@ -5,7 +5,7 @@ All Rights Reserved
 
 See LICENSE for the license information
 
-PinholeCameraCal3_S2 unit tests.
+SimpleCamera unit tests.
 Author: Frank Dellaert & Duy Nguyen Ta (Python)
 """
 import math
@@ -14,7 +14,7 @@ import unittest
 import numpy as np
 
 import gtsam
-from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2
+from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera
 from gtsam.utils.test_case import GtsamTestCase
 
 K = Cal3_S2(625, 625, 0, 0, 0)
@@ -23,14 +23,14 @@ class TestSimpleCamera(GtsamTestCase):
 
     def test_constructor(self):
         pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5))
-        camera = PinholeCameraCal3_S2(pose1, K)
+        camera = SimpleCamera(pose1, K)
         self.gtsamAssertEquals(camera.calibration(), K, 1e-9)
         self.gtsamAssertEquals(camera.pose(), pose1, 1e-9)
 
     def test_level2(self):
         # Create a level camera, looking in Y-direction
         pose2 = Pose2(0.4,0.3,math.pi/2.0)
-        camera = PinholeCameraCal3_S2.Level(K, pose2, 0.1)
+        camera = SimpleCamera.Level(K, pose2, 0.1)
 
         # expected
         x = Point3(1,0,0)
diff --git a/cython/gtsam/tests/test_StereoVOExample.py b/python/gtsam/tests/test_StereoVOExample.py
similarity index 92%
rename from cython/gtsam/tests/test_StereoVOExample.py
rename to python/gtsam/tests/test_StereoVOExample.py
index 3f5f57522..cefc08aab 100644
--- a/cython/gtsam/tests/test_StereoVOExample.py
+++ b/python/gtsam/tests/test_StereoVOExample.py
@@ -28,11 +28,11 @@ class TestStereoVOExample(GtsamTestCase):
         #  - No noise on measurements
 
         ## Create keys for variables
-        x1 = symbol(ord('x'),1) 
-        x2 = symbol(ord('x'),2) 
-        l1 = symbol(ord('l'),1) 
-        l2 = symbol(ord('l'),2) 
-        l3 = symbol(ord('l'),3)
+        x1 = symbol('x',1) 
+        x2 = symbol('x',2) 
+        l1 = symbol('l',1) 
+        l2 = symbol('l',2) 
+        l3 = symbol('l',3)
 
         ## Create graph container and add factors to it
         graph = gtsam.NonlinearFactorGraph()
@@ -44,7 +44,7 @@ class TestStereoVOExample(GtsamTestCase):
         ## Create realistic calibration and measurement noise model
         # format: fx fy skew cx cy baseline
         K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
-        stereo_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.0, 1.0, 1.0]))
+        stereo_model = gtsam.noiseModel.Diagonal.Sigmas(np.array([1.0, 1.0, 1.0]))
 
         ## Add measurements
         # pose 1
diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py
new file mode 100644
index 000000000..b43ad9b57
--- /dev/null
+++ b/python/gtsam/tests/test_Triangulation.py
@@ -0,0 +1,80 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Test Triangulation
+Author: Frank Dellaert & Fan Jiang (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam as g
+from gtsam.utils.test_case import GtsamTestCase
+from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \
+    PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3
+
+class TestVisualISAMExample(GtsamTestCase):
+    def test_TriangulationExample(self):
+        # Some common constants
+        sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
+
+        # Looking along X-axis, 1 meter above ground plane (x-y)
+        upright = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2)
+        pose1 = Pose3(upright, Point3(0, 0, 1))
+        camera1 = PinholeCameraCal3_S2(pose1, sharedCal)
+
+        # create second camera 1 meter to the right of first camera
+        pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
+        camera2 = PinholeCameraCal3_S2(pose2, sharedCal)
+
+        # landmark ~5 meters infront of camera
+        landmark = Point3(5, 0.5, 1.2)
+
+        # 1. Project two landmarks into two cameras and triangulate
+        z1 = camera1.project(landmark)
+        z2 = camera2.project(landmark)
+
+        # twoPoses
+        poses = Pose3Vector()
+        measurements = Point2Vector()
+
+        poses.append(pose1)
+        poses.append(pose2)
+        measurements.append(z1)
+        measurements.append(z2)
+
+        optimize = True
+        rank_tol = 1e-9
+
+        triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
+        self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9)
+
+        # 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
+        measurements = Point2Vector()
+        measurements.append(z1 - np.array([0.1, 0.5]))
+        measurements.append(z2 - np.array([-0.2, 0.3]))
+
+        triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
+        self.gtsamAssertEquals(landmark, triangulated_landmark,1e-2)
+        #
+        # # two Poses with Bundler Calibration
+        # bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480)
+        # camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal)
+        # camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal)
+        #
+        # z1 = camera1.project(landmark)
+        # z2 = camera2.project(landmark)
+        #
+        # measurements = Point2Vector()
+        # measurements.append(z1)
+        # measurements.append(z2)
+        #
+        # triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize)
+        # self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9)
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/cython/gtsam/tests/test_Values.py b/python/gtsam/tests/test_Values.py
similarity index 94%
rename from cython/gtsam/tests/test_Values.py
rename to python/gtsam/tests/test_Values.py
index 20634a21c..dddd11c40 100644
--- a/cython/gtsam/tests/test_Values.py
+++ b/python/gtsam/tests/test_Values.py
@@ -15,8 +15,7 @@ import numpy as np
 
 import gtsam
 from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2,
-                   Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values,
-                   imuBias_ConstantBias)
+                   Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, imuBias)
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -37,7 +36,7 @@ class TestValues(GtsamTestCase):
         values.insert(7, Cal3DS2())
         values.insert(8, Cal3Bundler())
         values.insert(9, E)
-        values.insert(10, imuBias_ConstantBias())
+        values.insert(10, imuBias.ConstantBias())
 
         # Special cases for Vectors and Matrices
         # Note that gtsam's Eigen Vectors and Matrices requires double-precision
@@ -70,8 +69,8 @@ class TestValues(GtsamTestCase):
         self.gtsamAssertEquals(values.atCal3DS2(7), Cal3DS2(), tol)
         self.gtsamAssertEquals(values.atCal3Bundler(8), Cal3Bundler(), tol)
         self.gtsamAssertEquals(values.atEssentialMatrix(9), E, tol)
-        self.gtsamAssertEquals(values.atimuBias_ConstantBias(
-            10), imuBias_ConstantBias(), tol)
+        self.gtsamAssertEquals(values.atConstantBias(
+            10), imuBias.ConstantBias(), tol)
 
         # special cases for Vector and Matrix:
         actualVector = values.atVector(11)
diff --git a/cython/gtsam/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py
similarity index 93%
rename from cython/gtsam/tests/test_VisualISAMExample.py
rename to python/gtsam/tests/test_VisualISAMExample.py
index 99d7e6160..6eb05eeee 100644
--- a/cython/gtsam/tests/test_VisualISAMExample.py
+++ b/python/gtsam/tests/test_VisualISAMExample.py
@@ -46,11 +46,11 @@ class TestVisualISAMExample(GtsamTestCase):
             isam, result = visual_isam.step(data, isam, result, truth, currentPose)
 
         for i in range(len(truth.cameras)):
-            pose_i = result.atPose3(symbol(ord('x'), i))
+            pose_i = result.atPose3(symbol('x', i))
             self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
 
         for j in range(len(truth.points)):
-            point_j = result.atPoint3(symbol(ord('l'), j))
+            point_j = result.atPoint3(symbol('l', j))
             self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
 
 if __name__ == "__main__":
diff --git a/cython/gtsam/tests/test_dataset.py b/python/gtsam/tests/test_dataset.py
similarity index 87%
rename from cython/gtsam/tests/test_dataset.py
rename to python/gtsam/tests/test_dataset.py
index 60fb9450d..87fc2ad54 100644
--- a/cython/gtsam/tests/test_dataset.py
+++ b/python/gtsam/tests/test_dataset.py
@@ -15,7 +15,7 @@ from __future__ import print_function
 import unittest
 
 import gtsam
-from gtsam import BetweenFactorPose3, BetweenFactorPose3s
+from gtsam import BetweenFactorPose3
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -37,8 +37,8 @@ class TestDataset(GtsamTestCase):
     def test_parse3Dfactors(self):
         """Test parsing into data structure."""
         factors = gtsam.parse3DFactors(self.pose3_example_g2o_file)
-        self.assertEqual(factors.size(), 6)
-        self.assertIsInstance(factors.at(0), BetweenFactorPose3)
+        self.assertEqual(len(factors), 6)
+        self.assertIsInstance(factors[0], BetweenFactorPose3)
 
 
 if __name__ == '__main__':
diff --git a/cython/gtsam/tests/test_dsf_map.py b/python/gtsam/tests/test_dsf_map.py
similarity index 100%
rename from cython/gtsam/tests/test_dsf_map.py
rename to python/gtsam/tests/test_dsf_map.py
diff --git a/cython/gtsam/tests/test_initialize_pose3.py b/python/gtsam/tests/test_initialize_pose3.py
similarity index 96%
rename from cython/gtsam/tests/test_initialize_pose3.py
rename to python/gtsam/tests/test_initialize_pose3.py
index 3aa7e3470..6d7f66653 100644
--- a/cython/gtsam/tests/test_initialize_pose3.py
+++ b/python/gtsam/tests/test_initialize_pose3.py
@@ -24,7 +24,7 @@ class TestValues(GtsamTestCase):
 
     def setUp(self):
 
-        model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
+        model = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
 
         # We consider a small graph:
         #                            symbolic FG
@@ -64,9 +64,8 @@ class TestValues(GtsamTestCase):
 
     def test_orientations(self):
         pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph)
-
         initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph)
-
+    
         # comparison is up to M_PI, that's why we add some multiples of 2*M_PI
         self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6)
         self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6)
@@ -77,7 +76,7 @@ class TestValues(GtsamTestCase):
         g2oFile = gtsam.findExampleDataFile("pose3example-grid")
         is3D = True
         inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D)
-        priorModel = gtsam.noiseModel_Unit.Create(6)
+        priorModel = gtsam.noiseModel.Unit.Create(6)
         inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel))
 
         initial = gtsam.InitializePose3.initialize(inputGraph)
diff --git a/cython/gtsam/tests/test_logging_optimizer.py b/python/gtsam/tests/test_logging_optimizer.py
similarity index 98%
rename from cython/gtsam/tests/test_logging_optimizer.py
rename to python/gtsam/tests/test_logging_optimizer.py
index 2560a72a2..47eb32e7b 100644
--- a/cython/gtsam/tests/test_logging_optimizer.py
+++ b/python/gtsam/tests/test_logging_optimizer.py
@@ -21,7 +21,7 @@ from gtsam.utils.test_case import GtsamTestCase
 from gtsam.utils.logging_optimizer import gtsam_optimize
 
 KEY = 0
-MODEL = gtsam.noiseModel_Unit.Create(3)
+MODEL = gtsam.noiseModel.Unit.Create(3)
 
 
 class TestOptimizeComet(GtsamTestCase):
diff --git a/cython/gtsam/tests/__init__.py b/python/gtsam/utils/__init__.py
similarity index 100%
rename from cython/gtsam/tests/__init__.py
rename to python/gtsam/utils/__init__.py
diff --git a/cython/gtsam/utils/circlePose3.py b/python/gtsam/utils/circlePose3.py
similarity index 79%
rename from cython/gtsam/utils/circlePose3.py
rename to python/gtsam/utils/circlePose3.py
index 7012548f4..e1def9427 100644
--- a/cython/gtsam/utils/circlePose3.py
+++ b/python/gtsam/utils/circlePose3.py
@@ -1,9 +1,10 @@
 import gtsam
+import math
 import numpy as np
-from math import pi, cos, sin
+from math import pi
 
 
-def circlePose3(numPoses=8, radius=1.0, symbolChar=0):
+def circlePose3(numPoses=8, radius=1.0, symbolChar='\0'):
     """
     circlePose3 generates a set of poses in a circle. This function
     returns those poses inside a gtsam.Values object, with sequential
@@ -18,10 +19,6 @@ def circlePose3(numPoses=8, radius=1.0, symbolChar=0):
     Vehicle at p0 is looking towards y axis (X-axis points towards world y)
     """
 
-    # Force symbolChar to be a single character
-    if type(symbolChar) is str:
-        symbolChar = ord(symbolChar[0])
-
     values = gtsam.Values()
     theta = 0.0
     dtheta = 2 * pi / numPoses
@@ -29,7 +26,7 @@ def circlePose3(numPoses=8, radius=1.0, symbolChar=0):
         np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F'))
     for i in range(numPoses):
         key = gtsam.symbol(symbolChar, i)
-        gti = gtsam.Point3(radius * cos(theta), radius * sin(theta), 0)
+        gti = gtsam.Point3(radius * math.cos(theta), radius * math.sin(theta), 0)
         oRi = gtsam.Rot3.Yaw(
             -theta)  # negative yaw goes counterclockwise, with Z down !
         gTi = gtsam.Pose3(gRo.compose(oRi), gti)
diff --git a/cython/gtsam/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py
similarity index 100%
rename from cython/gtsam/utils/logging_optimizer.py
rename to python/gtsam/utils/logging_optimizer.py
diff --git a/cython/gtsam/utils/plot.py b/python/gtsam/utils/plot.py
similarity index 94%
rename from cython/gtsam/utils/plot.py
rename to python/gtsam/utils/plot.py
index b55f5206a..0267da8c3 100644
--- a/cython/gtsam/utils/plot.py
+++ b/python/gtsam/utils/plot.py
@@ -109,7 +109,7 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
     # get rotation and translation (center)
     gRp = pose.rotation().matrix()  # rotation from pose to global
     t = pose.translation()
-    origin = np.array([t.x(), t.y()])
+    origin = t
 
     # draw the camera axes
     x_axis = origin + gRp[:, 0] * axis_length
@@ -169,9 +169,9 @@ def plot_point3_on_axes(axes, point, linespec, P=None):
         linespec (string): String representing formatting options for Matplotlib.
         P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
     """
-    axes.plot([point.x()], [point.y()], [point.z()], linespec)
+    axes.plot([point[0]], [point[1]], [point[2]], linespec)
     if P is not None:
-        plot_covariance_ellipse_3d(axes, point.vector(), P)
+        plot_covariance_ellipse_3d(axes, point, P)
 
 
 def plot_point3(fignum, point, linespec, P=None,
@@ -221,9 +221,8 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None,
     keys = values.keys()
 
     # Plot points and covariance matrices
-    for i in range(keys.size()):
+    for key in keys:
         try:
-            key = keys.at(i)
             point = values.atPoint3(key)
             if marginals is not None:
                 covariance = marginals.marginalCovariance(key)
@@ -253,7 +252,7 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
     """
     # get rotation and translation (center)
     gRp = pose.rotation().matrix()  # rotation from pose to global
-    origin = pose.translation().vector()
+    origin = pose.translation()
 
     # draw the camera axes
     x_axis = origin + gRp[:, 0] * axis_length
@@ -319,19 +318,17 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
         title (string): The title of the plot.
         axis_labels (iterable[string]): List of axis labels to set.
     """
-    pose3Values = gtsam.utilities_allPose3s(values)
+    pose3Values = gtsam.utilities.allPose3s(values)
     keys = gtsam.KeyVector(pose3Values.keys())
-    lastIndex = None
+    lastKey = None
 
-    for i in range(keys.size()):
-        key = keys.at(i)
+    for key in keys:
         try:
             pose = pose3Values.atPose3(key)
         except:
             print("Warning: no Pose3 at key: {0}".format(key))
 
-        if lastIndex is not None:
-            lastKey = keys.at(lastIndex)
+        if lastKey is not None:
             try:
                 lastPose = pose3Values.atPose3(lastKey)
             except:
@@ -346,11 +343,10 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
             fig = plot_pose3(fignum, lastPose,  P=covariance,
                              axis_length=scale, axis_labels=axis_labels)
 
-        lastIndex = i
+        lastKey = key
 
     # Draw final pose
-    if lastIndex is not None:
-        lastKey = keys.at(lastIndex)
+    if lastKey is not None:
         try:
             lastPose = pose3Values.atPose3(lastKey)
             if marginals:
@@ -387,13 +383,12 @@ def plot_incremental_trajectory(fignum, values, start=0,
     fig = plt.figure(fignum)
     axes = fig.gca(projection='3d')
 
-    pose3Values = gtsam.utilities_allPose3s(values)
+    pose3Values = gtsam.utilities.allPose3s(values)
     keys = gtsam.KeyVector(pose3Values.keys())
 
-    for i in range(start, keys.size()):
-        key = keys.at(i)
+    for key in keys[start:]:
         if values.exists(key):
-            pose_i = values.atPose3(keys.at(i))
+            pose_i = values.atPose3(key)
             plot_pose3(fignum, pose_i, scale)
 
     # Update the plot space to encompass all plotted points
diff --git a/cython/gtsam/utils/test_case.py b/python/gtsam/utils/test_case.py
similarity index 80%
rename from cython/gtsam/utils/test_case.py
rename to python/gtsam/utils/test_case.py
index 7df1e6ee9..3effd7f65 100644
--- a/cython/gtsam/utils/test_case.py
+++ b/python/gtsam/utils/test_case.py
@@ -21,7 +21,11 @@ class GtsamTestCase(unittest.TestCase):
             Keyword Arguments:
                 tol {float} -- tolerance passed to 'equals', default 1e-9
         """
-        equal = actual.equals(expected, tol)
+        import numpy
+        if isinstance(expected, numpy.ndarray):
+            equal = numpy.allclose(actual, expected, atol=tol)
+        else:
+            equal = actual.equals(expected, tol)
         if not equal:
             raise self.failureException(
                 "Values are not equal:\n{}!={}".format(actual, expected))
diff --git a/cython/gtsam/utils/visual_data_generator.py b/python/gtsam/utils/visual_data_generator.py
similarity index 73%
rename from cython/gtsam/utils/visual_data_generator.py
rename to python/gtsam/utils/visual_data_generator.py
index 5ce72fe68..32ccbc8fa 100644
--- a/cython/gtsam/utils/visual_data_generator.py
+++ b/python/gtsam/utils/visual_data_generator.py
@@ -1,9 +1,10 @@
 from __future__ import print_function
 
 import numpy as np
-
+import math
+from math import pi
 import gtsam
-from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3
+from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2
 
 
 class Options:
@@ -30,8 +31,8 @@ class GroundTruth:
 
     def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
         self.K = K
-        self.cameras = [gtsam.Pose3()] * nrCameras
-        self.points = [gtsam.Point3(0, 0, 0)] * nrPoints
+        self.cameras = [Pose3()] * nrCameras
+        self.points = [Point3(0, 0, 0)] * nrPoints
 
     def print_(self, s=""):
         print(s)
@@ -55,20 +56,20 @@ class Data:
 
     def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
         self.K = K
-        self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras]
+        self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras]
         self.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
         self.odometry = [Pose3()] * nrCameras
 
         # Set Noise parameters
         self.noiseModels = Data.NoiseModels()
-        self.noiseModels.posePrior = gtsam.noiseModel_Diagonal.Sigmas(
+        self.noiseModels.posePrior = gtsam.noiseModel.Diagonal.Sigmas(
             np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]))
-        # noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas(
+        # noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas(
         #    np.array([0.001,0.001,0.001,0.1,0.1,0.1]))
-        self.noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas(
+        self.noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas(
             np.array([0.05, 0.05, 0.05, 0.2, 0.2, 0.2]))
-        self.noiseModels.pointPrior = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
-        self.noiseModels.measurement = gtsam.noiseModel_Isotropic.Sigma(2, 1.0)
+        self.noiseModels.pointPrior = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
+        self.noiseModels.measurement = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)
 
 
 def generate_data(options):
@@ -84,8 +85,8 @@ def generate_data(options):
     if options.triangle:  # Create a triangle target, just 3 points on a plane
         r = 10
         for j in range(len(truth.points)):
-            theta = j * 2 * np.pi / nrPoints
-            truth.points[j] = Point3(r * np.cos(theta), r * np.sin(theta), 0)
+            theta = j * 2 * pi / nrPoints
+            truth.points[j] = Point3(r * math.cos(theta), r * math.sin(theta), 0)
     else:  # 3D landmarks as vertices of a cube
         truth.points = [
             Point3(10, 10, 10), Point3(-10, 10, 10),
@@ -98,12 +99,12 @@ def generate_data(options):
     height = 10
     r = 40
     for i in range(options.nrCameras):
-        theta = i * 2 * np.pi / options.nrCameras
-        t = gtsam.Point3(r * np.cos(theta), r * np.sin(theta), height)
-        truth.cameras[i] = gtsam.SimpleCamera.Lookat(t,
-                                                     gtsam.Point3(0, 0, 0),
-                                                     gtsam.Point3(0, 0, 1),
-                                                     truth.K)
+        theta = i * 2 * pi / options.nrCameras
+        t = Point3(r * math.cos(theta), r * math.sin(theta), height)
+        truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
+                                                       Point3(0, 0, 0),
+                                                       Point3(0, 0, 1),
+                                                       truth.K)
         # Create measurements
         for j in range(nrPoints):
             # All landmarks seen in every frame
diff --git a/cython/gtsam/utils/visual_isam.py b/python/gtsam/utils/visual_isam.py
similarity index 88%
rename from cython/gtsam/utils/visual_isam.py
rename to python/gtsam/utils/visual_isam.py
index b0ebe68c3..a8fed4b23 100644
--- a/cython/gtsam/utils/visual_isam.py
+++ b/python/gtsam/utils/visual_isam.py
@@ -25,7 +25,7 @@ def initialize(data, truth, options):
     newFactors = gtsam.NonlinearFactorGraph()
     initialEstimates = gtsam.Values()
     for i in range(2):
-        ii = symbol(ord('x'), i)
+        ii = symbol('x', i)
         if i == 0:
             if options.hardConstraint:  # add hard constraint
                 newFactors.add(
@@ -41,10 +41,10 @@ def initialize(data, truth, options):
     # Add visual measurement factors from two first poses and initialize
     # observed landmarks
     for i in range(2):
-        ii = symbol(ord('x'), i)
+        ii = symbol('x', i)
         for k in range(len(data.Z[i])):
             j = data.J[i][k]
-            jj = symbol(ord('l'), j)
+            jj = symbol('l', j)
             newFactors.add(
                 gtsam.GenericProjectionFactorCal3_S2(data.Z[i][
                     k], data.noiseModels.measurement, ii, jj, data.K))
@@ -59,8 +59,8 @@ def initialize(data, truth, options):
     # Add odometry between frames 0 and 1
     newFactors.add(
         gtsam.BetweenFactorPose3(
-            symbol(ord('x'), 0),
-            symbol(ord('x'), 1), data.odometry[1], data.noiseModels.odometry))
+            symbol('x', 0),
+            symbol('x', 1), data.odometry[1], data.noiseModels.odometry))
 
     # Update ISAM
     if options.batchInitialization:  # Do a full optimize for first two poses
@@ -98,28 +98,28 @@ def step(data, isam, result, truth, currPoseIndex):
     odometry = data.odometry[prevPoseIndex]
     newFactors.add(
         gtsam.BetweenFactorPose3(
-            symbol(ord('x'), prevPoseIndex),
-            symbol(ord('x'), currPoseIndex), odometry,
+            symbol('x', prevPoseIndex),
+            symbol('x', currPoseIndex), odometry,
             data.noiseModels.odometry))
 
     # Add visual measurement factors and initializations as necessary
     for k in range(len(data.Z[currPoseIndex])):
         zij = data.Z[currPoseIndex][k]
         j = data.J[currPoseIndex][k]
-        jj = symbol(ord('l'), j)
+        jj = symbol('l', j)
         newFactors.add(
             gtsam.GenericProjectionFactorCal3_S2(
                 zij, data.noiseModels.measurement,
-                symbol(ord('x'), currPoseIndex), jj, data.K))
+                symbol('x', currPoseIndex), jj, data.K))
         # TODO: initialize with something other than truth
         if not result.exists(jj) and not initialEstimates.exists(jj):
             lmInit = truth.points[j]
             initialEstimates.insert(jj, lmInit)
 
     # Initial estimates for the new pose.
-    prevPose = result.atPose3(symbol(ord('x'), prevPoseIndex))
+    prevPose = result.atPose3(symbol('x', prevPoseIndex))
     initialEstimates.insert(
-        symbol(ord('x'), currPoseIndex), prevPose.compose(odometry))
+        symbol('x', currPoseIndex), prevPose.compose(odometry))
 
     # Update ISAM
     # figure(1)tic
diff --git a/cython/gtsam_unstable/__init__.py b/python/gtsam_unstable/__init__.py
similarity index 100%
rename from cython/gtsam_unstable/__init__.py
rename to python/gtsam_unstable/__init__.py
diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/python/gtsam_unstable/examples/FixedLagSmootherExample.py
similarity index 84%
rename from cython/gtsam_unstable/examples/FixedLagSmootherExample.py
rename to python/gtsam_unstable/examples/FixedLagSmootherExample.py
index 786701e0f..7d2cea8ae 100644
--- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
+++ b/python/gtsam_unstable/examples/FixedLagSmootherExample.py
@@ -16,16 +16,6 @@ import numpy as np
 import gtsam
 import gtsam_unstable
 
-
-def _timestamp_key_value(key, value):
-    """
-
-    """
-    return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
-        key, value
-    )
-
-
 def BatchFixedLagSmootherExample():
     """
     Runs a batch fixed smoother on an agent with two odometry
@@ -45,21 +35,21 @@ def BatchFixedLagSmootherExample():
 
     # Create  a prior on the first pose, placing it at the origin
     prior_mean = gtsam.Pose2(0, 0, 0)
-    prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
+    prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
     X1 = 0
     new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
     new_values.insert(X1, prior_mean)
-    new_timestamps.insert(_timestamp_key_value(X1, 0.0))
+    new_timestamps.insert((X1, 0.0))
 
     delta_time = 0.25
     time = 0.25
 
     while time <= 3.0:
-        previous_key = 1000 * (time - delta_time)
-        current_key = 1000 * time
+        previous_key = int(1000 * (time - delta_time))
+        current_key = int(1000 * time)
 
         # assign current key to the current timestamp
-        new_timestamps.insert(_timestamp_key_value(current_key, time))
+        new_timestamps.insert((current_key, time))
 
         # Add a guess for this pose to the new values
         # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s]
@@ -69,14 +59,14 @@ def BatchFixedLagSmootherExample():
         # Add odometry factors from two different sources with different error
         # stats
         odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
-        odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
+        odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas(
             np.array([0.1, 0.1, 0.05]))
         new_factors.push_back(gtsam.BetweenFactorPose2(
             previous_key, current_key, odometry_measurement_1, odometry_noise_1
         ))
 
         odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
-        odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
+        odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas(
             np.array([0.05, 0.05, 0.05]))
         new_factors.push_back(gtsam.BetweenFactorPose2(
             previous_key, current_key, odometry_measurement_2, odometry_noise_2
diff --git a/cython/gtsam_unstable/examples/TimeOfArrivalExample.py b/python/gtsam_unstable/examples/TimeOfArrivalExample.py
similarity index 98%
rename from cython/gtsam_unstable/examples/TimeOfArrivalExample.py
rename to python/gtsam_unstable/examples/TimeOfArrivalExample.py
index 6ba06f0f2..59f008a05 100644
--- a/cython/gtsam_unstable/examples/TimeOfArrivalExample.py
+++ b/python/gtsam_unstable/examples/TimeOfArrivalExample.py
@@ -12,7 +12,7 @@ Author: Frank Dellaert
 # pylint: disable=invalid-name, no-name-in-module
 
 from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
-                   NonlinearFactorGraph, Point3, Values, noiseModel_Isotropic)
+                   NonlinearFactorGraph, Point3, Values, noiseModel)
 from gtsam_unstable import Event, TimeOfArrival, TOAFactor
 
 # units
@@ -64,7 +64,7 @@ def create_graph(microphones, simulatedTOA):
     graph = NonlinearFactorGraph()
 
     # Create a noise model for the TOA error
-    model = noiseModel_Isotropic.Sigma(1, 0.5 * MS)
+    model = noiseModel.Isotropic.Sigma(1, 0.5 * MS)
 
     K = len(microphones)
     key = 0
diff --git a/cython/gtsam/utils/__init__.py b/python/gtsam_unstable/examples/__init__.py
similarity index 100%
rename from cython/gtsam/utils/__init__.py
rename to python/gtsam_unstable/examples/__init__.py
diff --git a/python/gtsam_unstable/gtsam_unstable.tpl b/python/gtsam_unstable/gtsam_unstable.tpl
new file mode 100644
index 000000000..1d9dfaa40
--- /dev/null
+++ b/python/gtsam_unstable/gtsam_unstable.tpl
@@ -0,0 +1,44 @@
+/**
+ * @file  gtsam.cpp
+ * @brief   The auto-generated wrapper C++ source code.
+ * @author  Duy-Nguyen Ta, Fan Jiang, Matthew Sklar
+ * @date  Aug. 18, 2020
+ *
+ * ** THIS FILE IS AUTO-GENERATED, DO NOT MODIFY! **
+ */
+
+// Include relevant boost libraries required by GTSAM
+{include_boost}
+
+#include <pybind11/eigen.h>
+#include <pybind11/stl_bind.h>
+#include <pybind11/pybind11.h>
+#include "gtsam/base/serialization.h"
+#include "gtsam/nonlinear/utilities.h"  // for RedirectCout.
+
+// These are the included headers listed in `gtsam_unstable.i`
+{includes}
+#include <boost/serialization/export.hpp>
+
+{boost_class_export}
+
+{hoder_type}
+
+#include "python/gtsam_unstable/preamble.h"
+
+using namespace std;
+
+namespace py = pybind11;
+
+PYBIND11_MODULE({module_name}, m_) {{
+    m_.doc() = "pybind11 wrapper of {module_name}";
+
+    // Note here we need to import the dependent library
+    py::module::import("gtsam");
+
+{wrapped_namespace}
+
+#include "python/gtsam_unstable/specializations.h"
+
+}}
+
diff --git a/cython/gtsam_unstable/examples/__init__.py b/python/gtsam_unstable/preamble.h
similarity index 100%
rename from cython/gtsam_unstable/examples/__init__.py
rename to python/gtsam_unstable/preamble.h
diff --git a/cython/gtsam_unstable/tests/__init__.py b/python/gtsam_unstable/specializations.h
similarity index 100%
rename from cython/gtsam_unstable/tests/__init__.py
rename to python/gtsam_unstable/specializations.h
diff --git a/python/gtsam_unstable/tests/__init__.py b/python/gtsam_unstable/tests/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/python/gtsam_unstable/tests/test_FixedLagSmootherExample.py
similarity index 88%
rename from cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
rename to python/gtsam_unstable/tests/test_FixedLagSmootherExample.py
index 8d3af311f..c1ccd1ea1 100644
--- a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
+++ b/python/gtsam_unstable/tests/test_FixedLagSmootherExample.py
@@ -16,13 +16,6 @@ import gtsam
 import gtsam_unstable
 from gtsam.utils.test_case import GtsamTestCase
 
-
-def _timestamp_key_value(key, value):
-    return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
-        key, value
-    )
-
-
 class TestFixedLagSmootherExample(GtsamTestCase):
     '''
     Tests the fixed lag smoother wrapper
@@ -47,14 +40,14 @@ class TestFixedLagSmootherExample(GtsamTestCase):
 
         # Create  a prior on the first pose, placing it at the origin
         prior_mean = gtsam.Pose2(0, 0, 0)
-        prior_noise = gtsam.noiseModel_Diagonal.Sigmas(
+        prior_noise = gtsam.noiseModel.Diagonal.Sigmas(
             np.array([0.3, 0.3, 0.1]))
         X1 = 0
         new_factors.push_back(
             gtsam.PriorFactorPose2(
                 X1, prior_mean, prior_noise))
         new_values.insert(X1, prior_mean)
-        new_timestamps.insert(_timestamp_key_value(X1, 0.0))
+        new_timestamps.insert((X1, 0.0))
 
         delta_time = 0.25
         time = 0.25
@@ -80,11 +73,11 @@ class TestFixedLagSmootherExample(GtsamTestCase):
         # and its two odometers measure the change. The smoothed
         # result is then compared to the ground truth
         while time <= 3.0:
-            previous_key = 1000 * (time - delta_time)
-            current_key = 1000 * time
+            previous_key = int(1000 * (time - delta_time))
+            current_key = int(1000 * time)
 
             # assign current key to the current timestamp
-            new_timestamps.insert(_timestamp_key_value(current_key, time))
+            new_timestamps.insert((current_key, time))
 
             # Add a guess for this pose to the new values
             # Assume that the robot moves at 2 m/s. Position is time[s] *
@@ -95,7 +88,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
             # Add odometry factors from two different sources with different
             # error stats
             odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
-            odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
+            odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas(
                 np.array([0.1, 0.1, 0.05]))
             new_factors.push_back(
                 gtsam.BetweenFactorPose2(
@@ -105,7 +98,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
                     odometry_noise_1))
 
             odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
-            odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
+            odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas(
                 np.array([0.05, 0.05, 0.05]))
             new_factors.push_back(
                 gtsam.BetweenFactorPose2(
diff --git a/python/requirements.txt b/python/requirements.txt
new file mode 100644
index 000000000..481d27d8e
--- /dev/null
+++ b/python/requirements.txt
@@ -0,0 +1,2 @@
+numpy>=1.11.0
+pyparsing>=2.4.2
diff --git a/cython/setup.py.in b/python/setup.py.in
similarity index 70%
rename from cython/setup.py.in
rename to python/setup.py.in
index 98a05c9f6..55431a9ad 100644
--- a/cython/setup.py.in
+++ b/python/setup.py.in
@@ -1,25 +1,20 @@
 import os
 import sys
+
 try:
     from setuptools import setup, find_packages
 except ImportError:
     from distutils.core import setup, find_packages
 
-packages = find_packages()
-
+packages = find_packages(where=".")
+print("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
+    '': [
+        './*.so',
+        './*.dll',
+    ]
 }
 
-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()
 
@@ -33,8 +28,6 @@ setup(
     license='Simplified BSD license',
     keywords='slam sam robotics localization mapping optimization',
     long_description=readme_contents,
-    long_description_content_type='text/markdown',
-    python_requires='>=2.7',
     # https://pypi.org/pypi?%3Aaction=list_classifiers
     classifiers=[
         'Development Status :: 5 - Production/Stable',
@@ -48,8 +41,9 @@ setup(
         'Programming Language :: Python :: 2',
         'Programming Language :: Python :: 3',
     ],
-
     packages=packages,
     package_data=package_data,
-    install_requires=install_requires
+    test_suite="gtsam.tests",
+    install_requires=["numpy"],
+    zip_safe=False,
 )
diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp
index 2616ab103..dc19801a2 100644
--- a/tests/testNonlinearOptimizer.cpp
+++ b/tests/testNonlinearOptimizer.cpp
@@ -541,7 +541,6 @@ TEST(NonlinearOptimizer, subclass_solver) {
 }
 
 /* ************************************************************************* */
-#include <wrap/utilities.h>
 TEST( NonlinearOptimizer, logfile )
 {
   NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph());
diff --git a/timing/timeFrobeniusFactor.cpp b/timing/timeShonanFactor.cpp
similarity index 92%
rename from timing/timeFrobeniusFactor.cpp
rename to timing/timeShonanFactor.cpp
index 924213a33..207d54a4d 100644
--- a/timing/timeFrobeniusFactor.cpp
+++ b/timing/timeShonanFactor.cpp
@@ -10,8 +10,8 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    timeFrobeniusFactor.cpp
- * @brief   time FrobeniusFactor with BAL file
+ * @file    timeShonanFactor.cpp
+ * @brief   time ShonanFactor with BAL file
  * @author  Frank Dellaert
  * @date    2019
  */
@@ -27,7 +27,7 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/dataset.h>
-#include <gtsam/slam/FrobeniusFactor.h>
+#include <gtsam/sfm/ShonanFactor.h>
 
 #include <iostream>
 #include <random>
@@ -42,7 +42,7 @@ 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]");
+    throw runtime_error("Usage: timeShonanFactor [g2oFile]");
   }
 
   string g2oFile;
@@ -70,7 +70,7 @@ int main(int argc, char* argv[]) {
     const auto &keys = m.keys();
     const Rot3 &Rij = m.measured();
     const auto &model = m.noiseModel();
-    graph.emplace_shared<FrobeniusWormholeFactor>(
+    graph.emplace_shared<ShonanFactor3>(
         keys[0], keys[1], Rij, 4, model, G);
   }
 
diff --git a/update_wrap.sh b/update_wrap.sh
new file mode 100755
index 000000000..61f55e32e
--- /dev/null
+++ b/update_wrap.sh
@@ -0,0 +1,13 @@
+#!/bin/bash
+REF=${1-master} # branch or tag; defaults to 'master' if parameter 1 not present
+REMOTE=wrap # just a name to identify the remote
+REPO=git@github.com:borglab/wrap.git # replace this with your repository URL
+FOLDER=wrap # where to mount the subtree
+
+git remote add $REMOTE --no-tags $REPO
+if [[ -d $FOLDER ]]; then # update the existing subtree
+    git subtree pull $REMOTE $REF --prefix=$FOLDER --squash -m "Merging '$REF' into '$FOLDER'"
+else # add the subtree
+    git subtree add  $REMOTE $REF --prefix=$FOLDER --squash -m "Merging '$REF' into '$FOLDER'"
+fi
+git remote remove $REMOTE
\ No newline at end of file
diff --git a/wrap/.github/workflows/ci.yml b/wrap/.github/workflows/ci.yml
new file mode 100644
index 000000000..2e38bc3dd
--- /dev/null
+++ b/wrap/.github/workflows/ci.yml
@@ -0,0 +1,52 @@
+name: Python CI
+
+on: [push, pull_request]
+
+jobs:
+  build:
+    name: ${{ matrix.name }} 🐍 ${{ matrix.python_version }}
+    runs-on: ${{ matrix.os }}
+
+    env:
+      PYTHON_VERSION: ${{ matrix.python_version }}
+    strategy:
+      fail-fast: false
+      matrix:
+        # Github Actions requires a single row to be added to the build matrix.
+        # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
+        name: [
+          ubuntu-18.04
+        ]
+
+        python_version: [3]
+        include:
+          - name: ubuntu-18.04
+            os: ubuntu-18.04
+
+    steps:
+      - name: Checkout
+        uses: actions/checkout@master
+      - name: Install (Linux)
+        if: runner.os == 'Linux'
+        run: |
+          sudo apt-get -y update
+          
+          sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
+      - name: Install (macOS)
+        if: runner.os == 'macOS'
+        run: |
+          brew install cmake ninja boost
+      - name: Build (Linux)
+        if: runner.os == 'Linux'
+        run: |
+          sudo pip$PYTHON_VERSION install -r requirements.txt
+          cd tests
+          python$PYTHON_VERSION test_pybind_wrapper.py
+          python$PYTHON_VERSION test_matlab_wrapper.py
+      - name: Build (macOS)
+        if: runner.os == 'macOS'
+        run: |
+          pip$PYTHON_VERSION install -r requirements.txt
+          cd tests
+          python$PYTHON_VERSION test_pybind_wrapper.py
+          python$PYTHON_VERSION test_matlab_wrapper.py
\ No newline at end of file
diff --git a/wrap/.gitignore b/wrap/.gitignore
new file mode 100644
index 000000000..38da6d9d1
--- /dev/null
+++ b/wrap/.gitignore
@@ -0,0 +1,2 @@
+__pycache__/
+.vscode/
diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp
deleted file mode 100644
index f85aed72e..000000000
--- a/wrap/Argument.cpp
+++ /dev/null
@@ -1,316 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Argument.ccp
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#include "Argument.h"
-#include "Class.h"
-
-#include <boost/lexical_cast.hpp>
-
-#include <iostream>
-#include <fstream>
-#include <sstream>
-
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-Argument Argument::expandTemplate(const TemplateSubstitution& ts) const {
-  Argument instArg = *this;
-  instArg.type = ts.tryToSubstitite(type);
-  return instArg;
-}
-
-/* ************************************************************************* */
-ArgumentList ArgumentList::expandTemplate(
-    const TemplateSubstitution& ts) const {
-  ArgumentList instArgList;
-  for(const Argument& arg: *this) {
-    Argument instArg = arg.expandTemplate(ts);
-    instArgList.push_back(instArg);
-  }
-  return instArgList;
-}
-
-/* ************************************************************************* */
-string Argument::matlabClass(const string& delim) const {
-  string result;
-  for(const string& ns: type.namespaces())
-    result += ns + delim;
-  if (type.name() == "string" || type.name() == "unsigned char"
-      || type.name() == "char")
-    return result + "char";
-  if (type.name() == "Vector" || type.name() == "Matrix")
-    return result + "double";
-  if (type.name() == "int" || type.name() == "size_t")
-    return result + "numeric";
-  if (type.name() == "bool")
-    return result + "logical";
-  return result + type.name();
-}
-
-/* ************************************************************************* */
-void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
-  file.oss << "  ";
-
-  string cppType = type.qualifiedName("::");
-  string matlabUniqueType = type.qualifiedName();
-  bool isNotScalar = !type.isScalar();
-
-  // We cannot handle scalar non const references
-  if (!isNotScalar && is_ref && !is_const) {
-    throw std::runtime_error("Cannot unwrap a scalar non-const reference");
-  }
-
-  if (is_ptr && type.category != Qualified::EIGEN)
-    // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer
-    file.oss << "boost::shared_ptr<" << cppType << "> " << name
-        << " = unwrap_shared_ptr< ";
-  else if (is_ref && isNotScalar && type.category != Qualified::EIGEN)
-    // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer
-    file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< ";
-  else
-    // Not a pointer, or a reference to a scalar type. Therefore, emit an "unwrap" call
-    // unwrap is specified in matlab.h as a series of template specializations
-    // that know how to unpack the expected MATLAB object
-    // example: double tol = unwrap< double >(in[2]);
-    // example: Vector v = unwrap< Vector >(in[1]);
-    file.oss << cppType << " " << name << " = unwrap< ";
-
-  file.oss << cppType << " >(" << matlabName;
-  if( (is_ptr || is_ref) && isNotScalar && type.category != Qualified::EIGEN)
-    file.oss << ", \"ptr_" << matlabUniqueType << "\"";
-  file.oss << ");" << endl;
-}
-
-/* ************************************************************************* */
-void Argument::proxy_check(FileWriter& proxyFile, const string& s) const {
-  proxyFile.oss << "isa(" << s << ",'" << matlabClass(".") << "')";
-  if (type.name() == "Vector")
-    proxyFile.oss << " && size(" << s << ",2)==1";
-}
-
-/* ************************************************************************* */
-void Argument::emit_cython_pxd(
-    FileWriter& file, const std::string& className,
-    const std::vector<std::string>& templateArgs) const {
-  string cythonType = type.pxdClassName();
-  if (cythonType == "This") cythonType = className;
-  else if (type.isEigen())
-    cythonType = "const " + cythonType + "&";
-  else if (type.match(templateArgs))
-    cythonType = type.name();
-
-  // add modifier
-  if (!type.isEigen()) {
-    if (is_ptr) cythonType = "shared_ptr[" + cythonType + "]&";
-    if (is_ref) cythonType = cythonType + "&";
-    if (is_const) cythonType = "const " + cythonType;
-  }
-
-  file.oss << cythonType << " " << name;
-}
-
-/* ************************************************************************* */
-void Argument::emit_cython_pyx(FileWriter& file) const {
-  file.oss << type.pyxArgumentType() << " " << name;
-}
-
-/* ************************************************************************* */
-std::string Argument::pyx_convertEigenTypeAndStorageOrder() const {
-  if (!type.isEigen())
-    return "";
-  return name + " = " + name + ".astype(float, order=\'F\', copy=False)";
-}
-
-/* ************************************************************************* */
-std::string Argument::pyx_asParam() const {
-  string cythonType = type.pxdClassName();
-  string cythonVar;
-  if (type.isNonBasicType()) {
-    cythonVar = name + "." + type.shared_pxd_obj_in_pyx();
-    if (!is_ptr) cythonVar = "deref(" + cythonVar + ")";
-  } else if (type.isEigen()) {
-    cythonVar = "<" + cythonType + ">" + "(Map[" + cythonType + "](" + name + "))";
-  } else {
-    cythonVar = name;
-  }
-  return cythonVar;
-}
-
-/* ************************************************************************* */
-string ArgumentList::types() const {
-  string str;
-  bool first = true;
-  for(Argument arg: *this) {
-    if (!first)
-      str += ",";
-    str += arg.type.name();
-    first = false;
-  }
-  return str;
-}
-
-/* ************************************************************************* */
-string ArgumentList::signature() const {
-  string sig;
-  bool cap = false;
-
-  for(Argument arg: *this) {
-    for(char ch: arg.type.name())
-      if (isupper(ch)) {
-        sig += ch;
-        //If there is a capital letter, we don't want to read it below
-        cap = true;
-      }
-    if (!cap)
-      sig += arg.type.name()[0];
-    //Reset to default
-    cap = false;
-  }
-
-  return sig;
-}
-
-/* ************************************************************************* */
-string ArgumentList::names() const {
-  string str;
-  bool first = true;
-  for(Argument arg: *this) {
-    if (!first)
-      str += ",";
-    str += arg.name;
-    first = false;
-  }
-  return str;
-}
-
-/* ************************************************************************* */
-bool ArgumentList::allScalar() const {
-  for(Argument arg: *this)
-    if (!arg.type.isScalar())
-      return false;
-  return true;
-}
-
-/* ************************************************************************* */
-void ArgumentList::matlab_unwrap(FileWriter& file, int start) const {
-  int index = start;
-  for(Argument arg: *this) {
-    stringstream buf;
-    buf << "in[" << index << "]";
-    arg.matlab_unwrap(file, buf.str());
-    index++;
-  }
-}
-
-/* ************************************************************************* */
-void ArgumentList::emit_prototype(FileWriter& file, const string& name) const {
-  file.oss << name << "(";
-  bool first = true;
-  for(Argument arg: *this) {
-    if (!first)
-      file.oss << ", ";
-    file.oss << arg.type.name() << " " << arg.name;
-    first = false;
-  }
-  file.oss << ")";
-}
-
-/* ************************************************************************* */
-void ArgumentList::emit_cython_pxd(
-    FileWriter& file, const std::string& className,
-    const std::vector<std::string>& templateArgs) const {
-  for (size_t j = 0; j<size(); ++j) {
-    at(j).emit_cython_pxd(file, className, templateArgs);
-    if (j<size()-1) file.oss << ", ";
-  }
-}
-
-/* ************************************************************************* */
-void ArgumentList::emit_cython_pyx(FileWriter& file) const {
-  for (size_t j = 0; j < size(); ++j) {
-    at(j).emit_cython_pyx(file);
-    if (j < size() - 1) file.oss << ", ";
-  }
-}
-
-/* ************************************************************************* */
-std::string ArgumentList::pyx_convertEigenTypeAndStorageOrder(const std::string& indent) const {
-  string ret, conversion;
-  for (size_t j = 0; j < size(); ++j) {
-    conversion = at(j).pyx_convertEigenTypeAndStorageOrder();
-    if (!conversion.empty())
-      ret += indent + conversion + "\n";
-  }
-  return ret;
-}
-
-/* ************************************************************************* */
-std::string ArgumentList::pyx_asParams() const {
-  string ret;
-  for (size_t j = 0; j < size(); ++j) {
-    ret += at(j).pyx_asParam();
-    if (j < size() - 1) ret += ", ";
-  }
-  return ret;
-}
-
-/* ************************************************************************* */
-std::string ArgumentList::pyx_paramsList() const {
-  string s;
-  for (size_t j = 0; j < size(); ++j) {
-    s += "'" + at(j).name + "'";
-    if (j < size() - 1) s += ", ";
-  }
-  return s;
-}
-
-/* ************************************************************************* */
-std::string ArgumentList::pyx_castParamsToPythonType(
-    const std::string& indent) const {
-  if (size() == 0)
-    return "";
-
-  // cast params to their correct python argument type to pass in the function call later
-  string s;
-  for (size_t j = 0; j < size(); ++j)
-    s += indent + at(j).name + " = <" + at(j).type.pyxArgumentType()
-        + ">(__params[" + std::to_string(j) + "])\n";
-  return s;
-}
-
-/* ************************************************************************* */
-void ArgumentList::proxy_check(FileWriter& proxyFile) const {
-  // Check nr of arguments
-  proxyFile.oss << "if length(varargin) == " << size();
-  if (size() > 0)
-    proxyFile.oss << " && ";
-  // ...and their type.names
-  bool first = true;
-  for (size_t i = 0; i < size(); i++) {
-    if (!first)
-      proxyFile.oss << " && ";
-    string s = "varargin{" + boost::lexical_cast<string>(i + 1) + "}";
-    (*this)[i].proxy_check(proxyFile, s);
-    first = false;
-  }
-  proxyFile.oss << "\n";
-}
-
-/* ************************************************************************* */
-
diff --git a/wrap/Argument.h b/wrap/Argument.h
deleted file mode 100644
index c08eb0be9..000000000
--- a/wrap/Argument.h
+++ /dev/null
@@ -1,242 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Argument.h
- * @brief arguments to constructors and methods
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#pragma once
-
-#include "TemplateSubstitution.h"
-#include "FileWriter.h"
-#include "ReturnValue.h"
-
-namespace wrap {
-
-/// Argument class
-struct Argument {
-  Qualified type;
-  std::string name;
-  bool is_const, is_ref, is_ptr;
-
-  Argument() :
-      is_const(false), is_ref(false), is_ptr(false) {
-  }
-
-  Argument(const Qualified& t, const std::string& n) :
-      type(t), name(n), is_const(false), is_ref(false), is_ptr(false) {
-  }
-
-  bool isSameSignature(const Argument& other) const {
-    return type == other.type
-        && is_const == other.is_const && is_ref == other.is_ref
-        && is_ptr == other.is_ptr;
-  }
-
-  bool operator==(const Argument& other) const {
-    return type == other.type && name == other.name
-        && is_const == other.is_const && is_ref == other.is_ref
-        && is_ptr == other.is_ptr;
-  }
-
-  Argument expandTemplate(const TemplateSubstitution& ts) const;
-
-  /// return MATLAB class for use in isa(x,class)
-  std::string matlabClass(const std::string& delim = "") const;
-
-  /// MATLAB code generation, MATLAB to C++
-  void matlab_unwrap(FileWriter& file, const std::string& matlabName) const;
-
-  /**
-   * emit checking argument to MATLAB proxy
-   * @param proxyFile output stream
-   */
-  void proxy_check(FileWriter& proxyFile, const std::string& s) const;
-
-  /**
-   * emit arguments for cython pxd
-   * @param file output stream
-   */
-  void emit_cython_pxd(FileWriter& file, const std::string& className,
-                       const std::vector<std::string>& templateArgs) const;
-  void emit_cython_pyx(FileWriter& file) const;
-  std::string pyx_asParam() const;
-  std::string pyx_convertEigenTypeAndStorageOrder() const;
-
-  friend std::ostream& operator<<(std::ostream& os, const Argument& arg) {
-    os << (arg.is_const ? "const " : "") << arg.type << (arg.is_ptr ? "*" : "")
-        << (arg.is_ref ? "&" : "");
-    return os;
-  }
-
-};
-
-/// Argument list is just a container with Arguments
-struct ArgumentList: public std::vector<Argument> {
-
-  /// create a comma-separated string listing all argument types (not used)
-  std::string types() const;
-
-  /// create a short "signature" string
-  std::string signature() const;
-
-  /// create a comma-separated string listing all argument names, used in m-files
-  std::string names() const;
-
-  /// Check if all arguments scalar
-  bool allScalar() const;
-
-  ArgumentList expandTemplate(const TemplateSubstitution& ts) const;
-
-  bool isSameSignature(const ArgumentList& other) const {
-    for(size_t i = 0; i<size(); ++i)
-      if (!at(i).isSameSignature(other[i])) return false;
-    return true;
-  }
-
-  // MATLAB code generation:
-
-  /**
-   * emit code to unwrap arguments
-   * @param file output stream
-   * @param start initial index for input array, set to 1 for method
-   */
-  void matlab_unwrap(FileWriter& file, int start = 0) const; // MATLAB to C++
-
-  /**
-   * emit MATLAB prototype
-   * @param file output stream
-   * @param name of method or function
-   */
-  void emit_prototype(FileWriter& file, const std::string& name) const;
-
-  /**
-   * emit arguments for cython pxd
-   * @param file output stream
-   */
-  void emit_cython_pxd(FileWriter& file, const std::string& className,
-                       const std::vector<std::string>& templateArgs) const;
-  void emit_cython_pyx(FileWriter& file) const;
-  std::string pyx_asParams() const;
-  std::string pyx_paramsList() const;
-  std::string pyx_castParamsToPythonType(const std::string& indent) const;
-  std::string pyx_convertEigenTypeAndStorageOrder(const std::string& indent) const;
-
-  /**
-   * emit checking arguments to MATLAB proxy
-   * @param proxyFile output stream
-   */
-  void proxy_check(FileWriter& proxyFile) const;
-
-  /// Output stream operator
-  friend std::ostream& operator<<(std::ostream& os,
-      const ArgumentList& argList) {
-    os << "(";
-    if (argList.size() > 0)
-      os << argList.front();
-    if (argList.size() > 1)
-      for (size_t i = 1; i < argList.size(); i++)
-        os << ", " << argList[i];
-    os << ")";
-    return os;
-  }
-
-};
-
-/* ************************************************************************* */
-// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
-struct ArgumentGrammar: public classic::grammar<ArgumentGrammar> {
-
-  wrap::Argument& result_; ///< successful parse will be placed in here
-  TypeGrammar argument_type_g; ///< Type parser for Argument::type
-
-  /// Construct type grammar and specify where result is placed
-  ArgumentGrammar(wrap::Argument& result) :
-      result_(result), argument_type_g(result.type) {
-  }
-
-  /// Definition of type grammar
-  template<typename ScannerT>
-  struct definition: BasicRules<ScannerT> {
-
-    typedef classic::rule<ScannerT> Rule;
-
-    Rule argument_p;
-
-    definition(ArgumentGrammar const& self) {
-      using namespace classic;
-
-      // NOTE: allows for pointers to all types
-      // Slightly more permissive than before on basis/eigen type qualification
-      // Also, currently parses Point2*&, can't make it work otherwise :-(
-      argument_p = !str_p("const")[assign_a(self.result_.is_const, T)] //
-          >> self.argument_type_g //
-          >> !ch_p('*')[assign_a(self.result_.is_ptr, T)]
-          >> !ch_p('&')[assign_a(self.result_.is_ref, T)]
-          >> BasicRules<ScannerT>::name_p[assign_a(self.result_.name)];
-    }
-
-    Rule const& start() const {
-      return argument_p;
-    }
-
-  };
-};
-// ArgumentGrammar
-
-/* ************************************************************************* */
-// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
-struct ArgumentListGrammar: public classic::grammar<ArgumentListGrammar> {
-
-  wrap::ArgumentList& result_; ///< successful parse will be placed in here
-
-  /// Construct type grammar and specify where result is placed
-  ArgumentListGrammar(wrap::ArgumentList& result) :
-      result_(result) {
-  }
-
-  /// Definition of type grammar
-  template<typename ScannerT>
-  struct definition {
-
-    const Argument arg0; ///< used to reset arg
-    Argument arg; ///< temporary argument for use during parsing
-    ArgumentGrammar argument_g; ///< single Argument parser
-
-    classic::rule<ScannerT> argument_p, argumentList_p;
-
-    definition(ArgumentListGrammar const& self) :
-        argument_g(arg) {
-      using namespace classic;
-
-      argument_p = argument_g //
-          [classic::push_back_a(self.result_, arg)] //
-          [assign_a(arg, arg0)];
-
-      argumentList_p = '(' >> !argument_p >> *(',' >> argument_p) >> ')';
-    }
-
-    classic::rule<ScannerT> const& start() const {
-      return argumentList_p;
-    }
-
-  };
-};
-// ArgumentListGrammar
-
-/* ************************************************************************* */
-
-}// \namespace wrap
-
diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt
deleted file mode 100644
index c04a44edb..000000000
--- a/wrap/CMakeLists.txt
+++ /dev/null
@@ -1,45 +0,0 @@
-# Build/install Wrap
-
-set(WRAP_BOOST_LIBRARIES
-  Boost::system
-  Boost::filesystem
-  Boost::thread
-)
-
-# Allow for disabling serialization to handle errors related to Clang's linker
-option(GTSAM_WRAP_SERIALIZATION "If enabled, allows for wrapped objects to be saved via boost.serialization" ON)
-
-# Build the executable itself
-file(GLOB wrap_srcs "*.cpp")
-file(GLOB wrap_headers "*.h")
-list(REMOVE_ITEM wrap_srcs ${CMAKE_CURRENT_SOURCE_DIR}/wrap.cpp)
-add_library(wrap_lib STATIC ${wrap_srcs} ${wrap_headers})
-target_include_directories(wrap_lib PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
-)
-if (NOT GTSAM_WRAP_SERIALIZATION)
-  target_compile_definitions(wrap_lib PUBLIC -DWRAP_DISABLE_SERIALIZE)
-endif()
-
-# Apply build flags:
-gtsam_apply_build_flags(wrap_lib)
-
-target_link_libraries(wrap_lib ${WRAP_BOOST_LIBRARIES})
-gtsam_assign_source_folders(${wrap_srcs} ${wrap_headers})
-add_executable(wrap wrap.cpp)
-target_link_libraries(wrap PRIVATE wrap_lib)
-
-# Set folder in Visual Studio
-file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
-set_target_properties(wrap_lib wrap PROPERTIES FOLDER "${relative_path}")
-
-# Install wrap binary and export target
-install(TARGETS wrap EXPORT GTSAM-exports DESTINATION ${CMAKE_INSTALL_BINDIR})
-list(APPEND GTSAM_EXPORTED_TARGETS wrap)
-set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
-
-# Install matlab header
-install(FILES matlab.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wrap)
-
-# Build tests
-add_subdirectory(tests)
diff --git a/wrap/Class.cpp b/wrap/Class.cpp
deleted file mode 100644
index 65ce9eab7..000000000
--- a/wrap/Class.cpp
+++ /dev/null
@@ -1,897 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Class.cpp
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#include "Class.h"
-#include "utilities.h"
-#include "Argument.h"
-#include <unordered_set>
-
-#include <boost/lexical_cast.hpp>
-#include <boost/range/adaptor/map.hpp>
-#include <boost/range/algorithm/copy.hpp>
-#include <boost/tuple/tuple.hpp>
-#include <boost/iterator/zip_iterator.hpp>
-#include <boost/range/combine.hpp>
-#include <boost/range/algorithm/remove_if.hpp>
-
-#include <vector>
-#include <iostream>
-#include <fstream>
-#include <sstream>
-#include <iterator>     // std::ostream_iterator
-//#include <cstdint> // on Linux GCC: fails with error regarding needing C++0x std flags
-//#include <cinttypes> // same failure as above
-#include <stdint.h> // works on Linux GCC
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-void Class::assignParent(const Qualified& parent) {
-  parentClass.reset(parent);
-}
-
-/* ************************************************************************* */
-boost::optional<string> Class::qualifiedParent() const {
-  boost::optional<string> result = boost::none;
-  if (parentClass)
-    result = parentClass->qualifiedName("::");
-  return result;
-}
-
-/* ************************************************************************* */
-static void handleException(const out_of_range& oor,
-    const Class::Methods& methods) {
-  cerr << "Class::method: key not found: " << oor.what() << ", methods are:\n";
-  using boost::adaptors::map_keys;
-  ostream_iterator<string> out_it(cerr, "\n");
-  boost::copy(methods | map_keys, out_it);
-}
-
-/* ************************************************************************* */
-// Method& Class::mutableMethod(Str key) {
-//   try {
-//     return methods_.at(key);
-//   } catch (const out_of_range& oor) {
-//     handleException(oor, methods_);
-//     throw runtime_error("Internal error in wrap");
-//   }
-// }
-
-/* ************************************************************************* */
-const Method& Class::method(Str key) const {
-  try {
-    return methods_.at(key);
-  } catch (const out_of_range& oor) {
-    handleException(oor, methods_);
-    throw runtime_error("Internal error in wrap");
-  }
-}
-
-/* ************************************************************************* */
-void Class::matlab_proxy(Str toolboxPath, Str wrapperName,
-    const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile,
-    vector<string>& functionNames) const {
-
-  // Create namespace folders
-  createNamespaceStructure(namespaces(), toolboxPath);
-
-  // open destination classFile
-  string classFile = matlabName(toolboxPath);
-  FileWriter proxyFile(classFile, verbose_, "%");
-
-  // get the name of actual matlab object
-  const string matlabQualName = qualifiedName(".");
-  const string matlabUniqueName = qualifiedName();
-  const string cppName = qualifiedName("::");
-
-  // emit class proxy code
-  // we want our class to inherit the handle class for memory purposes
-  const string parent =
-      parentClass ? parentClass->qualifiedName(".") : "handle";
-  comment_fragment(proxyFile);
-  proxyFile.oss << "classdef " << name() << " < " << parent << endl;
-  proxyFile.oss << "  properties\n";
-  proxyFile.oss << "    ptr_" << matlabUniqueName << " = 0\n";
-  proxyFile.oss << "  end\n";
-  proxyFile.oss << "  methods\n";
-
-  // Constructor
-  proxyFile.oss << "    function obj = " << name() << "(varargin)\n";
-  // Special pointer constructors - one in MATLAB to create an object and
-  // assign a pointer returned from a C++ function.  In turn this MATLAB
-  // constructor calls a special C++ function that just adds the object to
-  // its collector.  This allows wrapped functions to return objects in
-  // other wrap modules - to add these to their collectors the pointer is
-  // passed from one C++ module into matlab then back into the other C++
-  // module.
-  pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName,
-      functionNames);
-  wrapperFile.oss << "\n";
-
-  // Regular constructors
-  boost::optional<string> cppBaseName = qualifiedParent();
-  for (size_t i = 0; i < constructor.nrOverloads(); i++) {
-    ArgumentList args = constructor.argumentList(i);
-    const int id = (int) functionNames.size();
-    constructor.proxy_fragment(proxyFile, wrapperName, (bool) parentClass, id,
-        args);
-    const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile,
-        cppName, matlabUniqueName, cppBaseName, id, args);
-    wrapperFile.oss << "\n";
-    functionNames.push_back(wrapFunctionName);
-  }
-  proxyFile.oss << "      else\n";
-  proxyFile.oss << "        error('Arguments do not match any overload of "
-      << matlabQualName << " constructor');\n";
-  proxyFile.oss << "      end\n";
-  if (parentClass)
-    proxyFile.oss << "      obj = obj@" << parentClass->qualifiedName(".")
-        << "(uint64(" << ptr_constructor_key << "), base_ptr);\n";
-  proxyFile.oss << "      obj.ptr_" << matlabUniqueName << " = my_ptr;\n";
-  proxyFile.oss << "    end\n\n";
-
-  // Deconstructor
-  {
-    const int id = (int) functionNames.size();
-    deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id);
-    proxyFile.oss << "\n";
-    const string functionName = deconstructor.wrapper_fragment(wrapperFile,
-        cppName, matlabUniqueName, id);
-    wrapperFile.oss << "\n";
-    functionNames.push_back(functionName);
-  }
-  proxyFile.oss
-      << "    function display(obj), obj.print(''); end\n    %DISPLAY Calls print on the object\n";
-  proxyFile.oss
-      << "    function disp(obj), obj.display; end\n    %DISP Calls print on the object\n";
-
-  // Methods
-  for(const Methods::value_type& name_m: methods_) {
-    const Method& m = name_m.second;
-    m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName,
-        matlabUniqueName, wrapperName, typeAttributes, functionNames);
-    proxyFile.oss << "\n";
-    wrapperFile.oss << "\n";
-  }
-  if (hasSerialization)
-    serialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames);
-
-  proxyFile.oss << "  end\n";
-  proxyFile.oss << "\n";
-  proxyFile.oss << "  methods(Static = true)\n";
-
-  // Static methods
-  for(const StaticMethods::value_type& name_m: static_methods) {
-    const StaticMethod& m = name_m.second;
-    m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName,
-        matlabUniqueName, wrapperName, typeAttributes, functionNames);
-    proxyFile.oss << "\n";
-    wrapperFile.oss << "\n";
-  }
-  if (hasSerialization)
-    deserialization_fragments(proxyFile, wrapperFile, wrapperName,
-        functionNames);
-
-  proxyFile.oss << "  end\n";
-  proxyFile.oss << "end\n";
-
-  // Close file
-  proxyFile.emit(true);
-}
-
-/* ************************************************************************* */
-void Class::pointer_constructor_fragments(FileWriter& proxyFile,
-    FileWriter& wrapperFile, Str wrapperName,
-    vector<string>& functionNames) const {
-
-  const string matlabUniqueName = qualifiedName();
-  const string cppName = qualifiedName("::");
-
-  const int collectorInsertId = (int) functionNames.size();
-  const string collectorInsertFunctionName = matlabUniqueName
-      + "_collectorInsertAndMakeBase_"
-      + boost::lexical_cast<string>(collectorInsertId);
-  functionNames.push_back(collectorInsertFunctionName);
-
-  int upcastFromVoidId;
-  string upcastFromVoidFunctionName;
-  if (isVirtual) {
-    upcastFromVoidId = (int) functionNames.size();
-    upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_"
-        + boost::lexical_cast<string>(upcastFromVoidId);
-    functionNames.push_back(upcastFromVoidFunctionName);
-  }
-
-  // MATLAB constructor that assigns pointer to matlab object then calls c++
-  // function to add the object to the collector.
-  if (isVirtual) {
-    proxyFile.oss
-        << "      if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))";
-  } else {
-    proxyFile.oss << "      if nargin == 2";
-  }
-  proxyFile.oss << " && isa(varargin{1}, 'uint64') && varargin{1} == uint64("
-      << ptr_constructor_key << ")\n";
-  if (isVirtual) {
-    proxyFile.oss << "        if nargin == 2\n";
-    proxyFile.oss << "          my_ptr = varargin{2};\n";
-    proxyFile.oss << "        else\n";
-    proxyFile.oss << "          my_ptr = " << wrapperName << "("
-        << upcastFromVoidId << ", varargin{2});\n";
-    proxyFile.oss << "        end\n";
-  } else {
-    proxyFile.oss << "        my_ptr = varargin{2};\n";
-  }
-  if (!parentClass) // If this class has a base class, we'll get a base class pointer back
-    proxyFile.oss << "        ";
-  else
-    proxyFile.oss << "        base_ptr = ";
-  proxyFile.oss << wrapperName << "(" << collectorInsertId << ", my_ptr);\n"; // Call collector insert and get base class ptr
-
-  // C++ function to add pointer from MATLAB to collector.  The pointer always
-  // comes from a C++ return value; this mechanism allows the object to be added
-  // to a collector in a different wrap module.  If this class has a base class,
-  // a new pointer to the base class is allocated and returned.
-  wrapperFile.oss << "void " << collectorInsertFunctionName
-      << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
-  wrapperFile.oss << "{\n";
-  wrapperFile.oss << "  mexAtExit(&_deleteAllObjects);\n";
-  // Typedef boost::shared_ptr
-  wrapperFile.oss << "  typedef boost::shared_ptr<" << cppName << "> Shared;\n";
-  wrapperFile.oss << "\n";
-  // Get self pointer passed in
-  wrapperFile.oss
-      << "  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));\n";
-  // Add to collector
-  wrapperFile.oss << "  collector_" << matlabUniqueName << ".insert(self);\n";
-  // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy)
-  boost::optional<string> cppBaseName = qualifiedParent();
-  if (cppBaseName) {
-    wrapperFile.oss << "\n";
-    wrapperFile.oss << "  typedef boost::shared_ptr<" << *cppBaseName
-        << "> SharedBase;\n";
-    wrapperFile.oss
-        << "  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n";
-    wrapperFile.oss
-        << "  *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);\n";
-  }
-  wrapperFile.oss << "}\n";
-
-  // If this is a virtual function, C++ function to dynamic upcast it from a
-  // shared_ptr<void>.  This mechanism allows automatic dynamic creation of the
-  // real underlying derived-most class when a C++ method returns a virtual
-  // base class.
-  if (isVirtual)
-    wrapperFile.oss << "\n"
-        "void " << upcastFromVoidFunctionName
-        << "(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {\n"
-            "  mexAtExit(&_deleteAllObjects);\n"
-            "  typedef boost::shared_ptr<" << cppName
-        << "> Shared;\n"
-            "  boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));\n"
-            "  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"
-            "  Shared *self = new Shared(boost::static_pointer_cast<" << cppName
-        << ">(*asVoid));\n"
-            "  *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;\n"
-            "}\n";
-}
-
-/* ************************************************************************* */
-Class Class::expandTemplate(const TemplateSubstitution& ts) const {
-  Class inst = *this;
-  inst.methods_ = expandMethodTemplate(methods_, ts);
-  inst.static_methods = expandMethodTemplate(static_methods, ts);
-  inst.constructor = constructor.expandTemplate(ts);
-  inst.deconstructor.name = inst.name();
-  return inst;
-}
-
-/* ************************************************************************* */
-vector<Class> Class::expandTemplate(Str templateArg,
-    const vector<Qualified>& instantiations) const {
-  vector<Class> result;
-  for(const Qualified& instName: instantiations) {
-    Qualified expandedClass = (Qualified) (*this);
-    expandedClass.expand(instName.name());
-    const TemplateSubstitution ts(templateArg, instName, expandedClass);
-    Class inst = expandTemplate(ts);
-    inst.name_ = expandedClass.name();
-    inst.templateArgs.clear();
-    inst.typedefName = qualifiedName("::") + "<" + instName.qualifiedName("::")
-        + ">";
-    inst.templateInstTypeList.push_back(instName);
-    inst.templateClass = *this;
-    result.push_back(inst);
-  }
-  return result;
-}
-
-/* ************************************************************************* */
-vector<Class> Class::expandTemplate(Str templateArg,
-    const vector<int>& integers) const {
-  vector<Class> result;
-  for(int i: integers) {
-    Qualified expandedClass = (Qualified) (*this);
-    stringstream ss; ss << i;
-    string instName = ss.str();
-    expandedClass.expand(instName);
-    const TemplateSubstitution ts(templateArg, instName, expandedClass);
-    Class inst = expandTemplate(ts);
-    inst.name_ = expandedClass.name();
-    inst.templateArgs.clear();
-    inst.typedefName = qualifiedName("::") + "<" + instName + ">";
-    result.push_back(inst);
-  }
-  return result;
-}
-
-/* ************************************************************************* */
-void Class::addMethod(bool verbose, bool is_const, Str methodName,
-                      const ArgumentList& argumentList,
-                      const ReturnValue& returnValue, const Template& tmplate) {
-  // Check if templated
-  if (tmplate.valid()) {
-    try {
-      templateMethods_[methodName].addOverload(methodName, argumentList,
-                                               returnValue, is_const,
-                                               tmplate.argName(), verbose);
-    } catch (const std::runtime_error& e) {
-      throw std::runtime_error("Class::addMethod: error adding " + name_ +
-                               "::" + methodName + "\n" + e.what());
-    }
-    // Create method to expand
-    // For all values of the template argument, create a new method
-    for (const Qualified& instName : tmplate.argValues()) {
-      const TemplateSubstitution ts(tmplate.argName(), instName, *this);
-      // substitute template in arguments
-      ArgumentList expandedArgs = argumentList.expandTemplate(ts);
-      // do the same for return type
-      ReturnValue expandedRetVal = returnValue.expandTemplate(ts);
-      // Now stick in new overload stack with expandedMethodName key
-      // but note we use the same, unexpanded methodName in overload
-      string expandedMethodName = methodName + instName.name();
-      try {
-        methods_[expandedMethodName].addOverload(methodName, expandedArgs,
-                                                 expandedRetVal, is_const,
-                                                 instName, verbose);
-      } catch (const std::runtime_error& e) {
-        throw std::runtime_error("Class::addMethod: error adding " + name_ +
-                                 "::" + expandedMethodName + "\n" + e.what());
-      }
-    }
-  } else {
-    try {
-      // just add overload
-      methods_[methodName].addOverload(methodName, argumentList, returnValue,
-                                       is_const, boost::none, verbose);
-      nontemplateMethods_[methodName].addOverload(methodName, argumentList,
-                                                  returnValue, is_const,
-                                                  boost::none, verbose);
-    } catch (const std::runtime_error& e) {
-      throw std::runtime_error("Class::addMethod: error adding " + name_ +
-                               "::" + methodName + "\n" + e.what());
-    }
-  }
-}
-
-/* ************************************************************************* */
-void Class::erase_serialization(Methods& methods) {
-  Methods::iterator it = methods.find("serializable");
-  if (it != methods.end()) {
-#ifndef WRAP_DISABLE_SERIALIZE
-    isSerializable = true;
-#else
-    // cout << "Ignoring serializable() flag in class " << name << endl;
-#endif
-    methods.erase(it);
-  }
-
-  it = methods.find("serialize");
-  if (it != methods.end()) {
-#ifndef WRAP_DISABLE_SERIALIZE
-    isSerializable = true;
-    hasSerialization = true;
-#else
-    // cout << "Ignoring serialize() flag in class " << name << endl;
-#endif
-    methods.erase(it);
-  }
-}
-
-void Class::erase_serialization() {
-  erase_serialization(methods_);
-  erase_serialization(nontemplateMethods_);
-}
-
-/* ************************************************************************* */
-void Class::verifyAll(vector<string>& validTypes, bool& hasSerialiable) const {
-
-  hasSerialiable |= isSerializable;
-
-  // verify all of the function arguments
-  //TODO:verifyArguments<ArgumentList>(validTypes, constructor.args_list);
-  verifyArguments<StaticMethod>(validTypes, static_methods);
-  verifyArguments<Method>(validTypes, methods_);
-
-  // verify function return types
-  verifyReturnTypes<StaticMethod>(validTypes, static_methods);
-  verifyReturnTypes<Method>(validTypes, methods_);
-
-  // verify parents
-  boost::optional<string> parent = qualifiedParent();
-  if (parent
-      && find(validTypes.begin(), validTypes.end(), *parent)
-          == validTypes.end())
-    throw DependencyMissing(*parent, qualifiedName("::"));
-}
-
-/* ************************************************************************* */
-void Class::appendInheritedMethods(const Class& cls,
-    const vector<Class>& classes) {
-
-  if (cls.parentClass) {
-
-    // Find parent
-    for(const Class& parent: classes) {
-      // We found a parent class for our parent, TODO improve !
-      if (parent.name() == cls.parentClass->name()) {
-        methods_.insert(parent.methods_.begin(), parent.methods_.end());
-        appendInheritedMethods(parent, classes);
-      }
-    }
-  }
-}
-
-/* ************************************************************************* */
-void Class::removeInheritedNontemplateMethods(vector<Class>& classes) {
-  if (!parentClass) return;
-  // Find parent
-  auto parentIt = std::find_if(classes.begin(), classes.end(),
-      [&](const Class& cls) { return cls.name() == parentClass->name(); });
-  if (parentIt == classes.end()) return; // ignore if parent not found
-  Class& parent = *parentIt;
-
-  // Only check nontemplateMethods_
-  for(const string& methodName: nontemplateMethods_ | boost::adaptors::map_keys) {
-    // check if the method exists in its parent
-    // Check against parent's methods_ because all the methods of grand
-    // parent and grand-grand-parent, etc. are already included there
-    // This is to avoid looking into higher level grand parents...
-    auto it = parent.methods_.find(methodName);
-    if (it == parent.methods_.end()) continue; // if not: ignore!
-
-    Method& parentMethod = it->second;
-    Method& method = nontemplateMethods_[methodName];
-    // check if they have the same modifiers (const/static/templateArgs)
-    if (!method.isSameModifiers(parentMethod)) continue; // if not: ignore
-
-    // check and remove duplicate overloads
-    auto methodOverloads = boost::combine(method.returnVals_, method.argLists_);
-    auto parentMethodOverloads = boost::combine(parentMethod.returnVals_, parentMethod.argLists_);
-    auto result = boost::remove_if(
-        methodOverloads,
-        [&](boost::tuple<ReturnValue, ArgumentList> const& overload) {
-            bool found = std::find_if(
-                       parentMethodOverloads.begin(),
-                       parentMethodOverloads.end(),
-                       [&](boost::tuple<ReturnValue, ArgumentList> const&
-                               parentOverload) {
-                           return overload.get<0>() == parentOverload.get<0>() &&
-                                  overload.get<1>().isSameSignature(parentOverload.get<1>());
-                       }) != parentMethodOverloads.end();
-            return found;
-        });
-    // remove all duplicate overloads
-    method.returnVals_.erase(boost::get<0>(result.get_iterator_tuple()),
-                             method.returnVals_.end());
-    method.argLists_.erase(boost::get<1>(result.get_iterator_tuple()),
-                               method.argLists_.end());
-  }
-  // [Optional] remove the entire method if it has no overload
-  for (auto it = nontemplateMethods_.begin(), ite = nontemplateMethods_.end(); it != ite;)
-      if (it->second.nrOverloads() == 0) it = nontemplateMethods_.erase(it); else ++it;
-}
-
-/* ************************************************************************* */
-string Class::getTypedef() const {
-  string result;
-  for(Str namesp: namespaces()) {
-    result += ("namespace " + namesp + " { ");
-  }
-  result += ("typedef " + typedefName + " " + name() + ";");
-  for (size_t i = 0; i < namespaces().size(); ++i) {
-    result += " }";
-  }
-  return result;
-}
-
-/* ************************************************************************* */
-void Class::comment_fragment(FileWriter& proxyFile) const {
-  proxyFile.oss << "%class " << name() << ", see Doxygen page for details\n";
-  proxyFile.oss
-      << "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
-
-  constructor.comment_fragment(proxyFile);
-
-  if (!methods_.empty())
-    proxyFile.oss << "%\n%-------Methods-------\n";
-  for(const Methods::value_type& name_m: methods_)
-    name_m.second.comment_fragment(proxyFile);
-
-  if (!static_methods.empty())
-    proxyFile.oss << "%\n%-------Static Methods-------\n";
-  for(const StaticMethods::value_type& name_m: static_methods)
-    name_m.second.comment_fragment(proxyFile);
-
-  if (hasSerialization) {
-    proxyFile.oss << "%\n%-------Serialization Interface-------\n";
-    proxyFile.oss << "%string_serialize() : returns string\n";
-    proxyFile.oss << "%string_deserialize(string serialized) : returns "
-        << name() << "\n";
-  }
-
-  proxyFile.oss << "%\n";
-}
-
-/* ************************************************************************* */
-void Class::serialization_fragments(FileWriter& proxyFile,
-    FileWriter& wrapperFile, Str wrapperName,
-    vector<string>& functionNames) const {
-
-//void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-//{
-//  typedef boost::shared_ptr<Point3> Shared;
-//  checkArguments("string_serialize",nargout,nargin-1,0);
-//  Shared obj = unwrap_shared_ptr<Point3>(in[0], "ptr_Point3");
-//  ostringstream out_archive_stream;
-//  boost::archive::text_oarchive out_archive(out_archive_stream);
-//  out_archive << *obj;
-//  out[0] = wrap< string >(out_archive_stream.str());
-//}
-
-  int serialize_id = functionNames.size();
-  const string matlabQualName = qualifiedName(".");
-  const string matlabUniqueName = qualifiedName();
-  const string cppClassName = qualifiedName("::");
-  const string wrapFunctionNameSerialize = matlabUniqueName
-      + "_string_serialize_" + boost::lexical_cast<string>(serialize_id);
-  functionNames.push_back(wrapFunctionNameSerialize);
-
-  // call
-  //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-  wrapperFile.oss << "void " << wrapFunctionNameSerialize
-      << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
-  wrapperFile.oss << "{\n";
-  wrapperFile.oss << "  typedef boost::shared_ptr<" << cppClassName
-      << "> Shared;" << endl;
-
-  // check arguments - for serialize, no arguments
-  // example: checkArguments("string_serialize",nargout,nargin-1,0);
-  wrapperFile.oss
-      << "  checkArguments(\"string_serialize\",nargout,nargin-1,0);\n";
-
-  // get class pointer
-  // example: Shared obj = unwrap_shared_ptr<Point3>(in[0], "ptr_Point3");
-  wrapperFile.oss << "  Shared obj = unwrap_shared_ptr<" << cppClassName
-      << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
-
-  // Serialization boilerplate
-  wrapperFile.oss << "  ostringstream out_archive_stream;\n";
-  wrapperFile.oss
-      << "  boost::archive::text_oarchive out_archive(out_archive_stream);\n";
-  wrapperFile.oss << "  out_archive << *obj;\n";
-  wrapperFile.oss << "  out[0] = wrap< string >(out_archive_stream.str());\n";
-
-  // finish
-  wrapperFile.oss << "}\n";
-
-  // Generate code for matlab function
-//  function varargout string_serialize(this, varargin)
-//  % STRING_SERIALIZE usage: string_serialize() : returns string
-//  % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-//    if length(varargin) == 0
-//      varargout{1} = geometry_wrapper(15, this, varargin{:});
-//    else
-//      error('Arguments do not match any overload of function Point3.string_serialize');
-//    end
-//  end
-
-  proxyFile.oss
-      << "    function varargout = string_serialize(this, varargin)\n";
-  proxyFile.oss
-      << "      % STRING_SERIALIZE usage: string_serialize() : returns string\n";
-  proxyFile.oss
-      << "      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
-  proxyFile.oss << "      if length(varargin) == 0\n";
-  proxyFile.oss << "        varargout{1} = " << wrapperName << "("
-      << boost::lexical_cast<string>(serialize_id) << ", this, varargin{:});\n";
-  proxyFile.oss << "      else\n";
-  proxyFile.oss
-      << "        error('Arguments do not match any overload of function "
-      << matlabQualName << ".string_serialize');\n";
-  proxyFile.oss << "      end\n";
-  proxyFile.oss << "    end\n\n";
-
-  // Generate code for matlab save function
-//  function sobj = saveobj(obj)
-//    % SAVEOBJ Saves the object to a matlab-readable format
-//    sobj = obj.string_serialize();
-//  end
-
-  proxyFile.oss << "    function sobj = saveobj(obj)\n";
-  proxyFile.oss
-      << "      % SAVEOBJ Saves the object to a matlab-readable format\n";
-  proxyFile.oss << "      sobj = obj.string_serialize();\n";
-  proxyFile.oss << "    end\n";
-}
-
-/* ************************************************************************* */
-void Class::deserialization_fragments(FileWriter& proxyFile,
-    FileWriter& wrapperFile, Str wrapperName,
-    vector<string>& functionNames) const {
-  //void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-  //{
-  //  typedef boost::shared_ptr<Point3> Shared;
-  //  checkArguments("Point3.string_deserialize",nargout,nargin,1);
-  //  string serialized = unwrap< string >(in[0]);
-  //  istringstream in_archive_stream(serialized);
-  //  boost::archive::text_iarchive in_archive(in_archive_stream);
-  //  Shared output(new Point3(0,0,0));
-  //  in_archive >> *output;
-  //  out[0] = wrap_shared_ptr(output,"Point3", false);
-  //}
-  int deserialize_id = functionNames.size();
-  const string matlabQualName = qualifiedName(".");
-  const string matlabUniqueName = qualifiedName();
-  const string cppClassName = qualifiedName("::");
-  const string wrapFunctionNameDeserialize = matlabUniqueName
-      + "_string_deserialize_" + boost::lexical_cast<string>(deserialize_id);
-  functionNames.push_back(wrapFunctionNameDeserialize);
-
-  // call
-  wrapperFile.oss << "void " << wrapFunctionNameDeserialize
-      << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
-  wrapperFile.oss << "{\n";
-  wrapperFile.oss << "  typedef boost::shared_ptr<" << cppClassName
-      << "> Shared;" << endl;
-
-  // check arguments - for deserialize, 1 string argument
-  wrapperFile.oss << "  checkArguments(\"" << matlabUniqueName
-      << ".string_deserialize\",nargout,nargin,1);\n";
-
-  // string argument with deserialization boilerplate
-  wrapperFile.oss << "  string serialized = unwrap< string >(in[0]);\n";
-  wrapperFile.oss << "  istringstream in_archive_stream(serialized);\n";
-  wrapperFile.oss
-      << "  boost::archive::text_iarchive in_archive(in_archive_stream);\n";
-  wrapperFile.oss << "  Shared output(new " << cppClassName << "());\n";
-  wrapperFile.oss << "  in_archive >> *output;\n";
-  wrapperFile.oss << "  out[0] = wrap_shared_ptr(output,\"" << matlabQualName
-      << "\", false);\n";
-  wrapperFile.oss << "}\n";
-
-  // Generate matlab function
-//    function varargout = string_deserialize(varargin)
-//    % STRING_DESERIALIZE usage: string_deserialize() : returns Point3
-//    % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-//      if length(varargin) == 1
-//        varargout{1} = geometry_wrapper(18, varargin{:});
-//      else
-//        error('Arguments do not match any overload of function Point3.string_deserialize');
-//      end
-//    end
-
-  proxyFile.oss << "    function varargout = string_deserialize(varargin)\n";
-  proxyFile.oss
-      << "      % STRING_DESERIALIZE usage: string_deserialize() : returns "
-      << matlabQualName << "\n";
-  proxyFile.oss
-      << "      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
-  proxyFile.oss << "      if length(varargin) == 1\n";
-  proxyFile.oss << "        varargout{1} = " << wrapperName << "("
-      << boost::lexical_cast<string>(deserialize_id) << ", varargin{:});\n";
-  proxyFile.oss << "      else\n";
-  proxyFile.oss
-      << "        error('Arguments do not match any overload of function "
-      << matlabQualName << ".string_deserialize');\n";
-  proxyFile.oss << "      end\n";
-  proxyFile.oss << "    end\n\n";
-
-  // Generate matlab load function
-//    function obj = loadobj(sobj)
-//      % LOADOBJ Saves the object to a matlab-readable format
-//      obj = Point3.string_deserialize(sobj);
-//    end
-
-  proxyFile.oss << "    function obj = loadobj(sobj)\n";
-  proxyFile.oss
-      << "      % LOADOBJ Saves the object to a matlab-readable format\n";
-  proxyFile.oss << "      obj = " << matlabQualName
-      << ".string_deserialize(sobj);\n";
-  proxyFile.oss << "    end" << endl;
-}
-
-/* ************************************************************************* */
-string Class::getSerializationExport() const {
-  //BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal");
-  return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \""
-      + qualifiedName() + "\");";
-}
-
-/* ************************************************************************* */
-void Class::python_wrapper(FileWriter& wrapperFile) const {
-  wrapperFile.oss << "class_<" << name() << ">(\"" << name() << "\")\n";
-  constructor.python_wrapper(wrapperFile, name());
-  for(const StaticMethod& m: static_methods | boost::adaptors::map_values)
-    m.python_wrapper(wrapperFile, name());
-  for(const Method& m: methods_ | boost::adaptors::map_values)
-    m.python_wrapper(wrapperFile, name());
-  wrapperFile.oss << ";\n\n";
-}
-
-/* ************************************************************************* */
-void Class::emit_cython_pxd(FileWriter& pxdFile) const {
-  pxdFile.oss << "cdef extern from \"" << includeFile << "\"";
-  string ns = qualifiedNamespaces("::");
-  if (!ns.empty())
-    pxdFile.oss << " namespace \"" << ns << "\"";
-  pxdFile.oss << ":" << endl;
-  pxdFile.oss << "    cdef cppclass " << pxdClassName() << " \"" << qualifiedName("::") << "\"";
-  if (templateArgs.size()>0) {
-    pxdFile.oss << "[";
-    for(size_t i = 0; i<templateArgs.size(); ++i) {
-      pxdFile.oss << templateArgs[i];
-      if (i<templateArgs.size()-1) pxdFile.oss << ",";
-    }
-    pxdFile.oss << "]";
-  }
-  if (parentClass) pxdFile.oss << "(" <<  parentClass->pxdClassName() << ")";
-  pxdFile.oss << ":\n";
-
-  constructor.emit_cython_pxd(pxdFile, *this);
-  if (constructor.nrOverloads()>0) pxdFile.oss << "\n";
-
-  for(const StaticMethod& m: static_methods | boost::adaptors::map_values)
-    m.emit_cython_pxd(pxdFile, *this);
-  if (static_methods.size()>0) pxdFile.oss << "\n";
-
-  for(const Method& m: nontemplateMethods_ | boost::adaptors::map_values)
-    m.emit_cython_pxd(pxdFile, *this);
-
-  for(const TemplateMethod& m: templateMethods_ | boost::adaptors::map_values)
-    m.emit_cython_pxd(pxdFile, *this);
-  size_t numMethods = constructor.nrOverloads() + static_methods.size() +
-                      methods_.size() + templateMethods_.size();
-  if (numMethods == 0)
-      pxdFile.oss << "        pass\n";
-}
-/* ************************************************************************* */
-void Class::emit_cython_wrapper_pxd(FileWriter& pxdFile) const {
-  pxdFile.oss << "\ncdef class " << pyxClassName();
-  if (getParent())
-    pxdFile.oss << "(" << getParent()->pyxClassName() << ")";
-  pxdFile.oss << ":\n";
-  pxdFile.oss << "    cdef " << shared_pxd_class_in_pyx() << " "
-      << shared_pxd_obj_in_pyx() << "\n";
-  // cyCreateFromShared
-  pxdFile.oss << "    @staticmethod\n";
-  pxdFile.oss << "    cdef " << pyxClassName() << " cyCreateFromShared(const "
-      << shared_pxd_class_in_pyx() << "& other)\n";
-  for(const StaticMethod& m: static_methods | boost::adaptors::map_values)
-    m.emit_cython_wrapper_pxd(pxdFile, *this);
-  if (static_methods.size()>0) pxdFile.oss << "\n";
-}
-
-/* ************************************************************************* */
-void Class::pyxInitParentObj(FileWriter& pyxFile, const std::string& pyObj,
-                             const std::string& cySharedObj,
-                             const std::vector<Class>& allClasses) const {
-    if (parentClass) {
-      pyxFile.oss << pyObj << "." << parentClass->shared_pxd_obj_in_pyx() << " = "
-                  << "<" << parentClass->shared_pxd_class_in_pyx() << ">("
-                  << cySharedObj << ")\n";
-      // Find the parent class with name "parentClass" and point its cython obj
-      // to the same pointer
-      auto parent_it = find_if(allClasses.begin(), allClasses.end(),
-                               [this](const Class& cls) {
-                                   return cls.pxdClassName() ==
-                                          this->parentClass->pxdClassName();
-                               });
-      if (parent_it == allClasses.end()) {
-          cerr << "Can't find parent class: " << parentClass->pxdClassName();
-        throw std::runtime_error("Parent class not found!");
-      }
-      parent_it->pyxInitParentObj(pyxFile, pyObj, cySharedObj, allClasses);
-  }
-}
-
-/* ************************************************************************* */
-void Class::pyxDynamicCast(FileWriter& pyxFile, const Class& curLevel,
-                           const std::vector<Class>& allClasses) const {
-  std::string me = this->pyxClassName(), sharedMe = this->shared_pxd_class_in_pyx();
-  if (curLevel.parentClass) {
-    std::string parent = curLevel.parentClass->pyxClassName(),
-                parentObj = curLevel.parentClass->shared_pxd_obj_in_pyx(),
-                parentCythonClass = curLevel.parentClass->pxd_class_in_pyx();
-    pyxFile.oss << "def dynamic_cast_" << me << "_" << parent << "(" << parent
-                << " parent):\n";
-    pyxFile.oss << "    try:\n";
-    pyxFile.oss << "        return " << me << ".cyCreateFromShared(<" << sharedMe
-                << ">dynamic_pointer_cast[" << pxd_class_in_pyx() << ","
-                << parentCythonClass << "](parent." << parentObj
-                << "))\n";
-    pyxFile.oss << "    except:\n";
-    pyxFile.oss << "        raise TypeError('dynamic cast failed!')\n";
-    // Move up higher to one level: Find the parent class with name "parentClass"
-    auto parent_it = find_if(allClasses.begin(), allClasses.end(),
-                             [&curLevel](const Class& cls) {
-                                 return cls.pxdClassName() ==
-                                        curLevel.parentClass->pxdClassName();
-                             });
-    if (parent_it == allClasses.end()) {
-        cerr << "Can't find parent class: " << parentClass->pxdClassName();
-      throw std::runtime_error("Parent class not found!");
-    }
-    pyxDynamicCast(pyxFile, *parent_it, allClasses);
-  }
-}
-
-/* ************************************************************************* */
-void Class::emit_cython_pyx(FileWriter& pyxFile, const std::vector<Class>& allClasses) const {
-  pyxFile.oss << "cdef class " << pyxClassName();
-  if (parentClass) pyxFile.oss << "(" <<  parentClass->pyxClassName() << ")";
-  pyxFile.oss << ":\n";
-
-  // __init___
-  pyxFile.oss << "    def __init__(self, *args, **kwargs):\n";
-  pyxFile.oss << "        cdef list __params\n";
-  pyxFile.oss << "        self." << shared_pxd_obj_in_pyx() << " = " << shared_pxd_class_in_pyx() << "()\n";
-  pyxFile.oss << "        if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):\n            return\n";
-
-  // Constructors
-  constructor.emit_cython_pyx(pyxFile, *this);
-  pyxFile.oss << "        if (self." << shared_pxd_obj_in_pyx() << ".use_count()==0):\n";
-  pyxFile.oss << "            raise TypeError('" << pyxClassName()
-      << " construction failed!')\n";
-  pyxInitParentObj(pyxFile, "        self", "self." + shared_pxd_obj_in_pyx(), allClasses);
-  pyxFile.oss << "\n";
-
-  // cyCreateFromShared
-  pyxFile.oss << "    @staticmethod\n";
-  pyxFile.oss << "    cdef " << pyxClassName() << " cyCreateFromShared(const "
-              << shared_pxd_class_in_pyx() << "& other):\n"
-              << "        if other.get() == NULL:\n"
-              << "            raise RuntimeError('Cannot create object from a nullptr!')\n"
-              << "        cdef " << pyxClassName() << " return_value = " << pyxClassName() << "(cyCreateFromShared=True)\n"
-              << "        return_value." << shared_pxd_obj_in_pyx() << " = other\n";
-  pyxInitParentObj(pyxFile, "        return_value", "other", allClasses);
-  pyxFile.oss << "        return return_value" << "\n\n";
-
-  for(const StaticMethod& m: static_methods | boost::adaptors::map_values)
-    m.emit_cython_pyx(pyxFile, *this);
-  if (static_methods.size()>0) pyxFile.oss << "\n";
-
-  for(const Method& m: methods_ | boost::adaptors::map_values)
-    m.emit_cython_pyx(pyxFile, *this);
-
-  pyxDynamicCast(pyxFile, *this, allClasses);
-
-  pyxFile.oss << "\n\n";
-}
-
-/* ************************************************************************* */
diff --git a/wrap/Class.h b/wrap/Class.h
deleted file mode 100644
index 3df37fe67..000000000
--- a/wrap/Class.h
+++ /dev/null
@@ -1,315 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Class.h
- * @brief describe the C++ class that is being wrapped
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#pragma once
-
-#include "spirit.h"
-#include "Template.h"
-#include "Constructor.h"
-#include "Deconstructor.h"
-#include "Method.h"
-#include "StaticMethod.h"
-#include "TemplateMethod.h"
-#include "TypeAttributesTable.h"
-
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-variable"
-#endif
-#include <boost/lambda/bind.hpp>
-#include <boost/lambda/lambda.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
-
-namespace bl = boost::lambda;
-
-#include <boost/range/adaptor/map.hpp>
-#include <boost/optional.hpp>
-
-#include <string>
-#include <map>
-
-namespace wrap {
-
-/// Class has name, constructors, methods
-class Class: public Qualified {
-
-public:
-  typedef const std::string& Str;
-  typedef std::map<std::string, Method> Methods;
-  typedef std::map<std::string, StaticMethod> StaticMethods;
-  typedef std::map<std::string, TemplateMethod> TemplateMethods;
-
-private:
-
-  boost::optional<Qualified> parentClass; ///< The *single* parent
-  Methods methods_; ///< Class methods, including all expanded/instantiated template methods -- to be serialized to matlab and Python classes in Cython pyx
-  Methods nontemplateMethods_; ///< only nontemplate methods -- to be serialized into Cython pxd
-  TemplateMethods templateMethods_; ///< only template methods -- to be serialized into Cython pxd
-  // Method& mutableMethod(Str key);
-
-public:
-
-  StaticMethods static_methods; ///< Static methods
-
-  // Then the instance variables are set directly by the Module constructor
-  std::vector<std::string> templateArgs; ///< Template arguments
-  std::string typedefName; ///< The name to typedef *from*, if this class is actually a typedef, i.e. typedef [typedefName] [name]
-  std::vector<Qualified> templateInstTypeList; ///< the original typelist used to instantiate this class from a template.
-                                               ///< Empty if it's not an instantiation. Needed for template classes in Cython pxd.
-  boost::optional<Qualified> templateClass = boost::none; ///< qualified name of the original template class from which this class was instantiated.
-                                                          ///< boost::none if not an instantiation. Needed for template classes in Cython pxd.
-  bool isVirtual; ///< Whether the class is part of a virtual inheritance chain
-  bool isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports
-  bool hasSerialization; ///< Whether we should create the serialization functions
-  Constructor constructor; ///< Class constructors
-  Deconstructor deconstructor; ///< Deconstructor to deallocate C++ object
-  bool verbose_; ///< verbose flag
-  std::string includeFile;
-
-  /// Constructor creates an empty class
-  Class(bool verbose = true) :
-      parentClass(boost::none), isVirtual(false), isSerializable(false), hasSerialization(
-          false), deconstructor(verbose), verbose_(verbose) {
-  }
-
-  Class(const std::string& name, bool verbose = true)
-      : Qualified(name, Qualified::Category::CLASS),
-        parentClass(boost::none),
-        isVirtual(false),
-        isSerializable(false),
-        hasSerialization(false),
-        deconstructor(verbose),
-        verbose_(verbose) {}
-
-  void assignParent(const Qualified& parent);
-
-  boost::optional<std::string> qualifiedParent() const;
-  boost::optional<Qualified> getParent() const { return parentClass; }
-
-  size_t nrMethods() const {
-    return methods_.size();
-  }
-
-  const Method& method(Str key) const;
-
-  bool exists(Str name) const {
-      return methods_.find(name) != methods_.end();
-  }
-
-  // And finally MATLAB code is emitted, methods below called by Module::matlab_code
-  void matlab_proxy(Str toolboxPath, Str wrapperName,
-      const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile,
-      std::vector<std::string>& functionNames) const; ///< emit proxy class
-
-  Class expandTemplate(const TemplateSubstitution& ts) const;
-
-  std::vector<Class> expandTemplate(Str templateArg,
-      const std::vector<Qualified>& instantiations) const;
-
-  // Create new classes with integer template arguments
-  std::vector<Class> expandTemplate(Str templateArg,
-      const std::vector<int>& integers) const;
-
-  /// Add potentially overloaded, potentially templated method
-  void addMethod(bool verbose, bool is_const, Str methodName,
-      const ArgumentList& argumentList, const ReturnValue& returnValue,
-      const Template& tmplate);
-
-  /// Post-process classes for serialization markers
-  void erase_serialization(); // non-const !
-  void erase_serialization(Methods& methods); // non-const !
-
-  /// verify all of the function arguments
-  void verifyAll(std::vector<std::string>& functionNames,
-      bool& hasSerialiable) const;
-
-  void appendInheritedMethods(const Class& cls,
-      const std::vector<Class>& classes);
-
-  void removeInheritedNontemplateMethods(std::vector<Class>& classes);
-
-  /// The typedef line for this class, if this class is a typedef, otherwise returns an empty string.
-  std::string getTypedef() const;
-
-  /// Returns the string for an export flag
-  std::string getSerializationExport() const;
-
-  /// Creates a member function that performs serialization
-  void serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
-      Str wrapperName, std::vector<std::string>& functionNames) const;
-
-  /// Creates a static member function that performs deserialization
-  void deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
-      Str wrapperName, std::vector<std::string>& functionNames) const;
-
-  // emit python wrapper
-  void python_wrapper(FileWriter& wrapperFile) const;
-
-  // emit cython wrapper
-  void emit_cython_pxd(FileWriter& pxdFile) const;
-  void emit_cython_wrapper_pxd(FileWriter& pxdFile) const;
-  void emit_cython_pyx(FileWriter& pyxFile,
-                       const std::vector<Class>& allClasses) const;
-  void pyxInitParentObj(FileWriter& pyxFile, const std::string& pyObj,
-                        const std::string& cySharedObj,
-                        const std::vector<Class>& allClasses) const;
-  void pyxDynamicCast(FileWriter& pyxFile, const Class& curLevel,
-                           const std::vector<Class>& allClasses) const;
-
-  friend std::ostream& operator<<(std::ostream& os, const Class& cls) {
-    os << "class " << cls.name() << "{\n";
-    os << cls.constructor << ";\n";
-    for(const StaticMethod& m: cls.static_methods | boost::adaptors::map_values)
-      os << m << ";\n";
-    for(const Method& m: cls.methods_ | boost::adaptors::map_values)
-      os << m << ";\n";
-    os << "};" << std::endl;
-    return os;
-  }
-
-private:
-
-  void pointer_constructor_fragments(FileWriter& proxyFile,
-      FileWriter& wrapperFile, Str wrapperName,
-      std::vector<std::string>& functionNames) const;
-
-  void comment_fragment(FileWriter& proxyFile) const;
-};
-
-/* ************************************************************************* */
-// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
-struct ClassGrammar: public classic::grammar<ClassGrammar> {
-
-  Class& cls_; ///< successful parse will be placed in here
-  Template& template_; ///< result needs to be visible outside
-
-  /// Construct type grammar and specify where result is placed
-  ClassGrammar(Class& cls, Template& t) :
-      cls_(cls), template_(t) {
-  }
-
-  /// Definition of type grammar
-  template<typename ScannerT>
-  struct definition: BasicRules<ScannerT> {
-
-    using BasicRules<ScannerT>::name_p;
-    using BasicRules<ScannerT>::className_p;
-    using BasicRules<ScannerT>::comments_p;
-
-    // NOTE: allows for pointers to all types
-    ArgumentList args;
-    ArgumentListGrammar argumentList_g;
-
-    Constructor constructor0, constructor;
-
-    ReturnValue retVal0, retVal;
-    ReturnValueGrammar returnValue_g;
-
-    Template methodTemplate;
-    TemplateGrammar methodTemplate_g, classTemplate_g;
-
-    std::string methodName;
-    bool isConst, T, F;
-
-    // Parent class
-    Qualified possibleParent;
-    TypeGrammar classParent_g;
-
-    classic::rule<ScannerT> constructor_p, methodName_p, method_p,
-        staticMethodName_p, static_method_p, templateList_p, classParent_p,
-        functions_p, class_p;
-
-    definition(ClassGrammar const& self) :
-        argumentList_g(args), returnValue_g(retVal), //
-        methodTemplate_g(methodTemplate), classTemplate_g(self.template_), //
-        T(true), F(false), classParent_g(possibleParent) {
-
-      using namespace classic;
-      bool verbose = false; // TODO
-
-      // ConstructorGrammar
-      constructor_p = (className_p >> argumentList_g >> ';' >> !comments_p) //
-          [bl::bind(&Constructor::push_back, bl::var(constructor),
-              bl::var(args))] //
-          [clear_a(args)];
-
-      // MethodGrammar
-      methodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')];
-
-      // gtsam::Values retract(const gtsam::VectorValues& delta) const;
-      method_p = !methodTemplate_g
-          >> (returnValue_g >> methodName_p[assign_a(methodName)]
-              >> argumentList_g >> !str_p("const")[assign_a(isConst, T)] >> ';'
-              >> *comments_p) //
-          [bl::bind(&Class::addMethod, bl::var(self.cls_), verbose,
-              bl::var(isConst), bl::var(methodName), bl::var(args),
-              bl::var(retVal), bl::var(methodTemplate))] //
-          [assign_a(retVal, retVal0)][clear_a(args)] //
-          [clear_a(methodTemplate)][assign_a(isConst, F)];
-
-      // StaticMethodGrammar
-      staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')];
-
-      static_method_p = (str_p("static") >> returnValue_g
-          >> staticMethodName_p[assign_a(methodName)] >> argumentList_g >> ';'
-          >> *comments_p) //
-          [bl::bind(&StaticMethod::addOverload,
-              bl::var(self.cls_.static_methods)[bl::var(methodName)],
-              bl::var(methodName), bl::var(args), bl::var(retVal), boost::none,
-              verbose)] //
-          [assign_a(retVal, retVal0)][clear_a(args)];
-
-      // template<POSE, POINT>
-      templateList_p = (str_p("template") >> '<'
-          >> name_p[push_back_a(self.cls_.templateArgs)]
-          >> *(',' >> name_p[push_back_a(self.cls_.templateArgs)]) >> '>');
-
-      // parse a full class
-      classParent_p = (':' >> classParent_g >> '{') //
-          [bl::bind(&Class::assignParent, bl::var(self.cls_),
-              bl::var(possibleParent))][clear_a(possibleParent)];
-
-      functions_p = constructor_p | method_p | static_method_p;
-
-      // parse a full class
-      class_p = (!(classTemplate_g[push_back_a(self.cls_.templateArgs,
-          self.template_.argName())] | templateList_p)
-          >> !(str_p("virtual")[assign_a(self.cls_.isVirtual, T)])
-          >> str_p("class") >> className_p[assign_a(self.cls_.name_)]
-          >> (classParent_p | '{') >> //
-          *(functions_p | comments_p) >> str_p("};")) //
-          [bl::bind(&Constructor::initializeOrCheck, bl::var(constructor),
-              bl::var(self.cls_.name_), boost::none, verbose)][assign_a(
-              self.cls_.constructor, constructor)] //
-          [assign_a(self.cls_.deconstructor.name, self.cls_.name_)] //
-          [assign_a(constructor, constructor0)];
-    }
-
-    classic::rule<ScannerT> const& start() const {
-      return class_p;
-    }
-
-  };
-};
-// ClassGrammar
-
-}// \namespace wrap
-
diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp
deleted file mode 100644
index 74719b289..000000000
--- a/wrap/Constructor.cpp
+++ /dev/null
@@ -1,160 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Constructor.ccp
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#include <iostream>
-#include <fstream>
-#include <algorithm>
-
-#include <boost/lexical_cast.hpp>
-
-#include "utilities.h"
-#include "Constructor.h"
-#include "Class.h"
-
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-string Constructor::matlab_wrapper_name(Str className) const {
-  string str = "new_" + className;
-  return str;
-}
-
-/* ************************************************************************* */
-void Constructor::proxy_fragment(FileWriter& file,
-                                 const std::string& wrapperName, bool hasParent,
-                                 const int id, const ArgumentList args) const {
-  size_t nrArgs = args.size();
-  // check for number of arguments...
-  file.oss << "      elseif nargin == " << nrArgs;
-  if (nrArgs > 0) file.oss << " && ";
-  // ...and their types
-  bool first = true;
-  for (size_t i = 0; i < nrArgs; i++) {
-    if (!first) file.oss << " && ";
-    file.oss << "isa(varargin{" << i + 1 << "},'" << args[i].matlabClass(".")
-             << "')";
-    first = false;
-  }
-  // emit code for calling constructor
-  if (hasParent)
-    file.oss << "\n        [ my_ptr, base_ptr ] = ";
-  else
-    file.oss << "\n        my_ptr = ";
-  file.oss << wrapperName << "(" << id;
-  // emit constructor arguments
-  for (size_t i = 0; i < nrArgs; i++) {
-    file.oss << ", ";
-    file.oss << "varargin{" << i + 1 << "}";
-  }
-  file.oss << ");\n";
-}
-
-/* ************************************************************************* */
-string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName,
-                                     Str matlabUniqueName,
-                                     boost::optional<string> cppBaseClassName,
-                                     int id, const ArgumentList& al) const {
-  const string wrapFunctionName =
-      matlabUniqueName + "_constructor_" + boost::lexical_cast<string>(id);
-
-  file.oss << "void " << wrapFunctionName
-           << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])"
-           << endl;
-  file.oss << "{\n";
-  file.oss << "  mexAtExit(&_deleteAllObjects);\n";
-  // Typedef boost::shared_ptr
-  file.oss << "  typedef boost::shared_ptr<" << cppClassName << "> Shared;\n";
-  file.oss << "\n";
-
-  // Check to see if there will be any arguments and remove {} for consiseness
-  if (al.size() > 0) al.matlab_unwrap(file);  // unwrap arguments
-  file.oss << "  Shared *self = new Shared(new " << cppClassName << "("
-           << al.names() << "));" << endl;
-  file.oss << "  collector_" << matlabUniqueName << ".insert(self);\n";
-
-  if (verbose_)
-    file.oss << "  std::cout << \"constructed \" << self << std::endl;" << endl;
-  file.oss
-      << "  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);"
-      << endl;
-  file.oss << "  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;"
-           << endl;
-
-  // If we have a base class, return the base class pointer (MATLAB will call
-  // the base class collectorInsertAndMakeBase to add this to the collector and
-  // recurse the heirarchy)
-  if (cppBaseClassName) {
-    file.oss << "\n";
-    file.oss << "  typedef boost::shared_ptr<" << *cppBaseClassName
-             << "> SharedBase;\n";
-    file.oss << "  out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, "
-                "mxREAL);\n";
-    file.oss << "  *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new "
-                "SharedBase(*self);\n";
-  }
-
-  file.oss << "}" << endl;
-
-  return wrapFunctionName;
-}
-
-/* ************************************************************************* */
-void Constructor::python_wrapper(FileWriter& wrapperFile, Str className) const {
-  wrapperFile.oss << "  .def(\"" << name_ << "\", &" << className
-                  << "::" << name_ << ");\n";
-}
-
-/* ************************************************************************* */
-bool Constructor::hasDefaultConstructor() const {
-  for (size_t i = 0; i < nrOverloads(); i++) {
-    if (argumentList(i).size() == 0) return true;
-  }
-  return false;
-}
-
-/* ************************************************************************* */
-void Constructor::emit_cython_pxd(FileWriter& pxdFile, const Class& cls) const {
-  for (size_t i = 0; i < nrOverloads(); i++) {
-    ArgumentList args = argumentList(i);
-
-    // generate the constructor
-    pxdFile.oss << "        " << cls.pxdClassName() << "(";
-    args.emit_cython_pxd(pxdFile, cls.pxdClassName(), cls.templateArgs);
-    pxdFile.oss << ") " << "except +\n";
-  }
-}
-
-/* ************************************************************************* */
-void Constructor::emit_cython_pyx(FileWriter& pyxFile, const Class& cls) const {
-  for (size_t i = 0; i < nrOverloads(); i++) {
-    ArgumentList args = argumentList(i);
-    pyxFile.oss << "        try:\n";
-    pyxFile.oss << pyx_resolveOverloadParams(args, true, 3);
-    pyxFile.oss
-        << argumentList(i).pyx_convertEigenTypeAndStorageOrder("            ");
-
-    pyxFile.oss << "            self." << cls.shared_pxd_obj_in_pyx() << " = "
-        << cls.shared_pxd_class_in_pyx() << "(new " << cls.pxd_class_in_pyx()
-        << "(" << args.pyx_asParams() << "))\n";
-    pyxFile.oss << "        except (AssertionError, ValueError):\n";
-    pyxFile.oss << "            pass\n";
-  }
-}
-
-/* ************************************************************************* */
diff --git a/wrap/Constructor.h b/wrap/Constructor.h
deleted file mode 100644
index 172cd24a4..000000000
--- a/wrap/Constructor.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Constructor.h
- * @brief class describing a constructor + code generation
- * @author Frank Dellaert
- * @author Richard Roberts
- **/
-
-#pragma once
-
-#include "OverloadedFunction.h"
-#include <boost/optional.hpp>
-#include <string>
-#include <vector>
-
-namespace wrap {
-
-// Forward declaration
-class Class;
-
-// Constructor class
-struct Constructor: public OverloadedFunction {
-
-  typedef const std::string& Str;
-
-  /// Constructor creates an empty class
-  Constructor(bool verbose = false) {
-    verbose_ = verbose;
-  }
-
-  Constructor expandTemplate(const TemplateSubstitution& ts) const {
-    Constructor inst = *this;
-    inst.argLists_ = expandArgumentListsTemplate(ts);
-    inst.name_ = ts.expandedClassName();
-    return inst;
-  }
-
-  /// return true if the default constructor exists
-  bool hasDefaultConstructor() const;
-
-  // MATLAB code generation
-  // toolboxPath is main toolbox directory, e.g., ../matlab
-  // classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m
-
-  /// wrapper name
-  std::string matlab_wrapper_name(Str className) const;
-
-  void comment_fragment(FileWriter& proxyFile) const {
-    if (nrOverloads() > 0)
-      proxyFile.oss << "%\n%-------Constructors-------\n";
-    for (size_t i = 0; i < nrOverloads(); i++) {
-      proxyFile.oss << "%";
-      argumentList(i).emit_prototype(proxyFile, name_);
-      proxyFile.oss << "\n";
-    }
-  }
-
-  /**
-   * Create fragment to select constructor in proxy class, e.g.,
-   * if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end
-   */
-  void proxy_fragment(FileWriter& file, Str wrapperName, bool hasParent,
-      const int id, const ArgumentList args) const;
-
-  /// cpp wrapper
-  std::string wrapper_fragment(FileWriter& file, Str cppClassName,
-      Str matlabUniqueName, boost::optional<std::string> cppBaseClassName, int id,
-      const ArgumentList& al) const;
-
-  /// constructor function
-  void generate_construct(FileWriter& file, Str cppClassName,
-      std::vector<ArgumentList>& args_list) const;
-
-  // emit python wrapper
-  void python_wrapper(FileWriter& wrapperFile, Str className) const;
-
-  // emit cython wrapper
-  void emit_cython_pxd(FileWriter& pxdFile, const Class& cls) const;
-  void emit_cython_pyx(FileWriter& pyxFile, const Class& cls) const;
-
-  friend std::ostream& operator<<(std::ostream& os, const Constructor& m) {
-    for (size_t i = 0; i < m.nrOverloads(); i++)
-      os << m.name_ << m.argLists_[i];
-    return os;
-  }
-
-};
-
-} // \namespace wrap
diff --git a/wrap/Deconstructor.cpp b/wrap/Deconstructor.cpp
deleted file mode 100644
index 7bb366e3f..000000000
--- a/wrap/Deconstructor.cpp
+++ /dev/null
@@ -1,73 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Deconstructor.ccp
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#include <iostream>
-#include <fstream>
-
-#include <boost/lexical_cast.hpp>
-
-#include "utilities.h"
-#include "Deconstructor.h"
-
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-string Deconstructor::matlab_wrapper_name(const string& className) const {
-  string str = "delete_" + className;
-  return str;
-}
-
-/* ************************************************************************* */
-void Deconstructor::proxy_fragment(FileWriter& file,
-    const std::string& wrapperName,
-    const std::string& matlabUniqueName, int id) const {
-
-  file.oss << "    function delete(obj)\n";
-  file.oss << "      " << wrapperName << "(" << id << ", obj.ptr_" << matlabUniqueName << ");\n";
-  file.oss << "    end\n";
-}
-
-/* ************************************************************************* */
-string Deconstructor::wrapper_fragment(FileWriter& file,
-         const string& cppClassName,
-         const string& matlabUniqueName,
-         int id) const {
-
-  const string matlabName = matlab_wrapper_name(matlabUniqueName);
-
-  const string wrapFunctionName = matlabUniqueName + "_deconstructor_" + boost::lexical_cast<string>(id);
-
-  file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
-  file.oss << "{" << endl;
-  file.oss << "  typedef boost::shared_ptr<"  << cppClassName  << "> Shared;" << endl;
-  //Deconstructor takes 1 arg, the mxArray obj
-  file.oss << "  checkArguments(\"" << matlabName << "\",nargout,nargin," << "1" << ");" << endl;
-  file.oss << "  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));\n";
-  file.oss << "  Collector_" << matlabUniqueName << "::iterator item;\n";
-  file.oss << "  item = collector_" << matlabUniqueName << ".find(self);\n";
-  file.oss << "  if(item != collector_" << matlabUniqueName << ".end()) {\n";
-  file.oss << "    delete self;\n";
-  file.oss << "    collector_" << matlabUniqueName << ".erase(item);\n";
-  file.oss << "  }\n";
-  file.oss << "}" << endl;
-
-  return wrapFunctionName;
-}
-
-/* ************************************************************************* */
diff --git a/wrap/Deconstructor.h b/wrap/Deconstructor.h
deleted file mode 100644
index ee2f4ea19..000000000
--- a/wrap/Deconstructor.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Deconstructor.h
- * @brief class describing a constructor + code generation
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include "Argument.h"
-
-namespace wrap {
-
-// Deconstructor class
-struct Deconstructor {
-
-  /// Deconstructor creates an empty class
-  Deconstructor(bool verbose = true) :
-      verbose_(verbose) {
-  }
-
-  // Then the instance variables are set directly by the Module deconstructor
-  std::string name;
-  bool verbose_;
-
-  // MATLAB code generation
-  // toolboxPath is main toolbox directory, e.g., ../matlab
-  // classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m
-
-  /// wrapper name
-  std::string matlab_wrapper_name(const std::string& className) const;
-
-  /// m-file
-  void proxy_fragment(FileWriter& file,
-    const std::string& wrapperName,
-    const std::string& matlabUniqueName, int id) const;
-
-  /// cpp wrapper
-  std::string wrapper_fragment(FileWriter& file,
-       const std::string& cppClassName,
-       const std::string& matlabUniqueName,
-       int id) const;
-};
-
-} // \namespace wrap
-
diff --git a/wrap/FileWriter.cpp b/wrap/FileWriter.cpp
deleted file mode 100644
index c07de0eb0..000000000
--- a/wrap/FileWriter.cpp
+++ /dev/null
@@ -1,52 +0,0 @@
-/**
- * @file FileWriter.cpp
- *
- * @date Jan 15, 2012
- * @author Alex Cunningham
- */
-
-#include "FileWriter.h"
-#include "utilities.h"
-
-#include <fstream>
-#include <iostream>
-
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-FileWriter::FileWriter(const string& filename, bool verbose,
-    const string& comment_str) :
-    verbose_(verbose), filename_(filename), comment_str_(comment_str) {
-}
-
-/* ************************************************************************* */
-void FileWriter::emit(bool add_header, bool force_overwrite) const {
-  if (verbose_)
-    cerr << "generating " << filename_ << " ";
-  // read in file if it exists
-  string existing_contents;
-  bool file_exists = true;
-  try {
-    existing_contents = file_contents(filename_.c_str(), add_header);
-  } catch (const CantOpenFile& ) {
-    file_exists = false;
-  }
-
-  // Only write a file if it is new, an update, or overwrite is forced
-  string new_contents = oss.str();
-  if (force_overwrite || !file_exists || existing_contents != new_contents) {
-    // Binary to use LF line endings instead of CRLF
-    ofstream ofs(filename_.c_str(), ios::binary);
-    if (!ofs)
-      throw CantOpenFile(filename_);
-
-    // dump in stringstream
-    ofs << new_contents;
-    ofs.close();
-  }
-  if (verbose_)
-    cerr << " ...no update" << endl;
-}
-/* ************************************************************************* */
-
diff --git a/wrap/FileWriter.h b/wrap/FileWriter.h
deleted file mode 100644
index 12e033fdf..000000000
--- a/wrap/FileWriter.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/**
- * @file FileWriter.h
- *
- * @brief Wrapper for writing files and avoiding overwriting existing files
- * This class wraps a stream object and will check that the file is
- * actually different to write the new generated file.
- *
- * @date Jan 15, 2012
- * @author Alex Cunningham
- */
-
-#pragma once
-
-#include <sstream>
-
-namespace wrap {
-
-class FileWriter {
-protected:
-  bool verbose_;
-  std::string filename_;
-  std::string comment_str_;
-
-public:
-  std::ostringstream oss; ///< Primary stream for operating on the file
-
-  /** Create a writer with a filename and delimiter for the header comment */
-  FileWriter(const std::string& filename, bool verbose, const std::string& comment_str);
-
-  /** Writes the contents of the stringstream to the file, checking if actually new */
-  void emit(bool add_header, bool force=false) const;
-
-};
-
-} // \namespace wrap
diff --git a/wrap/ForwardDeclaration.h b/wrap/ForwardDeclaration.h
deleted file mode 100644
index 190387ecc..000000000
--- a/wrap/ForwardDeclaration.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Class.h
- * @brief describe the C++ class that is being wrapped
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#pragma once
-
-#include <string>
-
-namespace wrap {
-
-  class Class;
-
-  struct ForwardDeclaration {
-    Class cls;
-    bool isVirtual;
-    ForwardDeclaration() : isVirtual(false) {}
-    explicit ForwardDeclaration(const std::string& s) : cls(s), isVirtual(false) {}
-    std::string name() const { return cls.qualifiedName("::"); }
-  };
-
-}
diff --git a/wrap/FullyOverloadedFunction.cpp b/wrap/FullyOverloadedFunction.cpp
deleted file mode 100644
index 4db4c8713..000000000
--- a/wrap/FullyOverloadedFunction.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-#include "FullyOverloadedFunction.h"
-
-using namespace std;
-
-namespace wrap {
-const std::array<std::string, 2> FullyOverloadedFunction::pythonKeywords{
-    {"print", "lambda"}};
-
-/* ************************************************************************* */
-std::string FullyOverloadedFunction::pyx_functionCall(
-    const std::string& caller,
-    const std::string& funcName, size_t iOverload) const {
-
-  string ret;
-  if (!returnVals_[iOverload].isPair && !returnVals_[iOverload].type1.isPtr &&
-      returnVals_[iOverload].type1.isNonBasicType()) {
-    ret = returnVals_[iOverload].type1.make_shared_pxd_class_in_pyx() + "(";
-  }
-
-  // actual function call ...
-  if (!caller.empty()) ret += caller + ".";
-  ret += funcName;
-  if (templateArgValue_) ret += "[" + templateArgValue_->pxd_class_in_pyx() + "]";
-  //... with argument list
-  ret += "(" + argumentList(iOverload).pyx_asParams() + ")";
-
-  if (!returnVals_[iOverload].isPair && !returnVals_[iOverload].type1.isPtr &&
-      returnVals_[iOverload].type1.isNonBasicType())
-    ret += ")";
-
-  return ret;
-}
-
-}
diff --git a/wrap/FullyOverloadedFunction.h b/wrap/FullyOverloadedFunction.h
deleted file mode 100644
index 6b40f6a70..000000000
--- a/wrap/FullyOverloadedFunction.h
+++ /dev/null
@@ -1,147 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 FullyOverloadedFunction.h
- * @brief Function that can be fully overloaded: arguments and return values
- * @author Frank Dellaert
- * @date Nov 13, 2014
- **/
-
-#pragma once
-
-#include "OverloadedFunction.h"
-#include <array>
-
-namespace wrap {
-
-/**
- * Signature Overload (including return value)
- */
-class SignatureOverloads: public ArgumentOverloads {
-
-public:
-
-  std::vector<ReturnValue> returnVals_;
-
-public:
-
-  const ReturnValue& returnValue(size_t i) const {
-    return returnVals_.at(i);
-  }
-
-  void push_back(const ArgumentList& args, const ReturnValue& retVal) {
-    argLists_.push_back(args);
-    returnVals_.push_back(retVal);
-  }
-
-  void verifyReturnTypes(const std::vector<std::string>& validtypes,
-      const std::string& s) const {
-    for(const ReturnValue& retval: returnVals_) {
-      retval.type1.verify(validtypes, s);
-      if (retval.isPair)
-        retval.type2.verify(validtypes, s);
-    }
-  }
-
-  // TODO use transform ?
-  std::vector<ReturnValue> expandReturnValuesTemplate(
-      const TemplateSubstitution& ts) const {
-    std::vector<ReturnValue> result;
-    for(const ReturnValue& retVal: returnVals_) {
-      ReturnValue instRetVal = retVal.expandTemplate(ts);
-      result.push_back(instRetVal);
-    }
-    return result;
-  }
-
-  /// Expand templates, imperative !
-  void expandTemplate(const TemplateSubstitution& ts) {
-    // substitute template in arguments
-    argLists_ = expandArgumentListsTemplate(ts);
-    // do the same for return types
-    returnVals_ = expandReturnValuesTemplate(ts);
-  }
-
-  // emit a list of comments, one for each overload
-  void usage_fragment(FileWriter& proxyFile, const std::string& name) const {
-    unsigned int argLCount = 0;
-    for(ArgumentList argList: argLists_) {
-      argList.emit_prototype(proxyFile, name);
-      if (argLCount != nrOverloads() - 1)
-        proxyFile.oss << ", ";
-      else
-        proxyFile.oss << " : returns " << returnValue(0).returnType()
-                      << std::endl;
-      argLCount++;
-    }
-  }
-
-  // emit a list of comments, one for each overload
-  void comment_fragment(FileWriter& proxyFile, const std::string& name) const {
-    size_t i = 0;
-    for(ArgumentList argList: argLists_) {
-      proxyFile.oss << "%";
-      argList.emit_prototype(proxyFile, name);
-      proxyFile.oss << " : returns " << returnVals_[i++].returnType()
-                    << std::endl;
-    }
-  }
-
-  friend std::ostream& operator<<(std::ostream& os,
-      const SignatureOverloads& overloads) {
-    for (size_t i = 0; i < overloads.nrOverloads(); i++)
-      os << overloads.returnVals_[i] << overloads.argLists_[i] << std::endl;
-    return os;
-  }
-
-};
-
-class FullyOverloadedFunction: public Function, public SignatureOverloads {
-
-public:
-
-  bool addOverload(const std::string& name, const ArgumentList& args,
-      const ReturnValue& retVal, boost::optional<const Qualified> instName =
-          boost::none, bool verbose = false) {
-    bool first = initializeOrCheck(name, instName, verbose);
-    SignatureOverloads::push_back(args, retVal);
-    return first;
-  }
-
-  // emit cython pyx function call
-  std::string pyx_functionCall(const std::string& caller, const std::string& funcName,
-                        size_t iOverload) const;
-
-  /// Cython: Rename functions which names are python keywords
-  static const std::array<std::string, 2> pythonKeywords;
-  static std::string pyRename(const std::string& name) {
-    if (std::find(pythonKeywords.begin(), pythonKeywords.end(), name) ==
-        pythonKeywords.end())
-      return name;
-    else
-      return name + "_";
-  }
-};
-
-// Templated checking functions
-// TODO: do this via polymorphism, use transform ?
-
-template<class F>
-inline void verifyReturnTypes(const std::vector<std::string>& validTypes,
-    const std::map<std::string, F>& vt) {
-  typedef typename std::map<std::string, F>::value_type NamedMethod;
-  for(const NamedMethod& namedMethod: vt)
-    namedMethod.second.verifyReturnTypes(validTypes);
-}
-
-} // \namespace wrap
-
diff --git a/wrap/Function.cpp b/wrap/Function.cpp
deleted file mode 100644
index 80b0adbbe..000000000
--- a/wrap/Function.cpp
+++ /dev/null
@@ -1,78 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Function.ccp
- * @author Frank Dellaert
- * @date Nov 13, 2014
- **/
-
-#include "Function.h"
-#include "utilities.h"
-
-#include <boost/lexical_cast.hpp>
-#include <boost/algorithm/string.hpp>
-
-#include <iostream>
-#include <fstream>
-
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-bool Function::initializeOrCheck(const string& name,
-    boost::optional<const Qualified> instName, bool verbose) {
-
-  if (name.empty())
-    throw runtime_error("Function::initializeOrCheck called with empty name");
-
-  // Check if this overload is give to the correct method
-  if (name_.empty()) {
-    name_ = name;
-    templateArgValue_ = instName;
-    verbose_ = verbose;
-    return true;
-  } else {
-    if (name_ != name || verbose_ != verbose
-        || ((bool) templateArgValue_ != (bool) instName)
-        || ((bool) templateArgValue_ && (bool) instName
-            && !(*templateArgValue_ == *instName)))
-      throw runtime_error(
-          "Function::initializeOrCheck called with different arguments");
-
-    return false;
-  }
-}
-
-/* ************************************************************************* */
-void Function::emit_call(FileWriter& proxyFile, const ReturnValue& returnVal,
-    const string& wrapperName, int id) const {
-  returnVal.emit_matlab(proxyFile);
-  proxyFile.oss << wrapperName << "(" << id;
-  if (!isStatic())
-    proxyFile.oss << ", this";
-  proxyFile.oss << ", varargin{:});\n";
-}
-
-/* ************************************************************************* */
-void Function::emit_conditional_call(FileWriter& proxyFile,
-    const ReturnValue& returnVal, const ArgumentList& args,
-    const string& wrapperName, int id) const {
-
-  // Check all arguments
-  args.proxy_check(proxyFile);
-
-  // output call to C++ wrapper
-  proxyFile.oss << "        ";
-  emit_call(proxyFile, returnVal, wrapperName, id);
-}
-
-/* ************************************************************************* */
diff --git a/wrap/Function.h b/wrap/Function.h
deleted file mode 100644
index c39b3231c..000000000
--- a/wrap/Function.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Function.h
- * @brief Base class for global functions and methods
- * @author Frank Dellaert
- * @date Nov 13, 2014
- **/
-
-#pragma once
-
-#include "Argument.h"
-#include <boost/optional.hpp>
-
-namespace wrap {
-
-/// Function class
-class Function {
-
-protected:
-
-  std::string name_; ///< name of method
-  boost::optional<Qualified> templateArgValue_; ///< value of template argument if applicable
-  bool verbose_;
-
-public:
-
-  /**
-   * @brief first time, fill in instance variables, otherwise check if same
-   * @return true if first time, false thereafter
-   */
-  bool initializeOrCheck(const std::string& name,
-      boost::optional<const Qualified> instName = boost::none, bool verbose =
-          false);
-
-  std::string name() const {
-    return name_;
-  }
-
-  /// Only Methods are non-static
-  virtual bool isStatic() const {
-    return true;
-  }
-
-  std::string matlabName() const {
-    if (templateArgValue_)
-      return name_ + templateArgValue_->name();
-    else
-      return name_;
-  }
-
-  /// Emit function call to MATLAB (no argument checking)
-  void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal,
-      const std::string& wrapperName, int id) const;
-
-  /// Emit checking arguments and function call to MATLAB
-  void emit_conditional_call(FileWriter& proxyFile,
-      const ReturnValue& returnVal, const ArgumentList& args,
-      const std::string& wrapperName, int id) const;
-
-};
-
-} // \namespace wrap
-
diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp
deleted file mode 100644
index 02ab19657..000000000
--- a/wrap/GlobalFunction.cpp
+++ /dev/null
@@ -1,227 +0,0 @@
-/**
- * @file GlobalFunction.cpp
- *
- * @date Jul 22, 2012
- * @author Alex Cunningham
- */
-
-#include "GlobalFunction.h"
-#include "Class.h"
-#include "utilities.h"
-
-#include <boost/lexical_cast.hpp>
-
-namespace wrap {
-
-using namespace std;
-
-/* ************************************************************************* */
-void GlobalFunction::addOverload(const Qualified& overload,
-    const ArgumentList& args, const ReturnValue& retVal, const std::string& _includeFile,
-    boost::optional<const Qualified> instName, bool verbose) {
-  FullyOverloadedFunction::addOverload(overload.name(), args, retVal, instName,
-      verbose);
-  overloads.push_back(overload);
-  includeFile = _includeFile;
-}
-
-/* ************************************************************************* */
-void GlobalFunction::matlab_proxy(const string& toolboxPath,
-    const string& wrapperName, const TypeAttributesTable& typeAttributes,
-    FileWriter& file, vector<string>& functionNames) const {
-
-  // cluster overloads with same namespace
-  // create new GlobalFunction structures around namespaces - same namespaces and names are overloads
-  // map of namespace to global function
-  typedef map<string, GlobalFunction> GlobalFunctionMap;
-  GlobalFunctionMap grouped_functions;
-  for (size_t i = 0; i < overloads.size(); ++i) {
-    Qualified overload = overloads.at(i);
-    // use concatenated namespaces as key
-    string str_ns = qualifiedName("", overload.namespaces());
-    const ReturnValue& ret = returnValue(i);
-    const ArgumentList& args = argumentList(i);
-    grouped_functions[str_ns].addOverload(overload, args, ret);
-  }
-
-  size_t lastcheck = grouped_functions.size();
-  for(const GlobalFunctionMap::value_type& p: grouped_functions) {
-    p.second.generateSingleFunction(toolboxPath, wrapperName, typeAttributes,
-        file, functionNames);
-    if (--lastcheck != 0)
-      file.oss << endl;
-  }
-}
-
-/* ************************************************************************* */
-void GlobalFunction::generateSingleFunction(const string& toolboxPath,
-    const string& wrapperName, const TypeAttributesTable& typeAttributes,
-    FileWriter& file, vector<string>& functionNames) const {
-
-  // create the folder for the namespace
-  const Qualified& overload1 = overloads.front();
-  createNamespaceStructure(overload1.namespaces(), toolboxPath);
-
-  // open destination mfunctionFileName
-  string mfunctionFileName = overload1.matlabName(toolboxPath);
-  FileWriter mfunctionFile(mfunctionFileName, verbose_, "%");
-
-  // get the name of actual matlab object
-  const string matlabQualName = overload1.qualifiedName(".");
-  const string matlabUniqueName = overload1.qualifiedName("");
-  const string cppName = overload1.qualifiedName("::");
-
-  mfunctionFile.oss << "function varargout = " << name_ << "(varargin)\n";
-
-  for (size_t i = 0; i < nrOverloads(); ++i) {
-    const ArgumentList& args = argumentList(i);
-    const ReturnValue& returnVal = returnValue(i);
-
-    const int id = functionNames.size();
-
-    // Output proxy matlab code
-    mfunctionFile.oss << "      " << (i == 0 ? "" : "else");
-    emit_conditional_call(mfunctionFile, returnVal, args, wrapperName, id);
-
-    // Output C++ wrapper code
-
-    const string wrapFunctionName = matlabUniqueName + "_"
-        + boost::lexical_cast<string>(id);
-
-    // call
-    file.oss << "void " << wrapFunctionName
-        << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
-    // start
-    file.oss << "{\n";
-
-    // check arguments
-    // NOTE: for static functions, there is no object passed
-    file.oss << "  checkArguments(\"" << matlabUniqueName
-        << "\",nargout,nargin," << args.size() << ");\n";
-
-    // unwrap arguments, see Argument.cpp
-    args.matlab_unwrap(file, 0); // We start at 0 because there is no self object
-
-    // call method with default type and wrap result
-    if (returnVal.type1.name() != "void")
-      returnVal.wrap_result(cppName + "(" + args.names() + ")", file,
-          typeAttributes);
-    else
-      file.oss << cppName + "(" + args.names() + ");\n";
-
-    // finish
-    file.oss << "}\n";
-
-    // Add to function list
-    functionNames.push_back(wrapFunctionName);
-  }
-
-  mfunctionFile.oss << "      else\n";
-  mfunctionFile.oss
-      << "        error('Arguments do not match any overload of function "
-      << matlabQualName << "');" << endl;
-  mfunctionFile.oss << "      end" << endl;
-
-  // Close file
-  mfunctionFile.emit(true);
-}
-
-/* ************************************************************************* */
-void GlobalFunction::python_wrapper(FileWriter& wrapperFile) const {
-  wrapperFile.oss << "def(\"" << name_ << "\", " << name_ << ");\n";
-}
-
-/* ************************************************************************* */
-void GlobalFunction::emit_cython_pxd(FileWriter& file) const {
-  file.oss << "cdef extern from \"" << includeFile << "\" namespace \""
-                << overloads[0].qualifiedNamespaces("::")
-                << "\":" << endl;
-  for (size_t i = 0; i < nrOverloads(); ++i) {
-    file.oss << "        ";
-    returnVals_[i].emit_cython_pxd(file, "", vector<string>());
-    file.oss << pxdName() + " \"" + overloads[0].qualifiedName("::") +
-                    "\"(";
-    argumentList(i).emit_cython_pxd(file, "", vector<string>());
-    file.oss << ")";
-    file.oss << " except +";
-    file.oss << "\n";
-  }
-}
-
-/* ************************************************************************* */
-void GlobalFunction::emit_cython_pyx_no_overload(FileWriter& file) const {
-  string funcName = pyxName();
-
-  // Function definition
-  file.oss << "def " << funcName;
-
-  // modify name of function instantiation as python doesn't allow overloads
-  // e.g. template<T={A,B,C}> funcName(...) --> funcNameA, funcNameB, funcNameC
-  if (templateArgValue_) file.oss << templateArgValue_->pyxClassName();
-
-  // funtion arguments
-  file.oss << "(";
-  argumentList(0).emit_cython_pyx(file);
-  file.oss << "):\n";
-
-  /// Call cython corresponding function and return
-  file.oss << argumentList(0).pyx_convertEigenTypeAndStorageOrder("    ");
-  string ret = pyx_functionCall("", pxdName(), 0);
-  if (!returnVals_[0].isVoid()) {
-    file.oss << "    cdef " << returnVals_[0].pyx_returnType()
-             << " ret = " << ret << "\n";
-    file.oss << "    return " << returnVals_[0].pyx_casting("ret") << "\n";
-  } else {
-    file.oss << "    " << ret << "\n";
-  }
-}
-
-/* ************************************************************************* */
-void GlobalFunction::emit_cython_pyx(FileWriter& file) const {
-  string funcName = pyxName();
-
-  size_t N = nrOverloads();
-  if (N == 1) {
-    emit_cython_pyx_no_overload(file);
-    return;
-  }
-
-  // Dealing with overloads..
-  file.oss << "def " << funcName << "(*args, **kwargs):\n";
-  for (size_t i = 0; i < N; ++i) {
-    file.oss << "    success, results = " << funcName << "_" << i
-             << "(args, kwargs)\n";
-    file.oss << "    if success:\n            return results\n";
-  }
-  file.oss << "    raise TypeError('Could not find the correct overload')\n";
-
-  for (size_t i = 0; i < N; ++i) {
-    ArgumentList args = argumentList(i);
-    file.oss << "def " + funcName + "_" + to_string(i) + "(args, kwargs):\n";
-    file.oss << "    cdef list __params\n";
-    if (!returnVals_[i].isVoid()) {
-      file.oss << "    cdef " << returnVals_[i].pyx_returnType() << " return_value\n";
-    }
-    file.oss << "    try:\n";
-    file.oss << pyx_resolveOverloadParams(args, false, 2); // lazy: always return None even if it's a void function
-
-    /// Call corresponding cython function
-    file.oss << argumentList(i).pyx_convertEigenTypeAndStorageOrder("        ");
-    // catch exception which indicates the parameters passed are incorrect.
-    file.oss << "    except:\n";
-    file.oss << "        return False, None\n\n";
-
-    string call = pyx_functionCall("", pxdName(), i);
-    if (!returnVals_[i].isVoid()) {
-      file.oss << "    return_value = " << call << "\n";
-      file.oss << "    return True, " << returnVals_[i].pyx_casting("return_value") << "\n";
-    } else {
-      file.oss << "    " << call << "\n";
-      file.oss << "    return True, None\n";
-    }
-  }
-}
-/* ************************************************************************* */
-
-} // \namespace wrap
-
diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h
deleted file mode 100644
index 099cefa70..000000000
--- a/wrap/GlobalFunction.h
+++ /dev/null
@@ -1,148 +0,0 @@
-/**
- * @file GlobalFunction.h
- *
- * @brief Implements codegen for a global function wrapped in matlab
- *
- * @date Jul 22, 2012
- * @author Alex Cunningham
- */
-
-#pragma once
-
-#include "FullyOverloadedFunction.h"
-
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-variable"
-#endif
-#include <boost/lambda/bind.hpp>
-#include <boost/lambda/lambda.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
-
-namespace bl = boost::lambda;
-
-namespace wrap {
-
-struct GlobalFunction: public FullyOverloadedFunction {
-
-  std::vector<Qualified> overloads; ///< Stack of qualified names
-  std::string includeFile;
-
-  // adds an overloaded version of this function,
-  void addOverload(const Qualified& overload, const ArgumentList& args,
-      const ReturnValue& retVal, const std::string& _includeFile = "", boost::optional<const Qualified> instName =
-          boost::none, bool verbose = false);
-
-  void verifyArguments(const std::vector<std::string>& validArgs) const {
-    SignatureOverloads::verifyArguments(validArgs, name_);
-  }
-
-  void verifyReturnTypes(const std::vector<std::string>& validtypes) const {
-    SignatureOverloads::verifyReturnTypes(validtypes, name_);
-  }
-
-  // codegen function called from Module to build the cpp and matlab versions of the function
-  void matlab_proxy(const std::string& toolboxPath,
-      const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
-      FileWriter& file, std::vector<std::string>& functionNames) const;
-
-  // emit python wrapper
-  void python_wrapper(FileWriter& wrapperFile) const;
-
-  // function name in Cython pxd
-  std::string pxdName() const { return "pxd_" + pyRename(name_); }
-  // function name in Python pyx
-  std::string pyxName() const {
-    std::string result = "";
-    for(size_t i=0; i<overloads[0].namespaces_.size(); i++){
-      if (i >= 1) {
-        result += (overloads[0].namespaces_[i] + "_");
-      }
-    }
-    result += pyRename(name_);
-    return result;
-  }
-
-  // emit cython wrapper
-  void emit_cython_pxd(FileWriter& pxdFile) const;
-  void emit_cython_pyx(FileWriter& pyxFile) const;
-  void emit_cython_pyx_no_overload(FileWriter& pyxFile) const;
-
-private:
-
-  // Creates a single global function - all in same namespace
-  void generateSingleFunction(const std::string& toolboxPath,
-      const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
-      FileWriter& file, std::vector<std::string>& functionNames) const;
-
-};
-
-typedef std::map<std::string, GlobalFunction> GlobalFunctions;
-
-/* ************************************************************************* */
-// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
-struct GlobalFunctionGrammar: public classic::grammar<GlobalFunctionGrammar> {
-
-  GlobalFunctions& global_functions_; ///< successful parse will be placed in here
-  std::vector<std::string>& namespaces_;
-  std::string& includeFile;
-
-  /// Construct type grammar and specify where result is placed
-  GlobalFunctionGrammar(GlobalFunctions& global_functions,
-                        std::vector<std::string>& namespaces,
-                        std::string& includeFile)
-      : global_functions_(global_functions),
-        namespaces_(namespaces),
-        includeFile(includeFile) {}
-
-  /// Definition of type grammar
-  template<typename ScannerT>
-  struct definition: BasicRules<ScannerT> {
-
-//    using BasicRules<ScannerT>::name_p;
-//    using BasicRules<ScannerT>::className_p;
-    using BasicRules<ScannerT>::comments_p;
-
-    ArgumentList args;
-    ArgumentListGrammar argumentList_g;
-
-    ReturnValue retVal0, retVal;
-    ReturnValueGrammar returnValue_g;
-
-    Qualified globalFunction;
-
-    classic::rule<ScannerT> globalFunctionName_p, global_function_p;
-
-    definition(GlobalFunctionGrammar const& self) :
-        argumentList_g(args), returnValue_g(retVal) {
-
-      using namespace classic;
-      bool verbose = false; // TODO
-
-      globalFunctionName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')];
-
-      // parse a global function
-      global_function_p = (returnValue_g >> globalFunctionName_p[assign_a(
-                                                globalFunction.name_)] >>
-                           argumentList_g >> ';' >> *comments_p)    //
-          [assign_a(globalFunction.namespaces_, self.namespaces_)]  //
-          [bl::bind(
-              &GlobalFunction::addOverload,
-              bl::var(self.global_functions_)[bl::var(globalFunction.name_)],
-              bl::var(globalFunction), bl::var(args), bl::var(retVal), bl::var(self.includeFile),
-              boost::none, verbose)]  //
-          [assign_a(retVal, retVal0)][clear_a(globalFunction)][clear_a(args)];
-    }
-
-    classic::rule<ScannerT> const& start() const {
-      return global_function_p;
-    }
-
-  };
-};
-// GlobalFunctionGrammar
-
-}// \namespace wrap
-
diff --git a/wrap/Method.cpp b/wrap/Method.cpp
deleted file mode 100644
index 2a4b0b3af..000000000
--- a/wrap/Method.cpp
+++ /dev/null
@@ -1,205 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Method.ccp
- * @author Frank Dellaert
- * @author Richard Roberts
- **/
-
-#include "Method.h"
-#include "Class.h"
-#include "utilities.h"
-
-#include <boost/lexical_cast.hpp>
-#include <boost/algorithm/string.hpp>
-
-#include <iostream>
-#include <fstream>
-
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-bool Method::addOverload(Str name, const ArgumentList& args,
-                         const ReturnValue& retVal, bool is_const,
-                         boost::optional<const Qualified> instName,
-                         bool verbose) {
-  bool first = MethodBase::addOverload(name, args, retVal, instName, verbose);
-  if (first)
-    is_const_ = is_const;
-  else if (is_const && !is_const_)
-    throw std::runtime_error(
-        "Method::addOverload: " + name +
-        " now designated as const whereas before it was not");
-  else if (!is_const && is_const_)
-    throw std::runtime_error(
-        "Method::addOverload: " + name +
-        " now designated as non-const whereas before it was");
-  return first;
-}
-
-/* ************************************************************************* */
-void Method::proxy_header(FileWriter& proxyFile) const {
-  proxyFile.oss << "    function varargout = " << matlabName()
-                << "(this, varargin)\n";
-}
-
-/* ************************************************************************* */
-string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
-                            Str matlabUniqueName,
-                            const ArgumentList& args) const {
-  // check arguments
-  // extra argument obj -> nargin-1 is passed !
-  // example: checkArguments("equals",nargout,nargin-1,2);
-  wrapperFile.oss << "  checkArguments(\"" << matlabName()
-                  << "\",nargout,nargin-1," << args.size() << ");\n";
-
-  // get class pointer
-  // example: auto obj = unwrap_shared_ptr< Test >(in[0], "Test");
-  wrapperFile.oss << "  auto obj = unwrap_shared_ptr<" << cppClassName
-                  << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
-
-  // unwrap arguments, see Argument.cpp, we start at 1 as first is obj
-  args.matlab_unwrap(wrapperFile, 1);
-
-  // call method and wrap result
-  // example: out[0]=wrap<bool>(obj->return_field(t));
-  string expanded = "obj->" + name_;
-  if (templateArgValue_)
-    expanded += ("<" + templateArgValue_->qualifiedName("::") + ">");
-
-  return expanded;
-}
-
-/* ************************************************************************* */
-void Method::emit_cython_pxd(FileWriter& file, const Class& cls) const {
-  for (size_t i = 0; i < nrOverloads(); ++i) {
-    file.oss << "        ";
-    returnVals_[i].emit_cython_pxd(file, cls.pxdClassName(), cls.templateArgs);
-    const string renamed = pyRename(name_);
-    if (renamed != name_) {
-      file.oss << pyRename(name_) + " \"" + name_ + "\"" << "(";
-    } else {
-      file.oss << name_ << "(";
-    }
-    argumentList(i).emit_cython_pxd(file, cls.pxdClassName(), cls.templateArgs);
-    file.oss << ")";
-    // if (is_const_) file.oss << " const";
-    file.oss << " except +";
-    file.oss << "\n";
-  }
-}
-
-/* ************************************************************************* */
-void Method::emit_cython_pyx_no_overload(FileWriter& file,
-                                         const Class& cls) const {
-  string funcName = pyRename(name_);
-
-  // leverage python's special treatment for print
-  if (funcName == "print_") {
-    file.oss << "    def __repr__(self):\n";
-    file.oss << "        strBuf = RedirectCout()\n";
-    file.oss << "        self.print_('')\n";
-    file.oss << "        return strBuf.str()\n";
-  }
-
-  // Function definition
-  file.oss << "    def " << funcName;
-
-  // modify name of function instantiation as python doesn't allow overloads
-  // e.g. template<T={A,B,C}> funcName(...) --> funcNameA, funcNameB, funcNameC
-  if (templateArgValue_) file.oss << templateArgValue_->pyxClassName();
-
-  // function arguments
-  file.oss << "(self";
-  if (argumentList(0).size() > 0) file.oss << ", ";
-  argumentList(0).emit_cython_pyx(file);
-  file.oss << "):\n";
-
-  /// Call cython corresponding function and return
-  file.oss << argumentList(0).pyx_convertEigenTypeAndStorageOrder("        ");
-  string caller = "self." + cls.shared_pxd_obj_in_pyx() + ".get()";
-  string ret = pyx_functionCall(caller, funcName, 0);
-  if (!returnVals_[0].isVoid()) {
-    file.oss << "        cdef " << returnVals_[0].pyx_returnType()
-             << " ret = " << ret << "\n";
-    file.oss << "        return " << returnVals_[0].pyx_casting("ret") << "\n";
-  } else {
-    file.oss << "        " << ret << "\n";
-  }
-}
-
-/* ************************************************************************* */
-void Method::emit_cython_pyx(FileWriter& file, const Class& cls) const {
-  string funcName = pyRename(name_);
-  // For template function: modify name of function instantiation as python
-  // doesn't allow overloads
-  // e.g. template<T={A,B,C}> funcName(...) --> funcNameA, funcNameB, funcNameC
-  string instantiatedName =
-      (templateArgValue_) ? funcName + templateArgValue_->pyxClassName() :
-          funcName;
-
-  size_t N = nrOverloads();
-  // It's easy if there's no overload
-  if (N == 1) {
-    emit_cython_pyx_no_overload(file, cls);
-    return;
-  }
-
-  // Dealing with overloads..
-  file.oss << "    def " << instantiatedName << "(self, *args, **kwargs):\n";
-  file.oss << "        cdef list __params\n";
-
-  // Define return values for all possible overloads
-  vector<string> return_type; // every overload has a return type, possibly void
-  map<string, string> return_value; // we only define one return value for every distinct type
-  size_t j = 1;
-  for (size_t i = 0; i < nrOverloads(); ++i) {
-    if (returnVals_[i].isVoid()) {
-      return_type.push_back("void");
-    } else {
-      const string type = returnVals_[i].pyx_returnType();
-      return_type.push_back(type);
-      if (return_value.count(type) == 0) {
-        const string value = "return_value_" + to_string(j++);
-        return_value[type] = value;
-        file.oss << "        cdef " << type << " " << value << "\n";
-      }
-    }
-  }
-
-  for (size_t i = 0; i < nrOverloads(); ++i) {
-    ArgumentList args = argumentList(i);
-    file.oss << "        try:\n";
-    file.oss << pyx_resolveOverloadParams(args, false, 3); // lazy: always return None even if it's a void function
-
-    /// Call corresponding cython function
-    file.oss << args.pyx_convertEigenTypeAndStorageOrder("            ");
-    string caller = "self." + cls.shared_pxd_obj_in_pyx() + ".get()";
-    string call = pyx_functionCall(caller, funcName, i);
-    if (!returnVals_[i].isVoid()) {
-      const string type = return_type[i];
-      const string value = return_value[type];
-      file.oss << "            " << value << " = " << call << "\n";
-      file.oss << "            return " << returnVals_[i].pyx_casting(value)
-          << "\n";
-    } else {
-      file.oss << "            " << call << "\n";
-      file.oss << "            return\n";
-    }
-    file.oss << "        except (AssertionError, ValueError):\n";
-    file.oss << "            pass\n";
-  }
-  file.oss
-      << "        raise TypeError('Incorrect arguments or types for method call.')\n\n";
-}
-/* ************************************************************************* */
diff --git a/wrap/Method.h b/wrap/Method.h
deleted file mode 100644
index 4d3c8d909..000000000
--- a/wrap/Method.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Method.h
- * @brief describes and generates code for methods
- * @author Frank Dellaert
- * @author Richard Roberts
- **/
-
-#pragma once
-
-#include "MethodBase.h"
-
-namespace wrap {
-
-/// Method class
-class Method: public MethodBase {
-
-protected:
-  bool is_const_;
-
-public:
-
-  typedef const std::string& Str;
-
-  bool addOverload(Str name, const ArgumentList& args,
-      const ReturnValue& retVal, bool is_const,
-      boost::optional<const Qualified> instName = boost::none, bool verbose =
-          false);
-
-  bool isStatic() const override {
-    return false;
-  }
-
-  virtual bool isConst() const {
-    return is_const_;
-  }
-
-  bool isSameModifiers(const Method& other) const {
-      return is_const_ == other.is_const_ &&
-             ((templateArgValue_ && other.templateArgValue_) ||
-              (!templateArgValue_ && !other.templateArgValue_));
-  }
-
-  friend std::ostream& operator<<(std::ostream& os, const Method& m) {
-    for (size_t i = 0; i < m.nrOverloads(); i++)
-      os << m.returnVals_[i] << " " << m.name_ << m.argLists_[i];
-    return os;
-  }
-
-  void emit_cython_pxd(FileWriter& file, const Class& cls) const;
-  void emit_cython_pyx(FileWriter& file, const Class& cls) const;
-  void emit_cython_pyx_no_overload(FileWriter& file, const Class& cls) const;
-
-private:
-
-  // Emit method header
-  void proxy_header(FileWriter& proxyFile) const override;
-
-  std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
-      Str matlabUniqueName, const ArgumentList& args) const override;
-};
-
-} // \namespace wrap
-
diff --git a/wrap/MethodBase.cpp b/wrap/MethodBase.cpp
deleted file mode 100644
index a2ed68780..000000000
--- a/wrap/MethodBase.cpp
+++ /dev/null
@@ -1,135 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 MethodBase.ccp
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#include "Method.h"
-#include "Class.h"
-#include "utilities.h"
-
-#include <boost/lexical_cast.hpp>
-#include <boost/algorithm/string.hpp>
-
-#include <iostream>
-#include <fstream>
-
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-void MethodBase::proxy_wrapper_fragments(
-    FileWriter& proxyFile, FileWriter& wrapperFile, Str cppClassName,
-    Str matlabQualName, Str matlabUniqueName, Str wrapperName,
-    const TypeAttributesTable& typeAttributes,
-    vector<string>& functionNames) const {
-  // emit header, e.g., function varargout = templatedMethod(this, varargin)
-  proxy_header(proxyFile);
-
-  // Emit comments for documentation
-  string up_name = boost::to_upper_copy(matlabName());
-  proxyFile.oss << "      % " << up_name << " usage: ";
-  usage_fragment(proxyFile, matlabName());
-
-  // Emit URL to Doxygen page
-  proxyFile.oss << "      % "
-                << "Doxygen can be found at "
-                   "http://research.cc.gatech.edu/borg/sites/edu.borg/html/"
-                   "index.html" << endl;
-
-  // Handle special case of single overload with all numeric arguments
-  if (nrOverloads() == 1 && argumentList(0).allScalar()) {
-    // Output proxy matlab code
-    // TODO: document why is it OK to not check arguments in this case
-    proxyFile.oss << "      ";
-    const int id = (int)functionNames.size();
-    emit_call(proxyFile, returnValue(0), wrapperName, id);
-
-    // Output C++ wrapper code
-    const string wrapFunctionName = wrapper_fragment(
-        wrapperFile, cppClassName, matlabUniqueName, 0, id, typeAttributes);
-
-    // Add to function list
-    functionNames.push_back(wrapFunctionName);
-  } else {
-    // Check arguments for all overloads
-    for (size_t i = 0; i < nrOverloads(); ++i) {
-      // Output proxy matlab code
-      proxyFile.oss << "      " << (i == 0 ? "" : "else");
-      const int id = (int)functionNames.size();
-      emit_conditional_call(proxyFile, returnValue(i), argumentList(i),
-                            wrapperName, id);
-
-      // Output C++ wrapper code
-      const string wrapFunctionName = wrapper_fragment(
-          wrapperFile, cppClassName, matlabUniqueName, i, id, typeAttributes);
-
-      // Add to function list
-      functionNames.push_back(wrapFunctionName);
-    }
-    proxyFile.oss << "      else\n";
-    proxyFile.oss
-        << "        error('Arguments do not match any overload of function "
-        << matlabQualName << "." << name_ << "');" << endl;
-    proxyFile.oss << "      end\n";
-  }
-
-  proxyFile.oss << "    end\n";
-}
-
-/* ************************************************************************* */
-string MethodBase::wrapper_fragment(
-    FileWriter& wrapperFile, Str cppClassName, Str matlabUniqueName,
-    int overload, int id, const TypeAttributesTable& typeAttributes) const {
-  // generate code
-
-  const string wrapFunctionName =
-      matlabUniqueName + "_" + name_ + "_" + boost::lexical_cast<string>(id);
-
-  const ArgumentList& args = argumentList(overload);
-  const ReturnValue& returnVal = returnValue(overload);
-
-  // call
-  wrapperFile.oss
-      << "void " << wrapFunctionName
-      << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
-  // start
-  wrapperFile.oss << "{\n";
-
-  // get call
-  // for static methods: cppClassName::staticMethod<TemplateVal>
-  // for instance methods: obj->instanceMethod<TemplateVal>
-  string expanded =
-      wrapper_call(wrapperFile, cppClassName, matlabUniqueName, args);
-
-  expanded += ("(" + args.names() + ")");
-  if (returnVal.type1.name() != "void")
-    returnVal.wrap_result(expanded, wrapperFile, typeAttributes);
-  else
-    wrapperFile.oss << "  " + expanded + ";\n";
-
-  // finish
-  wrapperFile.oss << "}\n";
-
-  return wrapFunctionName;
-}
-
-/* ************************************************************************* */
-void MethodBase::python_wrapper(FileWriter& wrapperFile, Str className) const {
-  wrapperFile.oss << "  .def(\"" << name_ << "\", &" << className
-                  << "::" << name_ << ");\n";
-}
-
-/* ************************************************************************* */
diff --git a/wrap/MethodBase.h b/wrap/MethodBase.h
deleted file mode 100644
index ee72a6a53..000000000
--- a/wrap/MethodBase.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 MethodBase.h
- * @brief describes and generates code for static methods
- * @author Frank Dellaert
- * @author Alex Cunningham
- * @author Richard Roberts
- **/
-
-#pragma once
-
-#include "FullyOverloadedFunction.h"
-
-namespace wrap {
-
-// Forward declaration
-class Class;
-
-/// MethodBase class
-struct MethodBase : public FullyOverloadedFunction {
-  typedef const std::string& Str;
-
-  // emit a list of comments, one for each overload
-  void comment_fragment(FileWriter& proxyFile) const {
-    SignatureOverloads::comment_fragment(proxyFile, matlabName());
-  }
-
-  void verifyArguments(const std::vector<std::string>& validArgs) const {
-    SignatureOverloads::verifyArguments(validArgs, name_);
-  }
-
-  void verifyReturnTypes(const std::vector<std::string>& validtypes) const {
-    SignatureOverloads::verifyReturnTypes(validtypes, name_);
-  }
-
-  // MATLAB code generation
-  // classPath is class directory, e.g., ../matlab/@Point2
-  void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
-                               Str cppClassName, Str matlabQualName,
-                               Str matlabUniqueName, Str wrapperName,
-                               const TypeAttributesTable& typeAttributes,
-                               std::vector<std::string>& functionNames) const;
-
-  // emit python wrapper
-  void python_wrapper(FileWriter& wrapperFile, Str className) const;
-
-protected:
-  virtual void proxy_header(FileWriter& proxyFile) const = 0;
-
-  std::string wrapper_fragment(
-      FileWriter& wrapperFile, Str cppClassName, Str matlabUniqueName,
-      int overload, int id,
-      const TypeAttributesTable& typeAttributes) const;  ///< cpp wrapper
-
-  virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
-                                   Str matlabUniqueName,
-                                   const ArgumentList& args) const = 0;
-};
-
-}  // \namespace wrap
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
deleted file mode 100644
index 780c6f8da..000000000
--- a/wrap/Module.cpp
+++ /dev/null
@@ -1,649 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Module.ccp
- * @author Frank Dellaert
- * @author Alex Cunningham
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#include "Module.h"
-#include "FileWriter.h"
-#include "TypeAttributesTable.h"
-#include "utilities.h"
-
-#include <boost/filesystem.hpp>
-#include <boost/lexical_cast.hpp>
-
-#include <iostream>
-#include <algorithm>
-
-using namespace std;
-using namespace wrap;
-using namespace BOOST_SPIRIT_CLASSIC_NS;
-namespace bl = boost::lambda;
-namespace fs = boost::filesystem;
-
-/* ************************************************************************* */
-// We parse an interface file into a Module object.
-// The grammar is defined using the boost/spirit combinatorial parser.
-// For example, str_p("const") parses the string "const", and the >>
-// operator creates a sequence parser. The grammar below, composed of rules
-// and with start rule [class_p], doubles as the specs for our interface files.
-/* ************************************************************************* */
-
-/* ************************************************************************* */
-// If a number of template arguments were given, generate a number of expanded
-// class names, e.g., PriorFactor -> PriorFactorPose2, and add those classes
-static void handle_possible_template(vector<Class>& classes,
-                                     vector<Class>& uninstantiatedClasses,
-                                     const Class& cls, const Template& t) {
-  uninstantiatedClasses.push_back(cls);
-  if (cls.templateArgs.empty() || t.empty()) {
-    classes.push_back(cls);
-  } else {
-    if (cls.templateArgs.size() != 1)
-      throw std::runtime_error(
-          "In-line template instantiations only handle a single template argument");
-    string arg = cls.templateArgs.front();
-    vector<Class> classInstantiations =
-        (t.nrValues() > 0) ? cls.expandTemplate(arg, t.argValues()) :
-            cls.expandTemplate(arg, t.intList());
-    for(const Class& c: classInstantiations)
-      classes.push_back(c);
-  }
-}
-
-static void push_typedef_pair(vector<TypedefPair>& typedefs,
-                              const Qualified& oldType,
-                              const Qualified& newType,
-                              const string& includeFile) {
-    typedefs.push_back(TypedefPair(oldType, newType, includeFile));
-}
-
-/* ************************************************************************* */
-Module::Module(const std::string& moduleName, bool enable_verbose)
-: name(moduleName), verbose(enable_verbose)
-{
-}
-
-/* ************************************************************************* */
-Module::Module(const string& interfacePath,
-         const string& moduleName, bool enable_verbose)
-: name(moduleName), verbose(enable_verbose)
-{
-  // read interface file
-  string interfaceFile = interfacePath + "/" + moduleName + ".h";
-  string contents = file_contents(interfaceFile);
-
-  // execute parsing
-  parseMarkup(contents);
-}
-
-/* ************************************************************************* */
-void Module::parseMarkup(const std::string& data) {
-  // The parse imperatively :-( updates variables gradually during parse
-  // The one with postfix 0 are used to reset the variables after parse.
-
-  //----------------------------------------------------------------------------
-  // Grammar with actions that build the Class object. Actions are
-  // defined within the square brackets [] and are executed whenever a
-  // rule is successfully parsed. Define BOOST_SPIRIT_DEBUG to debug.
-  // The grammar is allows a very restricted C++ header
-  // lexeme_d turns off white space skipping
-  // http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html
-  // ----------------------------------------------------------------------------
-
-  // Define Rule and instantiate basic rules
-  typedef rule<phrase_scanner_t> Rule;
-  BasicRules<phrase_scanner_t> basic;
-
-  vector<string> namespaces; // current namespace tag
-  string currentInclude;
-
-  // parse a full class
-  Class cls0(verbose),cls(verbose);
-  Template classTemplate;
-  ClassGrammar class_g(cls,classTemplate);
-  Rule class_p = class_g  //
-      [assign_a(cls.namespaces_, namespaces)]
-      [assign_a(cls.includeFile, currentInclude)][bl::bind(
-          &handle_possible_template, bl::var(classes),
-          bl::var(uninstantiatedClasses), bl::var(cls),
-          bl::var(classTemplate))][clear_a(classTemplate)]  //
-      [assign_a(cls, cls0)];
-
-  // parse "gtsam::Pose2" and add to singleInstantiation.typeList
-  TemplateInstantiationTypedef singleInstantiation, singleInstantiation0;
-  TypeListGrammar<'<','>'> typelist_g(singleInstantiation.typeList);
-
-  // typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
-  TypeGrammar instantiationClass_g(singleInstantiation.class_);
-  Rule templateSingleInstantiation_p =
-    (str_p("typedef") >> instantiationClass_g >>
-    typelist_g >>
-    basic.className_p[assign_a(singleInstantiation.name_)] >>
-    ';')
-    [assign_a(singleInstantiation.namespaces_, namespaces)]
-    [push_back_a(templateInstantiationTypedefs, singleInstantiation)]
-    [assign_a(singleInstantiation, singleInstantiation0)];
-
-  Qualified oldType, newType;
-  TypeGrammar typedefOldClass_g(oldType), typedefNewClass_g(newType);
-  Rule typedef_p =
-      (str_p("typedef") >> typedefOldClass_g >> typedefNewClass_g >>
-       ';')
-      [assign_a(oldType.namespaces_, namespaces)]
-      [assign_a(newType.namespaces_, namespaces)]
-      [bl::bind(&push_typedef_pair, bl::var(typedefs), bl::var(oldType),
-                     bl::var(newType), bl::var(currentInclude))];
-
-  // Create grammar for global functions
-  GlobalFunctionGrammar global_function_g(global_functions, namespaces,
-                                          currentInclude);
-
-  Rule include_p = str_p("#include") >> ch_p('<') >>
-                   (*(anychar_p - '>'))[push_back_a(includes)]
-                   [assign_a(currentInclude)] >>
-                   ch_p('>');
-
-#ifdef __clang__
-#pragma clang diagnostic push
-#pragma clang diagnostic ignored "-Wuninitialized"
-#endif
-
-  Rule namespace_def_p =
-      (str_p("namespace")
-      >> basic.namespace_p[push_back_a(namespaces)]
-      >> ch_p('{')
-      >> *(include_p | class_p | templateSingleInstantiation_p | typedef_p | global_function_g | namespace_def_p | basic.comments_p)
-      >> ch_p('}'))
-      [pop_a(namespaces)];
-
-#ifdef __clang__
-#pragma clang diagnostic pop
-#endif
-
-  // parse forward declaration
-  ForwardDeclaration fwDec0, fwDec;
-  Class fwParentClass;
-  TypeGrammar className_g(fwDec.cls);
-  TypeGrammar classParent_g(fwParentClass);
-  Rule classParent_p = (':' >> classParent_g >> ';') //
-         [bl::bind(&Class::assignParent, bl::var(fwDec.cls),
-              bl::var(fwParentClass))][clear_a(fwParentClass)];
-
-  Rule forward_declaration_p =
-      !(str_p("virtual")[assign_a(fwDec.isVirtual, T)])
-      >> str_p("class") >> className_g
-      >> (classParent_p | ';')
-      [push_back_a(forward_declarations, fwDec)]
-      [assign_a(cls,cls0)] // also clear class to avoid partial parse
-      [assign_a(fwDec, fwDec0)];
-
-  Rule module_content_p = basic.comments_p | include_p | class_p
-      | templateSingleInstantiation_p | forward_declaration_p
-      | global_function_g | namespace_def_p;
-
-  Rule module_p = *module_content_p >> !end_p;
-
-  // and parse contents
-  parse_info<const char*> info = parse(data.c_str(), module_p, space_p);
-  if(!info.full) {
-    printf("parsing stopped at \n%.20s\n",info.stop);
-    cout << "Stopped in:\n"
-      "class '" << cls.name_ << "'" << endl;
-    throw ParseFailed((int)info.length);
-  }
-
-  // Post-process classes for serialization markers
-  for(Class& cls: classes)
-    cls.erase_serialization();
-
-  for(Class& cls: uninstantiatedClasses)
-    cls.erase_serialization();
-
-  // Explicitly add methods to the classes from parents so it shows in documentation
-  for(Class& cls: classes)
-    cls.appendInheritedMethods(cls, classes);
-
-  // - Remove inherited methods for Cython classes in the pxd, otherwise Cython can't decide which one to call.
-  // - Only inherited nontemplateMethods_ in uninstantiatedClasses need to be removed
-  // because that what we serialized to the pxd.
-  // - However, we check against the class parent's *methods_* to avoid looking into
-  // its grand parent and grand-grand parent, etc., because all those are already
-  // added in its direct parent.
-  // - So this must be called *after* the above code appendInheritedMethods!!
-  for(Class& cls: uninstantiatedClasses)
-      cls.removeInheritedNontemplateMethods(uninstantiatedClasses);
-
-  // Expand templates - This is done first so that template instantiations are
-  // counted in the list of valid types, have their attributes and dependencies
-  // checked, etc.
-  expandedClasses = ExpandTypedefInstantiations(classes,
-      templateInstantiationTypedefs);
-
-  // Dependency check list
-  vector<string> validTypes = GenerateValidTypes(expandedClasses,
-      forward_declarations, typedefs);
-
-  // Check that all classes have been defined somewhere
-  verifyArguments<GlobalFunction>(validTypes, global_functions);
-  verifyReturnTypes<GlobalFunction>(validTypes, global_functions);
-
-  hasSerialiable = false;
-  for(const Class& cls: expandedClasses)
-    cls.verifyAll(validTypes,hasSerialiable);
-
-  // Create type attributes table and check validity
-  typeAttributes.addClasses(expandedClasses);
-  typeAttributes.addForwardDeclarations(forward_declarations);
-  for (const TypedefPair& p: typedefs)
-    typeAttributes.addType(p.newType);
-  // add Eigen types as template arguments are also checked ?
-  vector<ForwardDeclaration> eigen;
-  eigen.push_back(ForwardDeclaration("Vector"));
-  eigen.push_back(ForwardDeclaration("Matrix"));
-  typeAttributes.addForwardDeclarations(eigen);
-  typeAttributes.checkValidity(expandedClasses);
-}
-
-/* ************************************************************************* */
-void Module::generate_matlab_wrapper(const string& toolboxPath) const {
-
-  fs::create_directories(toolboxPath);
-
-  // create the unified .cpp switch file
-  const string wrapperName = name + "_wrapper";
-  string wrapperFileName = toolboxPath + "/" + wrapperName + ".cpp";
-  FileWriter wrapperFile(wrapperFileName, verbose, "//");
-  wrapperFile.oss << "#include <wrap/matlab.h>\n";
-  wrapperFile.oss << "#include <map>\n";
-  wrapperFile.oss << "\n";
-
-  // Include boost.serialization archive headers before other class headers
-  if (hasSerialiable) {
-    wrapperFile.oss << "#include <boost/archive/text_iarchive.hpp>\n";
-    wrapperFile.oss << "#include <boost/archive/text_oarchive.hpp>\n";
-    wrapperFile.oss << "#include <boost/serialization/export.hpp>\n\n";
-  }
-
-  // Generate includes while avoiding redundant includes
-  generateIncludes(wrapperFile);
-
-  // create typedef classes - we put this at the top of the wrap file so that
-  // collectors and method arguments can use these typedefs
-  for(const Class& cls: expandedClasses)
-    if(!cls.typedefName.empty())
-      wrapperFile.oss << cls.getTypedef() << "\n";
-  wrapperFile.oss << "\n";
-
-  // Generate boost.serialization export flags (needs typedefs from above)
-  if (hasSerialiable) {
-    for(const Class& cls: expandedClasses)
-      if(cls.isSerializable)
-        wrapperFile.oss << cls.getSerializationExport() << "\n";
-    wrapperFile.oss << "\n";
-  }
-
-  // Generate collectors and cleanup function to be called from mexAtExit
-  WriteCollectorsAndCleanupFcn(wrapperFile, name, expandedClasses);
-
-  // generate RTTI registry (for returning derived-most types)
-  WriteRTTIRegistry(wrapperFile, name, expandedClasses);
-
-  vector<string> functionNames; // Function names stored by index for switch
-
-  // create proxy class and wrapper code
-  for(const Class& cls: expandedClasses)
-    cls.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames);
-
-  // create matlab files and wrapper code for global functions
-  for(const GlobalFunctions::value_type& p: global_functions)
-    p.second.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames);
-
-  // finish wrapper file
-  wrapperFile.oss << "\n";
-  finish_wrapper(wrapperFile, functionNames);
-
-  wrapperFile.emit(true);
-}
-
-/* ************************************************************************* */
-void Module::generate_cython_wrapper(const string& toolboxPath, const std::string& pxdImports) const {
-  fs::create_directories(toolboxPath);
-  string pxdFileName = toolboxPath + "/" + name + ".pxd";
-  FileWriter pxdFile(pxdFileName, verbose, "#");
-  pxdFile.oss << pxdImports << "\n";
-  emit_cython_pxd(pxdFile);
-  string pyxFileName = toolboxPath + "/" + name + ".pyx";
-  FileWriter pyxFile(pyxFileName, verbose, "#");
-  emit_cython_pyx(pyxFile);
-}
-
-/* ************************************************************************* */
-void Module::emit_cython_pxd(FileWriter& pxdFile) const {
-  // headers
-  pxdFile.oss << "from gtsam_eigency.core cimport *\n"
-                 "from libcpp.string cimport string\n"
-                 "from libcpp.vector cimport vector\n"
-                 "from libcpp.pair cimport pair\n"
-                 "from libcpp.set cimport set\n"
-                 "from libcpp.map cimport map\n"
-                 "from libcpp cimport bool\n\n";
-
-  // boost shared_ptr
-  pxdFile.oss << "cdef extern from \"boost/shared_ptr.hpp\" namespace \"boost\":\n"
-                 "    cppclass shared_ptr[T]:\n"
-                 "        shared_ptr()\n"
-                 "        shared_ptr(T*)\n"
-                 "        T* get()\n"
-                 "        long use_count() const\n"
-                 "        T& operator*()\n\n"
-                 "    cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n\n";
-
-  // gtsam alignment-friendly shared_ptr
-  pxdFile.oss << "cdef extern from \"gtsam/base/make_shared.h\" namespace \"gtsam\":\n"
-                 "    cdef shared_ptr[T] make_shared[T](const T& r)\n\n";
-
-  for(const TypedefPair& types: typedefs)
-    types.emit_cython_pxd(pxdFile);
-
-  //... wrap all classes
-  for (const Class& cls : uninstantiatedClasses) {
-    cls.emit_cython_pxd(pxdFile);
-
-    for (const Class& expCls : expandedClasses) {
-      bool matchingNonTemplated = !expCls.templateClass
-          && expCls.pxdClassName() == cls.pxdClassName();
-      bool isTemplatedFromCls = expCls.templateClass
-          && expCls.templateClass->pxdClassName() == cls.pxdClassName();
-
-      // ctypedef for template instantiations
-      if (isTemplatedFromCls) {
-        pxdFile.oss << "\n";
-        pxdFile.oss << "ctypedef " << expCls.templateClass->pxdClassName()
-            << "[";
-        for (size_t i = 0; i < expCls.templateInstTypeList.size(); ++i)
-          pxdFile.oss << expCls.templateInstTypeList[i].pxdClassName()
-              << ((i == expCls.templateInstTypeList.size() - 1) ? "" : ", ");
-        pxdFile.oss << "] " << expCls.pxdClassName() << "\n";
-      }
-
-      // Python wrapper class
-      if (isTemplatedFromCls || matchingNonTemplated) {
-        expCls.emit_cython_wrapper_pxd(pxdFile);
-      }
-    }
-    pxdFile.oss << "\n\n";
-  }
-
-  //... wrap global functions
-  for(const GlobalFunctions::value_type& p: global_functions)
-    p.second.emit_cython_pxd(pxdFile);
-
-  pxdFile.emit(true);
-}
-
-/* ************************************************************************* */
-void Module::emit_cython_pyx(FileWriter& pyxFile) const {
-  // directives...
-  // allow str to automatically coerce to std::string and back (for python3)
-  pyxFile.oss << "# cython: c_string_type=str, c_string_encoding=ascii\n\n";
-
-  // headers...
-  string pxdHeader = name;
-  pyxFile.oss << "cimport numpy as np\n"
-                 "import numpy as npp\n"
-                 "cimport " << pxdHeader << "\n"
-                 "from ."<< pxdHeader << " cimport shared_ptr\n"
-                 "from ."<< pxdHeader << " cimport dynamic_pointer_cast\n"
-                 "from ."<< pxdHeader << " cimport make_shared\n";
-
-  pyxFile.oss << "# C helper function that copies all arguments into a positional list.\n"
-                 "cdef list process_args(list keywords, tuple args, dict kwargs):\n"
-                 "   cdef str keyword\n"
-                 "   cdef int n = len(args), m = len(keywords)\n"
-                 "   cdef list params = list(args)\n"
-                 "   assert len(args)+len(kwargs) == m, 'Expected {} arguments'.format(m)\n"
-                 "   try:\n"
-                 "       return params + [kwargs[keyword] for keyword in keywords[n:]]\n"
-                 "   except:\n"
-                 "       raise ValueError('Epected arguments ' + str(keywords))\n";
-
-  // import all typedefs, e.g. from gtsam_wrapper cimport Key, so we don't need to say gtsam.Key
-  for(const Qualified& q: Qualified::BasicTypedefs) {
-    pyxFile.oss << "from " << pxdHeader << " cimport " << q.pxdClassName() << "\n";
-  }
-  pyxFile.oss << "from gtsam_eigency.core cimport *\n"
-                 "from libcpp cimport bool\n\n"
-                 "from libcpp.pair cimport pair\n"
-                 "from libcpp.string cimport string\n"
-                 "from cython.operator cimport dereference as deref\n\n\n";
-
-  // all classes include all forward declarations
-  std::vector<Class> allClasses = expandedClasses;
-  for(const ForwardDeclaration& fd: forward_declarations)
-    allClasses.push_back(fd.cls);
-
-  for(const Class& cls: expandedClasses)
-    cls.emit_cython_pyx(pyxFile, allClasses);
-  pyxFile.oss << "\n";
-
-  //... wrap global functions
-  for(const GlobalFunctions::value_type& p: global_functions)
-    p.second.emit_cython_pyx(pyxFile);
-  pyxFile.emit(true);
-}
-
-/* ************************************************************************* */
-void Module::generateIncludes(FileWriter& file) const {
-
-  // collect includes
-  vector<string> all_includes(includes);
-
-  // sort and remove duplicates
-  sort(all_includes.begin(), all_includes.end());
-  vector<string>::const_iterator last_include = unique(all_includes.begin(), all_includes.end());
-  vector<string>::const_iterator it = all_includes.begin();
-  // add includes to file
-  for (; it != last_include; ++it)
-    file.oss << "#include <" << *it << ">" << endl;
-  file.oss << "\n";
-}
-
-
-/* ************************************************************************* */
-  void Module::finish_wrapper(FileWriter& file, const std::vector<std::string>& functionNames) const {
-    file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
-    file.oss << "{\n";
-    file.oss << "  mstream mout;\n"; // Send stdout to MATLAB console
-    file.oss << "  std::streambuf *outbuf = std::cout.rdbuf(&mout);\n\n";
-    file.oss << "  _" << name << "_RTTIRegister();\n\n";
-    file.oss << "  int id = unwrap<int>(in[0]);\n\n";
-    file.oss << "  try {\n";
-    file.oss << "    switch(id) {\n";
-    for(size_t id = 0; id < functionNames.size(); ++id) {
-      file.oss << "    case " << id << ":\n";
-      file.oss << "      " << functionNames[id] << "(nargout, out, nargin-1, in+1);\n";
-      file.oss << "      break;\n";
-    }
-    file.oss << "    }\n";
-    file.oss << "  } catch(const std::exception& e) {\n";
-    file.oss << "    mexErrMsgTxt((\"Exception from gtsam:\\n\" + std::string(e.what()) + \"\\n\").c_str());\n";
-    file.oss << "  }\n";
-    file.oss << "\n";
-    file.oss << "  std::cout.rdbuf(outbuf);\n"; // Restore cout
-    file.oss << "}\n";
-  }
-
-/* ************************************************************************* */
-vector<Class> Module::ExpandTypedefInstantiations(const vector<Class>& classes, const vector<TemplateInstantiationTypedef> instantiations) {
-
-  vector<Class> expandedClasses = classes;
-
-  for(const TemplateInstantiationTypedef& inst: instantiations) {
-    // Add the new class to the list
-    expandedClasses.push_back(inst.findAndExpand(classes));
-  }
-
-  // Remove all template classes
-  for(size_t i = 0; i < expandedClasses.size(); ++i)
-    if(!expandedClasses[i].templateArgs.empty()) {
-      expandedClasses.erase(expandedClasses.begin() + size_t(i));
-      -- i;
-    }
-
-  return expandedClasses;
-}
-
-/* ************************************************************************* */
-vector<string> Module::GenerateValidTypes(const vector<Class>& classes, const vector<ForwardDeclaration>& forwardDeclarations, const vector<TypedefPair>& typedefs) {
-  vector<string> validTypes;
-  for(const ForwardDeclaration& fwDec: forwardDeclarations) {
-    validTypes.push_back(fwDec.name());
-  }
-  validTypes.push_back("void");
-  validTypes.push_back("string");
-  validTypes.push_back("int");
-  validTypes.push_back("bool");
-  validTypes.push_back("char");
-  validTypes.push_back("unsigned char");
-  validTypes.push_back("size_t");
-  validTypes.push_back("double");
-  validTypes.push_back("Vector");
-  validTypes.push_back("Matrix");
-  //Create a list of parsed classes for dependency checking
-  for(const Class& cls: classes) {
-    validTypes.push_back(cls.qualifiedName("::"));
-  }
-  for(const TypedefPair& p: typedefs) {
-    validTypes.push_back(p.newType.qualifiedName("::"));
-  }
-
-  return validTypes;
-}
-
-/* ************************************************************************* */
-void Module::WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector<Class>& classes) {
-  // Generate all collectors
-  for(const Class& cls: classes) {
-    const string matlabUniqueName = cls.qualifiedName(),
-      cppName = cls.qualifiedName("::");
-    wrapperFile.oss << "typedef std::set<boost::shared_ptr<" << cppName << ">*> "
-      << "Collector_" << matlabUniqueName << ";\n";
-    wrapperFile.oss << "static Collector_" << matlabUniqueName <<
-      " collector_" << matlabUniqueName << ";\n";
-  }
-
-  // generate mexAtExit cleanup function
-  wrapperFile.oss <<
-    "\nvoid _deleteAllObjects()\n"
-    "{\n"
-    "  mstream mout;\n" // Send stdout to MATLAB console
-    "  std::streambuf *outbuf = std::cout.rdbuf(&mout);\n\n"
-    "  bool anyDeleted = false;\n";
-  for(const Class& cls: classes) {
-    const string matlabUniqueName = cls.qualifiedName();
-    const string cppName = cls.qualifiedName("::");
-    const string collectorType = "Collector_" + matlabUniqueName;
-    const string collectorName = "collector_" + matlabUniqueName;
-    // The extra curly-braces around the for loops work around a limitation in MSVC (existing
-    // since 2005!) preventing more than 248 blocks.
-    wrapperFile.oss <<
-      "  { for(" << collectorType << "::iterator iter = " << collectorName << ".begin();\n"
-      "      iter != " << collectorName << ".end(); ) {\n"
-      "    delete *iter;\n"
-      "    " << collectorName << ".erase(iter++);\n"
-      "    anyDeleted = true;\n"
-      "  } }\n";
-  }
-  wrapperFile.oss <<
-    "  if(anyDeleted)\n"
-    "    cout <<\n"
-    "      \"WARNING:  Wrap modules with variables in the workspace have been reloaded due to\\n\"\n"
-    "      \"calling destructors, call 'clear all' again if you plan to now recompile a wrap\\n\"\n"
-    "      \"module, so that your recompiled module is used instead of the old one.\" << endl;\n"
-    "  std::cout.rdbuf(outbuf);\n" // Restore cout
-    "}\n\n";
-}
-
-/* ************************************************************************* */
-void Module::WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& moduleName, const std::vector<Class>& classes) {
-  wrapperFile.oss <<
-    "void _" << moduleName << "_RTTIRegister() {\n"
-    "  const mxArray *alreadyCreated = mexGetVariablePtr(\"global\", \"gtsam_" + moduleName + "_rttiRegistry_created\");\n"
-    "  if(!alreadyCreated) {\n"
-    "    std::map<std::string, std::string> types;\n";
-  for(const Class& cls: classes) {
-    if(cls.isVirtual)
-      wrapperFile.oss <<
-      "    types.insert(std::make_pair(typeid(" << cls.qualifiedName("::") << ").name(), \"" << cls.qualifiedName(".") << "\"));\n";
-  }
-  wrapperFile.oss << "\n";
-
-  wrapperFile.oss <<
-    "    mxArray *registry = mexGetVariable(\"global\", \"gtsamwrap_rttiRegistry\");\n"
-    "    if(!registry)\n"
-    "      registry = mxCreateStructMatrix(1, 1, 0, NULL);\n"
-    "    typedef std::pair<std::string, std::string> StringPair;\n"
-    "    for(const StringPair& rtti_matlab: types) {\n"
-    "      int fieldId = mxAddField(registry, rtti_matlab.first.c_str());\n"
-    "      if(fieldId < 0)\n"
-    "        mexErrMsgTxt(\"gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly\");\n"
-    "      mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());\n"
-    "      mxSetFieldByNumber(registry, 0, fieldId, matlabName);\n"
-    "    }\n"
-    "    if(mexPutVariable(\"global\", \"gtsamwrap_rttiRegistry\", registry) != 0)\n"
-    "      mexErrMsgTxt(\"gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly\");\n"
-    "    mxDestroyArray(registry);\n"
-    "    \n"
-    "    mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);\n"
-    "    if(mexPutVariable(\"global\", \"gtsam_" + moduleName + "_rttiRegistry_created\", newAlreadyCreated) != 0)\n"
-    "      mexErrMsgTxt(\"gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly\");\n"
-    "    mxDestroyArray(newAlreadyCreated);\n"
-    "  }\n"
-    "}\n"
-    "\n";
-}
-
-/* ************************************************************************* */
-void Module::generate_python_wrapper(const string& toolboxPath) const {
-
-  fs::create_directories(toolboxPath);
-
-  // create the unified .cpp switch file
-  const string wrapperName = name + "_python";
-  string wrapperFileName = toolboxPath + "/" + wrapperName + ".cpp";
-  FileWriter wrapperFile(wrapperFileName, verbose, "//");
-  wrapperFile.oss << "#include <boost/python.hpp>\n\n";
-  wrapperFile.oss << "using namespace boost::python;\n";
-  wrapperFile.oss << "BOOST_PYTHON_MODULE(" + name + ")\n";
-  wrapperFile.oss << "{\n";
-
-  // write out classes
-  for(const Class& cls: expandedClasses) {
-    cls.python_wrapper(wrapperFile);
-  }
-
-  // write out global functions
-  for(const GlobalFunctions::value_type& p: global_functions)
-    p.second.python_wrapper(wrapperFile);
-
-  // finish wrapper file
-  wrapperFile.oss << "}\n";
-
-  wrapperFile.emit(true);
-}
-
-/* ************************************************************************* */
diff --git a/wrap/Module.h b/wrap/Module.h
deleted file mode 100644
index 2a8344551..000000000
--- a/wrap/Module.h
+++ /dev/null
@@ -1,95 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Module.h
- * @brief describes module to be wrapped
- * @author Frank Dellaert
- * @author Richard Roberts
- **/
-
-#pragma once
-
-#include "Class.h"
-#include "GlobalFunction.h"
-#include "TemplateInstantiationTypedef.h"
-#include "ForwardDeclaration.h"
-#include "TypedefPair.h"
-
-#include <string>
-#include <vector>
-#include <map>
-
-namespace wrap {
-
-/**
- * A module just has a name and a list of classes
- */
-struct Module {
-
-  // Filled during parsing:
-  std::string name; ///< module name
-  bool verbose; ///< verbose flag
-  std::vector<Class> classes; ///< list of classes
-  std::vector<Class> uninstantiatedClasses; ///< list of template classes after instantiated
-  std::vector<TemplateInstantiationTypedef> templateInstantiationTypedefs; ///< list of template instantiations
-  std::vector<ForwardDeclaration> forward_declarations;
-  std::vector<std::string> includes; ///< Include statements
-  GlobalFunctions global_functions;
-  std::vector<TypedefPair> typedefs;
-
-  // After parsing:
-  std::vector<Class> expandedClasses;
-  bool hasSerialiable;
-  TypeAttributesTable typeAttributes;
-
-  /// constructor that parses interface file
-  Module(const std::string& interfacePath, const std::string& moduleName,
-      bool enable_verbose = true);
-
-  /// Dummy constructor that does no parsing - use only for testing
-  Module(const std::string& moduleName, bool enable_verbose = true);
-
-  /// non-const function that performs parsing - typically called by constructor
-  /// Throws exception on failure
-  void parseMarkup(const std::string& data);
-
-  /// MATLAB code generation:
-  void generate_matlab_wrapper(const std::string& path) const;
-
-  /// Cython code generation:
-  void generate_cython_wrapper(const std::string& path, const std::string& pxdImports = "") const;
-  void emit_cython_pxd(FileWriter& file) const;
-  void emit_cython_pyx(FileWriter& file) const;
-
-  void generateIncludes(FileWriter& file) const;
-
-  void finish_wrapper(FileWriter& file,
-      const std::vector<std::string>& functionNames) const;
-
-  /// Python code generation:
-  void generate_python_wrapper(const std::string& path) const;
-
-private:
-  static std::vector<Class> ExpandTypedefInstantiations(
-      const std::vector<Class>& classes,
-      const std::vector<TemplateInstantiationTypedef> instantiations);
-  static std::vector<std::string> GenerateValidTypes(
-      const std::vector<Class>& classes,
-      const std::vector<ForwardDeclaration>& forwardDeclarations,
-      const std::vector<TypedefPair>& typedefs);
-  static void WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile,
-      const std::string& moduleName, const std::vector<Class>& classes);
-  static void WriteRTTIRegistry(FileWriter& wrapperFile,
-      const std::string& moduleName, const std::vector<Class>& classes);
-};
-
-} // \namespace wrap
diff --git a/wrap/OverloadedFunction.h b/wrap/OverloadedFunction.h
deleted file mode 100644
index 6bcb72d94..000000000
--- a/wrap/OverloadedFunction.h
+++ /dev/null
@@ -1,140 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 OverloadedFunction.h
- * @brief Function that can overload its arguments only
- * @author Frank Dellaert
- * @date Nov 13, 2014
- **/
-
-#pragma once
-
-#include "Function.h"
-#include "Argument.h"
-#include <unordered_set>
-namespace wrap {
-
-/**
- * ArgumentList Overloads
- */
-class ArgumentOverloads {
-public:
-  std::vector<ArgumentList> argLists_;
-
-public:
-  size_t nrOverloads() const { return argLists_.size(); }
-
-  const ArgumentList& argumentList(size_t i) const { return argLists_.at(i); }
-
-  void push_back(const ArgumentList& args) { argLists_.push_back(args); }
-
-  std::vector<ArgumentList> expandArgumentListsTemplate(
-      const TemplateSubstitution& ts) const {
-    std::vector<ArgumentList> result;
-    for (const ArgumentList& argList : argLists_) {
-      ArgumentList instArgList = argList.expandTemplate(ts);
-      result.push_back(instArgList);
-    }
-    return result;
-  }
-
-  /// Expand templates, imperative !
-  virtual void ExpandTemplate(const TemplateSubstitution& ts) {
-    argLists_ = expandArgumentListsTemplate(ts);
-  }
-
-  void verifyArguments(const std::vector<std::string>& validArgs,
-                       const std::string s) const {
-    for (const ArgumentList& argList : argLists_) {
-      for (Argument arg : argList) {
-        std::string fullType = arg.type.qualifiedName("::");
-        if (find(validArgs.begin(), validArgs.end(), fullType) ==
-            validArgs.end())
-          throw DependencyMissing(fullType, "checking argument of " + s);
-      }
-    }
-  }
-
-  friend std::ostream& operator<<(std::ostream& os,
-                                  const ArgumentOverloads& overloads) {
-    for (const ArgumentList& argList : overloads.argLists_)
-      os << argList << std::endl;
-    return os;
-  }
-
-  std::string pyx_resolveOverloadParams(const ArgumentList& args, bool isVoid,
-      size_t indentLevel = 2) const {
-    std::string indent;
-    for (size_t i = 0; i < indentLevel; ++i)
-      indent += "    ";
-    std::string s;
-    s += indent + "__params = process_args([" + args.pyx_paramsList()
-        + "], args, kwargs)\n";
-    s += args.pyx_castParamsToPythonType(indent);
-    if (args.size() > 0) {
-      for (size_t i = 0; i < args.size(); ++i) {
-        // For python types we can do the assert after the assignment and save list accesses
-        if (args[i].type.isNonBasicType() || args[i].type.isEigen()) {
-          std::string param = args[i].name;
-          s += indent + "assert isinstance(" + param + ", "
-              + args[i].type.pyxArgumentType() + ")";
-          if (args[i].type.isEigen()) {
-            s += " and " + param + ".ndim == "
-                + ((args[i].type.pyxClassName() == "Vector") ? "1" : "2");
-          }
-          s += "\n";
-        }
-      }
-    }
-    return s;
-  }
-};
-
-class OverloadedFunction : public Function, public ArgumentOverloads {
-public:
-  bool addOverload(const std::string& name, const ArgumentList& args,
-                   boost::optional<const Qualified> instName = boost::none,
-                   bool verbose = false) {
-    bool first = initializeOrCheck(name, instName, verbose);
-    ArgumentOverloads::push_back(args);
-    return first;
-  }
-
-private:
-};
-
-// Templated checking functions
-// TODO: do this via polymorphism, use transform ?
-
-template <class F>
-static std::map<std::string, F> expandMethodTemplate(
-    const std::map<std::string, F>& methods, const TemplateSubstitution& ts) {
-  std::map<std::string, F> result;
-  typedef std::pair<const std::string, F> NamedMethod;
-  for (NamedMethod namedMethod : methods) {
-    F instMethod = namedMethod.second;
-    instMethod.expandTemplate(ts);
-    namedMethod.second = instMethod;
-    result.insert(namedMethod);
-  }
-  return result;
-}
-
-template <class F>
-inline void verifyArguments(const std::vector<std::string>& validArgs,
-                            const std::map<std::string, F>& vt) {
-  typedef typename std::map<std::string, F>::value_type NamedMethod;
-  for (const NamedMethod& namedMethod : vt)
-    namedMethod.second.verifyArguments(validArgs);
-}
-
-}  // \namespace wrap
diff --git a/wrap/Qualified.cpp b/wrap/Qualified.cpp
deleted file mode 100644
index 947e51d54..000000000
--- a/wrap/Qualified.cpp
+++ /dev/null
@@ -1,5 +0,0 @@
-#include <wrap/Qualified.h>
-
-namespace wrap {
-  std::vector<Qualified>  Qualified::BasicTypedefs;
-}
diff --git a/wrap/Qualified.h b/wrap/Qualified.h
deleted file mode 100644
index 416db239d..000000000
--- a/wrap/Qualified.h
+++ /dev/null
@@ -1,370 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Qualified.h
- * @brief Qualified name
- * @author Frank Dellaert
- * @date Nov 11, 2014
- **/
-
-#pragma once
-
-#include <wrap/spirit.h>
-#include <string>
-#include <vector>
-#include <iostream>
-
-namespace wrap {
-
-/**
- * Class to encapuslate a qualified name, i.e., with (nested) namespaces
- */
-class Qualified {
-
-//protected:
-public:
-
-  std::vector<std::string> namespaces_; ///< Stack of namespaces
-  std::string name_; ///< type name
-  static std::vector<Qualified> BasicTypedefs;
-
-  friend struct TypeGrammar;
-  friend class TemplateSubstitution;
-
-public:
-
-  /// the different categories
-  typedef enum {
-    CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4
-  } Category;
-  Category category;
-
-  /// Default constructor
-  Qualified() :
-      category(VOID) {
-  }
-
-  /// Construct from name and optional category
-  Qualified(const std::string& n, Category c = CLASS) :
-      name_(n), category(c) {
-  }
-
-  /// Construct from scoped name and optional category
-  Qualified(const std::string& ns1, const std::string& n, Category c = CLASS) :
-      name_(n), category(c) {
-    namespaces_.push_back(ns1);
-  }
-
-  /// Construct from doubly scoped name and optional category
-  Qualified(const std::string& ns1, const std::string& ns2,
-      const std::string& n, Category c = CLASS) :
-      name_(n), category(c) {
-    namespaces_.push_back(ns1);
-    namespaces_.push_back(ns2);
-  }
-
-  /// Construct from arbitrarily scoped name
-  Qualified(std::vector<std::string> ns, const std::string& name) :
-      namespaces_(ns), name_(name), category(CLASS) {
-  }
-
-  // Destructor
-  virtual ~Qualified() {}
-
-  std::string name() const {
-    return name_;
-  }
-
-  std::vector<std::string> namespaces() const {
-    return namespaces_;
-  }
-
-  // Qualified is 'abused' as template argument name as well
-  // this function checks whether *this matches with templateArg
-  bool match(const std::string& templateArg) const {
-    return (name_ == templateArg && namespaces_.empty()); //TODO && category == CLASS);
-  }
-
-  bool match(const std::vector<std::string>& templateArgs) const {
-    for(const std::string& s: templateArgs)
-      if (match(s)) return true;
-    return false;
-  }
-
-  void rename(const Qualified& q) {
-    namespaces_ = q.namespaces_;
-    name_ = q.name_;
-    category = q.category;
-  }
-
-  void expand(const std::string& expansion) {
-    name_ += expansion;
-  }
-
-  bool operator==(const Qualified& other) const {
-    return namespaces_ == other.namespaces_ && name_ == other.name_
-        && category == other.category;
-  }
-
-  bool empty() const {
-    return namespaces_.empty() && name_.empty();
-  }
-
-  virtual void clear() {
-    namespaces_.clear();
-    name_.clear();
-    category = VOID;
-  }
-
-  bool isScalar() const {
-    return (name() == "bool" || name() == "char"
-        || name() == "unsigned char" || name() == "int"
-        || name() == "size_t" || name() == "double");
-  }
-
-  bool isVoid() const {
-    return name() == "void";
-  }
-
-  bool isString() const {
-    return name() == "string";
-  }
-
-  bool isEigen() const {
-    return name() == "Vector" || name() == "Matrix";
-  }
-
-  bool isBasicTypedef() const {
-    return std::find(Qualified::BasicTypedefs.begin(),
-                     Qualified::BasicTypedefs.end(),
-                     *this) != Qualified::BasicTypedefs.end();
-  }
-
-  bool isNonBasicType() const {
-    return name() != "This" && !isString() && !isScalar() && !isEigen() &&
-           !isVoid() && !isBasicTypedef();
-  }
-
-public:
-
-  static Qualified MakeClass(std::vector<std::string> namespaces,
-      const std::string& name) {
-    return Qualified(namespaces, name);
-  }
-
-  static Qualified MakeEigen(const std::string& name) {
-    return Qualified(name, EIGEN);
-  }
-
-  static Qualified MakeBasis(const std::string& name) {
-    return Qualified(name, BASIS);
-  }
-
-  static Qualified MakeVoid() {
-    return Qualified("void", VOID);
-  }
-
-  /// Return a qualified namespace using given delimiter
-  std::string qualifiedNamespaces(const std::string& delimiter = "") const {
-    std::string result;
-    for (std::size_t i = 0; i < namespaces_.size(); ++i)
-      result += (namespaces_[i] + ((i<namespaces_.size()-1)?delimiter:""));
-    return result;
-  }
-
-  /// Return a qualified string using given delimiter
-  std::string qualifiedName(const std::string& delimiter = "", size_t fromLevel = 0) const {
-    std::string result;
-    for (std::size_t i = fromLevel; i < namespaces_.size(); ++i)
-      result += (namespaces_[i] + delimiter);
-    result += name_;
-    return result;
-  }
-
-  /// Return a matlab file name, i.e. "toolboxPath/+ns1/+ns2/name.m"
-  std::string matlabName(const std::string& toolboxPath) const {
-    std::string result = toolboxPath;
-    for (std::size_t i = 0; i < namespaces_.size(); ++i)
-      result += ("/+" + namespaces_[i]);
-    result += "/" + name_ + ".m";
-    return result;
-  }
-
-  /// name of Cython classes in pxd
-  /// Normal classes: innerNamespace_ClassName, e.g. GaussianFactor, noiseModel_Gaussian
-  /// Eigen type: Vector --> VectorXd, Matrix --> MatrixXd
-  std::string pxdClassName() const {
-    if (isEigen())
-      return name_ + "Xd";
-    else if (isNonBasicType())
-      return "C" + qualifiedName("_", 1);
-    else return name_;
-  }
-
-  /// name of Python classes in pyx
-  /// They have the same name with the corresponding Cython classes in pxd
-  /// But note that they are different: These are Python classes in the pyx file
-  /// To refer to a Cython class in pyx, we need to add "pxd.", e.g. pxd.noiseModel_Gaussian
-  /// see the other function pxd_class_in_pyx for that purpose.
-  std::string pyxClassName() const {
-    if (isEigen())
-      return name_;
-    else
-      return qualifiedName("_", 1);
-  }
-
-  /// Python type of function arguments in pyx to interface with normal python scripts
-  /// Eigen types become np.ndarray (There's no Eigen types, e.g. VectorXd, in
-  /// Python. We have to pass in numpy array in the arguments, which will then be
-  /// converted to Eigen types in Cython)
-  std::string pyxArgumentType() const {
-    if (isEigen())
-      return "np.ndarray";
-    else
-      return qualifiedName("_", 1);
-  }
-
-  /// return the Cython class in pxd corresponding to a Python class in pyx
-  std::string pxd_class_in_pyx() const {
-    if (isNonBasicType()) {
-      return pxdClassName();
-    } else if (isEigen()) {
-      return name_ + "Xd";
-    } else  // basic types and not Eigen
-      return name_;
-  }
-
-  /// the internal Cython shared obj in a Python class wrappper
-  std::string shared_pxd_obj_in_pyx() const {
-    return pxdClassName() + "_";
-  }
-
-  std::string make_shared_pxd_class_in_pyx() const {
-    return "make_shared[" + pxd_class_in_pyx() + "]";
-  }
-
-  std::string shared_pxd_class_in_pyx() const {
-    return "shared_ptr[" + pxd_class_in_pyx() + "]";
-  }
-
-  friend std::ostream& operator<<(std::ostream& os, const Qualified& q) {
-    os << q.qualifiedName("::");
-    return os;
-  }
-};
-
-/* ************************************************************************* */
-// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
-struct TypeGrammar: classic::grammar<TypeGrammar> {
-
-  wrap::Qualified& result_; ///< successful parse will be placed in here
-
-  /// Construct type grammar and specify where result is placed
-  TypeGrammar(wrap::Qualified& result) :
-      result_(result) {
-  }
-
-  /// Definition of type grammar
-  template<typename ScannerT>
-  struct definition: BasicRules<ScannerT> {
-
-    typedef classic::rule<ScannerT> Rule;
-
-    Rule void_p, basisType_p, eigenType_p, namespace_del_p, class_p, type_p;
-
-    definition(TypeGrammar const& self) {
-
-      using namespace wrap;
-      using namespace classic;
-      typedef BasicRules<ScannerT> Basic;
-
-      // HACK: use const values instead of using enums themselves - somehow this doesn't result in values getting assigned to gibberish
-      static const Qualified::Category EIGEN = Qualified::EIGEN;
-      static const Qualified::Category BASIS = Qualified::BASIS;
-      static const Qualified::Category CLASS = Qualified::CLASS;
-      static const Qualified::Category VOID = Qualified::VOID;
-
-      void_p = str_p("void") //
-          [assign_a(self.result_.name_)] //
-          [assign_a(self.result_.category, VOID)];
-
-      basisType_p = Basic::basisType_p //
-          [assign_a(self.result_.name_)] //
-          [assign_a(self.result_.category, BASIS)];
-
-      eigenType_p = Basic::eigenType_p //
-          [assign_a(self.result_.name_)] //
-          [assign_a(self.result_.category, EIGEN)];
-
-      namespace_del_p = Basic::namespace_p //
-      [push_back_a(self.result_.namespaces_)] >> str_p("::");
-
-      class_p = *namespace_del_p >> Basic::className_p //
-          [assign_a(self.result_.name_)] //
-          [assign_a(self.result_.category, CLASS)];
-
-      type_p = void_p | basisType_p | class_p | eigenType_p;
-    }
-
-    Rule const& start() const {
-      return type_p;
-    }
-
-  };
-};
-// type_grammar
-
-/* ************************************************************************* */
-// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
-template<char OPEN, char CLOSE>
-struct TypeListGrammar: public classic::grammar<TypeListGrammar<OPEN, CLOSE> > {
-
-  typedef std::vector<wrap::Qualified> TypeList;
-  TypeList& result_; ///< successful parse will be placed in here
-
-  /// Construct type grammar and specify where result is placed
-  TypeListGrammar(TypeList& result) :
-      result_(result) {
-  }
-
-  /// Definition of type grammar
-  template<typename ScannerT>
-  struct definition {
-
-    wrap::Qualified type; ///< temporary for use during parsing
-    TypeGrammar type_g; ///< Individual Type grammars
-
-    classic::rule<ScannerT> type_p, typeList_p;
-
-    definition(TypeListGrammar const& self) :
-        type_g(type) {
-      using namespace classic;
-
-      type_p = type_g[push_back_a(self.result_, type)][clear_a(type)];
-
-      typeList_p = OPEN >> !type_p >> *(',' >> type_p) >> CLOSE;
-    }
-
-    classic::rule<ScannerT> const& start() const {
-      return typeList_p;
-    }
-
-  };
-};
-// TypeListGrammar
-
-/* ************************************************************************* */
-// Needed for other parsers in Argument.h and ReturnType.h
-static const bool T = true;
-
-} // \namespace wrap
-
diff --git a/wrap/README.md b/wrap/README.md
index 014577b5a..f72c3f652 100644
--- a/wrap/README.md
+++ b/wrap/README.md
@@ -1,27 +1,76 @@
-# WRAP README
 
-The wrap library wraps the GTSAM library into a MATLAB toolbox. 
+# WRAP
 
-It was designed to be more general than just wrapping GTSAM, but a small amount of GTSAM specific code exists in matlab.h, the include file that is included by the mex files. The GTSAM-specific functionality consists primarily of handling of Eigen Matrix and Vector classes.  
+The wrap library wraps the GTSAM library into a Python library or MATLAB toolbox.
+It was designed to be more general than just wrapping GTSAM. For notes on creating a wrap interface, see `gtsam.h` for what features can be wrapped into a toolbox, as well as the current state of the toolbox for GTSAM.
 
-For notes on creating a wrap interface, see gtsam.h for what features can be wrapped into a toolbox, as well as the current state of the toolbox for gtsam. For more technical details on the interface, please read comments in matlab.h
+## Prerequisites: Pybind11 and pyparsing
+
+1. This library uses `pybind11`, which is included as a subdirectory in GTSAM.
+2. The `interface_parser.py` in this library uses `pyparsing` to parse the interface file `gtsam.h`. Please install it first in your current Python environment before attempting the build.
+    ```
+    python3 -m pip install pyparsing
+    ```
+
+## GTSAM Python wrapper
+
+**WARNING: On macOS, you have to statically build GTSAM to use the wrapper.**
+
+1. Set `GTSAM_BUILD_PYTHON=ON` while configuring the build with `cmake`.
+1. What you can do in the `build` folder:
+    1. Just run python then import GTSAM and play around:
+        ```
+
+        import gtsam
+        gtsam.__dir__()
+        ```
+
+    1. Run the unittests:
+        ```
+        python -m unittest discover
+        ```
+    1. Edit the unittests in `python/gtsam/*.py` and simply rerun the test.
+    They were symlinked to `<build_folder>/gtsam/*.py` to facilitate fast development.
+        ```
+        python -m unittest gtsam/tests/test_Pose3.py
+        ```
+        - NOTE: You might need to re-run `cmake ..` if files are deleted or added.
+1. Do `make install` and `cd <gtsam_install_folder>/python`. Here, you can:
+    1. Run the unittests:
+        ```
+        python setup.py test
+        ```
+    2. Install `gtsam` to your current Python environment.
+        ```
+        python setup.py install
+        ```
+        - NOTE: It's a good idea to create a virtual environment otherwise it will be installed in your system Python's site-packages.
+
+
+## Old GTSAM Wrapper
+
+*Outdated note from the original wrap.*
+
+TODO: Update this.
+
+It was designed to be more general than just wrapping GTSAM, but a small amount of GTSAM specific code exists in `matlab.h`, the include file that is included by the `mex` files. The GTSAM-specific functionality consists primarily of handling of Eigen Matrix and Vector classes.
+
+For notes on creating a wrap interface, see `gtsam.h` for what features can be wrapped into a toolbox, as well as the current state of the toolbox for GTSAM. For more technical details on the interface, please read comments in `matlab.h`
 
 Some good things to know:
 
 OBJECT CREATION
 
-- Classes are created by special constructors, e.g., new_GaussianFactorGraph_.cpp.
-	These constructors are called from the MATLAB class @GaussianFactorGraph.
-	new_GaussianFactorGraph_ calls wrap_constructed in matlab.h, see documentation there
-	
+- Classes are created by special constructors, e.g., `new_GaussianFactorGraph_.cpp`.
+	These constructors are called from the MATLAB class `@GaussianFactorGraph`.
+	`new_GaussianFactorGraph_` calls wrap_constructed in `matlab.h`, see documentation there
+
 METHOD (AND CONSTRUCTOR) ARGUMENTS
 
 - Simple argument types of methods, such as "double", will be converted in the
-  mex wrappers by calling unwrap<double>, defined in matlab.h
+  `mex` wrappers by calling unwrap<double>, defined in matlab.h
 - Vector and Matrix arguments are normally passed by reference in GTSAM, but
-  in gtsam.h you need to pretend they are passed by value, to trigger the 
-  generation of the correct conversion routines unwrap<Vector> and unwrap<Matrix>
+  in `gtsam.h` you need to pretend they are passed by value, to trigger the
+  generation of the correct conversion routines `unwrap<Vector>` and `unwrap<Matrix>`
 - passing classes as arguments works, provided they are passed by reference.
 	This triggers a call to unwrap_shared_ptr
-
-   
\ No newline at end of file
diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp
deleted file mode 100644
index fdf86d975..000000000
--- a/wrap/ReturnType.cpp
+++ /dev/null
@@ -1,101 +0,0 @@
-/**
- * @file ReturnType.cpp
- * @date Nov 13, 2014
- * @author Frank Dellaert
- */
-
-#include "ReturnType.h"
-#include "Class.h"
-#include "utilities.h"
-#include <iostream>
-
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-void ReturnType::wrap_result(const string& out, const string& result,
-                             FileWriter& wrapperFile,
-                             const TypeAttributesTable& typeAttributes) const {
-  string cppType = qualifiedName("::"), matlabType = qualifiedName(".");
-
-  if (category == CLASS) {
-    // Handle Classes
-    string objCopy, ptrType;
-    const bool isVirtual = typeAttributes.attributes(cppType).isVirtual;
-    if (isPtr)
-      objCopy = result;  // a shared pointer can always be passed as is
-    else {
-      // but if we want an actual new object, things get more complex
-      if (isVirtual)
-        // A virtual class needs to be cloned, so the whole hierarchy is
-        // returned
-        objCopy = result + ".clone()";
-      else {
-        // ...but a non-virtual class can just be copied
-        objCopy = "boost::make_shared<" + cppType + ">(" + result + ")";
-      }
-    }
-    // e.g. out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false);
-    wrapperFile.oss << out << " = wrap_shared_ptr(" << objCopy << ",\""
-                    << matlabType << "\", " << (isVirtual ? "true" : "false")
-                    << ");\n";
-
-  } else if (isPtr) {
-    // Handle shared pointer case for BASIS/EIGEN/VOID
-    // This case does not actually occur in GTSAM wrappers, so untested!
-    wrapperFile.oss << "  {\n  boost::shared_ptr<" << qualifiedName("::")
-                    << "> shared(" << result << ");" << endl;
-    wrapperFile.oss << out << " = wrap_shared_ptr(shared,\"" << matlabType
-                    << "\");\n  }\n";
-
-  } else if (matlabType != "void")
-    // Handle normal case case for BASIS/EIGEN
-    wrapperFile.oss << out << " = wrap< " << qualifiedName("::") << " >(" << result
-                    << ");\n";
-}
-
-/* ************************************************************************* */
-void ReturnType::emit_cython_pxd(
-    FileWriter& file, const std::string& className,
-    const std::vector<std::string>& templateArgs) const {
-  string cythonType;
-  if (name() == "This")
-    cythonType = className;
-  else if (match(templateArgs))
-    cythonType = name();
-  else
-    cythonType = pxdClassName();
-  if (isPtr) cythonType = "shared_ptr[" + cythonType + "]";
-  file.oss << cythonType;
-}
-
-/* ************************************************************************* */
-std::string ReturnType::pyx_returnType(bool addShared) const {
-  string retType = pxd_class_in_pyx();
-  if (isPtr || (isNonBasicType() && addShared))
-    retType = "shared_ptr[" + retType + "]";
-  return retType;
-}
-
-/* ************************************************************************* */
-std::string ReturnType::pyx_casting(const std::string& var,
-                                    bool isSharedVar) const {
-  if (isEigen()) {
-    string s = "ndarray_copy(" + var + ")";
-    if (pyxClassName() == "Vector")
-      return s + ".squeeze()";
-    else return s;
-  }
-  else if (isNonBasicType()) {
-    if (isPtr || isSharedVar)
-      return pyxClassName() + ".cyCreateFromShared(" + var + ")";
-    else {
-      // construct a shared_ptr if var is not a shared ptr
-      return pyxClassName() + ".cyCreateFromShared(" + make_shared_pxd_class_in_pyx() +
-             + "(" + var + "))";
-    }
-  } else
-    return var;
-}
-
-/* ************************************************************************* */
diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h
deleted file mode 100644
index 8d78bb48f..000000000
--- a/wrap/ReturnType.h
+++ /dev/null
@@ -1,89 +0,0 @@
-/**
- * @file ReturnValue.h
- * @brief Encapsulates a return type of a method
- * @date Nov 13, 2014
- * @author Frank Dellaert
- */
-
-#include "Qualified.h"
-#include "FileWriter.h"
-#include "TypeAttributesTable.h"
-#include "utilities.h"
-#include <iostream>
-
-#pragma once
-
-namespace wrap {
-
-/**
- * Encapsulates return value of a method or function
- */
-struct ReturnType : public Qualified {
-  bool isPtr;
-
-  friend struct ReturnValueGrammar;
-
-  /// Makes a void type
-  ReturnType() : isPtr(false) {}
-
-  /// Constructor, no namespaces
-  ReturnType(const std::string& name, Category c = CLASS, bool ptr = false)
-      : Qualified(name, c), isPtr(ptr) {}
-
-  void clear() override {
-    Qualified::clear();
-    isPtr = false;
-  }
-
-  /// Check if this type is in a set of valid types
-  template <class TYPES>
-  void verify(TYPES validtypes, const std::string& s) const {
-    std::string key = qualifiedName("::");
-    if (find(validtypes.begin(), validtypes.end(), key) == validtypes.end())
-      throw DependencyMissing(key, "checking return type of " + s);
-  }
-
-  /// @param className the actual class name to use when "This" is specified
-  void emit_cython_pxd(FileWriter& file, const std::string& className,
-                       const std::vector<std::string>& templateArgs) const;
-
-  std::string pyx_returnType(bool addShared = true) const;
-  std::string pyx_casting(const std::string& var,
-                          bool isSharedVar = true) const;
-
-private:
-  friend struct ReturnValue;
-
-  /// Example: out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
-  void wrap_result(const std::string& out, const std::string& result,
-                   FileWriter& wrapperFile,
-                   const TypeAttributesTable& typeAttributes) const;
-};
-
-//******************************************************************************
-// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
-struct ReturnTypeGrammar : public classic::grammar<ReturnTypeGrammar> {
-  wrap::ReturnType& result_;  ///< successful parse will be placed in here
-
-  TypeGrammar type_g;
-
-  /// Construct ReturnType grammar and specify where result is placed
-  ReturnTypeGrammar(wrap::ReturnType& result)
-      : result_(result), type_g(result_) {}
-
-  /// Definition of type grammar
-  template <typename ScannerT>
-  struct definition {
-    classic::rule<ScannerT> type_p;
-
-    definition(ReturnTypeGrammar const& self) {
-      using namespace classic;
-      type_p = self.type_g >> !ch_p('*')[assign_a(self.result_.isPtr, T)];
-    }
-
-    classic::rule<ScannerT> const& start() const { return type_p; }
-  };
-};
-// ReturnTypeGrammar
-
-}  // \namespace wrap
diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp
deleted file mode 100644
index e58e85602..000000000
--- a/wrap/ReturnValue.cpp
+++ /dev/null
@@ -1,102 +0,0 @@
-/**
- * @file ReturnValue.cpp
- * @date Dec 1, 2011
- * @author Alex Cunningham
- * @author Andrew Melim
- * @author Richard Roberts
- */
-
-#include "ReturnValue.h"
-#include "utilities.h"
-#include <iostream>
-
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const {
-  ReturnValue instRetVal = *this;
-  instRetVal.type1 = ts.tryToSubstitite(type1);
-  if (isPair) instRetVal.type2 = ts.tryToSubstitite(type2);
-  return instRetVal;
-}
-
-/* ************************************************************************* */
-string ReturnValue::returnType() const {
-  if (isPair)
-    return "pair< " + type1.qualifiedName("::") + ", " +
-           type2.qualifiedName("::") + " >";
-  else
-    return type1.qualifiedName("::");
-}
-
-/* ************************************************************************* */
-string ReturnValue::matlab_returnType() const {
-  return isPair ? "[first,second]" : "result";
-}
-
-/* ************************************************************************* */
-void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile,
-                              const TypeAttributesTable& typeAttributes) const {
-  if (isPair) {
-    // For a pair, store the returned pair so we do not evaluate the function
-    // twice
-    wrapperFile.oss << "  auto pairResult = " << result
-                    << ";\n";
-    type1.wrap_result("  out[0]", "pairResult.first", wrapperFile,
-                      typeAttributes);
-    type2.wrap_result("  out[1]", "pairResult.second", wrapperFile,
-                      typeAttributes);
-  } else {  // Not a pair
-    type1.wrap_result("  out[0]", result, wrapperFile, typeAttributes);
-  }
-}
-
-/* ************************************************************************* */
-void ReturnValue::emit_matlab(FileWriter& proxyFile) const {
-  string output;
-  if (isPair)
-    proxyFile.oss << "[ varargout{1} varargout{2} ] = ";
-  else if (type1.category != ReturnType::VOID)
-    proxyFile.oss << "varargout{1} = ";
-}
-
-/* ************************************************************************* */
-void ReturnValue::emit_cython_pxd(
-    FileWriter& file, const std::string& className,
-    const std::vector<std::string>& templateArgs) const {
-  if (isPair) {
-    file.oss << "pair[";
-    type1.emit_cython_pxd(file, className, templateArgs);
-    file.oss << ",";
-    type2.emit_cython_pxd(file, className, templateArgs);
-    file.oss << "] ";
-  } else {
-    type1.emit_cython_pxd(file, className, templateArgs);
-    file.oss << " ";
-  }
-}
-
-/* ************************************************************************* */
-std::string ReturnValue::pyx_returnType() const {
-  if (isVoid()) return "";
-  if (isPair) {
-    return "pair [" + type1.pyx_returnType(false) + "," +
-           type2.pyx_returnType(false) + "]";
-  } else {
-    return type1.pyx_returnType(true);
-  }
-}
-
-/* ************************************************************************* */
-std::string ReturnValue::pyx_casting(const std::string& var) const {
-  if (isVoid()) return "";
-  if (isPair) {
-    return "(" + type1.pyx_casting(var + ".first", false) + "," +
-           type2.pyx_casting(var + ".second", false) + ")";
-  } else {
-    return type1.pyx_casting(var);
-  }
-}
-
-/* ************************************************************************* */
diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h
deleted file mode 100644
index 721132797..000000000
--- a/wrap/ReturnValue.h
+++ /dev/null
@@ -1,126 +0,0 @@
-/**
- * @file ReturnValue.h
- *
- * @brief Encapsulates a return value from a method
- * @date Dec 1, 2011
- * @author Alex Cunningham
- * @author Richard Roberts
- */
-
-#include "ReturnType.h"
-#include "TemplateSubstitution.h"
-#include "FileWriter.h"
-#include "TypeAttributesTable.h"
-#include "utilities.h"
-
-#pragma once
-
-namespace wrap {
-
-/**
- * Encapsulates return type of a method or function, possibly a pair
- */
-struct ReturnValue {
-
-  bool isPair;
-  ReturnType type1, type2;
-
-  friend struct ReturnValueGrammar;
-
-  /// Default constructor
-  ReturnValue() :
-      isPair(false) {
-  }
-
-  /// Construct from type
-  ReturnValue(const ReturnType& type) :
-      isPair(false), type1(type) {
-  }
-
-  /// Construct from pair type arguments
-  ReturnValue(const ReturnType& t1, const ReturnType& t2) :
-      isPair(true), type1(t1), type2(t2) {
-  }
-
-  /// Destructor
-  virtual ~ReturnValue() {}
-
-  virtual void clear() {
-    type1.clear();
-    type2.clear();
-    isPair = false;
-  }
-
-  bool isVoid() const {
-    return !isPair && !type1.isPtr && (type1.name() == "void");
-  }
-
-  bool operator==(const ReturnValue& other) const {
-    return isPair == other.isPair && type1 == other.type1
-        && type2 == other.type2;
-  }
-
-  /// Substitute template argument
-  ReturnValue expandTemplate(const TemplateSubstitution& ts) const;
-
-  std::string returnType() const;
-
-  std::string matlab_returnType() const;
-
-  void wrap_result(const std::string& result, FileWriter& wrapperFile,
-      const TypeAttributesTable& typeAttributes) const;
-
-  void emit_matlab(FileWriter& proxyFile) const;
-
-  /// @param className the actual class name to use when "This" is specified
-  void emit_cython_pxd(FileWriter& file, const std::string& className,
-                       const std::vector<std::string>& templateArgs) const;
-  std::string pyx_returnType() const;
-  std::string pyx_casting(const std::string& var) const;
-
-  friend std::ostream& operator<<(std::ostream& os, const ReturnValue& r) {
-    if (!r.isPair && r.type1.category == ReturnType::VOID)
-      os << "void";
-    else
-      os << r.returnType();
-    return os;
-  }
-
-};
-
-//******************************************************************************
-// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
-struct ReturnValueGrammar: public classic::grammar<ReturnValueGrammar> {
-
-  wrap::ReturnValue& result_; ///< successful parse will be placed in here
-  ReturnTypeGrammar returnType1_g, returnType2_g; ///< Type parsers
-
-  /// Construct type grammar and specify where result is placed
-  ReturnValueGrammar(wrap::ReturnValue& result) :
-      result_(result), returnType1_g(result.type1), returnType2_g(result.type2) {
-  }
-
-  /// Definition of type grammar
-  template<typename ScannerT>
-  struct definition {
-
-    classic::rule<ScannerT> pair_p, returnValue_p;
-
-    definition(ReturnValueGrammar const& self) {
-      using namespace classic;
-
-      pair_p = (str_p("pair") >> '<' >> self.returnType1_g >> ','
-          >> self.returnType2_g >> '>')[assign_a(self.result_.isPair, T)];
-
-      returnValue_p = pair_p | self.returnType1_g;
-    }
-
-    classic::rule<ScannerT> const& start() const {
-      return returnValue_p;
-    }
-
-  };
-};
-// ReturnValueGrammar
-
-}// \namespace wrap
diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp
deleted file mode 100644
index 0f812ea61..000000000
--- a/wrap/StaticMethod.cpp
+++ /dev/null
@@ -1,151 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 StaticMethod.ccp
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#include "StaticMethod.h"
-#include "utilities.h"
-#include "Class.h"
-
-#include <boost/lexical_cast.hpp>
-#include <boost/algorithm/string.hpp>
-
-#include <iostream>
-#include <fstream>
-
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-void StaticMethod::proxy_header(FileWriter& proxyFile) const {
-  string upperName = matlabName();
-  upperName[0] = toupper(upperName[0], locale());
-  proxyFile.oss << "    function varargout = " << upperName << "(varargin)\n";
-}
-
-/* ************************************************************************* */
-string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
-    Str matlabUniqueName, const ArgumentList& args) const {
-  // check arguments
-  // NOTE: for static functions, there is no object passed
-  wrapperFile.oss << "  checkArguments(\"" << matlabUniqueName << "." << name_
-      << "\",nargout,nargin," << args.size() << ");\n";
-
-  // unwrap arguments, see Argument.cpp
-  args.matlab_unwrap(wrapperFile, 0); // We start at 0 because there is no self object
-
-  // call method and wrap result
-  // example: out[0]=wrap<bool>(staticMethod(t));
-  string expanded = cppClassName + "::" + name_;
-  if (templateArgValue_)
-    expanded += ("<" + templateArgValue_->qualifiedName("::") + ">");
-
-  return expanded;
-}
-
-/* ************************************************************************* */
-void StaticMethod::emit_cython_pxd(FileWriter& file, const Class& cls) const {
-  for(size_t i = 0; i < nrOverloads(); ++i) {
-    file.oss << "        @staticmethod\n";
-    file.oss << "        ";
-    returnVals_[i].emit_cython_pxd(file, cls.pxdClassName(), cls.templateArgs);
-    file.oss << name_ + ((i>0)?"_" + to_string(i):"") << " \"" << name_ << "\"" << "(";
-    argumentList(i).emit_cython_pxd(file, cls.pxdClassName(), cls.templateArgs);
-    file.oss << ") except +\n";
-  }
-}
-
-/* ************************************************************************* */
-void StaticMethod::emit_cython_wrapper_pxd(FileWriter& file,
-    const Class& cls) const {
-  if (nrOverloads() > 1) {
-    for (size_t i = 0; i < nrOverloads(); ++i) {
-      string funcName = name_ + "_" + to_string(i);
-      file.oss << "    @staticmethod\n";
-      file.oss << "    cdef tuple " + funcName + "(tuple args, dict kwargs)\n";
-    }
-  }
-}
-
-/* ************************************************************************* */
-void StaticMethod::emit_cython_pyx_no_overload(FileWriter& file,
-                                               const Class& cls) const {
-  assert(nrOverloads() == 1);
-  file.oss << "    @staticmethod\n";
-  file.oss << "    def " << name_ << "(";
-  argumentList(0).emit_cython_pyx(file);
-  file.oss << "):\n";
-
-  /// Call cython corresponding function and return
-  file.oss << argumentList(0).pyx_convertEigenTypeAndStorageOrder("        ");
-  string call = pyx_functionCall(cls.pxd_class_in_pyx(), name_, 0);
-  file.oss << "        ";
-  if (!returnVals_[0].isVoid()) {
-    file.oss << "return " << returnVals_[0].pyx_casting(call) << "\n";
-  } else
-    file.oss << call << "\n";
-  file.oss << "\n";
-}
-
-/* ************************************************************************* */
-void StaticMethod::emit_cython_pyx(FileWriter& file, const Class& cls) const {
-  size_t N = nrOverloads();
-  if (N == 1) {
-    emit_cython_pyx_no_overload(file, cls);
-    return;
-  }
-
-  // Dealing with overloads..
-  file.oss << "    @staticmethod # overloaded\n";
-  file.oss << "    def " << name_ << "(*args, **kwargs):\n";
-  for (size_t i = 0; i < N; ++i) {
-    string funcName = name_ + "_" + to_string(i);
-    file.oss << "        success, results = " << cls.pyxClassName() << "."
-             << funcName << "(args, kwargs)\n";
-    file.oss << "        if success:\n            return results\n";
-  }
-  file.oss << "        raise TypeError('Could not find the correct overload')\n\n";
-
-  // Create cdef methods for all overloaded methods
-  for(size_t i = 0; i < N; ++i) {
-    string funcName = name_ + "_" + to_string(i);
-    file.oss << "    @staticmethod\n";
-    file.oss << "    cdef tuple " + funcName + "(tuple args, dict kwargs):\n";
-    file.oss << "        cdef list __params\n";
-    if (!returnVals_[i].isVoid()) {
-      file.oss << "        cdef " << returnVals_[i].pyx_returnType() << " return_value\n";
-    }
-    file.oss << "        try:\n";
-    ArgumentList args = argumentList(i);
-    file.oss << pyx_resolveOverloadParams(args, false, 3);
-
-    /// Call cython corresponding function and return
-    file.oss << args.pyx_convertEigenTypeAndStorageOrder("            ");
-    string pxdFuncName = name_ + ((i>0)?"_" + to_string(i):"");
-    string call = pyx_functionCall(cls.pxd_class_in_pyx(), pxdFuncName, i);
-    if (!returnVals_[i].isVoid()) {
-      file.oss << "            return_value = " << call << "\n";
-      file.oss << "            return True, " << returnVals_[i].pyx_casting("return_value") << "\n";
-    } else {
-      file.oss << "            " << call << "\n";
-      file.oss << "            return True, None\n";
-    }
-    file.oss << "        except:\n";
-    file.oss << "            return False, None\n\n";
-  }
-}
-
-/* ************************************************************************* */
diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h
deleted file mode 100644
index dbb918596..000000000
--- a/wrap/StaticMethod.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 StaticMethod.h
- * @brief describes and generates code for static methods
- * @author Frank Dellaert
- * @author Alex Cunningham
- * @author Richard Roberts
- **/
-
-#pragma once
-
-#include "MethodBase.h"
-
-namespace wrap {
-
-/// StaticMethod class
-struct StaticMethod: public MethodBase {
-
-  typedef const std::string& Str;
-
-  friend std::ostream& operator<<(std::ostream& os, const StaticMethod& m) {
-    for (size_t i = 0; i < m.nrOverloads(); i++)
-      os << "static " << m.returnVals_[i] << " " << m.name_ << m.argLists_[i];
-    return os;
-  }
-
-  void emit_cython_pxd(FileWriter& file, const Class& cls) const;
-  void emit_cython_wrapper_pxd(FileWriter& file, const Class& cls) const;
-  void emit_cython_pyx(FileWriter& file, const Class& cls) const;
-  void emit_cython_pyx_no_overload(FileWriter& file, const Class& cls) const;
-
-protected:
-
-  void proxy_header(FileWriter& proxyFile) const override;
-
-  std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
-      Str matlabUniqueName, const ArgumentList& args) const override;
-};
-
-} // \namespace wrap
-
diff --git a/wrap/Template.h b/wrap/Template.h
deleted file mode 100644
index 32f8e9761..000000000
--- a/wrap/Template.h
+++ /dev/null
@@ -1,146 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Template.h
- * @brief Template name
- * @author Frank Dellaert
- * @date Nov 11, 2014
- **/
-
-#pragma once
-
-#include <wrap/Qualified.h>
-
-namespace wrap {
-
-/// The template specification that goes before a method or a class
-class Template {
-  std::string argName_;
-  std::vector<Qualified> argValues_;
-  std::vector<int> intList_;
-  friend struct TemplateGrammar;
-public:
-  /// The only way to get values into a Template is via our friendly Grammar
-  Template() {
-  }
-  void clear() {
-    argName_.clear();
-    argValues_.clear();
-    intList_.clear();
-  }
-  const std::string& argName() const {
-    return argName_;
-  }
-  const std::vector<int>& intList() const {
-    return intList_;
-  }
-  const std::vector<Qualified>& argValues() const {
-    return argValues_;
-  }
-  bool empty() const {
-    return argValues_.empty() && intList_.empty();
-  }
-  size_t nrValues() const {
-    return argValues_.size();
-  }
-  const Qualified& operator[](size_t i) const {
-    return argValues_[i];
-  }
-  bool valid() const {
-    return !argName_.empty() && argValues_.size() > 0;
-  }
-
-};
-
-/* ************************************************************************* */
-// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
-struct IntListGrammar: public classic::grammar<IntListGrammar > {
-
-  typedef std::vector<int> IntList;
-  IntList& result_; ///< successful parse will be placed in here
-
-  /// Construct type grammar and specify where result is placed
-  IntListGrammar(IntList& result) :
-      result_(result) {
-  }
-
-  /// Definition of type grammar
-  template<typename ScannerT>
-  struct definition {
-
-    classic::rule<ScannerT> integer_p, intList_p;
-
-    definition(IntListGrammar const& self) {
-      using namespace classic;
-
-      integer_p = int_p[push_back_a(self.result_)];
-
-      intList_p = '{' >> !integer_p >> *(',' >> integer_p) >> '}';
-    }
-
-    classic::rule<ScannerT> const& start() const {
-      return intList_p;
-    }
-
-  };
-};
-// IntListGrammar
-
-/* ************************************************************************* */
-// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
-struct TemplateGrammar: public classic::grammar<TemplateGrammar> {
-
-  Template& result_; ///< successful parse will be placed in here
-  TypeListGrammar<'{', '}'> argValues_g; ///< TypeList parser
-  IntListGrammar intList_g; ///< TypeList parser
-
-  /// Construct type grammar and specify where result is placed
-  TemplateGrammar(Template& result) :
-      result_(result), argValues_g(result.argValues_), //
-      intList_g(result.intList_) {
-  }
-
-  /// Definition of type grammar
-  template<typename ScannerT>
-  struct definition: BasicRules<ScannerT> {
-
-    classic::rule<ScannerT> templateArgValues_p;
-
-    definition(TemplateGrammar const& self) {
-      using classic::str_p;
-      using classic::assign_a;
-      templateArgValues_p = (str_p("template") >> '<'
-          >> (BasicRules<ScannerT>::name_p)[assign_a(self.result_.argName_)]
-          >> '=' >> (self.argValues_g | self.intList_g) >> '>');
-    }
-
-    classic::rule<ScannerT> const& start() const {
-      return templateArgValues_p;
-    }
-
-  };
-};
-// TemplateGrammar
-
-/// Cool initializer for tests
-static inline boost::optional<Template> CreateTemplate(const std::string& s) {
-  Template result;
-  TemplateGrammar g(result);
-  bool success = parse(s.c_str(), g, classic::space_p).full;
-  if (success)
-    return result;
-  else
-    return boost::none;
-}
-
-} // \namespace wrap
-
diff --git a/wrap/TemplateInstantiationTypedef.cpp b/wrap/TemplateInstantiationTypedef.cpp
deleted file mode 100644
index 3f9b5de10..000000000
--- a/wrap/TemplateInstantiationTypedef.cpp
+++ /dev/null
@@ -1,69 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Class.cpp
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#include "TemplateInstantiationTypedef.h"
-
-#include "utilities.h"
-#include <iostream>
-#include <boost/optional.hpp>
-
-using namespace std;
-
-namespace wrap {
-
-Class TemplateInstantiationTypedef::findAndExpand(
-    const vector<Class>& classes) const {
-  // Find matching class
-  boost::optional<Class const &> matchedClass;
-  for(const Class& cls: classes) {
-    if (cls.name() == class_.name() && cls.namespaces() == class_.namespaces()
-        && cls.templateArgs.size() == typeList.size()) {
-      matchedClass.reset(cls);
-      break;
-    }
-  }
-
-  if (!matchedClass)
-    throw DependencyMissing(class_.qualifiedName("::"),
-        "instantiation into typedef name " + qualifiedName("::")
-            + ".  Ensure that the typedef provides the correct number of template arguments.");
-
-  // Instantiate it
-  Class classInst = *matchedClass;
-  for (size_t i = 0; i < typeList.size(); ++i) {
-    TemplateSubstitution ts(classInst.templateArgs[i], typeList[i], *this);
-    classInst = classInst.expandTemplate(ts);
-  }
-
-  // Fix class properties
-  classInst.name_ = name();
-  classInst.namespaces_ = namespaces();
-  classInst.templateArgs.clear();
-  classInst.typedefName = matchedClass->qualifiedName("::") + "<";
-  if (typeList.size() > 0)
-    classInst.typedefName += typeList[0].qualifiedName("::");
-  for (size_t i = 1; i < typeList.size(); ++i)
-    classInst.typedefName += (", " + typeList[i].qualifiedName("::"));
-  classInst.typedefName += ">";
-  classInst.templateClass = *matchedClass;
-  classInst.templateInstTypeList = typeList;
-
-  return classInst;
-}
-
-}
diff --git a/wrap/TemplateInstantiationTypedef.h b/wrap/TemplateInstantiationTypedef.h
deleted file mode 100644
index dfec272e3..000000000
--- a/wrap/TemplateInstantiationTypedef.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Class.h
- * @brief describe the C++ class that is being wrapped
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#pragma once
-
-#include <vector>
-#include <string>
-
-#include "Class.h"
-
-namespace wrap {
-
-struct TemplateInstantiationTypedef : public Qualified {
-  Qualified class_;
-  std::vector<Qualified> typeList;
-
-  Class findAndExpand(const std::vector<Class>& classes) const;
-};
-
-}
diff --git a/wrap/TemplateMethod.cpp b/wrap/TemplateMethod.cpp
deleted file mode 100644
index 10d2371c8..000000000
--- a/wrap/TemplateMethod.cpp
+++ /dev/null
@@ -1,53 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 TemplateMethod.ccp
- * @author Duy-Nguyen Ta
- **/
-
-#include "TemplateMethod.h"
-#include "Class.h"
-
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-void TemplateMethod::emit_cython_pxd(FileWriter& file, const Class& cls) const {
-  std::vector<std::string> templateArgs = cls.templateArgs;
-  templateArgs.push_back(argName);
-  for(size_t i = 0; i < nrOverloads(); ++i) {
-    file.oss << "        ";
-    returnVals_[i].emit_cython_pxd(file, cls.pxdClassName(), templateArgs);
-    file.oss << name_ << "[" << argName << "]" << "(";
-    argumentList(i).emit_cython_pxd(file, cls.pxdClassName(), templateArgs);
-    file.oss << ") except +\n";
-  }
-}
-
-/* ************************************************************************* */
-bool TemplateMethod::addOverload(Str name, const ArgumentList& args,
-    const ReturnValue& retVal, bool is_const,
-    std::string _argName, bool verbose) {
-  argName = _argName;
-  bool first = MethodBase::addOverload(name, args, retVal, boost::none, verbose);
-  if (first)
-    is_const_ = is_const;
-  else if (is_const && !is_const_)
-    throw std::runtime_error(
-        "Method::addOverload now designated as const whereas before it was not");
-  else if (!is_const && is_const_)
-    throw std::runtime_error(
-        "Method::addOverload now designated as non-const whereas before it was");
-  return first;
-}
-
-/* ************************************************************************* */
diff --git a/wrap/TemplateMethod.h b/wrap/TemplateMethod.h
deleted file mode 100644
index 399eeec5f..000000000
--- a/wrap/TemplateMethod.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 TemplateMethod.h
- * @brief describes and generates code for template methods
- * @author Duy-Nguyen Ta
- **/
-
-#pragma once
-
-#include "Method.h"
-
-namespace wrap {
-
-/// StaticMethod class
-struct TemplateMethod: public Method {
-  std::string argName; // name of template argument
-
-  void emit_cython_pxd(FileWriter& file, const Class& cls) const;
-  bool addOverload(Str name, const ArgumentList& args,
-                   const ReturnValue& retVal, bool is_const,
-                   std::string argName, bool verbose = false);
-
-  friend std::ostream& operator<<(std::ostream& os, const TemplateMethod& m) {
-    for (size_t i = 0; i < m.nrOverloads(); i++)
-      os << "template <" << m.argName << "> " << m.returnVals_[i] << " " << m.name_ << m.argLists_[i];
-    return os;
-  }
-
-};
-
-} // \namespace wrap
-
diff --git a/wrap/TemplateSubstitution.h b/wrap/TemplateSubstitution.h
deleted file mode 100644
index f214bc607..000000000
--- a/wrap/TemplateSubstitution.h
+++ /dev/null
@@ -1,76 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 TemplateSubstitution.h
- * @brief Auxiliary class for template substitutions
- * @author Frank Dellaert
- * @date Nov 13, 2014
- **/
-
-#pragma once
-
-#include "ReturnType.h"
-#include <string>
-#include <iostream>
-
-namespace wrap {
-
-/**
- * e.g. TemplateSubstitution("T", gtsam::Point2, gtsam::PriorFactorPoint2)
- */
-class TemplateSubstitution {
-
-  std::string templateArg_;
-  Qualified qualifiedType_, expandedClass_;
-
-public:
-
-  TemplateSubstitution(const std::string& a, const Qualified& t,
-      const Qualified& e) :
-      templateArg_(a), qualifiedType_(t), expandedClass_(e) {
-  }
-
-  std::string expandedClassName() const {
-    return expandedClass_.name();
-  }
-
-  // Substitute if needed
-  Qualified tryToSubstitite(const Qualified& type) const {
-    if (type.match(templateArg_))
-      return qualifiedType_;
-    else if (type.match("This"))
-      return expandedClass_;
-    else
-      return type;
-  }
-
-  // Substitute if needed
-  ReturnType tryToSubstitite(const ReturnType& type) const {
-    ReturnType instType = type;
-    if (type.match(templateArg_))
-      instType.rename(qualifiedType_);
-    else if (type.match("This"))
-      instType.rename(expandedClass_);
-    return instType;
-  }
-
-  friend std::ostream& operator<<(std::ostream& os,
-      const TemplateSubstitution& ts) {
-    os << ts.templateArg_ << '/' << ts.qualifiedType_.qualifiedName("::")
-        << " (" << ts.expandedClass_.qualifiedName("::") << ")";
-    return os;
-  }
-
-};
-
-} // \namespace wrap
-
diff --git a/wrap/TypeAttributesTable.cpp b/wrap/TypeAttributesTable.cpp
deleted file mode 100644
index 539f4b8ec..000000000
--- a/wrap/TypeAttributesTable.cpp
+++ /dev/null
@@ -1,93 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Class.cpp
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#include "TypeAttributesTable.h"
-#include "Class.h"
-#include "ForwardDeclaration.h"
-#include "utilities.h"
-
-#include <boost/range/adaptor/map.hpp>
-#include <boost/range/algorithm/copy.hpp>
-
-#include <iterator>     // std::ostream_iterator
-using namespace std;
-
-namespace wrap {
-
-/* ************************************************************************* */
-const TypeAttributes& TypeAttributesTable::attributes(const string& key) const {
-  try {
-    return table_.at(key);
-  } catch (const out_of_range& oor) {
-    cerr << "TypeAttributesTable::attributes: key " << key
-        << " not found. Valid keys are:\n";
-    using boost::adaptors::map_keys;
-    ostream_iterator<string> out_it(cerr, "\n");
-    boost::copy(table_ | map_keys, out_it);
-    throw runtime_error("Internal error in wrap");
-  }
-}
-
-/* ************************************************************************* */
-void TypeAttributesTable::addType(const Qualified& cls) {
-  if (!table_.insert(make_pair(cls.qualifiedName("::"), TypeAttributes(false)))
-           .second)
-    throw DuplicateDefinition("types " + cls.qualifiedName("::"));
-}
-
-/* ************************************************************************* */
-void TypeAttributesTable::addClasses(const vector<Class>& classes) {
-  for(const Class& cls: classes) {
-    if (!table_.insert(
-        make_pair(cls.qualifiedName("::"), TypeAttributes(cls.isVirtual))).second)
-      throw DuplicateDefinition("class " + cls.qualifiedName("::"));
-  }
-}
-
-/* ************************************************************************* */
-void TypeAttributesTable::addForwardDeclarations(
-    const vector<ForwardDeclaration>& forwardDecls) {
-  for(const ForwardDeclaration& fwDec: forwardDecls) {
-    if (!table_.insert(make_pair(fwDec.name(), TypeAttributes(fwDec.isVirtual))).second)
-      throw DuplicateDefinition("forward defined class " + fwDec.name());
-  }
-}
-
-/* ************************************************************************* */
-void TypeAttributesTable::checkValidity(const vector<Class>& classes) const {
-  for(const Class& cls: classes) {
-
-    boost::optional<string> parent = cls.qualifiedParent();
-    if (parent) {
-
-      // Check that class is virtual if it has a parent
-      if (!cls.isVirtual)
-        throw AttributeError(cls.qualifiedName("::"),
-            "Has a base class so needs to be declared virtual, change to 'virtual class "
-                + cls.name() + " ...'");
-
-      // Check that parent is virtual as well
-      if (!table_.at(*parent).isVirtual)
-        throw AttributeError(*parent,
-            "Is the base class of " + cls.qualifiedName("::")
-                + ", so needs to be declared virtual");
-    }
-  }
-}
-
-}
diff --git a/wrap/TypeAttributesTable.h b/wrap/TypeAttributesTable.h
deleted file mode 100644
index 4bbc239b4..000000000
--- a/wrap/TypeAttributesTable.h
+++ /dev/null
@@ -1,71 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Class.h
- * @brief describe the C++ class that is being wrapped
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#include <map>
-#include <string>
-#include <vector>
-
-#pragma once
-
-namespace wrap {
-
-// Forward declarations
-class Qualified;
-class Class;
-struct ForwardDeclaration;
-
-/** Attributes about valid classes, both for classes defined in this module and
- * also those forward-declared from others.  At the moment this only contains
- * whether the class is virtual, which is used to know how to copy the class,
- * and whether to try to convert it to a more derived type upon returning it.
- */
-struct TypeAttributes {
-  bool isVirtual;
-  TypeAttributes() :
-      isVirtual(false) {
-  }
-  TypeAttributes(bool isVirtual) :
-      isVirtual(isVirtual) {
-  }
-};
-
-/** Map of type names to attributes. */
-class TypeAttributesTable {
-
-  std::map<std::string, TypeAttributes> table_;
-
-public:
-
-  /// Constructor
-  TypeAttributesTable() {
-  }
-
-  void addClasses(const std::vector<Class>& classes);
-  void addType(const Qualified& types);
-  void addForwardDeclarations(
-      const std::vector<ForwardDeclaration>& forwardDecls);
-
-  /// Access attributes associated with key, informative failure
-  const TypeAttributes& attributes(const std::string& key) const;
-
-  /// Check that all virtual classes are properly defined
-  void checkValidity(const std::vector<Class>& classes) const;
-};
-
-}
diff --git a/wrap/TypedefPair.h b/wrap/TypedefPair.h
deleted file mode 100644
index c2224eaf2..000000000
--- a/wrap/TypedefPair.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#pragma once
-
-#include "Qualified.h"
-
-namespace wrap {
-struct TypedefPair {
-    Qualified oldType, newType;
-    std::string includeFile;
-
-    TypedefPair() {}
-    TypedefPair(const Qualified& _oldType, const Qualified& _newType,
-                const std::string& includeFile)
-        : oldType(_oldType), newType(_newType), includeFile(includeFile) {
-          if (!oldType.isNonBasicType() &&
-              std::find(Qualified::BasicTypedefs.begin(),
-                        Qualified::BasicTypedefs.end(),
-                        newType) == Qualified::BasicTypedefs.end())
-            Qualified::BasicTypedefs.push_back(newType);
-    }
-
-    void emit_cython_pxd(FileWriter& file) const {
-        file.oss << "cdef extern from \"" << includeFile << "\" namespace \""
-                 << oldType.qualifiedNamespaces("::") << "\":\n";
-        file.oss << "    ctypedef " << oldType.pxdClassName() << " "
-                 << newType.pxdClassName() << "\n";
-    }
-};
-}
diff --git a/wrap/cmake/PybindWrap.cmake b/wrap/cmake/PybindWrap.cmake
new file mode 100644
index 000000000..3091e4273
--- /dev/null
+++ b/wrap/cmake/PybindWrap.cmake
@@ -0,0 +1,235 @@
+# Unset these cached variables to avoid surprises when the python in the current
+# environment are different from the cached!
+unset(PYTHON_EXECUTABLE CACHE)
+unset(PYTHON_INCLUDE_DIR CACHE)
+unset(PYTHON_MAJOR_VERSION CACHE)
+
+# Allow override from command line
+if(NOT DEFINED WRAP_USE_CUSTOM_PYTHON_LIBRARY)
+  if(WRAP_PYTHON_VERSION STREQUAL "Default")
+    find_package(PythonInterp REQUIRED)
+    find_package(PythonLibs REQUIRED)
+  else()
+    find_package(PythonInterp
+                ${WRAP_PYTHON_VERSION}
+                EXACT
+                REQUIRED)
+    find_package(PythonLibs
+                ${WRAP_PYTHON_VERSION}
+                EXACT
+                REQUIRED)
+  endif()
+endif()
+
+set(DIR_OF_WRAP_PYBIND_CMAKE ${CMAKE_CURRENT_LIST_DIR})
+
+set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION})
+
+add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/../pybind11 pybind11)
+
+# User-friendly Pybind11 wrapping and installing function. Builds a Pybind11
+# module from the provided interface_header. For example, for the interface
+# header gtsam.h, this will build the wrap module 'gtsam_py.cc'.
+#
+# Arguments:
+# ~~~
+# interface_header:  The relative path to the wrapper interface definition file.
+# install_path: destination to install the library libs: libraries to link with
+# dependencies: Dependencies which need to be built before the wrapper
+function(pybind_wrap
+         target
+         interface_header
+         generated_cpp
+         module_name
+         top_namespace
+         ignore_classes
+         module_template
+         libs
+         dependencies)
+  set(ExtraMacroArgs ${ARGN})
+  list(GET ExtraMacroArgs 0 USE_BOOST)
+  if(USE_BOOST)
+    set(_WRAP_BOOST_ARG "--use-boost")
+  else(USE_BOOST)
+    set(_WRAP_BOOST_ARG "")
+  endif(USE_BOOST)
+  
+  add_custom_command(OUTPUT ${generated_cpp}
+                     COMMAND ${PYTHON_EXECUTABLE}
+                             ${CMAKE_SOURCE_DIR}/wrap/pybind_wrapper.py
+                             --src
+                             ${interface_header}
+                             --out
+                             ${generated_cpp}
+                             --module_name
+                             ${module_name}
+                             --top_module_namespaces
+                             "${top_namespace}"
+                             --ignore
+                             ${ignore_classes}
+                             --template
+                             ${module_template}
+                             ${_WRAP_BOOST_ARG}
+                     VERBATIM)
+  add_custom_target(pybind_wrap_${module_name} ALL DEPENDS ${generated_cpp})
+
+  # Late dependency injection, to make sure this gets called whenever the
+  # interface header or the wrap library are updated.
+  # ~~~
+  # See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes
+  # ~~~
+  add_custom_command(OUTPUT ${generated_cpp}
+                     DEPENDS ${interface_header}
+                             ${CMAKE_SOURCE_DIR}/wrap/interface_parser.py
+                             ${CMAKE_SOURCE_DIR}/wrap/pybind_wrapper.py
+                             ${CMAKE_SOURCE_DIR}/wrap/template_instantiator.py
+                     APPEND)
+
+  pybind11_add_module(${target} ${generated_cpp})
+
+  if(APPLE)
+    # `type_info` objects will become "weak private external" if the templated class is initialized implicitly even if we explicitly
+    # export them with `WRAP_EXPORT`. If that happens, the `type_info` for the same templated class will diverge between shared
+    # libraries, causing `dynamic_cast` to fail. This is mitigated by telling Clang to mimic the MSVC behavior.
+    # See https://developer.apple.com/library/archive/technotes/tn2185/_index.html#//apple_ref/doc/uid/DTS10004200-CH1-SUBSECTION2
+    # https://github.com/CppMicroServices/CppMicroServices/pull/82/files
+    # https://www.russellmcc.com/posts/2013-08-03-rtti.html
+    target_compile_options(${target} PRIVATE "-fvisibility-ms-compat")
+  endif()
+  add_dependencies(${target} pybind_wrap_${module_name})
+  if(NOT "${libs}" STREQUAL "")
+    target_link_libraries(${target} PRIVATE "${libs}")
+  endif()
+  if(NOT "${dependencies}" STREQUAL "")
+    add_dependencies(${target} ${dependencies})
+  endif()
+endfunction()
+
+# Helper function to install python scripts and handle multiple build types
+# where the scripts should be installed to all build type toolboxes
+#
+# Arguments:
+# ~~~
+# source_directory: The source directory to be installed. "The last component
+#     of each directory name is appended to the destination directory but a
+#     trailing slash may be used to avoid this because it leaves the last
+#     component empty."
+#     (https://cmake.org/cmake/help/v3.3/command/install.html?highlight=install#installing-directories)
+# dest_directory: The destination directory to install to.
+# patterns: list of file patterns to install
+# ~~~
+function(install_python_scripts
+         source_directory
+         dest_directory
+         patterns)
+  set(patterns_args "")
+  set(exclude_patterns "")
+
+  foreach(pattern ${patterns})
+    list(APPEND patterns_args PATTERN "${pattern}")
+  endforeach()
+  if(WRAP_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 "")
+      endif()
+      # Split up filename to strip trailing '/' in WRAP_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}"
+              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_python_files source_files dest_directory)
+
+  if(WRAP_BUILD_TYPE_POSTFIXES)
+    foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
+      string(TOUPPER "${build_type}" build_type_upper)
+      set(build_type_tag "")
+      # Split up filename to strip trailing '/' in WRAP_PY_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()
+
+# ~~~
+# https://stackoverflow.com/questions/13959434/cmake-out-of-source-build-python-files
+# ~~~
+function(create_symlinks source_folder dest_folder)
+  if(${source_folder} STREQUAL ${dest_folder})
+    return()
+  endif()
+
+  file(GLOB files
+       LIST_DIRECTORIES true
+       RELATIVE "${source_folder}"
+       "${source_folder}/*")
+  foreach(path_file ${files})
+    get_filename_component(folder ${path_file} PATH)
+    get_filename_component(ext ${path_file} EXT)
+    set(ignored_ext ".tpl" ".h")
+    list (FIND ignored_ext "${ext}" _index)
+    if (${_index} GREATER -1)
+      continue()
+    endif ()
+    # Create REAL folder
+    file(MAKE_DIRECTORY "${dest_folder}")
+
+    # Delete symlink if it exists
+    file(REMOVE "${dest_folder}/${path_file}")
+
+    # Get OS dependent path to use in `execute_process`
+    file(TO_NATIVE_PATH "${dest_folder}/${path_file}" link)
+    file(TO_NATIVE_PATH "${source_folder}/${path_file}" target)
+
+    # cmake-format: off
+    if(UNIX)
+      set(command ln -s ${target} ${link})
+    else()
+      set(command cmd.exe /c mklink ${link} ${target})
+    endif()
+    # cmake-format: on
+
+    execute_process(COMMAND ${command}
+                    RESULT_VARIABLE result
+                    ERROR_VARIABLE output)
+
+    if(NOT ${result} EQUAL 0)
+      message(
+        FATAL_ERROR
+          "Could not create symbolic link for: ${target} --> ${output}")
+    endif()
+
+  endforeach(path_file)
+endfunction(create_symlinks)
diff --git a/wrap/docs/doc_template.py b/wrap/docs/doc_template.py
new file mode 100644
index 000000000..68f4adb58
--- /dev/null
+++ b/wrap/docs/doc_template.py
@@ -0,0 +1,72 @@
+import xml.etree.ElementTree as ET
+
+
+class Doc():
+    def __init__(self, tree):
+        self.tree = tree
+
+    def get_tree(self):
+        """Get this Doc's tree.
+
+        Returns:
+            The xml.etree.ElementTree object of the documentation.
+        """
+        return self.tree
+
+    def __eq__(self, other):
+        if other is None or other.get_tree() is None:
+            return None
+
+        return ET.tostring(self.tree.getroot()) == \
+            ET.tostring(other.get_tree().getroot())
+
+
+class ClassDoc(Doc):
+    pass
+
+
+class FreeDoc(Doc):
+    pass
+
+
+class Docs():
+    def __init__(self, class_docs, free_docs):
+        # These are dicts that map file_path -> Doc
+        self.class_docs = class_docs
+        self.free_docs = free_docs
+
+    def get_class_docs(self, class_name):
+        '''Get the documentation for the class.
+
+        Arguments:
+        class_name -- the name of the class
+
+        Returns:
+        The ClassDoc with the class's documentation. None if the class does not
+        exist.
+        '''
+        return self.class_docs.get(class_name)
+
+    def get_free_docs(self, free_func_name):
+        '''Get the documentation for a free function.
+
+        Arguments:
+        free_func_name -- the name of the free function
+
+        Returns:
+        The FreeDoc with the free function's documentation. None if the class
+        does not exist.
+        '''
+        return self.free_docs.get(free_func_name)
+
+    def get_class_docs_keys_list(self):
+        return list(self.class_docs)
+
+    def get_free_docs_keys_list(self):
+        return list(self.free_docs)
+
+    def get_class_docs_values_list(self):
+        return list(self.class_docs.values())
+
+    def get_free_docs_values_list(self):
+        return list(self.free_docs.values())
diff --git a/wrap/docs/docs.py b/wrap/docs/docs.py
new file mode 100644
index 000000000..68f4adb58
--- /dev/null
+++ b/wrap/docs/docs.py
@@ -0,0 +1,72 @@
+import xml.etree.ElementTree as ET
+
+
+class Doc():
+    def __init__(self, tree):
+        self.tree = tree
+
+    def get_tree(self):
+        """Get this Doc's tree.
+
+        Returns:
+            The xml.etree.ElementTree object of the documentation.
+        """
+        return self.tree
+
+    def __eq__(self, other):
+        if other is None or other.get_tree() is None:
+            return None
+
+        return ET.tostring(self.tree.getroot()) == \
+            ET.tostring(other.get_tree().getroot())
+
+
+class ClassDoc(Doc):
+    pass
+
+
+class FreeDoc(Doc):
+    pass
+
+
+class Docs():
+    def __init__(self, class_docs, free_docs):
+        # These are dicts that map file_path -> Doc
+        self.class_docs = class_docs
+        self.free_docs = free_docs
+
+    def get_class_docs(self, class_name):
+        '''Get the documentation for the class.
+
+        Arguments:
+        class_name -- the name of the class
+
+        Returns:
+        The ClassDoc with the class's documentation. None if the class does not
+        exist.
+        '''
+        return self.class_docs.get(class_name)
+
+    def get_free_docs(self, free_func_name):
+        '''Get the documentation for a free function.
+
+        Arguments:
+        free_func_name -- the name of the free function
+
+        Returns:
+        The FreeDoc with the free function's documentation. None if the class
+        does not exist.
+        '''
+        return self.free_docs.get(free_func_name)
+
+    def get_class_docs_keys_list(self):
+        return list(self.class_docs)
+
+    def get_free_docs_keys_list(self):
+        return list(self.free_docs)
+
+    def get_class_docs_values_list(self):
+        return list(self.class_docs.values())
+
+    def get_free_docs_values_list(self):
+        return list(self.free_docs.values())
diff --git a/wrap/docs/parser/conf_doxygen.py b/wrap/docs/parser/conf_doxygen.py
new file mode 100644
index 000000000..2cf66c07f
--- /dev/null
+++ b/wrap/docs/parser/conf_doxygen.py
@@ -0,0 +1,2427 @@
+# Doxyfile 1.8.11
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a double hash (##) is considered a comment and is placed in
+# front of the TAG it is preceding.
+#
+# All text after a single hash (#) is considered a comment and will be ignored.
+# The format is:
+# TAG = value [value, ...]
+# For lists, items can also be appended using:
+# TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (\" \").
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# This tag specifies the encoding used for all characters in the config file
+# that follow. The default is UTF-8 which is also the encoding used for all text
+# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
+# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
+# for the list of possible encodings.
+# The default value is: UTF-8.
+
+DOXYFILE_ENCODING      = UTF-8
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
+# double-quotes, unless you are using Doxywizard) that should identify the
+# project for which the documentation is generated. This name is used in the
+# title of most generated pages and in a few other places.
+# The default value is: My Project.
+
+PROJECT_NAME           = "GTSAM"
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
+# could be handy for archiving the generated documentation or if some version
+# control system is used.
+
+PROJECT_NUMBER         = 0.0
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer a
+# quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF          = 
+
+# With the PROJECT_LOGO tag one can specify a logo or an icon that is included
+# in the documentation. The maximum height of the logo should not exceed 55
+# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
+# the logo to the output directory.
+
+PROJECT_LOGO           =
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
+# into which the generated documentation will be written. If a relative path is
+# entered, it will be relative to the location where doxygen was started. If
+# left blank the current directory will be used.
+
+OUTPUT_DIRECTORY       =
+
+# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
+# option can be useful when feeding doxygen a huge amount of source files, where
+# putting all generated files in the same directory would otherwise causes
+# performance problems for the file system.
+# The default value is: NO.
+
+CREATE_SUBDIRS         = NO
+
+# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
+# characters to appear in the names of generated files. If set to NO, non-ASCII
+# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
+# U+3044.
+# The default value is: NO.
+
+ALLOW_UNICODE_NAMES    = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all constant output in the proper language.
+# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
+# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
+# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
+# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
+# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
+# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
+# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
+# Ukrainian and Vietnamese.
+# The default value is: English.
+
+OUTPUT_LANGUAGE        = English
+
+# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member
+# descriptions after the members that are listed in the file and class
+# documentation (similar to Javadoc). Set to NO to disable this.
+# The default value is: YES.
+
+BRIEF_MEMBER_DESC      = YES
+
+# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief
+# description of a member or function before the detailed description
+#
+# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
+# brief descriptions will be completely suppressed.
+# The default value is: YES.
+
+REPEAT_BRIEF           = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator that is
+# used to form the text in various listings. Each string in this list, if found
+# as the leading text of the brief description, will be stripped from the text
+# and the result, after processing the whole list, is used as the annotated
+# text. Otherwise, the brief description is used as-is. If left blank, the
+# following values are used ($name is automatically replaced with the name of
+# the entity):The $name class, The $name widget, The $name file, is, provides,
+# specifies, contains, represents, a, an and the.
+
+ABBREVIATE_BRIEF       =
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
+# doxygen will generate a detailed section even if there is only a brief
+# description.
+# The default value is: NO.
+
+ALWAYS_DETAILED_SEC    = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
+# inherited members of a class in the documentation of that class as if those
+# members were ordinary class members. Constructors, destructors and assignment
+# operators of the base classes will not be shown.
+# The default value is: NO.
+
+INLINE_INHERITED_MEMB  = NO
+
+# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path
+# before files name in the file list and in the header files. If set to NO the
+# shortest path that makes the file name unique will be used
+# The default value is: YES.
+
+FULL_PATH_NAMES        = YES
+
+# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
+# Stripping is only done if one of the specified strings matches the left-hand
+# part of the path. The tag can be used to show relative paths in the file list.
+# If left blank the directory from which doxygen is run is used as the path to
+# strip.
+#
+# Note that you can specify absolute paths here, but also relative paths, which
+# will be relative from the directory where doxygen is started.
+# This tag requires that the tag FULL_PATH_NAMES is set to YES.
+
+STRIP_FROM_PATH        =
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
+# path mentioned in the documentation of a class, which tells the reader which
+# header file to include in order to use a class. If left blank only the name of
+# the header file containing the class definition is used. Otherwise one should
+# specify the list of include paths that are normally passed to the compiler
+# using the -I flag.
+
+STRIP_FROM_INC_PATH    =
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
+# less readable) file names. This can be useful is your file systems doesn't
+# support long names like on DOS, Mac, or CD-ROM.
+# The default value is: NO.
+
+SHORT_NAMES            = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
+# first line (until the first dot) of a Javadoc-style comment as the brief
+# description. If set to NO, the Javadoc-style will behave just like regular Qt-
+# style comments (thus requiring an explicit @brief command for a brief
+# description.)
+# The default value is: NO.
+
+JAVADOC_AUTOBRIEF      = NO
+
+# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
+# line (until the first dot) of a Qt-style comment as the brief description. If
+# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
+# requiring an explicit \brief command for a brief description.)
+# The default value is: NO.
+
+QT_AUTOBRIEF           = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
+# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
+# a brief description. This used to be the default behavior. The new default is
+# to treat a multi-line C++ comment block as a detailed description. Set this
+# tag to YES if you prefer the old behavior instead.
+#
+# Note that setting this tag to YES also means that rational rose comments are
+# not recognized any more.
+# The default value is: NO.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
+# documentation from any documented member that it re-implements.
+# The default value is: YES.
+
+INHERIT_DOCS           = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new
+# page for each member. If set to NO, the documentation of a member will be part
+# of the file/class/namespace that contains it.
+# The default value is: NO.
+
+SEPARATE_MEMBER_PAGES  = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
+# uses this value to replace tabs by spaces in code fragments.
+# Minimum value: 1, maximum value: 16, default value: 4.
+
+TAB_SIZE               = 4
+
+# This tag can be used to specify a number of aliases that act as commands in
+# the documentation. An alias has the form:
+# name=value
+# For example adding
+# "sideeffect=@par Side Effects:\n"
+# will allow you to put the command \sideeffect (or @sideeffect) in the
+# documentation, which will result in a user-defined paragraph with heading
+# "Side Effects:". You can put \n's in the value part of an alias to insert
+# newlines.
+
+ALIASES                =
+
+# This tag can be used to specify a number of word-keyword mappings (TCL only).
+# A mapping has the form "name=value". For example adding "class=itcl::class"
+# will allow you to use the command class in the itcl::class meaning.
+
+TCL_SUBST              =
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
+# only. Doxygen will then generate output that is more tailored for C. For
+# instance, some of the names that are used will be different. The list of all
+# members will be omitted, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_FOR_C  = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
+# Python sources only. Doxygen will then generate output that is more tailored
+# for that language. For instance, namespaces will be presented as packages,
+# qualified scopes will look different, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_JAVA   = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources. Doxygen will then generate output that is tailored for Fortran.
+# The default value is: NO.
+
+OPTIMIZE_FOR_FORTRAN   = NO
+
+# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
+# sources. Doxygen will then generate output that is tailored for VHDL.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_VHDL   = NO
+
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given
+# extension. Doxygen has a built-in mapping, but you can override or extend it
+# using this tag. The format is ext=language, where ext is a file extension, and
+# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
+# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran:
+# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran:
+# Fortran. In the later case the parser tries to guess whether the code is fixed
+# or free formatted code, this is the default for Fortran type files), VHDL. For
+# instance to make doxygen treat .inc files as Fortran files (default is PHP),
+# and .f files as C (default is Fortran), use: inc=Fortran f=C.
+#
+# Note: For files without extension you can use no_extension as a placeholder.
+#
+# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
+# the files are not read by doxygen.
+
+EXTENSION_MAPPING      =
+
+# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
+# according to the Markdown format, which allows for more readable
+# documentation. See http://daringfireball.net/projects/markdown/ for details.
+# The output of markdown processing is further processed by doxygen, so you can
+# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
+# case of backward compatibilities issues.
+# The default value is: YES.
+
+MARKDOWN_SUPPORT       = YES
+
+# When enabled doxygen tries to link words that correspond to documented
+# classes, or namespaces to their corresponding documentation. Such a link can
+# be prevented in individual cases by putting a % sign in front of the word or
+# globally by setting AUTOLINK_SUPPORT to NO.
+# The default value is: YES.
+
+AUTOLINK_SUPPORT       = YES
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
+# to include (a tag file for) the STL sources as input, then you should set this
+# tag to YES in order to let doxygen match functions declarations and
+# definitions whose arguments contain STL classes (e.g. func(std::string);
+# versus func(std::string) {}). This also make the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+# The default value is: NO.
+
+BUILTIN_STL_SUPPORT    = NO
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+# The default value is: NO.
+
+CPP_CLI_SUPPORT        = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
+# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
+# will parse them like normal C++ but will assume all classes use public instead
+# of private inheritance when no explicit protection keyword is present.
+# The default value is: NO.
+
+SIP_SUPPORT            = NO
+
+# For Microsoft's IDL there are propget and propput attributes to indicate
+# getter and setter methods for a property. Setting this option to YES will make
+# doxygen to replace the get and set methods by a property in the documentation.
+# This will only work if the methods are indeed getting or setting a simple
+# type. If this is not the case, or you want to show the methods anyway, you
+# should set this option to NO.
+# The default value is: YES.
+
+IDL_PROPERTY_SUPPORT   = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
+# tag is set to YES then doxygen will reuse the documentation of the first
+# member in the group (if any) for the other members of the group. By default
+# all members of a group must be documented explicitly.
+# The default value is: NO.
+
+DISTRIBUTE_GROUP_DOC   = NO
+
+# If one adds a struct or class to a group and this option is enabled, then also
+# any nested class or struct is added to the same group. By default this option
+# is disabled and one has to add nested compounds explicitly via \ingroup.
+# The default value is: NO.
+
+GROUP_NESTED_COMPOUNDS = NO
+
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
+# (for instance a group of public functions) to be put as a subgroup of that
+# type (e.g. under the Public Functions section). Set it to NO to prevent
+# subgrouping. Alternatively, this can be done per class using the
+# \nosubgrouping command.
+# The default value is: YES.
+
+SUBGROUPING            = YES
+
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
+# are shown inside the group in which they are included (e.g. using \ingroup)
+# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
+# and RTF).
+#
+# Note that this feature does not work in combination with
+# SEPARATE_MEMBER_PAGES.
+# The default value is: NO.
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
+# with only public data fields or simple typedef fields will be shown inline in
+# the documentation of the scope in which they are defined (i.e. file,
+# namespace, or group documentation), provided this scope is documented. If set
+# to NO, structs, classes, and unions are shown on a separate page (for HTML and
+# Man pages) or section (for LaTeX and RTF).
+# The default value is: NO.
+
+INLINE_SIMPLE_STRUCTS  = NO
+
+# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
+# enum is documented as struct, union, or enum with the name of the typedef. So
+# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
+# with name TypeT. When disabled the typedef will appear as a member of a file,
+# namespace, or class. And the struct will be named TypeS. This can typically be
+# useful for C code in case the coding convention dictates that all compound
+# types are typedef'ed and only the typedef is referenced, never the tag name.
+# The default value is: NO.
+
+TYPEDEF_HIDES_STRUCT   = NO
+
+# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
+# cache is used to resolve symbols given their name and scope. Since this can be
+# an expensive process and often the same symbol appears multiple times in the
+# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
+# doxygen will become slower. If the cache is too large, memory is wasted. The
+# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
+# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
+# symbols. At the end of a run doxygen will report the cache usage and suggest
+# the optimal cache size from a speed point of view.
+# Minimum value: 0, maximum value: 9, default value: 0.
+
+LOOKUP_CACHE_SIZE      = 0
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in
+# documentation are documented, even if no documentation was available. Private
+# class members and static file members will be hidden unless the
+# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
+# Note: This will also disable the warnings about undocumented members that are
+# normally produced when WARNINGS is set to YES.
+# The default value is: NO.
+
+EXTRACT_ALL            =
+
+# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will
+# be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIVATE        = NO
+
+# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal
+# scope will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PACKAGE        = NO
+
+# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be
+# included in the documentation.
+# The default value is: NO.
+
+EXTRACT_STATIC         = NO
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined
+# locally in source files will be included in the documentation. If set to NO,
+# only classes defined in header files are included. Does not have any effect
+# for Java sources.
+# The default value is: YES.
+
+EXTRACT_LOCAL_CLASSES  = YES
+
+# This flag is only useful for Objective-C code. If set to YES, local methods,
+# which are defined in the implementation section but not in the interface are
+# included in the documentation. If set to NO, only methods in the interface are
+# included.
+# The default value is: NO.
+
+EXTRACT_LOCAL_METHODS  = NO
+
+# If this flag is set to YES, the members of anonymous namespaces will be
+# extracted and appear in the documentation as a namespace called
+# 'anonymous_namespace{file}', where file will be replaced with the base name of
+# the file that contains the anonymous namespace. By default anonymous namespace
+# are hidden.
+# The default value is: NO.
+
+EXTRACT_ANON_NSPACES   = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
+# undocumented members inside documented classes or files. If set to NO these
+# members will be included in the various overviews, but no documentation
+# section is generated. This option has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_MEMBERS     = NO
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
+# undocumented classes that are normally visible in the class hierarchy. If set
+# to NO, these classes will be included in the various overviews. This option
+# has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_CLASSES     = NO
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
+# (class|struct|union) declarations. If set to NO, these declarations will be
+# included in the documentation.
+# The default value is: NO.
+
+HIDE_FRIEND_COMPOUNDS  = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
+# documentation blocks found inside the body of a function. If set to NO, these
+# blocks will be appended to the function's detailed documentation block.
+# The default value is: NO.
+
+HIDE_IN_BODY_DOCS      = NO
+
+# The INTERNAL_DOCS tag determines if documentation that is typed after a
+# \internal command is included. If the tag is set to NO then the documentation
+# will be excluded. Set it to YES to include the internal documentation.
+# The default value is: NO.
+
+INTERNAL_DOCS          = NO
+
+# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
+# names in lower-case letters. If set to YES, upper-case letters are also
+# allowed. This is useful if you have classes or files whose names only differ
+# in case and if your file system supports case sensitive file names. Windows
+# and Mac users are advised to set this option to NO.
+# The default value is: system dependent.
+
+CASE_SENSE_NAMES       = YES
+
+# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
+# their full class and namespace scopes in the documentation. If set to YES, the
+# scope will be hidden.
+# The default value is: NO.
+
+HIDE_SCOPE_NAMES       = NO
+
+# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will
+# append additional text to a page's title, such as Class Reference. If set to
+# YES the compound reference will be hidden.
+# The default value is: NO.
+
+HIDE_COMPOUND_REFERENCE= NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
+# the files that are included by a file in the documentation of that file.
+# The default value is: YES.
+
+SHOW_INCLUDE_FILES     = YES
+
+# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
+# grouped member an include statement to the documentation, telling the reader
+# which file to include in order to use the member.
+# The default value is: NO.
+
+SHOW_GROUPED_MEMB_INC  = NO
+
+# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
+# files with double quotes in the documentation rather than with sharp brackets.
+# The default value is: NO.
+
+FORCE_LOCAL_INCLUDES   = NO
+
+# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
+# documentation for inline members.
+# The default value is: YES.
+
+INLINE_INFO            = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
+# (detailed) documentation of file and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order.
+# The default value is: YES.
+
+SORT_MEMBER_DOCS       = YES
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
+# descriptions of file, namespace and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order. Note that
+# this will also influence the order of the classes in the class list.
+# The default value is: NO.
+
+SORT_BRIEF_DOCS        = NO
+
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
+# (brief and detailed) documentation of class members so that constructors and
+# destructors are listed first. If set to NO the constructors will appear in the
+# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
+# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
+# member documentation.
+# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
+# detailed member documentation.
+# The default value is: NO.
+
+SORT_MEMBERS_CTORS_1ST = NO
+
+# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
+# of group names into alphabetical order. If set to NO the group names will
+# appear in their defined order.
+# The default value is: NO.
+
+SORT_GROUP_NAMES       = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
+# fully-qualified names, including namespaces. If set to NO, the class list will
+# be sorted only by class name, not including the namespace part.
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the alphabetical
+# list.
+# The default value is: NO.
+
+SORT_BY_SCOPE_NAME     = NO
+
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
+# type resolution of all parameters of a function it will reject a match between
+# the prototype and the implementation of a member function even if there is
+# only one candidate or it is obvious which candidate to choose by doing a
+# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
+# accept a match between prototype and implementation in such cases.
+# The default value is: NO.
+
+STRICT_PROTO_MATCHING  = NO
+
+# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo
+# list. This list is created by putting \todo commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TODOLIST      = YES
+
+# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test
+# list. This list is created by putting \test commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TESTLIST      = YES
+
+# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug
+# list. This list is created by putting \bug commands in the documentation.
+# The default value is: YES.
+
+GENERATE_BUGLIST       = YES
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO)
+# the deprecated list. This list is created by putting \deprecated commands in
+# the documentation.
+# The default value is: YES.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional documentation
+# sections, marked by \if <section_label> ... \endif and \cond <section_label>
+# ... \endcond blocks.
+
+ENABLED_SECTIONS       =
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
+# initial value of a variable or macro / define can have for it to appear in the
+# documentation. If the initializer consists of more lines than specified here
+# it will be hidden. Use a value of 0 to hide initializers completely. The
+# appearance of the value of individual variables and macros / defines can be
+# controlled using \showinitializer or \hideinitializer command in the
+# documentation regardless of this setting.
+# Minimum value: 0, maximum value: 10000, default value: 30.
+
+MAX_INITIALIZER_LINES  = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
+# the bottom of the documentation of classes and structs. If set to YES, the
+# list will mention the files that were used to generate the documentation.
+# The default value is: YES.
+
+SHOW_USED_FILES        = YES
+
+# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
+# will remove the Files entry from the Quick Index and from the Folder Tree View
+# (if specified).
+# The default value is: YES.
+
+SHOW_FILES             = YES
+
+# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
+# page. This will remove the Namespaces entry from the Quick Index and from the
+# Folder Tree View (if specified).
+# The default value is: YES.
+
+SHOW_NAMESPACES        = YES
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that
+# doxygen should invoke to get the current version for each file (typically from
+# the version control system). Doxygen will invoke the program by executing (via
+# popen()) the command command input-file, where command is the value of the
+# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
+# by doxygen. Whatever the program writes to standard output is used as the file
+# version. For an example see the documentation.
+
+FILE_VERSION_FILTER    =
+
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. To create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option. You can
+# optionally specify a file name after the option, if omitted DoxygenLayout.xml
+# will be used as the name of the layout file.
+#
+# Note that if you run doxygen from a directory containing a file called
+# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
+# tag is left empty.
+
+LAYOUT_FILE            =
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
+# the reference definitions. This must be a list of .bib files. The .bib
+# extension is automatically appended if omitted. This requires the bibtex tool
+# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
+# For LaTeX the style of the bibliography can be controlled using
+# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
+# search path. See also \cite for info how to create references.
+
+CITE_BIB_FILES         =
+
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated to
+# standard output by doxygen. If QUIET is set to YES this implies that the
+# messages are off.
+# The default value is: NO.
+
+QUIET                  = YES
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES
+# this implies that the warnings are on.
+#
+# Tip: Turn warnings on while writing the documentation.
+# The default value is: YES.
+
+WARNINGS               = YES
+
+# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate
+# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
+# will automatically be disabled.
+# The default value is: YES.
+
+WARN_IF_UNDOCUMENTED   = YES
+
+# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
+# potential errors in the documentation, such as not documenting some parameters
+# in a documented function, or documenting parameters that don't exist or using
+# markup commands wrongly.
+# The default value is: YES.
+
+WARN_IF_DOC_ERROR      = YES
+
+# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
+# are documented, but have no documentation for their parameters or return
+# value. If set to NO, doxygen will only warn about wrong or incomplete
+# parameter documentation, but not about the absence of documentation.
+# The default value is: NO.
+
+WARN_NO_PARAMDOC       = NO
+
+# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when
+# a warning is encountered.
+# The default value is: NO.
+
+WARN_AS_ERROR          = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that doxygen
+# can produce. The string should contain the $file, $line, and $text tags, which
+# will be replaced by the file and line number from which the warning originated
+# and the warning text. Optionally the format may contain $version, which will
+# be replaced by the version of the file (if it could be obtained via
+# FILE_VERSION_FILTER)
+# The default value is: $file:$line: $text.
+
+WARN_FORMAT            = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning and error
+# messages should be written. If left blank the output is written to standard
+# error (stderr).
+
+WARN_LOGFILE           =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag is used to specify the files and/or directories that contain
+# documented source files. You may enter file names like myfile.cpp or
+# directories like /usr/src/myproject. Separate the files or directories with
+# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
+# Note: If this tag is empty the current directory is searched.
+
+INPUT                  = 
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
+# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
+# documentation (see: http://www.gnu.org/software/libiconv) for the list of
+# possible encodings.
+# The default value is: UTF-8.
+
+INPUT_ENCODING         = UTF-8
+
+# If the value of the INPUT tag contains directories, you can use the
+# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
+# *.h) to filter out the source-files in the directories.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# read by doxygen.
+#
+# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp,
+# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h,
+# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc,
+# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.pyw, *.f90, *.f, *.for, *.tcl,
+# *.vhd, *.vhdl, *.ucf, *.qsf, *.as and *.js.
+
+FILE_PATTERNS          =
+
+# The RECURSIVE tag can be used to specify whether or not subdirectories should
+# be searched for input files as well.
+# The default value is: NO.
+
+RECURSIVE              = NO
+
+# The EXCLUDE tag can be used to specify files and/or directories that should be
+# excluded from the INPUT source files. This way you can easily exclude a
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+#
+# Note that relative paths are relative to the directory from which doxygen is
+# run.
+
+EXCLUDE                =
+
+# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
+# directories that are symbolic links (a Unix file system feature) are excluded
+# from the input.
+# The default value is: NO.
+
+EXCLUDE_SYMLINKS       = NO
+
+# If the value of the INPUT tag contains directories, you can use the
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
+# certain files from those directories.
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories for example use the pattern */test/*
+
+EXCLUDE_PATTERNS       =
+
+# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
+# (namespaces, classes, functions, etc.) that should be excluded from the
+# output. The symbol name can be a fully qualified name, a word, or if the
+# wildcard * is used, a substring. Examples: ANamespace, AClass,
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
+
+EXCLUDE_SYMBOLS        =
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or directories
+# that contain example code fragments that are included (see the \include
+# command).
+
+EXAMPLE_PATH           =
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank all
+# files are included.
+
+EXAMPLE_PATTERNS       =
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
+# searched for input files to be used with the \include or \dontinclude commands
+# irrespective of the value of the RECURSIVE tag.
+# The default value is: NO.
+
+EXAMPLE_RECURSIVE      = NO
+
+# The IMAGE_PATH tag can be used to specify one or more files or directories
+# that contain images that are to be included in the documentation (see the
+# \image command).
+
+IMAGE_PATH             =
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should
+# invoke to filter for each input file. Doxygen will invoke the filter program
+# by executing (via popen()) the command:
+#
+# <filter> <input-file>
+#
+# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
+# name of an input file. Doxygen will then use the output that the filter
+# program writes to standard output. If FILTER_PATTERNS is specified, this tag
+# will be ignored.
+#
+# Note that the filter must not add or remove lines; it is applied before the
+# code is scanned, but not when the output code is generated. If lines are added
+# or removed, the anchors will not be placed correctly.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+INPUT_FILTER           =
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
+# basis. Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match. The filters are a list of the form: pattern=filter
+# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
+# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
+# patterns match the file name, INPUT_FILTER is applied.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+FILTER_PATTERNS        =
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
+# INPUT_FILTER) will also be used to filter the input files that are used for
+# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
+# The default value is: NO.
+
+FILTER_SOURCE_FILES    = NO
+
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
+# it is also possible to disable source filtering for a specific pattern using
+# *.ext= (so without naming a filter).
+# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
+
+FILTER_SOURCE_PATTERNS =
+
+# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
+# is part of the input, its contents will be placed on the main page
+# (index.html). This can be useful if you have a project on for instance GitHub
+# and want to reuse the introduction page also for the doxygen output.
+
+USE_MDFILE_AS_MAINPAGE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
+# generated. Documented entities will be cross-referenced with these sources.
+#
+# Note: To get rid of all source code in the generated output, make sure that
+# also VERBATIM_HEADERS is set to NO.
+# The default value is: NO.
+
+SOURCE_BROWSER         = NO
+
+# Setting the INLINE_SOURCES tag to YES will include the body of functions,
+# classes and enums directly into the documentation.
+# The default value is: NO.
+
+INLINE_SOURCES         = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
+# special comment blocks from generated source code fragments. Normal C, C++ and
+# Fortran comments will always remain visible.
+# The default value is: YES.
+
+STRIP_CODE_COMMENTS    = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
+# function all documented functions referencing it will be listed.
+# The default value is: NO.
+
+REFERENCED_BY_RELATION = NO
+
+# If the REFERENCES_RELATION tag is set to YES then for each documented function
+# all documented entities called/used by that function will be listed.
+# The default value is: NO.
+
+REFERENCES_RELATION    = NO
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
+# to YES then the hyperlinks from functions in REFERENCES_RELATION and
+# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
+# link to the documentation.
+# The default value is: YES.
+
+REFERENCES_LINK_SOURCE = YES
+
+# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
+# source code will show a tooltip with additional information such as prototype,
+# brief description and links to the definition and documentation. Since this
+# will make the HTML file larger and loading of large files a bit slower, you
+# can opt to disable this feature.
+# The default value is: YES.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+SOURCE_TOOLTIPS        = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code will
+# point to the HTML generated by the htags(1) tool instead of doxygen built-in
+# source browser. The htags tool is part of GNU's global source tagging system
+# (see http://www.gnu.org/software/global/global.html). You will need version
+# 4.8.6 or higher.
+#
+# To use it do the following:
+# - Install the latest version of global
+# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
+# - Make sure the INPUT points to the root of the source tree
+# - Run doxygen as normal
+#
+# Doxygen will invoke htags (and that will in turn invoke gtags), so these
+# tools must be available from the command line (i.e. in the search path).
+#
+# The result: instead of the source browser generated by doxygen, the links to
+# source code will now point to the output of htags.
+# The default value is: NO.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+USE_HTAGS              = NO
+
+# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
+# verbatim copy of the header file for each class for which an include is
+# specified. Set to NO to disable this.
+# See also: Section \class.
+# The default value is: YES.
+
+VERBATIM_HEADERS       = YES
+
+# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the
+# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the
+# cost of reduced performance. This can be particularly helpful with template
+# rich C++ code for which doxygen's built-in parser lacks the necessary type
+# information.
+# Note: The availability of this option depends on whether or not doxygen was
+# generated with the -Duse-libclang=ON option for CMake.
+# The default value is: NO.
+
+CLANG_ASSISTED_PARSING = NO
+
+# If clang assisted parsing is enabled you can provide the compiler with command
+# line options that you would normally use when invoking the compiler. Note that
+# the include paths will already be set by doxygen for the files and directories
+# specified with INPUT and INCLUDE_PATH.
+# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES.
+
+CLANG_OPTIONS          =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
+# compounds will be generated. Enable this if the project contains a lot of
+# classes, structs, unions or interfaces.
+# The default value is: YES.
+
+ALPHABETICAL_INDEX     = YES
+
+# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
+# which the alphabetical index list will be split.
+# Minimum value: 1, maximum value: 20, default value: 5.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+COLS_IN_ALPHA_INDEX    = 5
+
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+IGNORE_PREFIX          =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output
+# The default value is: YES.
+
+GENERATE_HTML          = NO
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_OUTPUT            = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
+# generated HTML page (for example: .htm, .php, .asp).
+# The default value is: .html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FILE_EXTENSION    = .html
+
+# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
+# each generated HTML page. If the tag is left blank doxygen will generate a
+# standard header.
+#
+# To get valid HTML the header file that includes any scripts and style sheets
+# that doxygen needs, which is dependent on the configuration options used (e.g.
+# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
+# default header using
+# doxygen -w html new_header.html new_footer.html new_stylesheet.css
+# YourConfigFile
+# and then modify the file new_header.html. See also section "Doxygen usage"
+# for information on how to generate the default header that doxygen normally
+# uses.
+# Note: The header is subject to change so you typically have to regenerate the
+# default header when upgrading to a newer version of doxygen. For a description
+# of the possible markers and block names see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_HEADER            =
+
+# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
+# generated HTML page. If the tag is left blank doxygen will generate a standard
+# footer. See HTML_HEADER for more information on how to generate a default
+# footer and what special commands can be used inside the footer. See also
+# section "Doxygen usage" for information on how to generate the default footer
+# that doxygen normally uses.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FOOTER            =
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
+# sheet that is used by each HTML page. It can be used to fine-tune the look of
+# the HTML output. If left blank doxygen will generate a default style sheet.
+# See also section "Doxygen usage" for information on how to generate the style
+# sheet that doxygen normally uses.
+# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
+# it is more robust and this tag (HTML_STYLESHEET) will in the future become
+# obsolete.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_STYLESHEET        =
+
+# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# cascading style sheets that are included after the standard style sheets
+# created by doxygen. Using this option one can overrule certain style aspects.
+# This is preferred over using HTML_STYLESHEET since it does not replace the
+# standard style sheet and is therefore more robust against future updates.
+# Doxygen will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list). For an example see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_STYLESHEET  =
+
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
+# files will be copied as-is; there are no commands or markers available.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_FILES       =
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
+# will adjust the colors in the style sheet and background images according to
+# this color. Hue is specified as an angle on a colorwheel, see
+# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
+# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
+# purple, and 360 is red again.
+# Minimum value: 0, maximum value: 359, default value: 220.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_HUE    = 220
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
+# in the HTML output. For a value of 0 the output will use grayscales only. A
+# value of 255 will produce the most vivid colors.
+# Minimum value: 0, maximum value: 255, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_SAT    = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
+# luminance component of the colors in the HTML output. Values below 100
+# gradually make the output lighter, whereas values above 100 make the output
+# darker. The value divided by 100 is the actual gamma applied, so 80 represents
+# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
+# change the gamma.
+# Minimum value: 40, maximum value: 240, default value: 80.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_GAMMA  = 80
+
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to YES can help to show when doxygen was last run and thus if the
+# documentation is up to date.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP         = NO
+
+# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
+# documentation will contain sections that can be hidden and shown after the
+# page has loaded.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_SECTIONS  = NO
+
+# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
+# shown in the various tree structured indices initially; the user can expand
+# and collapse entries dynamically later on. Doxygen will expand the tree to
+# such a level that at most the specified number of entries are visible (unless
+# a fully collapsed tree already exceeds this amount). So setting the number of
+# entries 1 will produce a full collapsed tree by default. 0 is a special value
+# representing an infinite number of entries and will result in a full expanded
+# tree by default.
+# Minimum value: 0, maximum value: 9999, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_INDEX_NUM_ENTRIES = 100
+
+# If the GENERATE_DOCSET tag is set to YES, additional index files will be
+# generated that can be used as input for Apple's Xcode 3 integrated development
+# environment (see: http://developer.apple.com/tools/xcode/), introduced with
+# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
+# Makefile in the HTML output directory. Running make will produce the docset in
+# that directory and running make install will install the docset in
+# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
+# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
+# for more information.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_DOCSET        = NO
+
+# This tag determines the name of the docset feed. A documentation feed provides
+# an umbrella under which multiple documentation sets from a single provider
+# (such as a company or product suite) can be grouped.
+# The default value is: Doxygen generated docs.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_FEEDNAME        = "Doxygen generated docs"
+
+# This tag specifies a string that should uniquely identify the documentation
+# set bundle. This should be a reverse domain-name style string, e.g.
+# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_BUNDLE_ID       = org.doxygen.Project
+
+# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+# The default value is: org.doxygen.Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_ID    = org.doxygen.Publisher
+
+# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
+# The default value is: Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_NAME  = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
+# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
+# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
+# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
+# Windows.
+#
+# The HTML Help Workshop contains a compiler that can convert all HTML output
+# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
+# files are now used as the Windows 98 help format, and will replace the old
+# Windows help format (.hlp) on all Windows platforms in the future. Compressed
+# HTML files also contain an index, a table of contents, and you can search for
+# words in the documentation. The HTML workshop also contains a viewer for
+# compressed HTML files.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_HTMLHELP      = NO
+
+# The CHM_FILE tag can be used to specify the file name of the resulting .chm
+# file. You can add a path in front of the file if the result should not be
+# written to the html output directory.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_FILE               =
+
+# The HHC_LOCATION tag can be used to specify the location (absolute path
+# including file name) of the HTML help compiler (hhc.exe). If non-empty,
+# doxygen will try to run the HTML help compiler on the generated index.hhp.
+# The file has to be specified with full path.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+HHC_LOCATION           =
+
+# The GENERATE_CHI flag controls if a separate .chi index file is generated
+# (YES) or that it should be included in the master .chm file (NO).
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+GENERATE_CHI           = NO
+
+# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc)
+# and project file content.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_INDEX_ENCODING     =
+
+# The BINARY_TOC flag controls whether a binary table of contents is generated
+# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it
+# enables the Previous and Next buttons.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+BINARY_TOC             = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members to
+# the table of contents of the HTML help documentation and to the tree view.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+TOC_EXPAND             = NO
+
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
+# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
+# (.qch) of the generated HTML documentation.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_QHP           = NO
+
+# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
+# the file name of the resulting .qch file. The path specified is relative to
+# the HTML output folder.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QCH_FILE               =
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
+# Project output. For more information please see Qt Help Project / Namespace
+# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_NAMESPACE          = org.doxygen.Project
+
+# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
+# Help Project output. For more information please see Qt Help Project / Virtual
+# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
+# folders).
+# The default value is: doc.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_VIRTUAL_FOLDER     = doc
+
+# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
+# filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_NAME   =
+
+# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_ATTRS  =
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's filter section matches. Qt Help Project / Filter Attributes (see:
+# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_SECT_FILTER_ATTRS  =
+
+# The QHG_LOCATION tag can be used to specify the location of Qt's
+# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
+# generated .qhp file.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHG_LOCATION           =
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
+# generated, together with the HTML files, they form an Eclipse help plugin. To
+# install this plugin and make it available under the help contents menu in
+# Eclipse, the contents of the directory containing the HTML and XML files needs
+# to be copied into the plugins directory of eclipse. The name of the directory
+# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
+# After copying Eclipse needs to be restarted before the help appears.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_ECLIPSEHELP   = NO
+
+# A unique identifier for the Eclipse help plugin. When installing the plugin
+# the directory name containing the HTML and XML files should also have this
+# name. Each documentation set should have its own identifier.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
+
+ECLIPSE_DOC_ID         = org.doxygen.Project
+
+# If you want full control over the layout of the generated HTML pages it might
+# be necessary to disable the index and replace it with your own. The
+# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
+# of each HTML page. A value of NO enables the index and the value YES disables
+# it. Since the tabs in the index contain the same information as the navigation
+# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+DISABLE_INDEX          = NO
+
+# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
+# structure should be generated to display hierarchical information. If the tag
+# value is set to YES, a side panel will be generated containing a tree-like
+# index structure (just like the one that is generated for HTML Help). For this
+# to work a browser that supports JavaScript, DHTML, CSS and frames is required
+# (i.e. any modern browser). Windows users are probably better off using the
+# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can
+# further fine-tune the look of the index. As an example, the default style
+# sheet generated by doxygen has an example that shows how to put an image at
+# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
+# the same information as the tab index, you could consider setting
+# DISABLE_INDEX to YES when enabling this option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_TREEVIEW      = NO
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
+# doxygen will group on one line in the generated HTML documentation.
+#
+# Note that a value of 0 will completely suppress the enum values from appearing
+# in the overview section.
+# Minimum value: 0, maximum value: 20, default value: 4.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+ENUM_VALUES_PER_LINE   = 4
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
+# to set the initial width (in pixels) of the frame in which the tree is shown.
+# Minimum value: 0, maximum value: 1500, default value: 250.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+TREEVIEW_WIDTH         = 250
+
+# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to
+# external symbols imported via tag files in a separate window.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+EXT_LINKS_IN_WINDOW    = NO
+
+# Use this tag to change the font size of LaTeX formulas included as images in
+# the HTML documentation. When you change the font size after a successful
+# doxygen run you need to manually remove any form_*.png images from the HTML
+# output directory to force them to be regenerated.
+# Minimum value: 8, maximum value: 50, default value: 10.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_FONTSIZE       = 10
+
+# Use the FORMULA_TRANPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT    = YES
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
+# http://www.mathjax.org) which uses client side Javascript for the rendering
+# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX
+# installed or if you want to formulas look prettier in the HTML output. When
+# enabled you may also need to install MathJax separately and configure the path
+# to it using the MATHJAX_RELPATH option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+USE_MATHJAX            = NO
+
+# When MathJax is enabled you can set the default output format to be used for
+# the MathJax output. See the MathJax site (see:
+# http://docs.mathjax.org/en/latest/output.html) for more details.
+# Possible values are: HTML-CSS (which is slower, but has the best
+# compatibility), NativeMML (i.e. MathML) and SVG.
+# The default value is: HTML-CSS.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_FORMAT         = HTML-CSS
+
+# When MathJax is enabled you need to specify the location relative to the HTML
+# output directory using the MATHJAX_RELPATH option. The destination directory
+# should contain the MathJax.js script. For instance, if the mathjax directory
+# is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
+# Content Delivery Network so you can quickly see the result without installing
+# MathJax. However, it is strongly recommended to install a local copy of
+# MathJax from http://www.mathjax.org before deployment.
+# The default value is: http://cdn.mathjax.org/mathjax/latest.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_RELPATH        = http://cdn.mathjax.org/mathjax/latest
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
+# extension names that should be enabled during MathJax rendering. For example
+# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_EXTENSIONS     =
+
+# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
+# of code that will be used on startup of the MathJax code. See the MathJax site
+# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
+# example see the documentation.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_CODEFILE       =
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
+# the HTML output. The underlying search engine uses javascript and DHTML and
+# should work on any modern browser. Note that when using HTML help
+# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
+# there is already a search function so this one should typically be disabled.
+# For large projects the javascript based search engine can be slow, then
+# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
+# search using the keyboard; to jump to the search box use <access key> + S
+# (what the <access key> is depends on the OS and browser, but it is typically
+# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
+# key> to jump into the search results window, the results can be navigated
+# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
+# the search. The filter options can be selected when the cursor is inside the
+# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
+# to select a filter and <Enter> or <escape> to activate or cancel the filter
+# option.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+SEARCHENGINE           = YES
+
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a web server instead of a web client using Javascript. There
+# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
+# setting. When disabled, doxygen will generate a PHP script for searching and
+# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
+# and searching needs to be provided by external tools. See the section
+# "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SERVER_BASED_SEARCH    = NO
+
+# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
+# script for searching. Instead the search results are written to an XML file
+# which needs to be processed by an external indexer. Doxygen will invoke an
+# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
+# search results.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/).
+#
+# See the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH        = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will return the search results when EXTERNAL_SEARCH is enabled.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/). See the section "External Indexing and
+# Searching" for details.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHENGINE_URL       =
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
+# search data is written to a file for indexing by an external tool. With the
+# SEARCHDATA_FILE tag the name of this file can be specified.
+# The default file is: searchdata.xml.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHDATA_FILE        = searchdata.xml
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
+# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
+# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
+# projects and redirect the results back to the right project.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH_ID     =
+
+# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
+# projects other than the one defined by this configuration file, but that are
+# all added to the same external search index. Each project needs to have a
+# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
+# to a relative location where the documentation can be found. The format is:
+# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTRA_SEARCH_MAPPINGS  =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES, doxygen will generate LaTeX output.
+# The default value is: YES.
+
+GENERATE_LATEX         = NO
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_OUTPUT           = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked.
+#
+# Note that when enabling USE_PDFLATEX this option is only used for generating
+# bitmaps for formulas in the HTML output, but not in the Makefile that is
+# written to the output directory.
+# The default file is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_CMD_NAME         = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
+# index for LaTeX.
+# The default file is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+MAKEINDEX_CMD_NAME     = makeindex
+
+# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+COMPACT_LATEX          = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
+# printer.
+# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
+# 14 inches) and executive (7.25 x 10.5 inches).
+# The default value is: a4.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PAPER_TYPE             = a4
+
+# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
+# that should be included in the LaTeX output. The package can be specified just
+# by its name or with the correct syntax as to be used with the LaTeX
+# \usepackage command. To get the times font for instance you can specify :
+# EXTRA_PACKAGES=times or EXTRA_PACKAGES={times}
+# To use the option intlimits with the amsmath package you can specify:
+# EXTRA_PACKAGES=[intlimits]{amsmath}
+# If left blank no extra packages will be included.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+EXTRA_PACKAGES         =
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
+# generated LaTeX document. The header should contain everything until the first
+# chapter. If it is left blank doxygen will generate a standard header. See
+# section "Doxygen usage" for information on how to let doxygen write the
+# default header to a separate file.
+#
+# Note: Only use a user-defined header if you know what you are doing! The
+# following commands have a special meaning inside the header: $title,
+# $datetime, $date, $doxygenversion, $projectname, $projectnumber,
+# $projectbrief, $projectlogo. Doxygen will replace $title with the empty
+# string, for the replacement values of the other commands the user is referred
+# to HTML_HEADER.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HEADER           =
+
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
+# generated LaTeX document. The footer should contain everything after the last
+# chapter. If it is left blank doxygen will generate a standard footer. See
+# LATEX_HEADER for more information on how to generate a default footer and what
+# special commands can be used inside the footer.
+#
+# Note: Only use a user-defined footer if you know what you are doing!
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_FOOTER           =
+
+# The LATEX_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# LaTeX style sheets that are included after the standard style sheets created
+# by doxygen. Using this option one can overrule certain style aspects. Doxygen
+# will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list).
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_STYLESHEET =
+
+# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the LATEX_OUTPUT output
+# directory. Note that the files will be copied as-is; there are no commands or
+# markers available.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_FILES      =
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
+# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
+# contain links (just like the HTML output) instead of page references. This
+# makes the output suitable for online browsing using a PDF viewer.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PDF_HYPERLINKS         = YES
+
+# If the USE_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
+# the PDF file directly from the LaTeX files. Set this option to YES, to get a
+# higher quality PDF documentation.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+USE_PDFLATEX           = YES
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help. This option is also used
+# when generating formulas in HTML.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BATCHMODE        = NO
+
+# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
+# index chapters (such as File Index, Compound Index, etc.) in the output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HIDE_INDICES     = NO
+
+# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
+# code with syntax highlighting in the LaTeX output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_SOURCE_CODE      = NO
+
+# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
+# bibliography, e.g. plainnat, or ieeetr. See
+# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
+# The default value is: plain.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BIB_STYLE        = plain
+
+# If the LATEX_TIMESTAMP tag is set to YES then the footer of each generated
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_TIMESTAMP        = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES, doxygen will generate RTF output. The
+# RTF output is optimized for Word 97 and may not look too pretty with other RTF
+# readers/editors.
+# The default value is: NO.
+
+GENERATE_RTF           = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: rtf.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_OUTPUT             = rtf
+
+# If the COMPACT_RTF tag is set to YES, doxygen generates more compact RTF
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+COMPACT_RTF            = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
+# contain hyperlink fields. The RTF file will contain links (just like the HTML
+# output) instead of page references. This makes the output suitable for online
+# browsing using Word or some other Word compatible readers that support those
+# fields.
+#
+# Note: WordPad (write) and others do not support links.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_HYPERLINKS         = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's config
+# file, i.e. a series of assignments. You only have to provide replacements,
+# missing definitions are set to their default value.
+#
+# See also section "Doxygen usage" for information on how to generate the
+# default style sheet that doxygen normally uses.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_STYLESHEET_FILE    =
+
+# Set optional variables used in the generation of an RTF document. Syntax is
+# similar to doxygen's config file. A template extensions file can be generated
+# using doxygen -e rtf extensionFile.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_EXTENSIONS_FILE    =
+
+# If the RTF_SOURCE_CODE tag is set to YES then doxygen will include source code
+# with syntax highlighting in the RTF output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_SOURCE_CODE        = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES, doxygen will generate man pages for
+# classes and files.
+# The default value is: NO.
+
+GENERATE_MAN           = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it. A directory man3 will be created inside the directory specified by
+# MAN_OUTPUT.
+# The default directory is: man.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_OUTPUT             = man
+
+# The MAN_EXTENSION tag determines the extension that is added to the generated
+# man pages. In case the manual section does not start with a number, the number
+# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
+# optional.
+# The default value is: .3.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_EXTENSION          = .3
+
+# The MAN_SUBDIR tag determines the name of the directory created within
+# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
+# MAN_EXTENSION with the initial . removed.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_SUBDIR             =
+
+# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
+# will generate one additional man file for each entity documented in the real
+# man page(s). These additional files only source the real man page, but without
+# them the man command would be unable to find the correct page.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_LINKS              = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES, doxygen will generate an XML file that
+# captures the structure of the code including all documentation.
+# The default value is: NO.
+
+GENERATE_XML           = YES
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: xml.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_OUTPUT             = xml
+
+# If the XML_PROGRAMLISTING tag is set to YES, doxygen will dump the program
+# listings (including syntax highlighting and cross-referencing information) to
+# the XML output. Note that enabling this will significantly increase the size
+# of the XML output.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_PROGRAMLISTING     = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_DOCBOOK tag is set to YES, doxygen will generate Docbook files
+# that can be used to generate PDF.
+# The default value is: NO.
+
+GENERATE_DOCBOOK       = NO
+
+# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
+# front of it.
+# The default directory is: docbook.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_OUTPUT         = docbook
+
+# If the DOCBOOK_PROGRAMLISTING tag is set to YES, doxygen will include the
+# program listings (including syntax highlighting and cross-referencing
+# information) to the DOCBOOK output. Note that enabling this will significantly
+# increase the size of the DOCBOOK output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_PROGRAMLISTING = NO
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an
+# AutoGen Definitions (see http://autogen.sf.net) file that captures the
+# structure of the code including all documentation. Note that this feature is
+# still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_AUTOGEN_DEF   = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES, doxygen will generate a Perl module
+# file that captures the structure of the code including all documentation.
+#
+# Note that this feature is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_PERLMOD       = NO
+
+# If the PERLMOD_LATEX tag is set to YES, doxygen will generate the necessary
+# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
+# output from the Perl module output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_LATEX          = NO
+
+# If the PERLMOD_PRETTY tag is set to YES, the Perl module output will be nicely
+# formatted so it can be parsed by a human reader. This is useful if you want to
+# understand what is going on. On the other hand, if this tag is set to NO, the
+# size of the Perl module output will be much smaller and Perl will parse it
+# just the same.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_PRETTY         = YES
+
+# The names of the make variables in the generated doxyrules.make file are
+# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
+# so different doxyrules.make files included by the same Makefile don't
+# overwrite each other's variables.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_MAKEVAR_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES, doxygen will evaluate all
+# C-preprocessor directives found in the sources and include files.
+# The default value is: YES.
+
+ENABLE_PREPROCESSING   = YES
+
+# If the MACRO_EXPANSION tag is set to YES, doxygen will expand all macro names
+# in the source code. If set to NO, only conditional compilation will be
+# performed. Macro expansion can be done in a controlled way by setting
+# EXPAND_ONLY_PREDEF to YES.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+MACRO_EXPANSION        = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
+# the macro expansion is limited to the macros specified with the PREDEFINED and
+# EXPAND_AS_DEFINED tags.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_ONLY_PREDEF     = NO
+
+# If the SEARCH_INCLUDES tag is set to YES, the include files in the
+# INCLUDE_PATH will be searched if a #include is found.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SEARCH_INCLUDES        = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that
+# contain include files that are not input files but should be processed by the
+# preprocessor.
+# This tag requires that the tag SEARCH_INCLUDES is set to YES.
+
+INCLUDE_PATH           =
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
+# patterns (like *.h and *.hpp) to filter out the header-files in the
+# directories. If left blank, the patterns specified with FILE_PATTERNS will be
+# used.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+INCLUDE_FILE_PATTERNS  =
+
+# The PREDEFINED tag can be used to specify one or more macro names that are
+# defined before the preprocessor is started (similar to the -D option of e.g.
+# gcc). The argument of the tag is a list of macros of the form: name or
+# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
+# is assumed. To prevent a macro definition from being undefined via #undef or
+# recursively expanded use the := operator instead of the = operator.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+PREDEFINED             =
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
+# tag can be used to specify a list of macro names that should be expanded. The
+# macro definition that is found in the sources will be used. Use the PREDEFINED
+# tag if you want to use a different macro definition that overrules the
+# definition found in the source code.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_AS_DEFINED      =
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
+# remove all references to function-like macros that are alone on a line, have
+# an all uppercase name, and do not end with a semicolon. Such function macros
+# are typically used for boiler-plate code, and will confuse the parser if not
+# removed.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SKIP_FUNCTION_MACROS   = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tag files. For each tag
+# file the location of the external documentation should be added. The format of
+# a tag file without this location is as follows:
+# TAGFILES = file1 file2 ...
+# Adding location for the tag files is done as follows:
+# TAGFILES = file1=loc1 "file2 = loc2" ...
+# where loc1 and loc2 can be relative or absolute paths or URLs. See the
+# section "Linking to external documentation" for more information about the use
+# of tag files.
+# Note: Each tag file must have a unique name (where the name does NOT include
+# the path). If a tag file is not located in the directory in which doxygen is
+# run, you must also specify the path to the tagfile here.
+
+TAGFILES               =
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
+# tag file that is based on the input files it reads. See section "Linking to
+# external documentation" for more information about the usage of tag files.
+
+GENERATE_TAGFILE       =
+
+# If the ALLEXTERNALS tag is set to YES, all external class will be listed in
+# the class index. If set to NO, only the inherited external classes will be
+# listed.
+# The default value is: NO.
+
+ALLEXTERNALS           = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed
+# in the modules index. If set to NO, only the current project's groups will be
+# listed.
+# The default value is: YES.
+
+EXTERNAL_GROUPS        = YES
+
+# If the EXTERNAL_PAGES tag is set to YES, all external pages will be listed in
+# the related pages index. If set to NO, only the current project's pages will
+# be listed.
+# The default value is: YES.
+
+EXTERNAL_PAGES         = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script
+# interpreter (i.e. the result of 'which perl').
+# The default file (with absolute path) is: /usr/bin/perl.
+
+PERL_PATH              = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS         = YES
+
+# You can define message sequence charts within doxygen comments using the \msc
+# command. Doxygen will then run the mscgen tool (see:
+# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
+# documentation. The MSCGEN_PATH tag allows you to specify the directory where
+# the mscgen tool resides. If left empty the tool is assumed to be found in the
+# default search path.
+
+MSCGEN_PATH            =
+
+# You can include diagrams made with dia in doxygen documentation. Doxygen will
+# then run dia to produce the diagram and insert it in the documentation. The
+# DIA_PATH tag allows you to specify the directory where the dia binary resides.
+# If left empty dia is assumed to be found in the default search path.
+
+DIA_PATH               =
+
+# If set to YES the inheritance and collaboration graphs will hide inheritance
+# and usage relations if the target is undocumented or is not a class.
+# The default value is: YES.
+
+HIDE_UNDOC_RELATIONS   = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
+# available from the path. This tool is part of Graphviz (see:
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# Bell Labs. The other options in this section have no effect if this option is
+# set to NO
+# The default value is: YES.
+
+HAVE_DOT               = YES
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
+# to run in parallel. When set to 0 doxygen will base this on the number of
+# processors available in the system. You can set it explicitly to a value
+# larger than 0 to get control over the balance between CPU load and processing
+# speed.
+# Minimum value: 0, maximum value: 32, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_NUM_THREADS        = 0
+
+# When you want a differently looking font in the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTNAME           = Helvetica
+
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTSIZE           = 10
+
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTPATH           =
+
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CLASS_GRAPH            = YES
+
+# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
+# graph for each documented class showing the direct and indirect implementation
+# dependencies (inheritance, containment, and class references variables) of the
+# class with other documented classes.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+COLLABORATION_GRAPH    = YES
+
+# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
+# groups, showing the direct groups dependencies.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GROUP_GRAPHS           = YES
+
+# If the UML_LOOK tag is set to YES, doxygen will generate inheritance and
+# collaboration diagrams in a style similar to the OMG's Unified Modeling
+# Language.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LOOK               = NO
+
+# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
+# class node. If there are many fields or methods and many nodes the graph may
+# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
+# number of items for each type to make the size more manageable. Set this to 0
+# for no limit. Note that the threshold may be exceeded by 50% before the limit
+# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
+# but if the number exceeds 15, the total amount of fields shown is limited to
+# 10.
+# Minimum value: 0, maximum value: 100, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LIMIT_NUM_FIELDS   = 10
+
+# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
+# collaboration graphs will show the relations between templates and their
+# instances.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+TEMPLATE_RELATIONS     = NO
+
+# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
+# YES then doxygen will generate a graph for each documented file showing the
+# direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDE_GRAPH          = YES
+
+# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
+# set to YES then doxygen will generate a graph for each documented file showing
+# the direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDED_BY_GRAPH      = YES
+
+# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable call graphs for selected
+# functions only using the \callgraph command. Disabling a call graph can be
+# accomplished by means of the command \hidecallgraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALL_GRAPH             = NO
+
+# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable caller graphs for selected
+# functions only using the \callergraph command. Disabling a caller graph can be
+# accomplished by means of the command \hidecallergraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALLER_GRAPH           = NO
+
+# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
+# hierarchy of all classes instead of a textual one.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GRAPHICAL_HIERARCHY    = YES
+
+# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
+# dependencies a directory has on other directories in a graphical way. The
+# dependency relations are determined by the #include relations between the
+# files in the directories.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DIRECTORY_GRAPH        = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot. For an explanation of the image formats see the section
+# output formats in the documentation of the dot tool (Graphviz (see:
+# http://www.graphviz.org/)).
+# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
+# to make the SVG files visible in IE 9+ (other browsers do not have this
+# requirement).
+# Possible values are: png, png:cairo, png:cairo:cairo, png:cairo:gd, png:gd,
+# png:gd:gd, jpg, jpg:cairo, jpg:cairo:gd, jpg:gd, jpg:gd:gd, gif, gif:cairo,
+# gif:cairo:gd, gif:gd, gif:gd:gd, svg, png:gd, png:gd:gd, png:cairo,
+# png:cairo:gd, png:cairo:cairo, png:cairo:gdiplus, png:gdiplus and
+# png:gdiplus:gdiplus.
+# The default value is: png.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_IMAGE_FORMAT       = png
+
+# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
+# enable generation of interactive SVG images that allow zooming and panning.
+#
+# Note that this requires a modern browser other than Internet Explorer. Tested
+# and working are Firefox, Chrome, Safari, and Opera.
+# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
+# the SVG files visible. Older versions of IE do not have SVG support.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INTERACTIVE_SVG        = NO
+
+# The DOT_PATH tag can be used to specify the path where the dot tool can be
+# found. If left blank, it is assumed the dot tool can be found in the path.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_PATH               =
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that
+# contain dot files that are included in the documentation (see the \dotfile
+# command).
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOTFILE_DIRS           =
+
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
+
+MSCFILE_DIRS           =
+
+# The DIAFILE_DIRS tag can be used to specify one or more directories that
+# contain dia files that are included in the documentation (see the \diafile
+# command).
+
+DIAFILE_DIRS           =
+
+# When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the
+# path where java can find the plantuml.jar file. If left blank, it is assumed
+# PlantUML is not used or called during a preprocessing step. Doxygen will
+# generate a warning when it encounters a \startuml command in this case and
+# will not generate output for the diagram.
+
+PLANTUML_JAR_PATH      =
+
+# When using plantuml, the specified paths are searched for files specified by
+# the !include statement in a plantuml block.
+
+PLANTUML_INCLUDE_PATH  =
+
+# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
+# that will be shown in the graph. If the number of nodes in a graph becomes
+# larger than this value, doxygen will truncate the graph, which is visualized
+# by representing a node as a red box. Note that doxygen if the number of direct
+# children of the root node in a graph is already larger than
+# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
+# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
+# Minimum value: 0, maximum value: 10000, default value: 50.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_GRAPH_MAX_NODES    = 50
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
+# generated by dot. A depth value of 3 means that only nodes reachable from the
+# root by following a path via at most 3 edges will be shown. Nodes that lay
+# further from the root node will be omitted. Note that setting this option to 1
+# or 2 may greatly reduce the computation time needed for large code bases. Also
+# note that the size of a graph can be further restricted by
+# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
+# Minimum value: 0, maximum value: 1000, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+MAX_DOT_GRAPH_DEPTH    = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT        = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output
+# files in one run (i.e. multiple -o and -T options on the command line). This
+# makes dot run faster, but since only newer versions of dot (>1.8.10) support
+# this, this feature is disabled by default.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_MULTI_TARGETS      = NO
+
+# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
+# explaining the meaning of the various boxes and arrows in the dot generated
+# graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GENERATE_LEGEND        = YES
+
+# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate dot
+# files that are used to generate the various graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_CLEANUP            = YES
diff --git a/wrap/docs/parser/parse_doxygen_xml.py b/wrap/docs/parser/parse_doxygen_xml.py
new file mode 100644
index 000000000..54e9cbcbf
--- /dev/null
+++ b/wrap/docs/parser/parse_doxygen_xml.py
@@ -0,0 +1,78 @@
+import os
+import os.path as path
+import xml.etree.ElementTree as ET
+
+from docs.docs import ClassDoc, Doc, Docs, FreeDoc
+
+DOXYGEN_CONF = 'conf_doxygen.py'
+
+
+class ParseDoxygenXML():
+    def __init__(self, input_path, output_path):
+        """Parse the Doxygen generated XML files.
+
+        Arguments:
+        input_path -- path to the input folder or file
+        output_path -- path to the output folder
+        """
+        self.input_path = input_path
+        self.output_path = output_path
+
+    def run(self, quiet=True):
+        """Run the Doxygen XML parser.
+
+        Arguments:
+        quiet -- turn on/off the messages that are generated to standard
+                 output by Doxygen (default = True)
+
+        Returns:
+            A Docs template storing all the class and free documentation in the
+            file.
+        """
+        class_docs = {}
+        free_docs = {}
+
+        for root, dirs, files in os.walk(self.output_path):
+            for f in files:
+                if f.endswith('.xml'):
+                    file_path = path.join(root, f)
+                    tree = ET.parse(file_path)
+
+                    if tree.getroot().tag == 'compounddef':
+                        first_compound_def = tree.getroot()
+                    else:
+                        first_compound_def = tree.find(
+                            './/{}'.format('compounddef'))
+
+                    if first_compound_def is None:
+                        continue
+
+                    category = first_compound_def.get('kind')
+
+                    if category == 'class':
+                        class_docs[file_path] = ClassDoc(tree)
+
+        return Docs(class_docs, free_docs)
+
+
+# TODO: This should be done in the make file.
+def generate_xml(input_path, output_path, quiet=False):
+    '''Parse the file for documentation and output it as in an xml format'''
+    if not quiet:
+        print('--------------Generating XML--------------')
+
+    input_path = path.relpath(input_path, os.getcwd())
+    conf_path = path.relpath(path.join(path.dirname(__file__), DOXYGEN_CONF))
+    output_path = path.relpath(output_path)
+
+    if not path.isdir(output_path):
+        os.mkdir(output_path)
+
+    command = '( cat {conf_path} ; echo "INPUT={input_path}" ; echo "OUTPUT_DIRECTORY={output_path}" ; echo "EXTRACT_ALL={quiet}" ) | doxygen -'.format(
+        conf_path=conf_path,
+        input_path=input_path,
+        output_path=output_path,
+        quiet='YES' if quiet else 'NO'
+    )
+
+    os.system(command)
diff --git a/wrap/docs/parser/parse_xml.py b/wrap/docs/parser/parse_xml.py
new file mode 100644
index 000000000..813608ec8
--- /dev/null
+++ b/wrap/docs/parser/parse_xml.py
@@ -0,0 +1,122 @@
+import os
+
+from docs.doc_template import ClassDoc, Doc, Docs, FreeDoc
+import os.path as path
+import xml.etree.ElementTree as ET
+
+DOXYGEN_CONF = 'conf_doxygen.py'
+
+
+def parse(input_path, output_path, quiet=False, generate_xml_flag=True):
+    '''Parse the files for documentation and store it in templates.
+
+    Arguments:
+    input_path -- path to the input folder or file
+    output_path -- path to the output folder
+    quiet -- turn on/off the messages that are generated to standard output by
+             Doxygen (default = False)
+    generate_xml -- use Doxygen to generate xml (default = True)
+
+    Returns:
+        A Docs template storing all the documentation in the input.
+    '''
+    if generate_xml_flag:
+        generate_xml(input_path, output_path, quiet)
+
+    class_docs = {}
+    free_docs = {}
+
+    for root, dirs, files in os.walk(output_path):
+        for f in files:
+            if f.endswith('.xml'):
+                file_path = path.join(root, f)
+                doc = init_doc(file_path)
+
+                if isinstance(doc, ClassDoc):
+                    class_docs[file_path] = doc
+
+    return Docs(class_docs, free_docs)
+
+
+def generate_xml(input_path, output_path, quiet=False):
+    '''Parse the file for documentation and output it as in an xml format'''
+    if not quiet:
+        print('--------------Generating XML--------------')
+
+    input_path = path.relpath(input_path, os.getcwd())
+    conf_path = path.relpath(path.join(path.dirname(__file__), DOXYGEN_CONF))
+    output_path = path.relpath(output_path)
+
+    if not path.isdir(output_path):
+        os.mkdir(output_path)
+
+    command = '( cat {conf_path} ; echo "INPUT={input_path}" ; echo "OUTPUT_DIRECTORY={output_path}" ; echo "EXTRACT_ALL={quiet}" ) | doxygen -'.format(
+        conf_path=conf_path,
+        input_path=input_path,
+        output_path=output_path,
+        quiet='YES' if quiet else 'NO'
+    )
+
+    os.system(command)
+
+
+def categorize_xml(tree):
+    '''Determine the type of the object the xml tree represents.
+
+    Arguments:
+    tree -- an xml tree or path to xml string
+    '''
+    if isinstance(tree, str):
+        tree = ET.parse(tree)
+
+    first_compound_def = find_first_element_with_tag(tree, 'compounddef')
+
+    if first_compound_def is None:
+        return first_compound_def
+
+    return first_compound_def.get('kind')
+
+
+def init_doc(file_path):
+    '''Initialize documentation given its type.
+
+    Categorize the xml tree at file_path and initiliaze the corresponding doc
+    type with the xml tree and other relevant information.
+
+    Arguments:
+    file_path -- path to the xml tree
+
+    Returns:
+        An initialized Doc
+    '''
+    tree = ET.parse(file_path)
+
+    category = categorize_xml(tree)
+
+    if category == 'class':
+        return ClassDoc(tree)
+
+
+def find_first_element_with_tag(tree, tag):
+    if tree.getroot().tag == tag:
+        return tree.getroot()
+
+    return tree.find('.//{}'.format(tag))
+
+
+def find_all_elements(tree, name, tag):
+    return tree.find('.//{}'.format(tag))
+
+
+def find_method_element_text(method, name):
+    element = method.find(name)
+
+    if element is None:
+        return None
+
+    out = '' if element.text is None else element.text
+
+    for e in list(element):
+        out += e.text
+
+    return out
diff --git a/wrap/interface_parser.py b/wrap/interface_parser.py
new file mode 100644
index 000000000..73f9ebaa1
--- /dev/null
+++ b/wrap/interface_parser.py
@@ -0,0 +1,756 @@
+"""
+GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Parser to get the interface of a C++ source file
+Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar and Frank Dellaert
+"""
+import os
+import sys
+
+from pyparsing import (
+    alphas,
+    alphanums,
+    cppStyleComment,
+    delimitedList,
+    empty,
+    nums,
+    stringEnd,
+    CharsNotIn,
+    Forward,
+    Group,
+    Keyword,
+    Literal,
+    OneOrMore,
+    Optional,
+    Or,
+    ParseException,
+    ParserElement,
+    Suppress,
+    Word,
+    ZeroOrMore,
+)
+
+ParserElement.enablePackrat()
+
+IDENT = Word(alphas + '_', alphanums + '_') ^ Word(nums)
+POINTER, REF = map(Literal, "*&")
+LPAREN, RPAREN, LBRACE, RBRACE, COLON, SEMI_COLON = map(Suppress, "(){}:;")
+LOPBRACK, ROPBRACK, COMMA, EQUAL = map(Suppress, "<>,=")
+CONST, VIRTUAL, CLASS, STATIC, PAIR, TEMPLATE, TYPEDEF, INCLUDE = map(
+    Keyword,
+    [
+        "const",
+        "virtual",
+        "class",
+        "static",
+        "pair",
+        "template",
+        "typedef",
+        "#include",
+    ],
+)
+NAMESPACE = Keyword("namespace")
+BASIS_TYPES = map(
+    Keyword,
+    [
+        "void",
+        "bool",
+        "unsigned char",
+        "char",
+        "int",
+        "size_t",
+        "double",
+        "float",
+        "string",
+    ],
+)
+
+
+class Typename(object):
+    """
+    Type's name with full namespaces.
+    """
+
+    namespaces_name_rule = delimitedList(IDENT, "::")
+    instantiation_name_rule = delimitedList(IDENT, "::")
+    rule = Forward()
+
+    rule << (
+        namespaces_name_rule("namespaces_name")
+        + Optional(
+            (LOPBRACK + delimitedList(rule, ",")("instantiations") + ROPBRACK)
+        )
+    ).setParseAction(lambda t: Typename(t.namespaces_name, t.instantiations))
+
+    def __init__(self, namespaces_name, instantiations=[]):
+        self.namespaces = namespaces_name[:-1]
+        self.name = namespaces_name[-1]
+
+        if instantiations:
+            if not isinstance(instantiations, list):
+                self.instantiations = instantiations.asList()
+            else:
+                self.instantiations = instantiations
+        else:
+            self.instantiations = []
+        if self.name in ["Matrix", "Vector"] and not self.namespaces:
+            self.namespaces = ["gtsam"]
+
+    @staticmethod
+    def from_parse_result(parse_result):
+        return parse_result[0]
+
+    def __repr__(self):
+        return self.to_cpp()
+
+    def instantiated_name(self):
+        res = self.name
+        for instantiation in self.instantiations:
+            res += instantiation.instantiated_name()
+        return res
+
+    def to_cpp(self):
+        idx = 1 if self.namespaces and not self.namespaces[0] else 0
+        if self.instantiations:
+            cpp_name = self.name + "<{}>".format(
+                ", ".join([inst.to_cpp() for inst in self.instantiations])
+            )
+        else:
+            cpp_name = self.name
+        return '{}{}{}'.format(
+            "::".join(self.namespaces[idx:]),
+            "::" if self.namespaces[idx:] else "",
+            cpp_name,
+        )
+
+    def __eq__(self, other):
+        if isinstance(other, Typename):
+            return str(self) == str(other)
+        else:
+            return NotImplemented
+
+    def __ne__(self, other):
+        res = self.__eq__(other)
+        if res is NotImplemented:
+            return res
+        return not res
+
+
+class Type(object):
+    class _QualifiedType(object):
+        """
+        Type with qualifiers.
+        """
+
+        rule = (
+            Optional(CONST("is_const"))
+            + Typename.rule("typename")
+            + Optional(POINTER("is_ptr") | REF("is_ref"))
+        ).setParseAction(
+            lambda t: Type._QualifiedType(
+                Typename.from_parse_result(t.typename),
+                t.is_const,
+                t.is_ptr,
+                t.is_ref,
+            )
+        )
+
+        def __init__(self, typename, is_const, is_ptr, is_ref):
+            self.typename = typename
+            self.is_const = is_const
+            self.is_ptr = is_ptr
+            self.is_ref = is_ref
+
+    class _BasisType(object):
+        """
+        Basis types don't have qualifiers and only allow copy-by-value.
+        """
+
+        rule = Or(BASIS_TYPES).setParseAction(lambda t: Typename(t))
+
+    rule = (
+        _BasisType.rule("basis") | _QualifiedType.rule("qualified")  # BR
+    ).setParseAction(lambda t: Type.from_parse_result(t))
+
+    def __init__(self, typename, is_const, is_ptr, is_ref, is_basis):
+        self.typename = typename
+        self.is_const = is_const
+        self.is_ptr = is_ptr
+        self.is_ref = is_ref
+        self.is_basis = is_basis
+
+    @staticmethod
+    def from_parse_result(t):
+        if t.basis:
+            return Type(
+                typename=t.basis,
+                is_const='',
+                is_ptr='',
+                is_ref='',
+                is_basis=True,
+            )
+        elif t.qualified:
+            return Type(
+                typename=t.qualified.typename,
+                is_const=t.qualified.is_const,
+                is_ptr=t.qualified.is_ptr,
+                is_ref=t.qualified.is_ref,
+                is_basis=False,
+            )
+        else:
+            raise ValueError("Parse result is not a Type?")
+
+    def __repr__(self):
+        return '{} {}{}{}'.format(
+            self.typename, self.is_const, self.is_ptr, self.is_ref
+        )
+
+    def to_cpp(self, use_boost):
+        """
+        Treat all pointers as "const shared_ptr<T>&"
+        Treat Matrix and Vector as "const Matrix&" and "const Vector&" resp.
+        """
+        shared_ptr_ns = "boost" if use_boost else "std"
+        return (
+            "{const} {shared_ptr}{typename}"
+            "{shared_ptr_ropbracket}{ref}".format(
+                const="const"
+                if self.is_const
+                or self.is_ptr
+                or self.typename.name in ["Matrix", "Vector"]
+                else "",
+                typename=self.typename.to_cpp(),
+                shared_ptr="{}::shared_ptr<".format(shared_ptr_ns)
+                if self.is_ptr
+                else "",
+                shared_ptr_ropbracket=">" if self.is_ptr else "",
+                ref="&"
+                if self.is_ref
+                or self.is_ptr
+                or self.typename.name in ["Matrix", "Vector"]
+                else "",
+            )
+        )
+
+
+class Argument(object):
+    rule = (Type.rule("ctype") + IDENT("name")).setParseAction(
+        lambda t: Argument(t.ctype, t.name)
+    )
+
+    def __init__(self, ctype, name):
+        self.ctype = ctype
+        self.name = name
+
+    def __repr__(self):
+        return '{} {}'.format(self.ctype.__repr__(), self.name)
+
+
+class ArgumentList(object):
+    rule = Optional(delimitedList(Argument.rule)("args_list")).setParseAction(
+        lambda t: ArgumentList.from_parse_result(t.args_list)
+    )
+
+    def __init__(self, args_list):
+        self.args_list = args_list
+        for arg in args_list:
+            arg.parent = self
+
+    @staticmethod
+    def from_parse_result(parse_result):
+        if parse_result:
+            return ArgumentList(parse_result.asList())
+        else:
+            return ArgumentList([])
+
+    def __repr__(self):
+        return self.args_list.__repr__()
+
+    def args_names(self):
+        return [arg.name for arg in self.args_list]
+
+    def to_cpp(self, use_boost):
+        return [arg.ctype.to_cpp(use_boost) for arg in self.args_list]
+
+
+class ReturnType(object):
+    _pair = (
+        PAIR.suppress()
+        + LOPBRACK
+        + Type.rule("type1")
+        + COMMA
+        + Type.rule("type2")
+        + ROPBRACK
+    )
+    rule = (_pair ^ Type.rule("type1")).setParseAction(  # BR
+        lambda t: ReturnType(t.type1, t.type2)
+    )
+
+    def __init__(self, type1, type2):
+        self.type1 = type1
+        self.type2 = type2
+
+    def is_void(self):
+        return self.type1.typename.name == "void" and not self.type2
+
+    def __repr__(self):
+        return "{}{}".format(
+            self.type1, (', ' + self.type2.__repr__()) if self.type2 else ''
+        )
+
+    def to_cpp(self):
+        if self.type2:
+            return "std::pair<{type1},{type2}>".format(
+                type1=self.type1.to_cpp(), type2=self.type2.to_cpp()
+            )
+        else:
+            return self.type1.to_cpp()
+
+
+class Template(object):
+    class TypenameAndInstantiations(object):
+        rule = (
+            IDENT("typename")
+            + Optional(
+                EQUAL
+                + LBRACE
+                + ((delimitedList(Typename.rule)("instantiations")))
+                + RBRACE
+            )
+        ).setParseAction(
+            lambda t: Template.TypenameAndInstantiations(
+                t.typename, t.instantiations
+            )
+        )
+
+        def __init__(self, typename, instantiations):
+            self.typename = typename
+
+            if instantiations:
+                self.instantiations = instantiations.asList()
+            else:
+                self.instantiations = []
+
+    rule = (  # BR
+        TEMPLATE
+        + LOPBRACK
+        + delimitedList(TypenameAndInstantiations.rule)(
+            "typename_and_instantiations_list"
+        )
+        + ROPBRACK  # BR
+    ).setParseAction(
+        lambda t: Template(t.typename_and_instantiations_list.asList())
+    )
+
+    def __init__(self, typename_and_instantiations_list):
+        ti_list = typename_and_instantiations_list
+        self.typenames = [ti.typename for ti in ti_list]
+        self.instantiations = [ti.instantiations for ti in ti_list]
+
+
+class Method(object):
+    rule = (
+        Optional(Template.rule("template"))
+        + ReturnType.rule("return_type")
+        + IDENT("name")
+        + LPAREN
+        + ArgumentList.rule("args_list")
+        + RPAREN
+        + Optional(CONST("is_const"))
+        + SEMI_COLON  # BR
+    ).setParseAction(
+        lambda t: Method(
+            t.template, t.name, t.return_type, t.args_list, t.is_const
+        )
+    )
+
+    def __init__(self, template, name, return_type, args, is_const, parent=''):
+        self.template = template
+        self.name = name
+        self.return_type = return_type
+        self.args = args
+        self.is_const = is_const
+
+        self.parent = parent
+
+    def __repr__(self):
+        return "Method: {} {} {}({}){}".format(
+            self.template,
+            self.return_type,
+            self.name,
+            self.args,
+            self.is_const,
+        )
+
+
+class StaticMethod(object):
+    rule = (
+        STATIC
+        + ReturnType.rule("return_type")
+        + IDENT("name")
+        + LPAREN
+        + ArgumentList.rule("args_list")
+        + RPAREN
+        + SEMI_COLON  # BR
+    ).setParseAction(
+        lambda t: StaticMethod(t.name, t.return_type, t.args_list)
+    )
+
+    def __init__(self, name, return_type, args, parent=''):
+        self.name = name
+        self.return_type = return_type
+        self.args = args
+
+        self.parent = parent
+
+    def __repr__(self):
+        return "static {} {}{}".format(self.return_type, self.name, self.args)
+
+    def to_cpp(self):
+        return self.name
+
+
+class Constructor(object):
+    rule = (
+        IDENT("name")
+        + LPAREN
+        + ArgumentList.rule("args_list")
+        + RPAREN
+        + SEMI_COLON  # BR
+    ).setParseAction(lambda t: Constructor(t.name, t.args_list))
+
+    def __init__(self, name, args, parent=''):
+        self.name = name
+        self.args = args
+
+        self.parent = parent
+
+    def __repr__(self):
+        return "Constructor: {}".format(self.name)
+
+
+class Property(object):
+    rule = (Type.rule("ctype") + IDENT("name") + SEMI_COLON).setParseAction(
+        lambda t: Property(t.ctype, t.name)
+    )
+
+    def __init__(self, ctype, name, parent=''):
+        self.ctype = ctype
+        self.name = name
+        # Check type constraints: no pointer, no ref.
+        if self.ctype.is_ptr or self.ctype.is_ref:
+            raise ValueError("Can't deal with pointer/ref class properties.")
+
+        self.parent = parent
+
+    def __repr__(self):
+        return '{} {}'.format(self.ctype.__repr__(), self.name)
+
+
+def collect_namespaces(obj):
+    namespaces = []
+    ancestor = obj.parent
+    while ancestor and ancestor.name:
+        namespaces = [ancestor.name] + namespaces
+        ancestor = ancestor.parent
+    return [''] + namespaces
+
+
+class Class(object):
+    class MethodsAndProperties(object):
+        rule = ZeroOrMore(
+            Constructor.rule ^ StaticMethod.rule ^ Method.rule ^ Property.rule
+        ).setParseAction(lambda t: Class.MethodsAndProperties(t.asList()))
+
+        def __init__(self, methods_props):
+            self.ctors = []
+            self.methods = []
+            self.static_methods = []
+            self.properties = []
+            for m in methods_props:
+                if isinstance(m, Constructor):
+                    self.ctors.append(m)
+                elif isinstance(m, Method):
+                    self.methods.append(m)
+                elif isinstance(m, StaticMethod):
+                    self.static_methods.append(m)
+                elif isinstance(m, Property):
+                    self.properties.append(m)
+
+    _parent = COLON + Typename.rule("parent_class")
+    rule = (
+        Optional(Template.rule("template"))
+        + Optional(VIRTUAL("is_virtual"))
+        + CLASS
+        + IDENT("name")
+        + Optional(_parent)
+        + LBRACE
+        + MethodsAndProperties.rule("methods_props")
+        + RBRACE
+        + SEMI_COLON  # BR
+    ).setParseAction(
+        lambda t: Class(
+            t.template,
+            t.is_virtual,
+            t.name,
+            t.parent_class,
+            t.methods_props.ctors,
+            t.methods_props.methods,
+            t.methods_props.static_methods,
+            t.methods_props.properties,
+        )
+    )
+
+    def __init__(
+        self,
+        template,
+        is_virtual,
+        name,
+        parent_class,
+        ctors,
+        methods,
+        static_methods,
+        properties,
+        parent='',
+    ):
+        self.template = template
+        self.is_virtual = is_virtual
+        self.name = name
+        if parent_class:
+            self.parent_class = Typename.from_parse_result(parent_class)
+        else:
+            self.parent_class = ''
+
+        self.ctors = ctors
+        self.methods = methods
+        self.static_methods = static_methods
+        self.properties = properties
+        self.parent = parent
+        # Make sure ctors' names and class name are the same.
+        for ctor in self.ctors:
+            if ctor.name != self.name:
+                raise ValueError(
+                    "Error in constructor name! {} != {}".format(
+                        ctor.name, self.name
+                    )
+                )
+
+        for ctor in self.ctors:
+            ctor.parent = self
+        for method in self.methods:
+            method.parent = self
+        for static_method in self.static_methods:
+            static_method.parent = self
+        for _property in self.properties:
+            _property.parent = self
+
+    def namespaces(self):
+        return collect_namespaces(self)
+
+
+class TypedefTemplateInstantiation(object):
+    rule = (
+        TYPEDEF + Typename.rule("typename") + IDENT("new_name") + SEMI_COLON
+    ).setParseAction(
+        lambda t: TypedefTemplateInstantiation(
+            Typename.from_parse_result(t.typename), t.new_name
+        )
+    )
+
+    def __init__(self, typename, new_name, parent=''):
+        self.typename = typename
+        self.new_name = new_name
+        self.parent = parent
+
+
+class Include(object):
+    rule = (
+        INCLUDE + LOPBRACK + CharsNotIn('>')("header") + ROPBRACK
+    ).setParseAction(lambda t: Include(t.header))
+
+    def __init__(self, header, parent=''):
+        self.header = header
+        self.parent = parent
+
+    def __repr__(self):
+        return "#include <{}>".format(self.header)
+
+
+class ForwardDeclaration(object):
+    rule = (
+        Optional(VIRTUAL("is_virtual"))
+        + CLASS
+        + Typename.rule("name")
+        + Optional(COLON + Typename.rule("parent_type"))
+        + SEMI_COLON
+    ).setParseAction(
+        lambda t: ForwardDeclaration(t.is_virtual, t.name, t.parent_type)
+    )
+
+    def __init__(self, is_virtual, name, parent_type, parent=''):
+        self.is_virtual = is_virtual
+        self.name = name
+        if parent_type:
+            self.parent_type = Typename.from_parse_result(parent_type)
+        else:
+            self.parent_type = ''
+        self.parent = parent
+
+    def __repr__(self):
+        return "ForwardDeclaration: {} {}({})".format(
+            self.is_virtual, self.name, self.parent
+        )
+
+
+class GlobalFunction(object):
+    rule = (
+        ReturnType.rule("return_type")
+        + IDENT("name")
+        + LPAREN
+        + ArgumentList.rule("args_list")
+        + RPAREN
+        + SEMI_COLON
+    ).setParseAction(
+        lambda t: GlobalFunction(t.name, t.return_type, t.args_list)
+    )
+
+    def __init__(self, name, return_type, args_list, parent=''):
+        self.name = name
+        self.return_type = return_type
+        self.args = args_list
+        self.is_const = None
+
+        self.parent = parent
+        self.return_type.parent = self
+        self.args.parent = self
+
+    def __repr__(self):
+        return "GlobalFunction:  {}{}({})".format(
+            self.return_type, self.name, self.args
+        )
+
+    def to_cpp(self):
+        return self.name
+
+
+def find_sub_namespace(namespace, str_namespaces):
+    if not str_namespaces:
+        return [namespace]
+
+    sub_namespaces = (
+        ns for ns in namespace.content if isinstance(ns, Namespace)
+    )
+
+    found_namespaces = [
+        ns for ns in sub_namespaces if ns.name == str_namespaces[0]
+    ]
+    if not found_namespaces:
+        return None
+
+    res = []
+    for found_namespace in found_namespaces:
+        ns = find_sub_namespace(found_namespace, str_namespaces[1:])
+        if ns:
+            res += ns
+    return res
+
+
+class Namespace(object):
+    rule = Forward()
+    rule << (
+        NAMESPACE
+        + IDENT("name")
+        + LBRACE
+        + ZeroOrMore(  # BR
+            ForwardDeclaration.rule
+            ^ Include.rule
+            ^ Class.rule
+            ^ TypedefTemplateInstantiation.rule
+            ^ GlobalFunction.rule
+            ^ rule
+        )(
+            "content"
+        )  # BR
+        + RBRACE
+    ).setParseAction(lambda t: Namespace.from_parse_result(t))
+
+    def __init__(self, name, content, parent=''):
+        self.name = name
+        self.content = content
+        self.parent = parent
+        for child in self.content:
+            child.parent = self
+
+    @staticmethod
+    def from_parse_result(t):
+        if t.content:
+            content = t.content.asList()
+        else:
+            content = []
+        return Namespace(t.name, content)
+
+    def find_class(self, typename):
+        """
+        Find the Class object given its typename.
+        We have to traverse the tree of namespaces.
+        """
+        found_namespaces = find_sub_namespace(self, typename.namespaces)
+        res = []
+        for namespace in found_namespaces:
+            classes = (c for c in namespace.content if isinstance(c, Class))
+            res += [c for c in classes if c.name == typename.name]
+        if not res:
+            raise ValueError(
+                "Cannot find class {} in module!".format(typename.name)
+            )
+        elif len(res) > 1:
+            raise ValueError(
+                "Found more than one classes {} in module!".format(
+                    typename.name
+                )
+            )
+        else:
+            return res[0]
+
+    def top_level(self):
+        if self.name == '' or self.parent == '':
+            return self
+        else:
+            return self.parent.top_level()
+
+    def __repr__(self):
+        return "Namespace: {}\n\t{}".format(self.name, self.content)
+
+    def full_namespaces(self):
+        ancestors = collect_namespaces(self)
+        if self.name:
+            ancestors.append(self.name)
+        return ancestors
+
+
+class Module(object):
+    """
+    Module is just a global namespace.
+    """
+
+    rule = (
+        ZeroOrMore(
+            ForwardDeclaration.rule
+            ^ Include.rule
+            ^ Class.rule
+            ^ TypedefTemplateInstantiation.rule
+            ^ GlobalFunction.rule
+            ^ Namespace.rule
+        ).setParseAction(lambda t: Namespace('', t.asList()))
+        + stringEnd
+    )
+
+    rule.ignore(cppStyleComment)
+
+    @staticmethod
+    def parseString(str):
+        return Module.rule.parseString(str)[0]
diff --git a/wrap/matlab.h b/wrap/matlab.h
index 1af00bd00..4f3bfe96e 100644
--- a/wrap/matlab.h
+++ b/wrap/matlab.h
@@ -24,9 +24,13 @@
 
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
 
 using gtsam::Vector;
 using gtsam::Matrix;
+using gtsam::Point2;
+using gtsam::Point3;
 
 extern "C" {
 #include <mex.h>
@@ -195,6 +199,18 @@ mxArray* wrap<gtsam::Vector >(const gtsam::Vector& v) {
   return wrap_Vector(v);
 }
 
+// specialization to Eigen vector -> double vector
+template<>
+mxArray* wrap<gtsam::Point2 >(const gtsam::Point2& v) {
+  return wrap_Vector(v);
+}
+
+// specialization to Eigen vector -> double vector
+template<>
+mxArray* wrap<gtsam::Point3 >(const gtsam::Point3& v) {
+  return wrap_Vector(v);
+}
+
 // wrap a const Eigen MATRIX into a double matrix
 mxArray* wrap_Matrix(const gtsam::Matrix& A) {
   int m = A.rows(), n = A.cols();
@@ -312,6 +328,41 @@ gtsam::Vector unwrap< gtsam::Vector >(const mxArray* array) {
   return v;
 }
 
+// specialization to Point2
+template<>
+gtsam::Point2 unwrap< gtsam::Point2 >(const mxArray* array) {
+  int m = mxGetM(array), n = mxGetN(array);
+  if (mxIsDouble(array)==false || n!=1) error("unwrap<vector>: not a vector");
+#ifdef DEBUG_WRAP
+  mexPrintf("unwrap< gtsam::Vector > called with %dx%d argument\n", m,n);
+#endif
+  double* data = (double*)mxGetData(array);
+  gtsam::Vector v(m);
+  for (int i=0;i<m;i++,data++) v(i) = *data;
+#ifdef DEBUG_WRAP
+  gtsam::print(v);
+#endif
+  return v;
+}
+
+// specialization to Point3
+template<>
+gtsam::Point3 unwrap< gtsam::Point3 >(const mxArray* array) {
+  int m = mxGetM(array), n = mxGetN(array);
+  if (mxIsDouble(array)==false || n!=1) error("unwrap<vector>: not a vector");
+#ifdef DEBUG_WRAP
+  mexPrintf("unwrap< gtsam::Vector > called with %dx%d argument\n", m,n);
+#endif
+  double* data = (double*)mxGetData(array);
+  gtsam::Vector v(m);
+  for (int i=0;i<m;i++,data++) v(i) = *data;
+#ifdef DEBUG_WRAP
+  gtsam::print(v);
+#endif
+  return v;
+}
+
+
 // specialization to Eigen matrix
 template<>
 gtsam::Matrix unwrap< gtsam::Matrix >(const mxArray* array) {
diff --git a/wrap/matlab_wrapper.py b/wrap/matlab_wrapper.py
new file mode 100755
index 000000000..23f56c8b0
--- /dev/null
+++ b/wrap/matlab_wrapper.py
@@ -0,0 +1,1770 @@
+import os
+import argparse
+import textwrap
+
+import interface_parser as parser
+import template_instantiator as instantiator
+
+from functools import reduce
+from functools import partial
+
+
+class MatlabWrapper(object):
+    """ Wrap the given C++ code into Matlab.
+
+    Attributes
+        module: the C++ module being wrapped
+        module_name: name of the C++ module being wrapped
+        top_module_namespace: C++ namespace for the top module (default '')
+        ignore_classes: A list of classes to ignore (default [])
+    """
+    """Map the data type to its Matlab class.
+    Found in Argument.cpp in old wrapper
+    """
+    data_type = {
+        'string': 'char',
+        'char': 'char',
+        'unsigned char': 'unsigned char',
+        'Vector': 'double',
+        'Matrix': 'double',
+        'int': 'numeric',
+        'size_t': 'numeric',
+        'bool': 'logical'
+    }
+    """Map the data type into the type used in Matlab methods.
+    Found in matlab.h in old wrapper
+    """
+    data_type_param = {
+        'string': 'char',
+        'char': 'char',
+        'unsigned char': 'unsigned char',
+        'size_t': 'int',
+        'int': 'int',
+        'double': 'double',
+        'Point2': 'double',
+        'Point3': 'double',
+        'Vector': 'double',
+        'Matrix': 'double',
+        'bool': 'bool'
+    }
+    """Methods that should not be wrapped directly"""
+    whitelist = ['serializable', 'serialize']
+    """Datatypes that do not need to be checked in methods"""
+    not_check_type = []
+    """Data types that are primitive types"""
+    not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t']
+    """Ignore the namespace for these datatypes"""
+    ignore_namespace = ['Matrix', 'Vector', 'Point2', 'Point3']
+    """The amount of times the wrapper has created a call to
+    geometry_wrapper
+    """
+    wrapper_id = 0
+    """Map each wrapper id to what its collector function namespace, class,
+    type, and string format"""
+    wrapper_map = {}
+    """Set of all the includes in the namespace"""
+    includes = {}
+    """Set of all classes in the namespace"""
+    classes = []
+    classes_elems = {}
+    """Id for ordering global functions in the wrapper"""
+    global_function_id = 0
+    """Files and their content"""
+    content = []
+
+    def __init__(self, module, module_name, top_module_namespace='', ignore_classes=[]):
+        self.module = module
+        self.module_name = module_name
+        self.top_module_namespace = top_module_namespace
+        self.ignore_classes = ignore_classes
+        self.verbose = False
+
+    def _debug(self, message):
+        if not self.verbose:
+            return
+        import sys
+        print(message, file=sys.stderr)
+
+    def _add_include(self, include):
+        self.includes[include] = 0
+
+    def _add_class(self, instantiated_class):
+        if self.classes_elems.get(instantiated_class) is None:
+            self.classes_elems[instantiated_class] = 0
+            self.classes.append(instantiated_class)
+
+    def _update_wrapper_id(self, collector_function=None, id_diff=0):
+        """Get and define wrapper ids.
+
+        Generates the map of id -> collector function.
+
+        Args:
+            collector_function: tuple storing info about the wrapper function
+                (namespace, class instance, function type, function name,
+                extra)
+            id_diff: constant to add to the id in the map
+
+        Returns:
+            the current wrapper id
+        """
+        if collector_function is not None:
+            is_instantiated_class = isinstance(collector_function[1], instantiator.InstantiatedClass)
+
+            if is_instantiated_class:
+                function_name = collector_function[0] + \
+                                collector_function[1].name + '_' + collector_function[2]
+            else:
+                function_name = collector_function[1].name
+
+            self.wrapper_map[self.wrapper_id] = (collector_function[0], collector_function[1], collector_function[2],
+                                                 function_name + '_' + str(self.wrapper_id + id_diff),
+                                                 collector_function[3])
+
+        self.wrapper_id += 1
+
+        return self.wrapper_id - 1
+
+    def _qualified_name(self, names):
+        return 'handle' if names == '' else names
+
+    def _insert_spaces(self, x, y):
+        """Insert spaces at the beginning of each line
+
+        Args:
+            x: the statement currently generated
+            y: the addition to add to the statement
+        """
+        return x + '\n' + ('' if y == '' else '  ') + y
+
+    def _is_ptr(self, arg_type):
+        """Determine if the interface_parser.Type should be treated as a
+        pointer in the wrapper.
+        """
+        return arg_type.is_ptr or (arg_type.typename.name not in self.not_ptr_type
+                                   and arg_type.typename.name not in self.ignore_namespace
+                                   and arg_type.typename.name != 'string')
+
+    def _is_ref(self, arg_type):
+        """Determine if the interface_parser.Type should be treated as a
+        reference in the wrapper.
+        """
+        return arg_type.typename.name not in self.ignore_namespace and \
+               arg_type.typename.name not in self.not_ptr_type and \
+               arg_type.is_ref
+
+    def _group_methods(self, methods):
+        """Group overloaded methods together"""
+        method_map = {}
+        method_out = []
+
+        for method in methods:
+            method_index = method_map.get(method.name)
+
+            if method_index is None:
+                method_map[method.name] = len(method_out)
+                method_out.append([method])
+            else:
+                self._debug("[_group_methods] Merging {} with {}".format(method_index, method.name))
+                method_out[method_index].append(method)
+
+        return method_out
+
+    def _clean_class_name(self, instantiated_class):
+        """Reformatted the C++ class name to fit Matlab defined naming
+        standards
+        """
+        if len(instantiated_class.ctors) != 0:
+            return instantiated_class.ctors[0].name
+
+        return instantiated_class.name
+
+    @classmethod
+    def _format_type_name(cls, type_name, separator='::', include_namespace=True, constructor=False, method=False):
+        """
+        Args:
+            type_name: an interface_parser.Typename to reformat
+            separator: the statement to add between namespaces and typename
+            include_namespace: whether to include namespaces when reformatting
+            constructor: if the typename will be in a constructor
+            method: if the typename will be in a method
+
+        Raises:
+            constructor and method cannot both be true
+        """
+        if constructor and method:
+            raise Exception('Constructor and method parameters cannot both be True')
+
+        formatted_type_name = ''
+        name = type_name.name
+
+        if include_namespace:
+            for namespace in type_name.namespaces:
+                if name not in cls.ignore_namespace and namespace != '':
+                    formatted_type_name += namespace + separator
+
+            #self._debug("formatted_ns: {}, ns: {}".format(formatted_type_name, type_name.namespaces))
+        if constructor:
+            formatted_type_name += cls.data_type.get(name) or name
+        elif method:
+            formatted_type_name += cls.data_type_param.get(name) or name
+        else:
+            formatted_type_name += name
+
+        if len(type_name.instantiations) == 1:
+            if separator == "::":  # C++
+                formatted_type_name += '<{}>'.format(cls._format_type_name(type_name.instantiations[0],
+                                                                            include_namespace=include_namespace,
+                                                                            constructor=constructor, method=method))
+            else:
+                formatted_type_name += '{}'.format(cls._format_type_name(
+                    type_name.instantiations[0],
+                    separator=separator,
+                    include_namespace=False,
+                    constructor=constructor, method=method
+                ))
+        return formatted_type_name
+
+    @classmethod
+    def _format_return_type(cls, return_type, include_namespace=False, separator="::"):
+        """Format return_type.
+
+        Args:
+            return_type: an interface_parser.ReturnType to reformat
+            include_namespace: whether to include namespaces when reformatting
+        """
+        return_wrap = ''
+
+        if cls._return_count(return_type) == 1:
+            return_wrap = cls._format_type_name(
+                return_type.type1.typename,
+                separator=separator,
+                include_namespace=include_namespace
+            )
+        else:
+            return_wrap = 'pair< {type1}, {type2} >'.format(
+                type1=cls._format_type_name(
+                    return_type.type1.typename, separator=separator, include_namespace=include_namespace
+                ),
+                type2=cls._format_type_name(
+                    return_type.type2.typename, separator=separator, include_namespace=include_namespace
+                ))
+
+        return return_wrap
+
+    def _format_class_name(self, instantiated_class, separator=''):
+        """Format a template_instantiator.InstantiatedClass name."""
+        if instantiated_class.parent == '':
+            parent_full_ns = ['']
+        else:
+            parent_full_ns = instantiated_class.parent.full_namespaces()
+        # class_name = instantiated_class.parent.name
+        #
+        # if class_name != '':
+        #     class_name += separator
+        #
+        # class_name += instantiated_class.name
+        parentname = "".join([separator + x for x in parent_full_ns]) + separator
+
+        class_name = parentname[2 * len(separator):]
+
+        class_name += instantiated_class.name
+
+        return class_name
+
+    def _format_static_method(self, static_method, separator=''):
+        """Example:
+
+                gtsamPoint3.staticFunction
+        """
+        method = ''
+
+        if isinstance(static_method, parser.StaticMethod):
+            method += "".join([separator + x for x in static_method.parent.namespaces()]) + \
+                      separator + static_method.parent.name + separator
+
+        return method[2 * len(separator):]
+
+    def _format_instance_method(self, instance_method, separator=''):
+        """Example:
+
+                gtsamPoint3.staticFunction
+        """
+        method = ''
+
+        if isinstance(instance_method, instantiator.InstantiatedMethod):
+            method += "".join([separator + x for x in instance_method.parent.parent.full_namespaces()]) + \
+                      separator
+
+            method += instance_method.parent.name + separator + \
+                      instance_method.original.name + "<" + instance_method.instantiation.to_cpp() + ">"
+        return method[2 * len(separator):]
+
+    def _format_global_method(self, static_method, separator=''):
+        """Example:
+
+                gtsamPoint3.staticFunction
+        """
+        method = ''
+
+        if isinstance(static_method, parser.GlobalFunction):
+            method += "".join([separator + x for x in static_method.parent.full_namespaces()]) + \
+                      separator
+
+        return method[2 * len(separator):]
+
+    def _wrap_args(self, args):
+        """Wrap an interface_parser.ArgumentList into a list of arguments.
+
+        Returns:
+            A string representation of the arguments. For example:
+                'int x, double y'
+        """
+        arg_wrap = ''
+
+        for i, arg in enumerate(args.args_list, 1):
+            c_type = self._format_type_name(arg.ctype.typename, include_namespace=False)
+
+            arg_wrap += '{c_type} {arg_name}{comma}'.format(c_type=c_type,
+                                                            arg_name=arg.name,
+                                                            comma='' if i == len(args.args_list) else ', ')
+
+        return arg_wrap
+
+    def _wrap_variable_arguments(self, args, wrap_datatypes=True):
+        """ Wrap an interface_parser.ArgumentList into a statement of argument
+        checks.
+
+        Returns:
+            A string representation of a variable arguments for an if
+            statement. For example:
+                ' && isa(varargin{1},'double') && isa(varargin{2},'numeric')'
+        """
+        var_arg_wrap = ''
+
+        for i, arg in enumerate(args.args_list, 1):
+            name = arg.ctype.typename.name
+            if name in self.not_check_type:
+                continue
+
+            check_type = self.data_type_param.get(name)
+
+            if self.data_type.get(check_type):
+                check_type = self.data_type[check_type]
+
+            if check_type is None:
+                check_type = self._format_type_name(arg.ctype.typename, separator='.', constructor=not wrap_datatypes)
+
+            var_arg_wrap += " && isa(varargin{{{num}}},'{data_type}')".format(num=i, data_type=check_type)
+            if name == 'Vector':
+                var_arg_wrap += ' && size(varargin{{{num}}},2)==1'.format(num=i)
+            if name == 'Point2':
+                var_arg_wrap += ' && size(varargin{{{num}}},1)==2'.format(num=i)
+                var_arg_wrap += ' && size(varargin{{{num}}},2)==1'.format(num=i)
+            if name == 'Point3':
+                var_arg_wrap += ' && size(varargin{{{num}}},1)==3'.format(num=i)
+                var_arg_wrap += ' && size(varargin{{{num}}},2)==1'.format(num=i)
+
+        return var_arg_wrap
+
+    def _wrap_list_variable_arguments(self, args):
+        """ Wrap an interface_parser.ArgumentList into a list of argument
+        variables.
+
+        Returns:
+            A string representation of a list of variable arguments.
+            For example:
+                'varargin{1}, varargin{2}, varargin{3}'
+        """
+        var_list_wrap = ''
+        first = True
+
+        for i in range(1, len(args.args_list) + 1):
+            if first:
+                var_list_wrap += 'varargin{{{}}}'.format(i)
+                first = False
+            else:
+                var_list_wrap += ', varargin{{{}}}'.format(i)
+
+        return var_list_wrap
+
+    def _wrap_method_check_statement(self, args):
+        """Wrap the given arguments into either just a varargout call or a
+        call in an if statement that checks if the parameters are accurate.
+        """
+        check_statement = ''
+        arg_id = 1
+
+        if check_statement == '':
+            check_statement = \
+                'if length(varargin) == {param_count}'.format(
+                    param_count=len(args.args_list))
+
+        for i, arg in enumerate(args.args_list):
+            name = arg.ctype.typename.name
+
+            if name in self.not_check_type:
+                arg_id += 1
+                continue
+
+            check_type = self.data_type_param.get(name)
+
+            if self.data_type.get(check_type):
+                check_type = self.data_type[check_type]
+
+            if check_type is None:
+                check_type = self._format_type_name(arg.ctype.typename, separator='.')
+
+            check_statement += " && isa(varargin{{{id}}},'{ctype}')".format(id=arg_id, ctype=check_type)
+
+            if name == 'Vector':
+                check_statement += ' && size(varargin{{{num}}},2)==1'.format(num=arg_id)
+            if name == 'Point2':
+                check_statement += ' && size(varargin{{{num}}},1)==2'.format(num=arg_id)
+                check_statement += ' && size(varargin{{{num}}},2)==1'.format(num=arg_id)
+            if name == 'Point3':
+                check_statement += ' && size(varargin{{{num}}},1)==3'.format(num=arg_id)
+                check_statement += ' && size(varargin{{{num}}},2)==1'.format(num=arg_id)
+
+            arg_id += 1
+
+        check_statement = check_statement \
+            if check_statement == '' \
+            else check_statement + '\n'
+
+        return check_statement
+
+    def _wrapper_unwrap_arguments(self, args, arg_id=0, constructor=False):
+        """Format the interface_parser.Arguments.
+
+        Examples:
+            ((a), unsigned char a = unwrap< unsigned char >(in[1]);),
+            ((a), Test& t = *unwrap_shared_ptr< Test >(in[1], "ptr_Test");),
+            ((a), std::shared_ptr<Test> p1 = unwrap_shared_ptr< Test >(in[1], "ptr_Test");)
+        """
+        params = ''
+        body_args = ''
+
+        for arg in args.args_list:
+            if params != '':
+                params += ','
+
+            # import sys
+            # print("type: {}, is_ref: {}, is_ref: {}".format(arg.ctype, self._is_ref(arg.ctype), arg.ctype.is_ref), file=sys.stderr)
+            if self._is_ref(arg.ctype):  # and not constructor:
+                ctype_camel = self._format_type_name(arg.ctype.typename, separator='')
+                body_args += textwrap.indent(textwrap.dedent('''\
+                   {ctype}& {name} = *unwrap_shared_ptr< {ctype} >(in[{id}], "ptr_{ctype_camel}");
+                '''.format(ctype=self._format_type_name(arg.ctype.typename),
+                           ctype_camel=ctype_camel,
+                           name=arg.name,
+                           id=arg_id)),
+                                             prefix='  ')
+            elif self._is_ptr(arg.ctype) and \
+                    arg.ctype.typename.name not in self.ignore_namespace:
+                call_type = arg.ctype.is_ptr
+                body_args += textwrap.indent(textwrap.dedent('''\
+                    {std_boost}::shared_ptr<{ctype_sep}> {name} = unwrap_shared_ptr< {ctype_sep} >(in[{id}], "ptr_{ctype}");
+                '''.format(std_boost='boost' if constructor else 'boost',
+                           ctype_sep=self._format_type_name(arg.ctype.typename),
+                           ctype=self._format_type_name(arg.ctype.typename, separator=''),
+                           name=arg.name,
+                           id=arg_id)),
+                                             prefix='  ')
+                if call_type == "":
+                    params += "*"
+            else:
+                body_args += textwrap.indent(textwrap.dedent('''\
+                    {ctype} {name} = unwrap< {ctype} >(in[{id}]);
+                '''.format(ctype=arg.ctype.typename.name, name=arg.name, id=arg_id)),
+                                             prefix='  ')
+
+            params += arg.name
+
+            arg_id += 1
+
+        return params, body_args
+
+    @staticmethod
+    def _return_count(return_type):
+        """The amount of objects returned by the given
+        interface_parser.ReturnType.
+        """
+        return 1 if return_type.type2 == '' else 2
+
+    def _wrapper_name(self):
+        """Determine the name of wrapper function."""
+        return self.module_name + '_wrapper'
+
+    def class_serialize_comment(self, class_name, static_methods):
+        """Generate comments for serialize methods."""
+        comment_wrap = ''
+        static_methods = sorted(static_methods, key=lambda name: name.name)
+
+        for static_method in static_methods:
+            if comment_wrap == '':
+                comment_wrap = '%-------Static Methods-------\n'
+
+            comment_wrap += '%{name}({args}) : returns {return_type}\n'.format(name=static_method.name,
+                                                                               args=self._wrap_args(static_method.args),
+                                                                               return_type=self._format_return_type(
+                                                                                   static_method.return_type,
+                                                                                   include_namespace=True))
+
+        comment_wrap += textwrap.dedent('''\
+            %
+            %-------Serialization Interface-------
+            %string_serialize() : returns string
+            %string_deserialize(string serialized) : returns {class_name}
+            %
+        ''').format(class_name=class_name)
+
+        return comment_wrap
+
+    def class_comment(self, instantiated_class):
+        """Generate comments for the given class in Matlab.
+
+        Args
+            instantiated_class: the class being wrapped
+            ctors: a list of the constructors in the class
+            methods: a list of the methods in the class
+        """
+        class_name = instantiated_class.name
+        ctors = instantiated_class.ctors
+        methods = instantiated_class.methods
+        static_methods = instantiated_class.static_methods
+
+        comment = textwrap.dedent('''\
+            %class {class_name}, see Doxygen page for details
+            %at https://gtsam.org/doxygen/
+        ''').format(class_name=class_name)
+
+        if len(ctors) != 0:
+            comment += '%\n%-------Constructors-------\n'
+
+        # Write constructors
+        for ctor in ctors:
+            comment += '%{ctor_name}({args})\n'.format(ctor_name=ctor.name, args=self._wrap_args(ctor.args))
+
+        if len(methods) != 0:
+            comment += '%\n' \
+                       '%-------Methods-------\n'
+
+        methods = sorted(methods, key=lambda name: name.name)
+
+        # Write methods
+        for method in methods:
+            if method.name in self.whitelist:
+                continue
+
+            comment += '%{name}({args})'.format(name=method.name, args=self._wrap_args(method.args))
+
+            if method.return_type.type2 == '':
+                return_type = self._format_type_name(method.return_type.type1.typename)
+            else:
+                return_type = 'pair< {type1}, {type2} >'.format(
+                    type1=self._format_type_name(method.return_type.type1.typename),
+                    type2=self._format_type_name(method.return_type.type2.typename))
+
+            comment += ' : returns {return_type}\n'.format(return_type=return_type)
+
+        comment += '%\n'
+
+        if len(static_methods) != 0:
+            comment += self.class_serialize_comment(class_name, static_methods)
+
+        return comment
+
+    def generate_matlab_wrapper(self):
+        """Generate the C++ file for the wrapper."""
+        file_name = self._wrapper_name() + '.cpp'
+
+        wrapper_file = textwrap.dedent('''\
+            # include <wrap/matlab.h>
+            # include <map>
+        ''')
+
+        return file_name, wrapper_file
+
+    def wrap_method(self, methods):
+        """Wrap methods in the body of a class."""
+        if not isinstance(methods, list):
+            methods = [methods]
+
+        for method in methods:
+            output = ''
+
+        return ''
+
+    def wrap_methods(self, methods, globals=False, global_ns=None):
+        """Wrap a sequence of methods. Groups methods with the same names
+        together. If globals is True then output every method into its own
+        file.
+        """
+        output = ''
+        methods = self._group_methods(methods)
+
+        for method in methods:
+            if globals:
+                self._debug("[wrap_methods] wrapping: {}..{}={}".format(method[0].parent.name, method[0].name,
+                                                                        type(method[0].parent.name)))
+
+                method_text = self.wrap_global_function(method)
+                self.content.append(
+                    ("".join(['+' + x + '/'
+                              for x in global_ns.full_namespaces()[1:]])[:-1], [(method[0].name + '.m', method_text)]))
+            else:
+                method_text = self.wrap_method(method)
+                output += ''
+
+        return output
+
+    def wrap_global_function(self, function):
+        """Wrap the given global function."""
+        if not isinstance(function, list):
+            function = [function]
+
+        function_name = function[0].name
+
+        # Get all combinations of parameters
+        param_wrap = ''
+
+        for i, overload in enumerate(function):
+            param_wrap += '      if' if i == 0 else '      elseif'
+            param_wrap += ' length(varargin) == '
+
+            if len(overload.args.args_list) == 0:
+                param_wrap += '0\n'
+            else:
+                param_wrap += str(len(overload.args.args_list)) \
+                              + self._wrap_variable_arguments(overload.args, False) + '\n'
+
+            # Determine format of return and varargout statements
+            return_type_formatted = self._format_return_type(
+                overload.return_type,
+                include_namespace=True,
+                separator="."
+            )
+            varargout = self._format_varargout(overload.return_type, return_type_formatted)
+
+            param_wrap += textwrap.indent(textwrap.dedent('''\
+                {varargout}{module_name}_wrapper({num}, varargin{{:}});
+            ''').format(varargout=varargout, module_name=self.module_name,
+                        num=self._update_wrapper_id(collector_function=(function[0].parent.name, function[i],
+                                                                        'global_function', None))),
+                                          prefix='        ')
+
+        param_wrap += textwrap.indent(textwrap.dedent('''\
+            else
+              error('Arguments do not match any overload of function {func_name}');
+        ''').format(func_name=function_name),
+                                      prefix='      ')
+
+        global_function = textwrap.indent(textwrap.dedent('''\
+            function varargout = {m_method}(varargin)
+            {statements}      end
+        ''').format(m_method=function_name, statements=param_wrap),
+                                          prefix='')
+
+        return global_function
+
+    def wrap_class_constructors(self, namespace_name, inst_class, parent_name, ctors, is_virtual):
+        """Wrap class constructor.
+
+        Args:
+            namespace_name: the name of the namespace ('' if it does not exist)
+            inst_class: instance of the class
+            parent_name: the name of the parent class if it exists
+            ctors: the interface_parser.Constructor in the class
+            is_virtual: whether the class is part of a virtual inheritance
+                chain
+        """
+        has_parent = parent_name != ''
+        class_name = inst_class.name
+        if has_parent:
+            parent_name = self._format_type_name(parent_name, separator=".")
+        if type(ctors) != list:
+            ctors = [ctors]
+
+        # import sys
+        # if class_name:
+        #     print("[Constructor] class: {} ns: {}"
+        #           .format(class_name,
+        #                   inst_class.namespaces())
+        #           , file=sys.stderr)
+        # full_name = "".join(obj_type.full_namespaces()) + obj_type.name
+
+        methods_wrap = textwrap.indent(textwrap.dedent("""\
+            methods
+              function obj = {class_name}(varargin)
+        """).format(class_name=class_name),
+                                       prefix='')
+
+        if is_virtual:
+            methods_wrap += "    if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))"
+        else:
+            methods_wrap += '    if nargin == 2'
+
+        methods_wrap += " && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)\n"
+
+        if is_virtual:
+            methods_wrap += textwrap.indent(textwrap.dedent('''\
+                if nargin == 2
+                  my_ptr = varargin{{2}};
+                else
+                  my_ptr = {wrapper_name}({id}, varargin{{2}});
+                end
+            ''').format(wrapper_name=self._wrapper_name(), id=self._update_wrapper_id() + 1),
+                                            prefix='      ')
+        else:
+            methods_wrap += '      my_ptr = varargin{2};\n'
+
+        collector_base_id = self._update_wrapper_id((namespace_name, inst_class, 'collectorInsertAndMakeBase', None),
+                                                    id_diff=-1 if is_virtual else 0)
+
+        methods_wrap += '      {ptr}{wrapper_name}({id}, my_ptr);\n' \
+            .format(
+            ptr='base_ptr = ' if has_parent else '',
+            wrapper_name=self._wrapper_name(),
+            id=collector_base_id - (1 if is_virtual else 0))
+
+        for ctor in ctors:
+            wrapper_return = '[ my_ptr, base_ptr ] = ' \
+                if has_parent \
+                else 'my_ptr = '
+
+            methods_wrap += textwrap.indent(textwrap.dedent('''\
+                elseif nargin == {len}{varargin}
+                  {ptr}{wrapper}({num}{comma}{var_arg});
+            ''').format(len=len(ctor.args.args_list),
+                        varargin=self._wrap_variable_arguments(ctor.args, False),
+                        ptr=wrapper_return,
+                        wrapper=self._wrapper_name(),
+                        num=self._update_wrapper_id((namespace_name, inst_class, 'constructor', ctor)),
+                        comma='' if len(ctor.args.args_list) == 0 else ', ',
+                        var_arg=self._wrap_list_variable_arguments(ctor.args)),
+                                            prefix='    ')
+
+        base_obj = ''
+
+        if has_parent:
+            self._debug("class: {} ns: {}".format(parent_name, self._format_class_name(inst_class.parent,
+                                                                                       separator=".")))
+
+        if has_parent:
+            base_obj = '  obj = obj@{parent_name}(uint64(5139824614673773682), base_ptr);'.format(
+                parent_name=parent_name)
+
+        if base_obj:
+            base_obj = '\n' + base_obj
+
+        self._debug("class: {}, name: {}".format(inst_class.name, self._format_class_name(inst_class, separator=".")))
+        methods_wrap += textwrap.indent(textwrap.dedent('''\
+              else
+                error('Arguments do not match any overload of {class_name_doc} constructor');
+              end{base_obj}
+              obj.ptr_{class_name} = my_ptr;
+            end\n
+        ''').format(namespace=namespace_name,
+                    d='' if namespace_name == '' else '.',
+                    class_name_doc=self._format_class_name(inst_class, separator="."),
+                    class_name=self._format_class_name(inst_class, separator=""),
+                    base_obj=base_obj),
+                                        prefix='  ')
+
+        return methods_wrap
+
+    def wrap_class_properties(self, class_name):
+        """Generate properties of class."""
+        return textwrap.dedent('''\
+            properties
+              ptr_{} = 0
+            end
+        ''').format(class_name)
+
+    def wrap_class_deconstructor(self, namespace_name, inst_class):
+        """Generate the delete function for the Matlab class."""
+        class_name = inst_class.name
+
+        methods_text = textwrap.indent(textwrap.dedent("""\
+            function delete(obj)
+              {wrapper}({num}, obj.ptr_{class_name});
+            end\n
+        """).format(num=self._update_wrapper_id((namespace_name, inst_class, 'deconstructor', None)),
+                    wrapper=self._wrapper_name(),
+                    class_name="".join(inst_class.parent.full_namespaces()) + class_name),
+                                       prefix='  ')
+
+        return methods_text
+
+    def wrap_class_display(self):
+        """Generate the display function for the Matlab class."""
+        return textwrap.indent(textwrap.dedent("""\
+            function display(obj), obj.print(''); end
+            %DISPLAY Calls print on the object
+            function disp(obj), obj.display; end
+            %DISP Calls print on the object
+        """),
+                               prefix='  ')
+
+    def _group_class_methods(self, methods):
+        """Group overloaded methods together"""
+        method_map = {}
+        method_out = []
+
+        for method in methods:
+            method_index = method_map.get(method.name)
+
+            if method_index is None:
+                method_map[method.name] = len(method_out)
+                method_out.append([method])
+            else:
+                # import sys
+                # print("[_group_methods] Merging {} with {}".format(method_index, method.name))
+                method_out[method_index].append(method)
+
+        return method_out
+
+    @classmethod
+    def _format_varargout(cls, return_type, return_type_formatted):
+        """Determine format of return and varargout statements"""
+        if cls._return_count(return_type) == 1:
+            varargout = '' \
+                if return_type_formatted == 'void' \
+                else 'varargout{1} = '
+        else:
+            varargout = '[ varargout{1} varargout{2} ] = '
+
+        return varargout
+
+    def wrap_class_methods(self, namespace_name, inst_class, methods, serialize=[False]):
+        """Wrap the methods in the class.
+
+        Args:
+            namespace_name: the name of the class's namespace
+            inst_class: the instantiated class whose methods to wrap
+            methods: the methods to wrap in the order to wrap them
+            serialize: mutable param storing if one of the methods is serialize
+        """
+        method_text = ''
+
+        methods = self._group_class_methods(methods)
+
+        for method in methods:
+            method_name = method[0].name
+            if method_name in self.whitelist and method_name != 'serialize':
+                continue
+
+            if method_name == 'serialize':
+                serialize[0] = True
+                method_text += self.wrap_class_serialize_method(namespace_name, inst_class)
+            else:
+                # Generate method code
+                method_text += textwrap.indent(textwrap.dedent("""\
+                    function varargout = {method_name}(this, varargin)
+                    """).format(caps_name=method_name.upper(), method_name=method_name),
+                                               prefix='')
+
+                for overload in method:
+                    method_text += textwrap.indent(textwrap.dedent("""\
+                    % {caps_name} usage: {method_name}(""").format(caps_name=method_name.upper(),
+                                                                   method_name=method_name),
+                                                   prefix='  ')
+
+                    # Determine format of return and varargout statements
+                    return_type_formatted = self._format_return_type(
+                        overload.return_type,
+                        include_namespace=True,
+                        separator="."
+                    )
+                    varargout = self._format_varargout(overload.return_type, return_type_formatted)
+
+                    check_statement = self._wrap_method_check_statement(overload.args)
+                    class_name = namespace_name + ('' if namespace_name == '' else '.') + inst_class.name
+
+                    end_statement = '' \
+                        if check_statement == '' \
+                        else textwrap.indent(textwrap.dedent("""\
+                              return
+                            end
+                        """).format(
+                        class_name=class_name,
+                        method_name=overload.original.name), prefix='  ')
+
+                    method_text += textwrap.dedent("""\
+                        {method_args}) : returns {return_type}
+                          % Doxygen can be found at https://gtsam.org/doxygen/
+                          {check_statement}{spacing}{varargout}{wrapper}({num}, this, varargin{{:}});
+                        {end_statement}""").format(method_args=self._wrap_args(overload.args),
+                                                   return_type=return_type_formatted,
+                                                   num=self._update_wrapper_id(
+                                                       (namespace_name, inst_class, overload.original.name, overload)),
+                                                   check_statement=check_statement,
+                                                   spacing='' if check_statement == '' else '    ',
+                                                   varargout=varargout,
+                                                   wrapper=self._wrapper_name(),
+                                                   end_statement=end_statement)
+
+                final_statement = textwrap.indent(textwrap.dedent("""\
+                    error('Arguments do not match any overload of function {class_name}.{method_name}');
+                """.format(class_name=class_name, method_name=method_name)),
+                                                  prefix='  ')
+                method_text += final_statement + 'end\n\n'
+
+        return method_text
+
+    def wrap_static_methods(self, namespace_name, instantiated_class, serialize):
+        class_name = instantiated_class.name
+
+        method_text = 'methods(Static = true)\n'
+        static_methods = sorted(instantiated_class.static_methods, key=lambda name: name.name)
+
+        static_methods = self._group_class_methods(static_methods)
+
+        for static_method in static_methods:
+            format_name = list(static_method[0].name)
+            format_name[0] = format_name[0].upper()
+
+            method_text += textwrap.indent(textwrap.dedent('''\
+                    function varargout = {name}(varargin)
+                    '''.format(name=''.join(format_name))),
+                                           prefix="  ")
+
+            for static_overload in static_method:
+                check_statement = self._wrap_method_check_statement(static_overload.args)
+
+                end_statement = '' \
+                    if check_statement == '' \
+                    else textwrap.indent(textwrap.dedent("""
+                              return
+                            end
+                            """), prefix='')
+                method_text += textwrap.indent(textwrap.dedent('''\
+                      % {name_caps} usage: {name_upper_case}({args}) : returns {return_type}
+                      % Doxygen can be found at https://gtsam.org/doxygen/
+                      {check_statement}{spacing}varargout{{1}} = {wrapper}({id}, varargin{{:}});{end_statement}
+                      ''').format(name=''.join(format_name),
+                                  name_caps=static_overload.name.upper(),
+                                  name_upper_case=static_overload.name,
+                                  args=self._wrap_args(static_overload.args),
+                                  return_type=self._format_return_type(static_overload.return_type,
+                                                                       include_namespace=True, separator="."),
+                                  length=len(static_overload.args.args_list),
+                                  var_args_list=self._wrap_variable_arguments(static_overload.args),
+                                  check_statement=check_statement,
+                                  spacing='' if check_statement == '' else '  ',
+                                  wrapper=self._wrapper_name(),
+                                  id=self._update_wrapper_id(
+                                      (namespace_name, instantiated_class, static_overload.name, static_overload)),
+                                  class_name=instantiated_class.name,
+                                  end_statement=end_statement),
+                                               prefix='    ')
+
+            method_text += textwrap.indent(textwrap.dedent("""\
+                    error('Arguments do not match any overload of function {class_name}.{method_name}');
+                """.format(class_name=class_name, method_name=static_overload.name)),
+                                           prefix='    ')
+            method_text += textwrap.indent(textwrap.dedent('''\
+                                    end\n
+                '''.format(name=''.join(format_name))),
+                                           prefix="  ")
+
+        if serialize:
+            method_text += textwrap.indent(textwrap.dedent('''\
+                function varargout = string_deserialize(varargin)
+                  % STRING_DESERIALIZE usage: string_deserialize() : returns {class_name}
+                  % Doxygen can be found at https://gtsam.org/doxygen/
+                  if length(varargin) == 1
+                    varargout{{1}} = {wrapper}({id}, varargin{{:}});
+                  else
+                    error('Arguments do not match any overload of function {class_name}.string_deserialize');
+                  end
+                end\n
+                function obj = loadobj(sobj)
+                  % LOADOBJ Saves the object to a matlab-readable format
+                  obj = {class_name}.string_deserialize(sobj);
+                end
+            ''').format(class_name=namespace_name + '.' + instantiated_class.name,
+                        wrapper=self._wrapper_name(),
+                        id=self._update_wrapper_id(
+                            (namespace_name, instantiated_class, 'string_deserialize', 'deserialize'))),
+                                           prefix='  ')
+
+        return method_text
+
+    def wrap_instantiated_class(self, instantiated_class, namespace_name=''):
+        """Generate comments and code for given class.
+
+        Args:
+            instantiated_class: template_instantiator.InstantiatedClass
+                instance storing the class to wrap
+            namespace_name: the name of the namespace if there is one
+        """
+        file_name = self._clean_class_name(instantiated_class)
+        namespace_file_name = namespace_name + file_name
+
+        if instantiated_class.cpp_class() in self.ignore_classes:
+            return None
+
+        # Class comment
+        content_text = self.class_comment(instantiated_class)
+        content_text += self.wrap_methods(instantiated_class.methods)
+
+        # Class definition
+        # import sys
+        # if namespace_name:
+        #     print("nsname: {}, file_name_: {}, filename: {}"
+        #           .format(namespace_name,
+        #                   self._clean_class_name(instantiated_class), file_name)
+        #           , file=sys.stderr)
+        content_text += 'classdef {class_name} < {parent}\n'.format(
+            class_name=file_name, parent=str(self._qualified_name(instantiated_class.parent_class)).replace("::", "."))
+
+        # Class properties
+        content_text += '  ' + reduce(self._insert_spaces,
+                                      self.wrap_class_properties(namespace_file_name).splitlines()) + '\n'
+
+        # Class constructor
+        content_text += '  ' + reduce(
+            self._insert_spaces,
+            self.wrap_class_constructors(
+                namespace_name,
+                instantiated_class,
+                instantiated_class.parent_class,
+                instantiated_class.ctors,
+                instantiated_class.is_virtual,
+            ).splitlines()) + '\n'
+
+        # Delete function
+        content_text += '  ' + reduce(self._insert_spaces,
+                                      self.wrap_class_deconstructor(namespace_name,
+                                                                    instantiated_class).splitlines()) + '\n'
+
+        # Display function
+        content_text += '  ' + reduce(self._insert_spaces, self.wrap_class_display().splitlines()) + '\n'
+
+        # Class methods
+        serialize = [False]
+
+        if len(instantiated_class.methods) != 0:
+            methods = sorted(instantiated_class.methods, key=lambda name: name.name)
+            class_methods_wrapped = self.wrap_class_methods(namespace_name,
+                                                            instantiated_class,
+                                                            methods,
+                                                            serialize=serialize).splitlines()
+            if len(class_methods_wrapped) > 0:
+                content_text += '    ' + reduce(lambda x, y: x + '\n' +
+                                                             ('' if y == '' else '    ') + y,
+                                                class_methods_wrapped) + '\n'
+
+        # Static class methods
+        content_text += '  end\n\n  ' + reduce(
+            self._insert_spaces,
+            self.wrap_static_methods(namespace_name, instantiated_class, serialize[0]).splitlines()) + '\n'
+
+        content_text += textwrap.dedent('''\
+              end
+            end
+        ''')
+
+        return file_name + '.m', content_text
+
+    def wrap_namespace(self, namespace, parent=[]):
+        """Wrap a namespace by wrapping all of its components.
+
+        Args:
+            namespace: the interface_parser.namespace instance of the namespace
+            parent: parent namespace
+        """
+        test_output = ''
+        namespaces = namespace.full_namespaces()
+        inner_namespace = namespace.name != ''
+        wrapped = []
+        self._debug("wrapping ns: {}, parent: {}".format(namespace.full_namespaces(), parent))
+
+        matlab_wrapper = self.generate_matlab_wrapper()
+        self.content.append((matlab_wrapper[0], matlab_wrapper[1]))
+
+        current_scope = []
+        namespace_scope = []
+
+        for element in namespace.content:
+            if isinstance(element, parser.Include):
+                self._add_include(element)
+            elif isinstance(element, parser.Namespace):
+                self.wrap_namespace(element, namespaces)
+            elif isinstance(element, instantiator.InstantiatedClass):
+                self._add_class(element)
+
+                if inner_namespace:
+                    class_text = self.wrap_instantiated_class(element, "".join(namespace.full_namespaces()))
+
+                    if not class_text is None:
+                        namespace_scope.append(("".join(['+' + x + '/' for x in namespace.full_namespaces()[1:]])[:-1],
+                                                [(class_text[0], class_text[1])]))
+                else:
+                    class_text = self.wrap_instantiated_class(element)
+                    current_scope.append((class_text[0], class_text[1]))
+
+        self.content.extend(current_scope)
+
+        if inner_namespace:
+            self.content.append(namespace_scope)
+
+        # Global functions
+        all_funcs = [func for func in namespace.content if isinstance(func, parser.GlobalFunction)]
+
+        test_output += self.wrap_methods(all_funcs, True, global_ns=namespace)
+
+        return wrapped
+
+    def wrap_collector_function_shared_return(self, return_type_name, shared_obj, id, new_line=True):
+        new_line = '\n' if new_line else ''
+
+        return textwrap.indent(textwrap.dedent('''\
+            {{
+            boost::shared_ptr<{name}> shared({shared_obj});
+            out[{id}] = wrap_shared_ptr(shared,"{name}");
+            }}{new_line}''').format(name=self._format_type_name(return_type_name, include_namespace=False),
+                                    shared_obj=shared_obj,
+                                    id=id,
+                                    new_line=new_line),
+                               prefix='  ')
+
+    def wrap_collector_function_return_types(self, return_type, id):
+        return_type_text = '  out[' + str(id) + '] = '
+        pair_value = 'first' if id == 0 else 'second'
+        new_line = '\n' if id == 0 else ''
+
+        if self._is_ptr(return_type):
+            shared_obj = 'pairResult.' + pair_value
+
+            if not return_type.is_ptr:
+                shared_obj = 'boost::make_shared<{name}>({shared_obj})' \
+                    .format(
+                    name=self._format_type_name(return_type.typename),
+                    formatted_name=self._format_type_name(
+                        return_type.typename),
+                    shared_obj='pairResult.' + pair_value)
+
+            if return_type.typename.name in self.ignore_namespace:
+                return_type_text = self.wrap_collector_function_shared_return(return_type.typename, shared_obj, id,
+                                                                              True if id == 0 else False)
+            else:
+                return_type_text += 'wrap_shared_ptr({},"{}", false);{new_line}' \
+                    .format(
+                    shared_obj,
+                    self._format_type_name(
+                        return_type.typename, separator='.'),
+                    new_line=new_line)
+        else:
+            return_type_text += 'wrap< {} >(pairResult.{});{}'.format(
+                self._format_type_name(return_type.typename, separator='.'), pair_value, new_line)
+
+        return return_type_text
+
+    def wrap_collector_function_return(self, method):
+        expanded = ''
+
+        params = self._wrapper_unwrap_arguments(method.args, arg_id=1)[0]
+
+        return_1 = method.return_type.type1
+        return_count = self._return_count(method.return_type)
+        return_1_name = method.return_type.type1.typename.name
+        obj_start = ''
+
+        if isinstance(method, instantiator.InstantiatedMethod):
+            # method_name = method.original.name
+            method_name = method.to_cpp()
+            obj_start = 'obj->'
+
+            if method.instantiation:
+                # method_name += '<{}>'.format(
+                #     self._format_type_name(method.instantiation))
+                # method_name = self._format_instance_method(method, '::')
+                method = method.to_cpp()
+        elif isinstance(method, parser.GlobalFunction):
+            method_name = self._format_global_method(method, '::')
+            method_name += method.name
+        else:
+            if isinstance(method.parent, instantiator.InstantiatedClass):
+                method_name = method.parent.cpp_class() + "::"
+            else:
+                method_name = self._format_static_method(method, '::')
+            method_name += method.name
+
+        if "MeasureRange" in method_name:
+            self._debug("method: {}, method: {}, inst: {}".format(method_name, method.name, method.parent.cpp_class()))
+
+        obj = '  ' if return_1_name == 'void' else ''
+        obj += '{}{}({})'.format(obj_start, method_name, params)
+
+        if return_1_name != 'void':
+            if return_count == 1:
+                if self._is_ptr(return_1):
+                    sep_method_name = partial(self._format_type_name, return_1.typename, include_namespace=True)
+
+                    if return_1.typename.name in self.ignore_namespace:
+                        expanded += self.wrap_collector_function_shared_return(return_1.typename,
+                                                                               obj,
+                                                                               0,
+                                                                               new_line=False)
+
+                    if return_1.is_ptr:
+                        shared_obj = '{obj},"{method_name_sep}"'.format(obj=obj, method_name_sep=sep_method_name('.'))
+                    else:
+                        self._debug("Non-PTR: {}, {}".format(return_1, type(return_1)))
+                        self._debug("Inner type is: {}, {}".format(return_1.typename.name, sep_method_name('.')))
+                        self._debug("Inner type instantiations: {}".format(return_1.typename.instantiations))
+                        method_name_sep_dot = sep_method_name('.')
+                        shared_obj = 'boost::make_shared<{method_name_sep_col}>({obj}),"{method_name_sep_dot}"' \
+                            .format(
+                            method_name=return_1.typename.name,
+                            method_name_sep_col=sep_method_name(),
+                            method_name_sep_dot=method_name_sep_dot,
+                            obj=obj)
+
+                    if return_1.typename.name not in self.ignore_namespace:
+                        expanded += textwrap.indent('out[0] = wrap_shared_ptr({}, false);'.format(shared_obj),
+                                                    prefix='  ')
+                else:
+                    expanded += '  out[0] = wrap< {} >({});'.format(return_1.typename.name, obj)
+            elif return_count == 2:
+                return_2 = method.return_type.type2
+
+                expanded += '  auto pairResult = {};\n'.format(obj)
+                expanded += self.wrap_collector_function_return_types(return_1, 0)
+                expanded += self.wrap_collector_function_return_types(return_2, 1)
+        else:
+            expanded += obj + ';'
+
+        return expanded
+
+    def wrap_collector_function_upcast_from_void(self, class_name, id, cpp_name):
+        return textwrap.dedent('''\
+            void {class_name}_upcastFromVoid_{id}(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {{
+              mexAtExit(&_deleteAllObjects);
+              typedef boost::shared_ptr<{cpp_name}> Shared;
+              boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
+              out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+              Shared *self = new Shared(boost::static_pointer_cast<{cpp_name}>(*asVoid));
+              *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
+            }}\n
+        ''').format(class_name=class_name, cpp_name=cpp_name, id=id)
+
+    def generate_collector_function(self, id):
+        collector_func = self.wrapper_map.get(id)
+
+        if collector_func is None:
+            return ''
+
+        method_name = collector_func[3]
+        # import sys
+        # print("[Collector Gen] id: {}, obj: {}".format(id, method_name), file=sys.stderr)
+
+        collector_function = 'void {}(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n' \
+            .format(method_name)
+
+        if isinstance(collector_func[1], instantiator.InstantiatedClass):
+            body = '{\n'
+
+            extra = collector_func[4]
+
+            class_name = collector_func[0] + collector_func[1].name
+            class_name_separated = collector_func[1].cpp_class()
+            is_method = isinstance(extra, parser.Method)
+            is_static_method = isinstance(extra, parser.StaticMethod)
+
+            if collector_func[2] == 'collectorInsertAndMakeBase':
+                body += textwrap.indent(textwrap.dedent('''\
+                    mexAtExit(&_deleteAllObjects);
+                    typedef boost::shared_ptr<{class_name_sep}> Shared;\n
+                    Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
+                    collector_{class_name}.insert(self);
+                ''').format(class_name_sep=class_name_separated, class_name=class_name),
+                                        prefix='  ')
+
+                if collector_func[1].parent_class:
+                    body += textwrap.indent(textwrap.dedent('''
+                        typedef boost::shared_ptr<{}> SharedBase;
+                        out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+                        *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
+                    ''').format(collector_func[1].parent_class),
+                                            prefix='  ')
+            elif collector_func[2] == 'constructor':
+                base = ''
+                params, body_args = self._wrapper_unwrap_arguments(extra.args, constructor=True)
+
+                if collector_func[1].parent_class:
+                    base += textwrap.indent(textwrap.dedent('''
+                        typedef boost::shared_ptr<{}> SharedBase;
+                        out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+                        *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);
+                    ''').format(collector_func[1].parent_class),
+                                            prefix='  ')
+
+                body += textwrap.dedent('''\
+                      mexAtExit(&_deleteAllObjects);
+                      typedef boost::shared_ptr<{class_name_sep}> Shared;\n
+                    {body_args}  Shared *self = new Shared(new {class_name_sep}({params}));
+                      collector_{class_name}.insert(self);
+                      out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+                      *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+                    {base}''').format(class_name_sep=class_name_separated,
+                                      body_args=body_args,
+                                      params=params,
+                                      class_name=class_name,
+                                      base=base)
+            elif collector_func[2] == 'deconstructor':
+                body += textwrap.indent(textwrap.dedent('''\
+                    typedef boost::shared_ptr<{class_name_sep}> Shared;
+                    checkArguments("delete_{class_name}",nargout,nargin,1);
+                    Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
+                    Collector_{class_name}::iterator item;
+                    item = collector_{class_name}.find(self);
+                    if(item != collector_{class_name}.end()) {{
+                      delete self;
+                      collector_{class_name}.erase(item);
+                    }}
+                ''').format(class_name_sep=class_name_separated, class_name=class_name),
+                                        prefix='  ')
+            elif extra == 'serialize':
+                body += self.wrap_collector_function_serialize(collector_func[1].name,
+                                                               full_name=collector_func[1].cpp_class(),
+                                                               namespace=collector_func[0])
+            elif extra == 'deserialize':
+                body += self.wrap_collector_function_deserialize(collector_func[1].name,
+                                                                 full_name=collector_func[1].cpp_class(),
+                                                                 namespace=collector_func[0])
+            elif is_method or is_static_method:
+                method_name = ''
+
+                if is_static_method:
+                    method_name = self._format_static_method(extra) + '.'
+
+                method_name += extra.name
+
+                return_type = extra.return_type
+                return_count = self._return_count(return_type)
+
+                return_body = self.wrap_collector_function_return(extra)
+                params, body_args = self._wrapper_unwrap_arguments(extra.args, arg_id=1 if is_method else 0)
+
+                shared_obj = ''
+
+                if is_method:
+                    shared_obj = \
+                        '  auto obj = unwrap_shared_ptr<{class_name_sep}>(in[0], "ptr_{class_name}");\n'.format(
+                            class_name_sep=class_name_separated,
+                            class_name=class_name)
+                """
+                body += textwrap.dedent('''\
+                      typedef std::shared_ptr<{class_name_sep}> Shared;
+                      checkArguments("{method_name}",nargout,nargin{min1},{num_args});
+                    {shared_obj}
+                    {body_args}
+                    {return_body}
+                ''')
+                """
+                body += '  checkArguments("{method_name}",nargout,nargin{min1},' \
+                        '{num_args});\n' \
+                        '{shared_obj}' \
+                        '{body_args}' \
+                        '{return_body}\n'.format(
+                    class_name_sep=class_name_separated,
+                    min1='-1' if is_method else '',
+                    shared_obj=shared_obj,
+                    method_name=method_name,
+                    num_args=len(extra.args.args_list),
+                    class_name=class_name,
+                    body_args=body_args,
+                    return_body=return_body)
+
+            body += '}\n'
+
+            if extra not in ['serialize', 'deserialize']:
+                body += '\n'
+
+            collector_function += body
+        else:
+            # import sys
+            # print("other func: {}".format(collector_func[1]), file=sys.stderr)
+            body = textwrap.dedent('''\
+                {{
+                  checkArguments("{function_name}",nargout,nargin,{len});
+            ''').format(function_name=collector_func[1].name,
+                        id=self.global_function_id,
+                        len=len(collector_func[1].args.args_list))
+
+            body += self._wrapper_unwrap_arguments(collector_func[1].args)[1]
+            body += self.wrap_collector_function_return(collector_func[1]) + '\n}\n'
+
+            collector_function += body
+
+            self.global_function_id += 1
+
+        return collector_function
+
+    def mex_function(self):
+        cases = ''
+        next_case = None
+
+        for wrapper_id in range(self.wrapper_id):
+            id_val = self.wrapper_map.get(wrapper_id)
+            set_next_case = False
+
+            if id_val is None:
+                id_val = self.wrapper_map.get(wrapper_id + 1)
+
+                if id_val is None:
+                    continue
+
+                set_next_case = True
+
+            cases += textwrap.indent(textwrap.dedent('''\
+                case {}:
+                  {}(nargout, out, nargin-1, in+1);
+                  break;
+                ''').format(wrapper_id, next_case if next_case else id_val[3]),
+                                     prefix='    ')
+
+            if set_next_case:
+                next_case = '{}_upcastFromVoid_{}'.format(id_val[1].name, wrapper_id + 1)
+            else:
+                next_case = None
+
+        mex_function = textwrap.dedent('''
+            void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+            {{
+              mstream mout;
+              std::streambuf *outbuf = std::cout.rdbuf(&mout);\n
+              _{module_name}_RTTIRegister();\n
+              int id = unwrap<int>(in[0]);\n
+              try {{
+                switch(id) {{
+            {cases}    }}
+              }} catch(const std::exception& e) {{
+                mexErrMsgTxt(("Exception from gtsam:\\n" + std::string(e.what()) + "\\n").c_str());
+              }}\n
+              std::cout.rdbuf(outbuf);
+            }}
+        ''').format(module_name=self.module_name, cases=cases)
+
+        return mex_function
+
+    def generate_wrapper(self, namespace):
+        """Generate the c++ wrapper."""
+        # Includes
+        wrapper_file = textwrap.dedent('''\
+            #include <wrap/matlab.h>
+            #include <map>\n
+            #include <boost/archive/text_iarchive.hpp>
+            #include <boost/archive/text_oarchive.hpp>
+            #include <boost/serialization/export.hpp>\n
+        ''')
+
+        includes_list = sorted(list(self.includes.keys()), key=lambda include: include.header)
+
+        wrapper_file += reduce(lambda x, y: str(x) + '\n' + str(y), includes_list) + '\n'
+
+        typedef_instances = '\n'
+        typedef_collectors = ''
+        boost_class_export_guid = ''
+        delete_objs = textwrap.dedent('''\
+            void _deleteAllObjects()
+            {
+              mstream mout;
+              std::streambuf *outbuf = std::cout.rdbuf(&mout);\n
+              bool anyDeleted = false;
+        ''')
+        rtti_reg_start = textwrap.dedent('''\
+            void _{module_name}_RTTIRegister() {{
+              const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_{module_name}_rttiRegistry_created");
+              if(!alreadyCreated) {{
+                std::map<std::string, std::string> types;
+        ''').format(module_name=self.module_name)
+        rtti_reg_mid = ''
+        rtti_reg_end = textwrap.indent(textwrap.dedent('''
+                mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry");
+                if(!registry)
+                  registry = mxCreateStructMatrix(1, 1, 0, NULL);
+                typedef std::pair<std::string, std::string> StringPair;
+                for(const StringPair& rtti_matlab: types) {
+                  int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
+                  if(fieldId < 0)
+                    mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+                  mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());
+                  mxSetFieldByNumber(registry, 0, fieldId, matlabName);
+                }
+                if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0)
+                  mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+                mxDestroyArray(registry);
+        '''),
+                                       prefix='    ') + '    \n' + textwrap.dedent('''\
+                mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);
+                if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0)
+                  mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
+                mxDestroyArray(newAlreadyCreated);
+              }
+            }
+        ''')
+        ptr_ctor_frag = ''
+
+        for cls in self.classes:
+            if cls.cpp_class().strip() in self.ignore_classes:
+                continue
+
+            def _has_serialization(cls):
+                for m in cls.methods:
+                    if m.name in self.whitelist:
+                        return True
+                return False
+
+            if len(cls.instantiations):
+                cls_insts = ''
+
+                for i, inst in enumerate(cls.instantiations):
+                    if i != 0:
+                        cls_insts += ', '
+
+                    cls_insts += self._format_type_name(inst)
+
+                typedef_instances += 'typedef {original_class_name} {class_name_sep};\n'.format(
+                    namespace_name=namespace.name, original_class_name=cls.cpp_class(), class_name_sep=cls.name)
+
+                class_name_sep = cls.name
+                class_name = self._format_class_name(cls)
+
+                if len(cls.original.namespaces()) > 1 and _has_serialization(cls):
+                    boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format(class_name_sep, class_name)
+            else:
+                class_name_sep = cls.cpp_class()
+                class_name = self._format_class_name(cls)
+
+                if len(cls.original.namespaces()) > 1 and _has_serialization(cls):
+                    boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format(class_name_sep, class_name)
+
+            typedef_collectors += textwrap.dedent('''\
+                typedef std::set<boost::shared_ptr<{class_name_sep}>*> Collector_{class_name};
+                static Collector_{class_name} collector_{class_name};
+            ''').format(class_name_sep=class_name_sep, class_name=class_name)
+            delete_objs += textwrap.indent(textwrap.dedent('''\
+                {{ for(Collector_{class_name}::iterator iter = collector_{class_name}.begin();
+                    iter != collector_{class_name}.end(); ) {{
+                  delete *iter;
+                  collector_{class_name}.erase(iter++);
+                  anyDeleted = true;
+                }} }}
+            ''').format(class_name=class_name),
+                                           prefix='  ')
+
+            if cls.is_virtual:
+                rtti_reg_mid += '    types.insert(std::make_pair(typeid({}).name(), "{}"));\n'.format(
+                    class_name_sep, class_name)
+
+        set_next_case = False
+
+        for id in range(self.wrapper_id):
+            id_val = self.wrapper_map.get(id)
+            queue_set_next_case = set_next_case
+
+            set_next_case = False
+
+            if id_val is None:
+                id_val = self.wrapper_map.get(id + 1)
+
+                if id_val is None:
+                    continue
+
+                set_next_case = True
+
+            ptr_ctor_frag += self.generate_collector_function(id)
+
+            if queue_set_next_case:
+                ptr_ctor_frag += self.wrap_collector_function_upcast_from_void(id_val[1].name, id,
+                                                                               id_val[1].cpp_class())
+
+        wrapper_file += textwrap.dedent('''\
+            {typedef_instances}
+            {boost_class_export_guid}
+            {typedefs_collectors}
+            {delete_objs}  if(anyDeleted)
+                cout <<
+                  "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\\n"
+                  "calling destructors, call \'clear all\' again if you plan to now recompile a wrap\\n"
+                  "module, so that your recompiled module is used instead of the old one." << endl;
+              std::cout.rdbuf(outbuf);
+            }}\n
+            {rtti_register}
+            {pointer_constructor_fragment}{mex_function}''') \
+            .format(typedef_instances=typedef_instances,
+                    boost_class_export_guid=boost_class_export_guid,
+                    typedefs_collectors=typedef_collectors,
+                    delete_objs=delete_objs,
+                    rtti_register=rtti_reg_start + rtti_reg_mid + rtti_reg_end,
+                    pointer_constructor_fragment=ptr_ctor_frag,
+                    mex_function=self.mex_function())
+
+        self.content.append((self._wrapper_name() + '.cpp', wrapper_file))
+
+    def wrap_class_serialize_method(self, namespace_name, inst_class):
+        class_name = inst_class.name
+        wrapper_id = self._update_wrapper_id((namespace_name, inst_class, 'string_serialize', 'serialize'))
+
+        return textwrap.dedent('''\
+            function varargout = string_serialize(this, varargin)
+              % STRING_SERIALIZE usage: string_serialize() : returns string
+              % Doxygen can be found at https://gtsam.org/doxygen/
+              if length(varargin) == 0
+                varargout{{1}} = {wrapper}({wrapper_id}, this, varargin{{:}});
+              else
+                error('Arguments do not match any overload of function {class_name}.string_serialize');
+              end
+            end\n
+            function sobj = saveobj(obj)
+              % SAVEOBJ Saves the object to a matlab-readable format
+              sobj = obj.string_serialize();
+            end
+        ''').format(wrapper=self._wrapper_name(), wrapper_id=wrapper_id, class_name=namespace_name + '.' + class_name)
+
+    def wrap_collector_function_serialize(self, class_name, full_name='', namespace=''):
+        return textwrap.indent(textwrap.dedent('''\
+            typedef boost::shared_ptr<{full_name}> Shared;
+            checkArguments("string_serialize",nargout,nargin-1,0);
+            Shared obj = unwrap_shared_ptr<{full_name}>(in[0], "ptr_{namespace}{class_name}");
+            ostringstream out_archive_stream;
+            boost::archive::text_oarchive out_archive(out_archive_stream);
+            out_archive << *obj;
+            out[0] = wrap< string >(out_archive_stream.str());
+        ''').format(class_name=class_name, full_name=full_name, namespace=namespace),
+                               prefix='  ')
+
+    def wrap_collector_function_deserialize(self, class_name, full_name='', namespace=''):
+        return textwrap.indent(textwrap.dedent('''\
+            typedef boost::shared_ptr<{full_name}> Shared;
+            checkArguments("{namespace}{class_name}.string_deserialize",nargout,nargin,1);
+            string serialized = unwrap< string >(in[0]);
+            istringstream in_archive_stream(serialized);
+            boost::archive::text_iarchive in_archive(in_archive_stream);
+            Shared output(new {full_name}());
+            in_archive >> *output;
+            out[0] = wrap_shared_ptr(output,"{namespace}.{class_name}", false);
+        ''').format(class_name=class_name, full_name=full_name, namespace=namespace),
+                               prefix='  ')
+
+    def wrap(self):
+        self.wrap_namespace(self.module)
+        self.generate_wrapper(self.module)
+
+        return self.content
+
+
+def _generate_content(cc_content, path, verbose=False):
+    """Generate files and folders from matlab wrapper content.
+
+    Keyword arguments:
+    cc_content -- the content to generate formatted as
+        (file_name, file_content) or
+        (folder_name, [(file_name, file_content)])
+    path -- the path to the files parent folder within the main folder
+    """
+
+    def _debug(message):
+        if not verbose:
+            return
+        import sys
+        print(message, file=sys.stderr)
+
+    for c in cc_content:
+        if type(c) == list:
+            if len(c) == 0:
+                continue
+            _debug("c object: {}".format(c[0][0]))
+            path_to_folder = path + '/' + c[0][0]
+
+            if not os.path.isdir(path_to_folder):
+                try:
+                    os.makedirs(path_to_folder, exist_ok=True)
+                except OSError:
+                    pass
+
+            for sub_content in c:
+                import sys
+                _debug("sub object: {}".format(sub_content[1][0][0]))
+                _generate_content(sub_content[1], path_to_folder)
+        elif type(c[1]) == list:
+            path_to_folder = path + '/' + c[0]
+
+            _debug("[generate_content_global]: {}".format(path_to_folder))
+            if not os.path.isdir(path_to_folder):
+                try:
+                    os.makedirs(path_to_folder, exist_ok=True)
+                except OSError:
+                    pass
+            for sub_content in c[1]:
+                import sys
+                path_to_file = path_to_folder + '/' + sub_content[0]
+                _debug("[generate_global_method]: {}".format(path_to_file))
+                with open(path_to_file, 'w') as f:
+                    f.write(sub_content[1])
+        else:
+            path_to_file = path + '/' + c[0]
+
+            _debug("[generate_content]: {}".format(path_to_file))
+            if not os.path.isdir(path_to_file):
+                try:
+                    os.mkdir(path)
+                except OSError:
+                    pass
+
+            with open(path_to_file, 'w') as f:
+                f.write(c[1])
+
+
+if __name__ == "__main__":
+    arg_parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+    arg_parser.add_argument("--src", type=str, required=True, help="Input interface .h file.")
+    arg_parser.add_argument("--module_name", type=str, required=True, help="Name of the C++ class being wrapped.")
+    arg_parser.add_argument("--out", type=str, required=True, help="Name of the output folder.")
+    arg_parser.add_argument("--top_module_namespaces",
+                            type=str,
+                            default="",
+                            help="C++ namespace for the top module, e.g. `ns1::ns2::ns3`. "
+                                 "Only the content within this namespace and its sub-namespaces "
+                                 "will be wrapped. The content of this namespace will be available at "
+                                 "the top module level, and its sub-namespaces' in the submodules.\n"
+                                 "For example, `import <module_name>` gives you access to a Python "
+                                 "`<module_name>.Class` of the corresponding C++ `ns1::ns2::ns3::Class`"
+                                 ", and `from <module_name> import ns4` gives you access to a Python "
+                                 "`ns4.Class` of the C++ `ns1::ns2::ns3::ns4::Class`. ")
+    arg_parser.add_argument("--ignore",
+                            nargs='*',
+                            type=str,
+                            help="A space-separated list of classes to ignore. "
+                                 "Class names must include their full namespaces.")
+    args = arg_parser.parse_args()
+
+    top_module_namespaces = args.top_module_namespaces.split("::")
+    if top_module_namespaces[0]:
+        top_module_namespaces = [''] + top_module_namespaces
+
+    with open(args.src, 'r') as f:
+        content = f.read()
+
+    if not os.path.exists(args.src):
+        os.mkdir(args.src)
+
+    module = parser.Module.parseString(content)
+
+    instantiator.instantiate_namespace_inplace(module)
+
+    import sys
+
+    print("Ignoring classes: {}".format(args.ignore), file=sys.stderr)
+    wrapper = MatlabWrapper(module=module,
+                            module_name=args.module_name,
+                            top_module_namespace=top_module_namespaces,
+                            ignore_classes=args.ignore)
+
+    cc_content = wrapper.wrap()
+
+    _generate_content(cc_content, args.out)
diff --git a/wrap/python/pybind11/.appveyor.yml b/wrap/pybind11/.appveyor.yml
similarity index 100%
rename from wrap/python/pybind11/.appveyor.yml
rename to wrap/pybind11/.appveyor.yml
diff --git a/wrap/pybind11/.github/workflows/format.yml b/wrap/pybind11/.github/workflows/format.yml
new file mode 100644
index 000000000..e92f96e6e
--- /dev/null
+++ b/wrap/pybind11/.github/workflows/format.yml
@@ -0,0 +1,19 @@
+name: Format
+
+on:
+  workflow_dispatch:
+  pull_request:
+  push:
+    branches:
+    - master
+    - stable
+    - "v*"
+
+jobs:
+  pre-commit:
+    name: Format
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v2
+    - uses: actions/setup-python@v2
+    - uses: pre-commit/action@v2.0.0
diff --git a/wrap/python/pybind11/.gitignore b/wrap/pybind11/.gitignore
similarity index 96%
rename from wrap/python/pybind11/.gitignore
rename to wrap/pybind11/.gitignore
index 979fd4431..244bbcaa7 100644
--- a/wrap/python/pybind11/.gitignore
+++ b/wrap/pybind11/.gitignore
@@ -10,6 +10,7 @@ cmake_install.cmake
 *.sdf
 *.opensdf
 *.vcxproj
+*.vcxproj.user
 *.filters
 example.dir
 Win32
diff --git a/wrap/python/pybind11/.gitmodules b/wrap/pybind11/.gitmodules
similarity index 100%
rename from wrap/python/pybind11/.gitmodules
rename to wrap/pybind11/.gitmodules
diff --git a/wrap/pybind11/.pre-commit-config.yaml b/wrap/pybind11/.pre-commit-config.yaml
new file mode 100644
index 000000000..63e4c6f7d
--- /dev/null
+++ b/wrap/pybind11/.pre-commit-config.yaml
@@ -0,0 +1,28 @@
+repos:
+- repo: https://github.com/pre-commit/pre-commit-hooks
+  rev: v3.1.0
+  hooks:
+  - id: check-added-large-files
+  - id: check-case-conflict
+  - id: check-merge-conflict
+  - id: check-symlinks
+  - id: check-yaml
+  - id: debug-statements
+  - id: end-of-file-fixer
+  - id: mixed-line-ending
+  - id: requirements-txt-fixer
+  - id: trailing-whitespace
+  - id: fix-encoding-pragma
+
+- repo: https://github.com/Lucas-C/pre-commit-hooks
+  rev: v1.1.7
+  hooks:
+  - id: remove-tabs
+    exclude: (Makefile|debian/rules|.gitmodules)(\.in)?$
+
+- repo: https://gitlab.com/pycqa/flake8
+  rev: 3.8.2
+  hooks:
+  - id: flake8
+    additional_dependencies: [flake8-bugbear]
+    exclude: ^(docs/.*|tools/.*)$
diff --git a/wrap/python/pybind11/.readthedocs.yml b/wrap/pybind11/.readthedocs.yml
similarity index 100%
rename from wrap/python/pybind11/.readthedocs.yml
rename to wrap/pybind11/.readthedocs.yml
diff --git a/wrap/python/pybind11/.travis.yml b/wrap/pybind11/.travis.yml
similarity index 73%
rename from wrap/python/pybind11/.travis.yml
rename to wrap/pybind11/.travis.yml
index 4cc5cf07c..12067bca8 100644
--- a/wrap/python/pybind11/.travis.yml
+++ b/wrap/pybind11/.travis.yml
@@ -16,7 +16,9 @@ matrix:
     - PY_CMD=python3
     - $PY_CMD -m pip install --user --upgrade pip wheel setuptools
     install:
-    - $PY_CMD -m pip install --user --upgrade sphinx sphinx_rtd_theme breathe flake8 pep8-naming pytest
+    # breathe 4.14 doesn't work with bit fields. See https://github.com/michaeljones/breathe/issues/462
+    # Latest breathe + Sphinx causes warnings and errors out
+    - $PY_CMD -m pip install --user --upgrade "sphinx<3" sphinx_rtd_theme breathe==4.13.1 flake8 pep8-naming pytest
     - curl -fsSL https://sourceforge.net/projects/doxygen/files/rel-1.8.15/doxygen-1.8.15.linux.bin.tar.gz/download | tar xz
     - export PATH="$PWD/doxygen-1.8.15/bin:$PATH"
     script:
@@ -32,8 +34,7 @@ matrix:
     - |
       # Barebones build
       cmake -DCMAKE_BUILD_TYPE=Debug -DPYBIND11_WERROR=ON -DDOWNLOAD_CATCH=ON -DPYTHON_EXECUTABLE=$(which $PY_CMD) .
-      make pytest -j 2
-      make cpptest -j 2
+      make pytest -j 2 && make cpptest -j 2
   # The following are regular test configurations, including optional dependencies.
   # With regard to each other they differ in Python version, C++ standard and compiler.
   - os: linux
@@ -61,7 +62,7 @@ matrix:
   - os: linux
     dist: trusty
     env: PYTHON=2.7 CPP=14 GCC=6 CMAKE=1
-    name: Python 2.7, c++14, gcc 4.8, CMake test
+    name: Python 2.7, c++14, gcc 6, CMake test
     addons:
       apt:
         sources:
@@ -106,31 +107,77 @@ matrix:
           - lld-7
           - libc++-7-dev
           - libc++abi-7-dev  # Why is this necessary???
+  - os: linux
+    dist: xenial
+    env: PYTHON=3.8 CPP=17 GCC=7
+    name: Python 3.8, c++17, gcc 7
+    addons:
+      apt:
+        sources:
+          - deadsnakes
+          - ubuntu-toolchain-r-test
+        packages:
+          - g++-7
+          - python3.8-dev
+          - python3.8-venv
+  - os: linux
+    dist: xenial
+    env: PYTHON=3.9 CPP=17 GCC=7
+    name: Python 3.9 beta, c++17, gcc 7 (w/o numpy/scipy) # TODO: update build name when the numpy/scipy wheels become available
+    addons:
+      apt:
+        sources:
+          - deadsnakes
+          - ubuntu-toolchain-r-test
+        packages:
+          - g++-7
+          - python3.9-dev
+          - python3.9-venv
+    # Currently there are no numpy/scipy wheels available for python3.9
+    # TODO: remove next install and script clause when the wheels become available
+    install:
+      - $PY_CMD -m pip install --user --upgrade pytest
+    script:
+      - |
+        # Barebones build
+        cmake -DCMAKE_BUILD_TYPE=Debug -DPYBIND11_WERROR=ON -DDOWNLOAD_CATCH=ON -DPYTHON_EXECUTABLE=$(which $PY_CMD) .
+        make pytest -j 2 && make cpptest -j 2
   - os: osx
     name: Python 2.7, c++14, AppleClang 7.3, CMake test
     osx_image: xcode7.3
     env: PYTHON=2.7 CPP=14 CLANG CMAKE=1
   - os: osx
-    name: Python 3.7, c++14, AppleClang 9, Debug build
-    osx_image: xcode9
-    env: PYTHON=3.7 CPP=14 CLANG DEBUG=1
+    name: Python 3.8, c++14, AppleClang 9, Debug build
+    osx_image: xcode9.4
+    env: PYTHON=3.8 CPP=14 CLANG DEBUG=1
   # Test a PyPy 2.7 build
   - os: linux
     dist: trusty
-    env: PYPY=5.8 PYTHON=2.7 CPP=11 GCC=4.8
-    name: PyPy 5.8, Python 2.7, c++11, gcc 4.8
+    env: PYPY=7.3.1 PYTHON=2.7 CPP=11 GCC=4.8
+    name: PyPy 7.3, Python 2.7, c++11, gcc 4.8
     addons:
       apt:
         packages:
           - libblas-dev
           - liblapack-dev
           - gfortran
+  - os: linux
+    dist: xenial
+    env: PYPY=7.3.1 PYTHON=3.6 CPP=11 GCC=5
+    name: PyPy 7.3, Python 3.6, c++11, gcc 5
+    addons:
+      apt:
+        packages:
+          - libblas-dev
+          - liblapack-dev
+          - gfortran
+          - g++-5
   # Build in 32-bit mode and tests against the CMake-installed version
   - os: linux
     dist: trusty
     services: docker
     env: DOCKER=i386/debian:stretch PYTHON=3.5 CPP=14 GCC=6 INSTALL=1
-    name: Python 3.4, c++14, gcc 6, 32-bit
+    name: Python 3.5, c++14, gcc 6, 32-bit
     script:
       - |
         # Consolidated 32-bit Docker Build + Install
@@ -144,6 +191,9 @@ matrix:
           cmake ../pybind11-tests ${CMAKE_EXTRA_ARGS} -DPYBIND11_WERROR=ON
           make pytest -j 2"
         set +ex
+  allow_failures:
+    - name: Python 3.9 beta, c++17, gcc 7 (w/o numpy/scipy)
+    - name: PyPy 7.3, Python 2.7, c++11, gcc 4.8
 cache:
   directories:
   - $HOME/.local/bin
@@ -185,15 +235,15 @@ before_install:
     SCRIPT_RUN_PREFIX="docker exec --tty $containerid"
     $SCRIPT_RUN_PREFIX sh -c 'for s in 0 15; do sleep $s; apt-get update && apt-get -qy dist-upgrade && break; done'
   else
-    if [ "$PYPY" = "5.8" ]; then
-      curl -fSL https://bitbucket.org/pypy/pypy/downloads/pypy2-v5.8.0-linux64.tar.bz2 | tar xj
-      PY_CMD=$(echo `pwd`/pypy2-v5.8.0-linux64/bin/pypy)
+    if [ -n "$PYPY" ]; then
+      curl -fSL https://bitbucket.org/pypy/pypy/downloads/pypy$PYTHON-v$PYPY-linux64.tar.bz2 | tar xj
+      PY_CMD=$(echo `pwd`/pypy$PYTHON-v$PYPY-linux64/bin/pypy$PY)
       CMAKE_EXTRA_ARGS+=" -DPYTHON_EXECUTABLE:FILEPATH=$PY_CMD"
     else
       PY_CMD=python$PYTHON
       if [ "$TRAVIS_OS_NAME" = "osx" ]; then
         if [ "$PY" = "3" ]; then
-          brew update && brew upgrade python
+          brew update && brew unlink python@2 && (brew upgrade python || brew install python)
         else
           curl -fsSL https://bootstrap.pypa.io/get-pip.py | $PY_CMD - --user
         fi
@@ -227,14 +277,17 @@ install:
     fi
 
     export NPY_NUM_BUILD_JOBS=2
-    echo "Installing pytest, numpy, scipy..."
     local PIP_CMD=""
-    if [ -n $PYPY ]; then
+    if [ -n "$PYPY" ]; then
       # For expediency, install only versions that are available on the extra index.
+      echo "Not installing numpy, scipy as working wheels are not available"
+      # travis_wait 30 $PY_CMD -m pip install --user --upgrade --extra-index-url https://antocuni.github.io/pypy-wheels/manylinux2010 \
+      #    numpy scipy
+      echo "Installing pytest"
       travis_wait 30 \
-        $PY_CMD -m pip install --user --upgrade --extra-index-url https://imaginary.ca/trusty-pypi \
-          pytest numpy==1.15.4 scipy==1.2.0
+        $PY_CMD -m pip install --user --upgrade pytest
     else
+      echo "Installing pytest, numpy, scipy..."
       $PY_CMD -m pip install --user --upgrade pytest numpy scipy
     fi
     echo "done."
diff --git a/wrap/python/pybind11/CMakeLists.txt b/wrap/pybind11/CMakeLists.txt
similarity index 100%
rename from wrap/python/pybind11/CMakeLists.txt
rename to wrap/pybind11/CMakeLists.txt
diff --git a/wrap/python/pybind11/CONTRIBUTING.md b/wrap/pybind11/CONTRIBUTING.md
similarity index 87%
rename from wrap/python/pybind11/CONTRIBUTING.md
rename to wrap/pybind11/CONTRIBUTING.md
index 01596d94f..099791812 100644
--- a/wrap/python/pybind11/CONTRIBUTING.md
+++ b/wrap/pybind11/CONTRIBUTING.md
@@ -27,11 +27,16 @@ adhere to the following rules to make the process as smooth as possible:
   do add value by themselves.
 * Add tests for any new functionality and run the test suite (``make pytest``)
   to ensure that no existing features break.
-* Please run ``flake8`` and ``tools/check-style.sh`` to check your code matches
-  the project style. (Note that ``check-style.sh`` requires ``gawk``.)
+* Please run [``pre-commit``][pre-commit] and ``tools/check-style.sh`` to check
+  your code matches the project style. (Note that ``check-style.sh`` requires
+  ``gawk``.) Use `pre-commit run --all-files` before committing (or use
+  installed-mode, check pre-commit docs) to verify your code passes before
+  pushing to save time.
 * This project has a strong focus on providing general solutions using a
   minimal amount of code, thus small pull requests are greatly preferred.
 
+[pre-commit]: https://pre-commit.com
+
 ### Licensing of contributions
 
 pybind11 is provided under a BSD-style license that can be found in the
diff --git a/wrap/python/pybind11/ISSUE_TEMPLATE.md b/wrap/pybind11/ISSUE_TEMPLATE.md
similarity index 100%
rename from wrap/python/pybind11/ISSUE_TEMPLATE.md
rename to wrap/pybind11/ISSUE_TEMPLATE.md
diff --git a/wrap/python/pybind11/LICENSE b/wrap/pybind11/LICENSE
similarity index 100%
rename from wrap/python/pybind11/LICENSE
rename to wrap/pybind11/LICENSE
diff --git a/wrap/python/pybind11/MANIFEST.in b/wrap/pybind11/MANIFEST.in
similarity index 100%
rename from wrap/python/pybind11/MANIFEST.in
rename to wrap/pybind11/MANIFEST.in
diff --git a/wrap/python/pybind11/README.md b/wrap/pybind11/README.md
similarity index 100%
rename from wrap/python/pybind11/README.md
rename to wrap/pybind11/README.md
diff --git a/wrap/python/pybind11/docs/Doxyfile b/wrap/pybind11/docs/Doxyfile
similarity index 85%
rename from wrap/python/pybind11/docs/Doxyfile
rename to wrap/pybind11/docs/Doxyfile
index 1b9d1297c..24ece0d8d 100644
--- a/wrap/python/pybind11/docs/Doxyfile
+++ b/wrap/pybind11/docs/Doxyfile
@@ -18,3 +18,5 @@ ALIASES               += "endrst=\endverbatim"
 QUIET                  = YES
 WARNINGS               = YES
 WARN_IF_UNDOCUMENTED   = NO
+PREDEFINED             = DOXYGEN_SHOULD_SKIP_THIS \
+                         PY_MAJOR_VERSION=3
diff --git a/wrap/pybind11/docs/Makefile b/wrap/pybind11/docs/Makefile
new file mode 100644
index 000000000..511b47c2d
--- /dev/null
+++ b/wrap/pybind11/docs/Makefile
@@ -0,0 +1,192 @@
+# Makefile for Sphinx documentation
+#
+
+# You can set these variables from the command line.
+SPHINXOPTS    =
+SPHINXBUILD   = sphinx-build
+PAPER         =
+BUILDDIR      = .build
+
+# User-friendly check for sphinx-build
+ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1)
+$(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don't have Sphinx installed, grab it from http://sphinx-doc.org/)
+endif
+
+# Internal variables.
+PAPEROPT_a4     = -D latex_paper_size=a4
+PAPEROPT_letter = -D latex_paper_size=letter
+ALLSPHINXOPTS   = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) .
+# the i18n builder cannot share the environment and doctrees with the others
+I18NSPHINXOPTS  = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) .
+
+.PHONY: help clean html dirhtml singlehtml pickle json htmlhelp qthelp devhelp epub latex latexpdf text man changes linkcheck doctest coverage gettext
+
+help:
+	@echo "Please use \`make <target>' where <target> is one of"
+	@echo "  html       to make standalone HTML files"
+	@echo "  dirhtml    to make HTML files named index.html in directories"
+	@echo "  singlehtml to make a single large HTML file"
+	@echo "  pickle     to make pickle files"
+	@echo "  json       to make JSON files"
+	@echo "  htmlhelp   to make HTML files and a HTML help project"
+	@echo "  qthelp     to make HTML files and a qthelp project"
+	@echo "  applehelp  to make an Apple Help Book"
+	@echo "  devhelp    to make HTML files and a Devhelp project"
+	@echo "  epub       to make an epub"
+	@echo "  latex      to make LaTeX files, you can set PAPER=a4 or PAPER=letter"
+	@echo "  latexpdf   to make LaTeX files and run them through pdflatex"
+	@echo "  latexpdfja to make LaTeX files and run them through platex/dvipdfmx"
+	@echo "  text       to make text files"
+	@echo "  man        to make manual pages"
+	@echo "  texinfo    to make Texinfo files"
+	@echo "  info       to make Texinfo files and run them through makeinfo"
+	@echo "  gettext    to make PO message catalogs"
+	@echo "  changes    to make an overview of all changed/added/deprecated items"
+	@echo "  xml        to make Docutils-native XML files"
+	@echo "  pseudoxml  to make pseudoxml-XML files for display purposes"
+	@echo "  linkcheck  to check all external links for integrity"
+	@echo "  doctest    to run all doctests embedded in the documentation (if enabled)"
+	@echo "  coverage   to run coverage check of the documentation (if enabled)"
+
+clean:
+	rm -rf $(BUILDDIR)/*
+
+html:
+	$(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html
+	@echo
+	@echo "Build finished. The HTML pages are in $(BUILDDIR)/html."
+
+dirhtml:
+	$(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml
+	@echo
+	@echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml."
+
+singlehtml:
+	$(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml
+	@echo
+	@echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml."
+
+pickle:
+	$(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle
+	@echo
+	@echo "Build finished; now you can process the pickle files."
+
+json:
+	$(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json
+	@echo
+	@echo "Build finished; now you can process the JSON files."
+
+htmlhelp:
+	$(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp
+	@echo
+	@echo "Build finished; now you can run HTML Help Workshop with the" \
+	      ".hhp project file in $(BUILDDIR)/htmlhelp."
+
+qthelp:
+	$(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp
+	@echo
+	@echo "Build finished; now you can run "qcollectiongenerator" with the" \
+	      ".qhcp project file in $(BUILDDIR)/qthelp, like this:"
+	@echo "# qcollectiongenerator $(BUILDDIR)/qthelp/pybind11.qhcp"
+	@echo "To view the help file:"
+	@echo "# assistant -collectionFile $(BUILDDIR)/qthelp/pybind11.qhc"
+
+applehelp:
+	$(SPHINXBUILD) -b applehelp $(ALLSPHINXOPTS) $(BUILDDIR)/applehelp
+	@echo
+	@echo "Build finished. The help book is in $(BUILDDIR)/applehelp."
+	@echo "N.B. You won't be able to view it unless you put it in" \
+	      "~/Library/Documentation/Help or install it in your application" \
+	      "bundle."
+
+devhelp:
+	$(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp
+	@echo
+	@echo "Build finished."
+	@echo "To view the help file:"
+	@echo "# mkdir -p $$HOME/.local/share/devhelp/pybind11"
+	@echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/pybind11"
+	@echo "# devhelp"
+
+epub:
+	$(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub
+	@echo
+	@echo "Build finished. The epub file is in $(BUILDDIR)/epub."
+
+latex:
+	$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex
+	@echo
+	@echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex."
+	@echo "Run \`make' in that directory to run these through (pdf)latex" \
+	      "(use \`make latexpdf' here to do that automatically)."
+
+latexpdf:
+	$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex
+	@echo "Running LaTeX files through pdflatex..."
+	$(MAKE) -C $(BUILDDIR)/latex all-pdf
+	@echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex."
+
+latexpdfja:
+	$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex
+	@echo "Running LaTeX files through platex and dvipdfmx..."
+	$(MAKE) -C $(BUILDDIR)/latex all-pdf-ja
+	@echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex."
+
+text:
+	$(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text
+	@echo
+	@echo "Build finished. The text files are in $(BUILDDIR)/text."
+
+man:
+	$(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man
+	@echo
+	@echo "Build finished. The manual pages are in $(BUILDDIR)/man."
+
+texinfo:
+	$(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo
+	@echo
+	@echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo."
+	@echo "Run \`make' in that directory to run these through makeinfo" \
+	      "(use \`make info' here to do that automatically)."
+
+info:
+	$(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo
+	@echo "Running Texinfo files through makeinfo..."
+	make -C $(BUILDDIR)/texinfo info
+	@echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo."
+
+gettext:
+	$(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale
+	@echo
+	@echo "Build finished. The message catalogs are in $(BUILDDIR)/locale."
+
+changes:
+	$(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes
+	@echo
+	@echo "The overview file is in $(BUILDDIR)/changes."
+
+linkcheck:
+	$(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck
+	@echo
+	@echo "Link check complete; look for any errors in the above output " \
+	      "or in $(BUILDDIR)/linkcheck/output.txt."
+
+doctest:
+	$(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest
+	@echo "Testing of doctests in the sources finished, look at the " \
+	      "results in $(BUILDDIR)/doctest/output.txt."
+
+coverage:
+	$(SPHINXBUILD) -b coverage $(ALLSPHINXOPTS) $(BUILDDIR)/coverage
+	@echo "Testing of coverage in the sources finished, look at the " \
+	      "results in $(BUILDDIR)/coverage/python.txt."
+
+xml:
+	$(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml
+	@echo
+	@echo "Build finished. The XML files are in $(BUILDDIR)/xml."
+
+pseudoxml:
+	$(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml
+	@echo
+	@echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml."
diff --git a/wrap/python/pybind11/docs/_static/theme_overrides.css b/wrap/pybind11/docs/_static/theme_overrides.css
similarity index 100%
rename from wrap/python/pybind11/docs/_static/theme_overrides.css
rename to wrap/pybind11/docs/_static/theme_overrides.css
diff --git a/wrap/python/pybind11/docs/advanced/cast/chrono.rst b/wrap/pybind11/docs/advanced/cast/chrono.rst
similarity index 97%
rename from wrap/python/pybind11/docs/advanced/cast/chrono.rst
rename to wrap/pybind11/docs/advanced/cast/chrono.rst
index 8c6b3d7e5..fbd46057a 100644
--- a/wrap/python/pybind11/docs/advanced/cast/chrono.rst
+++ b/wrap/pybind11/docs/advanced/cast/chrono.rst
@@ -59,7 +59,7 @@ Provided conversions
 
 .. rubric:: Python to C++
 
-- ``datetime.datetime`` → ``std::chrono::system_clock::time_point``
+- ``datetime.datetime`` or ``datetime.date`` or ``datetime.time`` → ``std::chrono::system_clock::time_point``
     Date/time objects are converted into system clock timepoints. Any
     timezone information is ignored and the type is treated as a naive
     object.
diff --git a/wrap/python/pybind11/docs/advanced/cast/custom.rst b/wrap/pybind11/docs/advanced/cast/custom.rst
similarity index 100%
rename from wrap/python/pybind11/docs/advanced/cast/custom.rst
rename to wrap/pybind11/docs/advanced/cast/custom.rst
diff --git a/wrap/python/pybind11/docs/advanced/cast/eigen.rst b/wrap/pybind11/docs/advanced/cast/eigen.rst
similarity index 100%
rename from wrap/python/pybind11/docs/advanced/cast/eigen.rst
rename to wrap/pybind11/docs/advanced/cast/eigen.rst
diff --git a/wrap/python/pybind11/docs/advanced/cast/functional.rst b/wrap/pybind11/docs/advanced/cast/functional.rst
similarity index 100%
rename from wrap/python/pybind11/docs/advanced/cast/functional.rst
rename to wrap/pybind11/docs/advanced/cast/functional.rst
diff --git a/wrap/python/pybind11/docs/advanced/cast/index.rst b/wrap/pybind11/docs/advanced/cast/index.rst
similarity index 99%
rename from wrap/python/pybind11/docs/advanced/cast/index.rst
rename to wrap/pybind11/docs/advanced/cast/index.rst
index 54c10570b..724585c92 100644
--- a/wrap/python/pybind11/docs/advanced/cast/index.rst
+++ b/wrap/pybind11/docs/advanced/cast/index.rst
@@ -39,4 +39,3 @@ the last case of the above list.
    chrono
    eigen
    custom
-
diff --git a/wrap/python/pybind11/docs/advanced/cast/overview.rst b/wrap/pybind11/docs/advanced/cast/overview.rst
similarity index 100%
rename from wrap/python/pybind11/docs/advanced/cast/overview.rst
rename to wrap/pybind11/docs/advanced/cast/overview.rst
diff --git a/wrap/python/pybind11/docs/advanced/cast/stl.rst b/wrap/pybind11/docs/advanced/cast/stl.rst
similarity index 100%
rename from wrap/python/pybind11/docs/advanced/cast/stl.rst
rename to wrap/pybind11/docs/advanced/cast/stl.rst
diff --git a/wrap/python/pybind11/docs/advanced/cast/strings.rst b/wrap/pybind11/docs/advanced/cast/strings.rst
similarity index 100%
rename from wrap/python/pybind11/docs/advanced/cast/strings.rst
rename to wrap/pybind11/docs/advanced/cast/strings.rst
diff --git a/wrap/python/pybind11/docs/advanced/classes.rst b/wrap/pybind11/docs/advanced/classes.rst
similarity index 93%
rename from wrap/python/pybind11/docs/advanced/classes.rst
rename to wrap/pybind11/docs/advanced/classes.rst
index c9a0da5a1..031484c9d 100644
--- a/wrap/python/pybind11/docs/advanced/classes.rst
+++ b/wrap/pybind11/docs/advanced/classes.rst
@@ -82,7 +82,7 @@ helper class that is defined as follows:
 
 The macro :c:macro:`PYBIND11_OVERLOAD_PURE` should be used for pure virtual
 functions, and :c:macro:`PYBIND11_OVERLOAD` should be used for functions which have
-a default implementation.  There are also two alternate macros 
+a default implementation.  There are also two alternate macros
 :c:macro:`PYBIND11_OVERLOAD_PURE_NAME` and :c:macro:`PYBIND11_OVERLOAD_NAME` which
 take a string-valued name argument between the *Parent class* and *Name of the
 function* slots, which defines the name of function in Python. This is required
@@ -149,6 +149,11 @@ memory for the C++ portion of the instance will be left uninitialized, which
 will generally leave the C++ instance in an invalid state and cause undefined
 behavior if the C++ instance is subsequently used.
 
+.. versionadded:: 2.5.1
+
+   The default pybind11 metaclass will throw a ``TypeError`` when it detects
+   that ``__init__`` was not called by a derived class.
+
 Here is an example:
 
 .. code-block:: python
@@ -298,8 +303,8 @@ The classes are then registered with pybind11 using:
 .. code-block:: cpp
 
     py::class_<Animal, PyAnimal<>> animal(m, "Animal");
-    py::class_<Dog, PyDog<>> dog(m, "Dog");
-    py::class_<Husky, PyDog<Husky>> husky(m, "Husky");
+    py::class_<Dog, Animal, PyDog<>> dog(m, "Dog");
+    py::class_<Husky, Dog, PyDog<Husky>> husky(m, "Husky");
     // ... add animal, dog, husky definitions
 
 Note that ``Husky`` did not require a dedicated trampoline template class at
@@ -662,6 +667,7 @@ to Python.
             .def(py::self *= float())
             .def(float() * py::self)
             .def(py::self * float())
+            .def(-py::self)
             .def("__repr__", &Vector2::toString);
     }
 
@@ -767,13 +773,17 @@ An instance can now be pickled as follows:
     p.setExtra(15)
     data = pickle.dumps(p, 2)
 
-Note that only the cPickle module is supported on Python 2.7. The second
-argument to ``dumps`` is also crucial: it selects the pickle protocol version
-2, since the older version 1 is not supported. Newer versions are also fine—for
-instance, specify ``-1`` to always use the latest available version. Beware:
-failure to follow these instructions will cause important pybind11 memory
-allocation routines to be skipped during unpickling, which will likely lead to
-memory corruption and/or segmentation faults.
+
+.. note::
+    Note that only the cPickle module is supported on Python 2.7.
+
+    The second argument to ``dumps`` is also crucial: it selects the pickle
+    protocol version 2, since the older version 1 is not supported. Newer
+    versions are also fine—for instance, specify ``-1`` to always use the
+    latest available version. Beware: failure to follow these instructions
+    will cause important pybind11 memory allocation routines to be skipped
+    during unpickling, which will likely lead to memory corruption and/or
+    segmentation faults.
 
 .. seealso::
 
@@ -783,6 +793,38 @@ memory corruption and/or segmentation faults.
 
 .. [#f3] http://docs.python.org/3/library/pickle.html#pickling-class-instances
 
+Deepcopy support
+================
+
+Python normally uses references in assignments. Sometimes a real copy is needed
+to prevent changing all copies. The ``copy`` module [#f5]_ provides these
+capabilities.
+
+On Python 3, a class with pickle support is automatically also (deep)copy
+compatible. However, performance can be improved by adding custom
+``__copy__`` and ``__deepcopy__`` methods. With Python 2.7, these custom methods
+are mandatory for (deep)copy compatibility, because pybind11 only supports
+cPickle.
+
+For simple classes (deep)copy can be enabled by using the copy constructor,
+which should look as follows:
+
+.. code-block:: cpp
+
+    py::class_<Copyable>(m, "Copyable")
+        .def("__copy__",  [](const Copyable &self) {
+            return Copyable(self);
+        })
+        .def("__deepcopy__", [](const Copyable &self, py::dict) {
+            return Copyable(self);
+        }, "memo"_a);
+
+.. note::
+
+    Dynamic attributes will not be copied in this example.
+
+.. [#f5] https://docs.python.org/3/library/copy.html
+
 Multiple Inheritance
 ====================
 
@@ -1041,6 +1083,32 @@ described trampoline:
     ``.def("foo", static_cast<int (A::*)() const>(&Publicist::foo));``
     where ``int (A::*)() const`` is the type of ``A::foo``.
 
+Binding final classes
+=====================
+
+Some classes may not be appropriate to inherit from. In C++11, classes can
+use the ``final`` specifier to ensure that a class cannot be inherited from.
+The ``py::is_final`` attribute can be used to ensure that Python classes
+cannot inherit from a specified type. The underlying C++ type does not need
+to be declared final.
+
+.. code-block:: cpp
+
+    class IsFinal final {};
+
+    py::class_<IsFinal>(m, "IsFinal", py::is_final());
+
+When you try to inherit from such a class in Python, you will now get this
+error:
+
+.. code-block:: pycon
+
+    >>> class PyFinalChild(IsFinal):
+    ...     pass
+    TypeError: type 'IsFinal' is not an acceptable base type
+
+.. note:: This attribute is currently ignored on PyPy
+
 Custom automatic downcasters
 ============================
 
diff --git a/wrap/python/pybind11/docs/advanced/embedding.rst b/wrap/pybind11/docs/advanced/embedding.rst
similarity index 100%
rename from wrap/python/pybind11/docs/advanced/embedding.rst
rename to wrap/pybind11/docs/advanced/embedding.rst
diff --git a/wrap/python/pybind11/docs/advanced/exceptions.rst b/wrap/pybind11/docs/advanced/exceptions.rst
similarity index 97%
rename from wrap/python/pybind11/docs/advanced/exceptions.rst
rename to wrap/pybind11/docs/advanced/exceptions.rst
index 75ac24ae9..75ad7f7f4 100644
--- a/wrap/python/pybind11/docs/advanced/exceptions.rst
+++ b/wrap/pybind11/docs/advanced/exceptions.rst
@@ -28,6 +28,8 @@ exceptions:
 +--------------------------------------+--------------------------------------+
 | :class:`std::range_error`            | ``ValueError``                       |
 +--------------------------------------+--------------------------------------+
+| :class:`std::overflow_error`         | ``OverflowError``                    |
++--------------------------------------+--------------------------------------+
 | :class:`pybind11::stop_iteration`    | ``StopIteration`` (used to implement |
 |                                      | custom iterators)                    |
 +--------------------------------------+--------------------------------------+
diff --git a/wrap/python/pybind11/docs/advanced/functions.rst b/wrap/pybind11/docs/advanced/functions.rst
similarity index 96%
rename from wrap/python/pybind11/docs/advanced/functions.rst
rename to wrap/pybind11/docs/advanced/functions.rst
index 3e1a3ff0e..984b046ef 100644
--- a/wrap/python/pybind11/docs/advanced/functions.rst
+++ b/wrap/pybind11/docs/advanced/functions.rst
@@ -362,6 +362,34 @@ like so:
     py::class_<MyClass>("MyClass")
         .def("myFunction", py::arg("arg") = (SomeType *) nullptr);
 
+Keyword-only arguments
+======================
+
+Python 3 introduced keyword-only arguments by specifying an unnamed ``*``
+argument in a function definition:
+
+.. code-block:: python
+
+    def f(a, *, b):  # a can be positional or via keyword; b must be via keyword
+        pass
+
+    f(a=1, b=2)  # good
+    f(b=2, a=1)  # good
+    f(1, b=2)    # good
+    f(1, 2)      # TypeError: f() takes 1 positional argument but 2 were given
+
+Pybind11 provides a ``py::kwonly`` object that allows you to implement
+the same behaviour by specifying the object between positional and keyword-only
+argument annotations when registering the function:
+
+.. code-block:: cpp
+
+    m.def("f", [](int a, int b) { /* ... */ },
+          py::arg("a"), py::kwonly(), py::arg("b"));
+
+Note that, as in Python, you cannot combine this with a ``py::args`` argument.
+This feature does *not* require Python 3 to work.
+
 .. _nonconverting_arguments:
 
 Non-converting arguments
diff --git a/wrap/python/pybind11/docs/advanced/misc.rst b/wrap/pybind11/docs/advanced/misc.rst
similarity index 98%
rename from wrap/python/pybind11/docs/advanced/misc.rst
rename to wrap/pybind11/docs/advanced/misc.rst
index 5b38ec759..7798462df 100644
--- a/wrap/python/pybind11/docs/advanced/misc.rst
+++ b/wrap/pybind11/docs/advanced/misc.rst
@@ -218,7 +218,7 @@ collected:
 
 Both approaches also expose a potentially dangerous ``_cleanup`` attribute in
 Python, which may be undesirable from an API standpoint (a premature explicit
-call from Python might lead to undefined behavior). Yet another approach that 
+call from Python might lead to undefined behavior). Yet another approach that
 avoids this issue involves weak reference with a cleanup callback:
 
 .. code-block:: cpp
@@ -283,9 +283,9 @@ work, it is important that all lines are indented consistently, i.e.:
         ----------
     )mydelimiter");
 
-By default, pybind11 automatically generates and prepends a signature to the docstring of a function 
+By default, pybind11 automatically generates and prepends a signature to the docstring of a function
 registered with ``module::def()`` and ``class_::def()``. Sometimes this
-behavior is not desirable, because you want to provide your own signature or remove 
+behavior is not desirable, because you want to provide your own signature or remove
 the docstring completely to exclude the function from the Sphinx documentation.
 The class ``options`` allows you to selectively suppress auto-generated signatures:
 
@@ -298,8 +298,8 @@ The class ``options`` allows you to selectively suppress auto-generated signatur
         m.def("add", [](int a, int b) { return a + b; }, "A function which adds two numbers");
     }
 
-Note that changes to the settings affect only function bindings created during the 
-lifetime of the ``options`` instance. When it goes out of scope at the end of the module's init function, 
+Note that changes to the settings affect only function bindings created during the
+lifetime of the ``options`` instance. When it goes out of scope at the end of the module's init function,
 the default settings are restored to prevent unwanted side effects.
 
 .. [#f4] http://www.sphinx-doc.org
diff --git a/wrap/python/pybind11/docs/advanced/pycpp/index.rst b/wrap/pybind11/docs/advanced/pycpp/index.rst
similarity index 100%
rename from wrap/python/pybind11/docs/advanced/pycpp/index.rst
rename to wrap/pybind11/docs/advanced/pycpp/index.rst
diff --git a/wrap/python/pybind11/docs/advanced/pycpp/numpy.rst b/wrap/pybind11/docs/advanced/pycpp/numpy.rst
similarity index 90%
rename from wrap/python/pybind11/docs/advanced/pycpp/numpy.rst
rename to wrap/pybind11/docs/advanced/pycpp/numpy.rst
index 458f99e97..f1941392f 100644
--- a/wrap/python/pybind11/docs/advanced/pycpp/numpy.rst
+++ b/wrap/pybind11/docs/advanced/pycpp/numpy.rst
@@ -67,7 +67,7 @@ specification.
 To create a C++ function that can take a Python buffer object as an argument,
 simply use the type ``py::buffer`` as one of its arguments. Buffers can exist
 in a great variety of configurations, hence some safety checks are usually
-necessary in the function body. Below, you can see an basic example on how to
+necessary in the function body. Below, you can see a basic example on how to
 define a custom constructor for the Eigen double precision matrix
 (``Eigen::MatrixXd``) type, which supports initialization from compatible
 buffer objects (e.g. a NumPy matrix).
@@ -81,7 +81,7 @@ buffer objects (e.g. a NumPy matrix).
     constexpr bool rowMajor = Matrix::Flags & Eigen::RowMajorBit;
 
     py::class_<Matrix>(m, "Matrix", py::buffer_protocol())
-        .def("__init__", [](Matrix &m, py::buffer b) {
+        .def("__init__", [](py::buffer b) {
             typedef Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> Strides;
 
             /* Request a buffer descriptor from Python */
@@ -101,7 +101,7 @@ buffer objects (e.g. a NumPy matrix).
             auto map = Eigen::Map<Matrix, 0, Strides>(
                 static_cast<Scalar *>(info.ptr), info.shape[0], info.shape[1], strides);
 
-            new (&m) Matrix(map);
+            return Matrix(m);
         });
 
 For reference, the ``def_buffer()`` call for this Eigen data type should look
@@ -384,3 +384,45 @@ operation on the C++ side:
 
    py::array a = /* A NumPy array */;
    py::array b = a[py::make_tuple(0, py::ellipsis(), 0)];
+
+Memory view
+===========
+
+For a case when we simply want to provide a direct accessor to C/C++ buffer
+without a concrete class object, we can return a ``memoryview`` object. Suppose
+we wish to expose a ``memoryview`` for 2x4 uint8_t array, we can do the
+following:
+
+.. code-block:: cpp
+
+    const uint8_t buffer[] = {
+        0, 1, 2, 3,
+        4, 5, 6, 7
+    };
+    m.def("get_memoryview2d", []() {
+        return py::memoryview::from_buffer(
+            buffer,                                    // buffer pointer
+            { 2, 4 },                                  // shape (rows, cols)
+            { sizeof(uint8_t) * 4, sizeof(uint8_t) }   // strides in bytes
+        );
+    })
+
+This approach is meant for providing a ``memoryview`` for a C/C++ buffer not
+managed by Python. The user is responsible for managing the lifetime of the
+buffer. Using a ``memoryview`` created in this way after deleting the buffer in
+C++ side results in undefined behavior.
+
+We can also use ``memoryview::from_memory`` for a simple 1D contiguous buffer:
+
+.. code-block:: cpp
+
+    m.def("get_memoryview1d", []() {
+        return py::memoryview::from_memory(
+            buffer,               // buffer pointer
+            sizeof(uint8_t) * 8   // buffer size
+        );
+    })
+
+.. note::
+
+    ``memoryview::from_memory`` is not available in Python 2.
diff --git a/wrap/python/pybind11/docs/advanced/pycpp/object.rst b/wrap/pybind11/docs/advanced/pycpp/object.rst
similarity index 99%
rename from wrap/python/pybind11/docs/advanced/pycpp/object.rst
rename to wrap/pybind11/docs/advanced/pycpp/object.rst
index 117131edc..19a226a87 100644
--- a/wrap/python/pybind11/docs/advanced/pycpp/object.rst
+++ b/wrap/pybind11/docs/advanced/pycpp/object.rst
@@ -60,7 +60,7 @@ This example obtains a reference to the Python ``Decimal`` class.
 Calling Python functions
 ========================
 
-It is also possible to call Python classes, functions and methods 
+It is also possible to call Python classes, functions and methods
 via ``operator()``.
 
 .. code-block:: cpp
@@ -75,7 +75,7 @@ via ``operator()``.
     py::object makedirs = os.attr("makedirs");
     makedirs("/tmp/path/to/somewhere");
 
-One can convert the result obtained from Python to a pure C++ version 
+One can convert the result obtained from Python to a pure C++ version
 if a ``py::class_`` or type conversion is defined.
 
 .. code-block:: cpp
@@ -99,8 +99,8 @@ Python method.
     py::print(py::str(exp_pi));
 
 In the example above ``pi.attr("exp")`` is a *bound method*: it will always call
-the method for that same instance of the class. Alternately one can create an 
-*unbound method* via the Python class (instead of instance) and pass the ``self`` 
+the method for that same instance of the class. Alternately one can create an
+*unbound method* via the Python class (instead of instance) and pass the ``self``
 object explicitly, followed by other arguments.
 
 .. code-block:: cpp
diff --git a/wrap/python/pybind11/docs/advanced/pycpp/utilities.rst b/wrap/pybind11/docs/advanced/pycpp/utilities.rst
similarity index 100%
rename from wrap/python/pybind11/docs/advanced/pycpp/utilities.rst
rename to wrap/pybind11/docs/advanced/pycpp/utilities.rst
diff --git a/wrap/python/pybind11/docs/advanced/smart_ptrs.rst b/wrap/pybind11/docs/advanced/smart_ptrs.rst
similarity index 100%
rename from wrap/python/pybind11/docs/advanced/smart_ptrs.rst
rename to wrap/pybind11/docs/advanced/smart_ptrs.rst
diff --git a/wrap/python/pybind11/docs/basics.rst b/wrap/pybind11/docs/basics.rst
similarity index 99%
rename from wrap/python/pybind11/docs/basics.rst
rename to wrap/pybind11/docs/basics.rst
index 447250ed9..7bf4d426d 100644
--- a/wrap/python/pybind11/docs/basics.rst
+++ b/wrap/pybind11/docs/basics.rst
@@ -164,7 +164,7 @@ load and execute the example:
 Keyword arguments
 =================
 
-With a simple modification code, it is possible to inform Python about the
+With a simple code modification, it is possible to inform Python about the
 names of the arguments ("i" and "j" in this case).
 
 .. code-block:: cpp
diff --git a/wrap/python/pybind11/docs/benchmark.py b/wrap/pybind11/docs/benchmark.py
similarity index 99%
rename from wrap/python/pybind11/docs/benchmark.py
rename to wrap/pybind11/docs/benchmark.py
index 6dc0604ea..023477212 100644
--- a/wrap/python/pybind11/docs/benchmark.py
+++ b/wrap/pybind11/docs/benchmark.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import random
 import os
 import time
diff --git a/wrap/python/pybind11/docs/benchmark.rst b/wrap/pybind11/docs/benchmark.rst
similarity index 99%
rename from wrap/python/pybind11/docs/benchmark.rst
rename to wrap/pybind11/docs/benchmark.rst
index 59d533df9..02c2ccde7 100644
--- a/wrap/python/pybind11/docs/benchmark.rst
+++ b/wrap/pybind11/docs/benchmark.rst
@@ -93,5 +93,3 @@ favor.
 .. only:: latex
 
     .. image:: pybind11_vs_boost_python2.png
-
-
diff --git a/wrap/python/pybind11/docs/changelog.rst b/wrap/pybind11/docs/changelog.rst
similarity index 91%
rename from wrap/python/pybind11/docs/changelog.rst
rename to wrap/pybind11/docs/changelog.rst
index f99d3516a..2def2b071 100644
--- a/wrap/python/pybind11/docs/changelog.rst
+++ b/wrap/pybind11/docs/changelog.rst
@@ -6,11 +6,131 @@ Changelog
 Starting with version 1.8.0, pybind11 releases use a `semantic versioning
 <http://semver.org>`_ policy.
 
-
-v2.3.1 (Not yet released)
+v2.5.0 (Mar 31, 2020)
 -----------------------------------------------------
 
-* TBA
+* Use C++17 fold expressions in type casters, if available. This can
+  improve performance during overload resolution when functions have
+  multiple arguments.
+  `#2043 <https://github.com/pybind/pybind11/pull/2043>`_.
+
+* Changed include directory resolution in ``pybind11/__init__.py``
+  and installation in ``setup.py``. This fixes a number of open issues
+  where pybind11 headers could not be found in certain environments.
+  `#1995 <https://github.com/pybind/pybind11/pull/1995>`_.
+
+* C++20 ``char8_t`` and ``u8string`` support. `#2026
+  <https://github.com/pybind/pybind11/pull/2026>`_.
+
+* CMake: search for Python 3.9. `bb9c91
+  <https://github.com/pybind/pybind11/commit/bb9c91>`_.
+
+* Fixes for MSYS-based build environments.
+  `#2087 <https://github.com/pybind/pybind11/pull/2087>`_,
+  `#2053 <https://github.com/pybind/pybind11/pull/2053>`_.
+
+* STL bindings for ``std::vector<...>::clear``. `#2074
+  <https://github.com/pybind/pybind11/pull/2074>`_.
+
+* Read-only flag for ``py::buffer``. `#1466
+  <https://github.com/pybind/pybind11/pull/1466>`_.
+
+* Exception handling during module initialization.
+  `bf2b031 <https://github.com/pybind/pybind11/commit/bf2b031>`_.
+
+* Support linking against a CPython debug build.
+  `#2025 <https://github.com/pybind/pybind11/pull/2025>`_.
+
+* Fixed issues involving the availability and use of aligned ``new`` and
+  ``delete``. `#1988 <https://github.com/pybind/pybind11/pull/1988>`_,
+  `759221 <https://github.com/pybind/pybind11/commit/759221>`_.
+
+* Fixed a resource leak upon interpreter shutdown.
+  `#2020 <https://github.com/pybind/pybind11/pull/2020>`_.
+
+* Fixed error handling in the boolean caster.
+  `#1976 <https://github.com/pybind/pybind11/pull/1976>`_.
+
+v2.4.3 (Oct 15, 2019)
+-----------------------------------------------------
+
+* Adapt pybind11 to a C API convention change in Python 3.8. `#1950
+  <https://github.com/pybind/pybind11/pull/1950>`_.
+
+v2.4.2 (Sep 21, 2019)
+-----------------------------------------------------
+
+* Replaced usage of a C++14 only construct. `#1929
+  <https://github.com/pybind/pybind11/pull/1929>`_.
+
+* Made an ifdef future-proof for Python >= 4. `f3109d
+  <https://github.com/pybind/pybind11/commit/f3109d>`_.
+
+v2.4.1 (Sep 20, 2019)
+-----------------------------------------------------
+
+* Fixed a problem involving implicit conversion from enumerations to integers
+  on Python 3.8. `#1780 <https://github.com/pybind/pybind11/pull/1780>`_.
+
+v2.4.0 (Sep 19, 2019)
+-----------------------------------------------------
+
+* Try harder to keep pybind11-internal data structures separate when there
+  are potential ABI incompatibilities. Fixes crashes that occurred when loading
+  multiple pybind11 extensions that were e.g. compiled by GCC (libstdc++)
+  and Clang (libc++).
+  `#1588 <https://github.com/pybind/pybind11/pull/1588>`_ and
+  `c9f5a <https://github.com/pybind/pybind11/commit/c9f5a>`_.
+
+* Added support for ``__await__``, ``__aiter__``, and ``__anext__`` protocols.
+  `#1842 <https://github.com/pybind/pybind11/pull/1842>`_.
+
+* ``pybind11_add_module()``: don't strip symbols when compiling in
+  ``RelWithDebInfo`` mode. `#1980
+  <https://github.com/pybind/pybind11/pull/1980>`_.
+
+* ``enum_``: Reproduce Python behavior when comparing against invalid values
+  (e.g. ``None``, strings, etc.). Add back support for ``__invert__()``.
+  `#1912 <https://github.com/pybind/pybind11/pull/1912>`_,
+  `#1907 <https://github.com/pybind/pybind11/pull/1907>`_.
+
+* List insertion operation for ``py::list``.
+  Added ``.empty()`` to all collection types.
+  Added ``py::set::contains()`` and ``py::dict::contains()``.
+  `#1887 <https://github.com/pybind/pybind11/pull/1887>`_,
+  `#1884 <https://github.com/pybind/pybind11/pull/1884>`_,
+  `#1888 <https://github.com/pybind/pybind11/pull/1888>`_.
+
+* ``py::details::overload_cast_impl`` is available in C++11 mode, can be used
+  like ``overload_cast`` with an additional set of parantheses.
+  `#1581 <https://github.com/pybind/pybind11/pull/1581>`_.
+
+* Fixed ``get_include()`` on Conda.
+  `#1877 <https://github.com/pybind/pybind11/pull/1877>`_.
+
+* ``stl_bind.h``: negative indexing support.
+  `#1882 <https://github.com/pybind/pybind11/pull/1882>`_.
+
+* Minor CMake fix to add MinGW compatibility.
+  `#1851 <https://github.com/pybind/pybind11/pull/1851>`_.
+
+* GIL-related fixes.
+  `#1836 <https://github.com/pybind/pybind11/pull/1836>`_,
+  `8b90b <https://github.com/pybind/pybind11/commit/8b90b>`_.
+
+* Other very minor/subtle fixes and improvements.
+  `#1329 <https://github.com/pybind/pybind11/pull/1329>`_,
+  `#1910 <https://github.com/pybind/pybind11/pull/1910>`_,
+  `#1863 <https://github.com/pybind/pybind11/pull/1863>`_,
+  `#1847 <https://github.com/pybind/pybind11/pull/1847>`_,
+  `#1890 <https://github.com/pybind/pybind11/pull/1890>`_,
+  `#1860 <https://github.com/pybind/pybind11/pull/1860>`_,
+  `#1848 <https://github.com/pybind/pybind11/pull/1848>`_,
+  `#1821 <https://github.com/pybind/pybind11/pull/1821>`_,
+  `#1837 <https://github.com/pybind/pybind11/pull/1837>`_,
+  `#1833 <https://github.com/pybind/pybind11/pull/1833>`_,
+  `#1748 <https://github.com/pybind/pybind11/pull/1748>`_,
+  `#1852 <https://github.com/pybind/pybind11/pull/1852>`_.
 
 v2.3.0 (June 11, 2019)
 -----------------------------------------------------
@@ -105,7 +225,6 @@ v2.3.0 (June 11, 2019)
   `#1744 <https://github.com/pybind/pybind11/pull/1744>`_,
   `#1670 <https://github.com/pybind/pybind11/pull/1670>`_.
 
-
 v2.2.4 (September 11, 2018)
 -----------------------------------------------------
 
diff --git a/wrap/python/pybind11/docs/classes.rst b/wrap/pybind11/docs/classes.rst
similarity index 96%
rename from wrap/python/pybind11/docs/classes.rst
rename to wrap/pybind11/docs/classes.rst
index 1deec9b53..a63f6a196 100644
--- a/wrap/python/pybind11/docs/classes.rst
+++ b/wrap/pybind11/docs/classes.rst
@@ -422,6 +422,17 @@ on constness, the ``py::const_`` tag should be used:
        .def("foo_mutable", py::overload_cast<int, float>(&Widget::foo))
        .def("foo_const",   py::overload_cast<int, float>(&Widget::foo, py::const_));
 
+If you prefer the ``py::overload_cast`` syntax but have a C++11 compatible compiler only,
+you can use ``py::detail::overload_cast_impl`` with an additional set of parentheses:
+
+.. code-block:: cpp
+
+    template <typename... Args>
+    using overload_cast_ = pybind11::detail::overload_cast_impl<Args...>;
+
+    py::class_<Pet>(m, "Pet")
+        .def("set", overload_cast_<int>()(&Pet::set), "Set the pet's age")
+        .def("set", overload_cast_<const std::string &>()(&Pet::set), "Set the pet's name");
 
 .. [#cpp14] A compiler which supports the ``-std=c++14`` flag
             or Visual Studio 2015 Update 2 and newer.
diff --git a/wrap/python/pybind11/docs/compiling.rst b/wrap/pybind11/docs/compiling.rst
similarity index 94%
rename from wrap/python/pybind11/docs/compiling.rst
rename to wrap/pybind11/docs/compiling.rst
index c50c7d8af..bfb1cd805 100644
--- a/wrap/python/pybind11/docs/compiling.rst
+++ b/wrap/pybind11/docs/compiling.rst
@@ -105,18 +105,14 @@ on the target compiler, falling back to C++11 if C++14 support is not
 available.  Note, however, that this default is subject to change: future
 pybind11 releases are expected to migrate to newer C++ standards as they become
 available.  To override this, the standard flag can be given explicitly in
-``PYBIND11_CPP_STANDARD``:
+`CMAKE_CXX_STANDARD <https://cmake.org/cmake/help/v3.17/variable/CMAKE_CXX_STANDARD.html>`_:
 
 .. code-block:: cmake
 
     # Use just one of these:
-    # GCC/clang:
-    set(PYBIND11_CPP_STANDARD -std=c++11)
-    set(PYBIND11_CPP_STANDARD -std=c++14)
-    set(PYBIND11_CPP_STANDARD -std=c++1z) # Experimental C++17 support
-    # MSVC:
-    set(PYBIND11_CPP_STANDARD /std:c++14)
-    set(PYBIND11_CPP_STANDARD /std:c++latest) # Enables some MSVC C++17 features
+    set(CMAKE_CXX_STANDARD 11)
+    set(CMAKE_CXX_STANDARD 14)
+    set(CMAKE_CXX_STANDARD 17) # Experimental C++17 support
 
     add_subdirectory(pybind11)  # or find_package(pybind11)
 
@@ -287,3 +283,11 @@ code by introspecting existing C++ codebases using LLVM/Clang. See the
 [binder]_ documentation for details.
 
 .. [binder] http://cppbinder.readthedocs.io/en/latest/about.html
+
+[AutoWIG]_ is a Python library that wraps automatically compiled libraries into
+high-level languages. It parses C++ code using LLVM/Clang technologies and
+generates the wrappers using the Mako templating engine. The approach is automatic,
+extensible, and applies to very complex C++ libraries, composed of thousands of
+classes or incorporating modern meta-programming constructs.
+
+.. [AutoWIG] https://github.com/StatisKit/AutoWIG
diff --git a/wrap/python/pybind11/docs/conf.py b/wrap/pybind11/docs/conf.py
similarity index 98%
rename from wrap/python/pybind11/docs/conf.py
rename to wrap/pybind11/docs/conf.py
index d17e4ba30..0946f30e2 100644
--- a/wrap/python/pybind11/docs/conf.py
+++ b/wrap/pybind11/docs/conf.py
@@ -61,9 +61,9 @@ author = 'Wenzel Jakob'
 # built documents.
 #
 # The short X.Y version.
-version = '2.3'
+version = '2.5'
 # The full version, including alpha/beta/rc tags.
-release = '2.3.dev1'
+release = '2.5.dev1'
 
 # The language for content autogenerated by Sphinx. Refer to documentation
 # for a list of supported languages.
@@ -130,8 +130,8 @@ if not on_rtd:  # only import and set the theme if we're building docs locally
 else:
     html_context = {
         'css_files': [
-            '//media.readthedocs.org/css/sphinx_rtd_theme.css',            
-            '//media.readthedocs.org/css/readthedocs-doc-embed.css',    
+            '//media.readthedocs.org/css/sphinx_rtd_theme.css',
+            '//media.readthedocs.org/css/readthedocs-doc-embed.css',
             '_static/theme_overrides.css'
         ]
     }
@@ -238,7 +238,7 @@ latex_elements = {
 #'pointsize': '10pt',
 
 # Additional stuff for the LaTeX preamble.
-'preamble': '\DeclareUnicodeCharacter{00A0}{}',
+'preamble': r'\DeclareUnicodeCharacter{00A0}{}',
 
 # Latex figure (float) alignment
 #'figure_align': 'htbp',
diff --git a/wrap/python/pybind11/docs/faq.rst b/wrap/pybind11/docs/faq.rst
similarity index 92%
rename from wrap/python/pybind11/docs/faq.rst
rename to wrap/pybind11/docs/faq.rst
index 93ccf10e5..b68562910 100644
--- a/wrap/python/pybind11/docs/faq.rst
+++ b/wrap/pybind11/docs/faq.rst
@@ -5,7 +5,7 @@ Frequently asked questions
 ===========================================================
 
 1. Make sure that the name specified in PYBIND11_MODULE is identical to the
-filename of the extension library (without prefixes such as .so)
+filename of the extension library (without suffixes such as .so)
 
 2. If the above did not fix the issue, you are likely using an incompatible
 version of Python (for instance, the extension library was compiled against
@@ -248,6 +248,33 @@ that that were ``malloc()``-ed in another shared library, using data
 structures with incompatible ABIs, and so on. pybind11 is very careful not
 to make these types of mistakes.
 
+How can I properly handle Ctrl-C in long-running functions?
+===========================================================
+
+Ctrl-C is received by the Python interpreter, and holds it until the GIL
+is released, so a long-running function won't be interrupted.
+
+To interrupt from inside your function, you can use the ``PyErr_CheckSignals()``
+function, that will tell if a signal has been raised on the Python side.  This
+function merely checks a flag, so its impact is negligible. When a signal has
+been received, you must either explicitly interrupt execution by throwing
+``py::error_already_set`` (which will propagate the existing
+``KeyboardInterrupt``), or clear the error (which you usually will not want):
+
+.. code-block:: cpp
+
+    PYBIND11_MODULE(example, m)
+    {
+        m.def("long running_func", []()
+        {
+            for (;;) {
+                if (PyErr_CheckSignals() != 0)
+                    throw py::error_already_set();
+                // Long running iteration
+            }
+        });
+    }
+
 Inconsistent detection of Python version in CMake and pybind11
 ==============================================================
 
diff --git a/wrap/python/pybind11/docs/index.rst b/wrap/pybind11/docs/index.rst
similarity index 100%
rename from wrap/python/pybind11/docs/index.rst
rename to wrap/pybind11/docs/index.rst
diff --git a/wrap/python/pybind11/docs/intro.rst b/wrap/pybind11/docs/intro.rst
similarity index 100%
rename from wrap/python/pybind11/docs/intro.rst
rename to wrap/pybind11/docs/intro.rst
diff --git a/wrap/python/pybind11/docs/limitations.rst b/wrap/pybind11/docs/limitations.rst
similarity index 99%
rename from wrap/python/pybind11/docs/limitations.rst
rename to wrap/pybind11/docs/limitations.rst
index a1a4f1aff..59474f82f 100644
--- a/wrap/python/pybind11/docs/limitations.rst
+++ b/wrap/pybind11/docs/limitations.rst
@@ -17,4 +17,3 @@ These features could be implemented but would lead to a significant increase in
 complexity. I've decided to draw the line here to keep this project simple and
 compact. Users who absolutely require these features are encouraged to fork
 pybind11.
-
diff --git a/wrap/python/pybind11/docs/pybind11-logo.png b/wrap/pybind11/docs/pybind11-logo.png
similarity index 100%
rename from wrap/python/pybind11/docs/pybind11-logo.png
rename to wrap/pybind11/docs/pybind11-logo.png
diff --git a/wrap/python/pybind11/docs/pybind11_vs_boost_python1.png b/wrap/pybind11/docs/pybind11_vs_boost_python1.png
similarity index 100%
rename from wrap/python/pybind11/docs/pybind11_vs_boost_python1.png
rename to wrap/pybind11/docs/pybind11_vs_boost_python1.png
diff --git a/wrap/python/pybind11/docs/pybind11_vs_boost_python1.svg b/wrap/pybind11/docs/pybind11_vs_boost_python1.svg
similarity index 100%
rename from wrap/python/pybind11/docs/pybind11_vs_boost_python1.svg
rename to wrap/pybind11/docs/pybind11_vs_boost_python1.svg
diff --git a/wrap/python/pybind11/docs/pybind11_vs_boost_python2.png b/wrap/pybind11/docs/pybind11_vs_boost_python2.png
similarity index 100%
rename from wrap/python/pybind11/docs/pybind11_vs_boost_python2.png
rename to wrap/pybind11/docs/pybind11_vs_boost_python2.png
diff --git a/wrap/python/pybind11/docs/pybind11_vs_boost_python2.svg b/wrap/pybind11/docs/pybind11_vs_boost_python2.svg
similarity index 100%
rename from wrap/python/pybind11/docs/pybind11_vs_boost_python2.svg
rename to wrap/pybind11/docs/pybind11_vs_boost_python2.svg
diff --git a/wrap/python/pybind11/docs/reference.rst b/wrap/pybind11/docs/reference.rst
similarity index 100%
rename from wrap/python/pybind11/docs/reference.rst
rename to wrap/pybind11/docs/reference.rst
diff --git a/wrap/python/pybind11/docs/release.rst b/wrap/pybind11/docs/release.rst
similarity index 75%
rename from wrap/python/pybind11/docs/release.rst
rename to wrap/pybind11/docs/release.rst
index b31bbe97e..9846f971a 100644
--- a/wrap/python/pybind11/docs/release.rst
+++ b/wrap/pybind11/docs/release.rst
@@ -13,10 +13,6 @@ To release a new version of pybind11:
     - ``git push --tags``.
     - ``python setup.py sdist upload``.
     - ``python setup.py bdist_wheel upload``.
-- Update conda-forge (https://github.com/conda-forge/pybind11-feedstock) via PR
-    - download release package from Github: ``wget https://github.com/pybind/pybind11/archive/vX.Y.Z.tar.gz``
-    - compute checksum: ``shasum -a 256  vX.Y.Z.tar.gz``
-    - change version number and checksum in ``recipe/meta.yml``
 - Get back to work
     - Update ``_version.py`` (add 'dev' and increment minor).
     - Update version in ``docs/conf.py``
diff --git a/wrap/python/pybind11/docs/requirements.txt b/wrap/pybind11/docs/requirements.txt
similarity index 100%
rename from wrap/python/pybind11/docs/requirements.txt
rename to wrap/pybind11/docs/requirements.txt
diff --git a/wrap/python/pybind11/docs/upgrade.rst b/wrap/pybind11/docs/upgrade.rst
similarity index 100%
rename from wrap/python/pybind11/docs/upgrade.rst
rename to wrap/pybind11/docs/upgrade.rst
diff --git a/wrap/python/pybind11/include/pybind11/attr.h b/wrap/pybind11/include/pybind11/attr.h
similarity index 92%
rename from wrap/python/pybind11/include/pybind11/attr.h
rename to wrap/pybind11/include/pybind11/attr.h
index 6962d6fc5..54065fc9e 100644
--- a/wrap/python/pybind11/include/pybind11/attr.h
+++ b/wrap/pybind11/include/pybind11/attr.h
@@ -12,7 +12,7 @@
 
 #include "cast.h"
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
 
 /// \addtogroup annotations
 /// @{
@@ -23,6 +23,9 @@ struct is_method { handle class_; is_method(const handle &c) : class_(c) { } };
 /// Annotation for operators
 struct is_operator { };
 
+/// Annotation for classes that cannot be subclassed
+struct is_final { };
+
 /// Annotation for parent scope
 struct scope { handle value; scope(const handle &s) : value(s) { } };
 
@@ -110,7 +113,7 @@ struct call_guard<T, Ts...> {
 
 /// @} annotations
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 /* Forward declarations */
 enum op_id : int;
 enum op_type : int;
@@ -134,7 +137,8 @@ struct argument_record {
 struct function_record {
     function_record()
         : is_constructor(false), is_new_style_constructor(false), is_stateless(false),
-          is_operator(false), has_args(false), has_kwargs(false), is_method(false) { }
+          is_operator(false), is_method(false),
+          has_args(false), has_kwargs(false), has_kwonly_args(false) { }
 
     /// Function name
     char *name = nullptr; /* why no C++ strings? They generate heavier code.. */
@@ -172,18 +176,24 @@ struct function_record {
     /// True if this is an operator (__add__), etc.
     bool is_operator : 1;
 
+    /// True if this is a method
+    bool is_method : 1;
+
     /// True if the function has a '*args' argument
     bool has_args : 1;
 
     /// True if the function has a '**kwargs' argument
     bool has_kwargs : 1;
 
-    /// True if this is a method
-    bool is_method : 1;
+    /// True once a 'py::kwonly' is encountered (any following args are keyword-only)
+    bool has_kwonly_args : 1;
 
     /// Number of arguments (including py::args and/or py::kwargs, if present)
     std::uint16_t nargs;
 
+    /// Number of trailing arguments (counted in `nargs`) that are keyword-only
+    std::uint16_t nargs_kwonly = 0;
+
     /// Python method object
     PyMethodDef *def = nullptr;
 
@@ -201,7 +211,7 @@ struct function_record {
 struct type_record {
     PYBIND11_NOINLINE type_record()
         : multiple_inheritance(false), dynamic_attr(false), buffer_protocol(false),
-          default_holder(true), module_local(false) { }
+          default_holder(true), module_local(false), is_final(false) { }
 
     /// Handle to the parent scope
     handle scope;
@@ -254,6 +264,9 @@ struct type_record {
     /// Is the class definition local to the module shared object?
     bool module_local : 1;
 
+    /// Is the class inheritable from python classes?
+    bool is_final : 1;
+
     PYBIND11_NOINLINE void add_base(const std::type_info &base, void *(*caster)(void *)) {
         auto base_info = detail::get_type_info(base, false);
         if (!base_info) {
@@ -353,12 +366,20 @@ template <> struct process_attribute<is_new_style_constructor> : process_attribu
     static void init(const is_new_style_constructor &, function_record *r) { r->is_new_style_constructor = true; }
 };
 
+inline void process_kwonly_arg(const arg &a, function_record *r) {
+    if (!a.name || strlen(a.name) == 0)
+        pybind11_fail("arg(): cannot specify an unnamed argument after an kwonly() annotation");
+    ++r->nargs_kwonly;
+}
+
 /// Process a keyword argument attribute (*without* a default value)
 template <> struct process_attribute<arg> : process_attribute_default<arg> {
     static void init(const arg &a, function_record *r) {
         if (r->is_method && r->args.empty())
             r->args.emplace_back("self", nullptr, handle(), true /*convert*/, false /*none not allowed*/);
         r->args.emplace_back(a.name, nullptr, handle(), !a.flag_noconvert, a.flag_none);
+
+        if (r->has_kwonly_args) process_kwonly_arg(a, r);
     }
 };
 
@@ -390,6 +411,15 @@ template <> struct process_attribute<arg_v> : process_attribute_default<arg_v> {
 #endif
         }
         r->args.emplace_back(a.name, a.descr, a.value.inc_ref(), !a.flag_noconvert, a.flag_none);
+
+        if (r->has_kwonly_args) process_kwonly_arg(a, r);
+    }
+};
+
+/// Process a keyword-only-arguments-follow pseudo argument
+template <> struct process_attribute<kwonly> : process_attribute_default<kwonly> {
+    static void init(const kwonly &, function_record *r) {
+        r->has_kwonly_args = true;
     }
 };
 
@@ -416,6 +446,11 @@ struct process_attribute<dynamic_attr> : process_attribute_default<dynamic_attr>
     static void init(const dynamic_attr &, type_record *r) { r->dynamic_attr = true; }
 };
 
+template <>
+struct process_attribute<is_final> : process_attribute_default<is_final> {
+    static void init(const is_final &, type_record *r) { r->is_final = true; }
+};
+
 template <>
 struct process_attribute<buffer_protocol> : process_attribute_default<buffer_protocol> {
     static void init(const buffer_protocol &, type_record *r) { r->buffer_protocol = true; }
@@ -489,5 +524,5 @@ constexpr bool expected_num_args(size_t nargs, bool has_args, bool has_kwargs) {
     return named == 0 || (self + named + has_args + has_kwargs) == nargs;
 }
 
-NAMESPACE_END(detail)
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/buffer_info.h b/wrap/pybind11/include/pybind11/buffer_info.h
similarity index 71%
rename from wrap/python/pybind11/include/pybind11/buffer_info.h
rename to wrap/pybind11/include/pybind11/buffer_info.h
index 9f072fa73..8349a46b8 100644
--- a/wrap/python/pybind11/include/pybind11/buffer_info.h
+++ b/wrap/pybind11/include/pybind11/buffer_info.h
@@ -11,7 +11,7 @@
 
 #include "detail/common.h"
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
 
 /// Information record describing a Python buffer object
 struct buffer_info {
@@ -21,14 +21,15 @@ struct buffer_info {
     std::string format;           // For homogeneous buffers, this should be set to format_descriptor<T>::format()
     ssize_t ndim = 0;             // Number of dimensions
     std::vector<ssize_t> shape;   // Shape of the tensor (1 entry per dimension)
-    std::vector<ssize_t> strides; // Number of entries between adjacent entries (for each per dimension)
+    std::vector<ssize_t> strides; // Number of bytes between adjacent entries (for each per dimension)
+    bool readonly = false;        // flag to indicate if the underlying storage may be written to
 
     buffer_info() { }
 
     buffer_info(void *ptr, ssize_t itemsize, const std::string &format, ssize_t ndim,
-                detail::any_container<ssize_t> shape_in, detail::any_container<ssize_t> strides_in)
+                detail::any_container<ssize_t> shape_in, detail::any_container<ssize_t> strides_in, bool readonly=false)
     : ptr(ptr), itemsize(itemsize), size(1), format(format), ndim(ndim),
-      shape(std::move(shape_in)), strides(std::move(strides_in)) {
+      shape(std::move(shape_in)), strides(std::move(strides_in)), readonly(readonly) {
         if (ndim != (ssize_t) shape.size() || ndim != (ssize_t) strides.size())
             pybind11_fail("buffer_info: ndim doesn't match shape and/or strides length");
         for (size_t i = 0; i < (size_t) ndim; ++i)
@@ -36,20 +37,24 @@ struct buffer_info {
     }
 
     template <typename T>
-    buffer_info(T *ptr, detail::any_container<ssize_t> shape_in, detail::any_container<ssize_t> strides_in)
-    : buffer_info(private_ctr_tag(), ptr, sizeof(T), format_descriptor<T>::format(), static_cast<ssize_t>(shape_in->size()), std::move(shape_in), std::move(strides_in)) { }
+    buffer_info(T *ptr, detail::any_container<ssize_t> shape_in, detail::any_container<ssize_t> strides_in, bool readonly=false)
+    : buffer_info(private_ctr_tag(), ptr, sizeof(T), format_descriptor<T>::format(), static_cast<ssize_t>(shape_in->size()), std::move(shape_in), std::move(strides_in), readonly) { }
 
-    buffer_info(void *ptr, ssize_t itemsize, const std::string &format, ssize_t size)
-    : buffer_info(ptr, itemsize, format, 1, {size}, {itemsize}) { }
+    buffer_info(void *ptr, ssize_t itemsize, const std::string &format, ssize_t size, bool readonly=false)
+    : buffer_info(ptr, itemsize, format, 1, {size}, {itemsize}, readonly) { }
 
     template <typename T>
-    buffer_info(T *ptr, ssize_t size)
-    : buffer_info(ptr, sizeof(T), format_descriptor<T>::format(), size) { }
+    buffer_info(T *ptr, ssize_t size, bool readonly=false)
+    : buffer_info(ptr, sizeof(T), format_descriptor<T>::format(), size, readonly) { }
+
+    template <typename T>
+    buffer_info(const T *ptr, ssize_t size, bool readonly=true)
+    : buffer_info(const_cast<T*>(ptr), sizeof(T), format_descriptor<T>::format(), size, readonly) { }
 
     explicit buffer_info(Py_buffer *view, bool ownview = true)
     : buffer_info(view->buf, view->itemsize, view->format, view->ndim,
-            {view->shape, view->shape + view->ndim}, {view->strides, view->strides + view->ndim}) {
-        this->view = view;
+            {view->shape, view->shape + view->ndim}, {view->strides, view->strides + view->ndim}, view->readonly) {
+        this->m_view = view;
         this->ownview = ownview;
     }
 
@@ -68,27 +73,30 @@ struct buffer_info {
         ndim = rhs.ndim;
         shape = std::move(rhs.shape);
         strides = std::move(rhs.strides);
-        std::swap(view, rhs.view);
+        std::swap(m_view, rhs.m_view);
         std::swap(ownview, rhs.ownview);
+        readonly = rhs.readonly;
         return *this;
     }
 
     ~buffer_info() {
-        if (view && ownview) { PyBuffer_Release(view); delete view; }
+        if (m_view && ownview) { PyBuffer_Release(m_view); delete m_view; }
     }
 
+    Py_buffer *view() const { return m_view; }
+    Py_buffer *&view() { return m_view; }
 private:
     struct private_ctr_tag { };
 
     buffer_info(private_ctr_tag, void *ptr, ssize_t itemsize, const std::string &format, ssize_t ndim,
-                detail::any_container<ssize_t> &&shape_in, detail::any_container<ssize_t> &&strides_in)
-    : buffer_info(ptr, itemsize, format, ndim, std::move(shape_in), std::move(strides_in)) { }
+                detail::any_container<ssize_t> &&shape_in, detail::any_container<ssize_t> &&strides_in, bool readonly)
+    : buffer_info(ptr, itemsize, format, ndim, std::move(shape_in), std::move(strides_in), readonly) { }
 
-    Py_buffer *view = nullptr;
+    Py_buffer *m_view = nullptr;
     bool ownview = false;
 };
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 template <typename T, typename SFINAE = void> struct compare_buffer_info {
     static bool compare(const buffer_info& b) {
@@ -104,5 +112,5 @@ template <typename T> struct compare_buffer_info<T, detail::enable_if_t<std::is_
     }
 };
 
-NAMESPACE_END(detail)
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/cast.h b/wrap/pybind11/include/pybind11/cast.h
similarity index 93%
rename from wrap/python/pybind11/include/pybind11/cast.h
rename to wrap/pybind11/include/pybind11/cast.h
index 8d0fd5d90..d788992df 100644
--- a/wrap/python/pybind11/include/pybind11/cast.h
+++ b/wrap/pybind11/include/pybind11/cast.h
@@ -32,8 +32,12 @@
 #include <string_view>
 #endif
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
-NAMESPACE_BEGIN(detail)
+#if defined(__cpp_lib_char8_t) && __cpp_lib_char8_t >= 201811L
+#  define PYBIND11_HAS_U8STRING
+#endif
+
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 /// A life support system for temporary objects created by `type_caster::load()`.
 /// Adding a patient will keep it alive up until the enclosing function returns.
@@ -284,8 +288,8 @@ public:
         // Past-the-end iterator:
         iterator(size_t end) : curr(end) {}
     public:
-        bool operator==(const iterator &other) { return curr.index == other.curr.index; }
-        bool operator!=(const iterator &other) { return curr.index != other.curr.index; }
+        bool operator==(const iterator &other) const { return curr.index == other.curr.index; }
+        bool operator!=(const iterator &other) const { return curr.index != other.curr.index; }
         iterator &operator++() {
             if (!inst->simple_layout)
                 curr.vh += 1 + (*types)[curr.index]->holder_size_in_ptrs;
@@ -533,9 +537,17 @@ public:
             case return_value_policy::copy:
                 if (copy_constructor)
                     valueptr = copy_constructor(src);
-                else
-                    throw cast_error("return_value_policy = copy, but the "
-                                     "object is non-copyable!");
+                else {
+#if defined(NDEBUG)
+                    throw cast_error("return_value_policy = copy, but type is "
+                                     "non-copyable! (compile in debug mode for details)");
+#else
+                    std::string type_name(tinfo->cpptype->name());
+                    detail::clean_type_id(type_name);
+                    throw cast_error("return_value_policy = copy, but type " +
+                                     type_name + " is non-copyable!");
+#endif
+                }
                 wrapper->owned = true;
                 break;
 
@@ -544,9 +556,18 @@ public:
                     valueptr = move_constructor(src);
                 else if (copy_constructor)
                     valueptr = copy_constructor(src);
-                else
-                    throw cast_error("return_value_policy = move, but the "
-                                     "object is neither movable nor copyable!");
+                else {
+#if defined(NDEBUG)
+                    throw cast_error("return_value_policy = move, but type is neither "
+                                     "movable nor copyable! "
+                                     "(compile in debug mode for details)");
+#else
+                    std::string type_name(tinfo->cpptype->name());
+                    detail::clean_type_id(type_name);
+                    throw cast_error("return_value_policy = move, but type " +
+                                     type_name + " is neither movable nor copyable!");
+#endif
+                }
                 wrapper->owned = true;
                 break;
 
@@ -574,10 +595,10 @@ public:
             if (type->operator_new) {
                 vptr = type->operator_new(type->type_size);
             } else {
-                #if defined(PYBIND11_CPP17)
+                #if defined(__cpp_aligned_new) && (!defined(_MSC_VER) || _MSC_VER >= 1912)
                     if (type->type_align > __STDCPP_DEFAULT_NEW_ALIGNMENT__)
                         vptr = ::operator new(type->type_size,
-                                              (std::align_val_t) type->type_align);
+                                              std::align_val_t(type->type_align));
                     else
                 #endif
                 vptr = ::operator new(type->type_size);
@@ -775,17 +796,27 @@ template <typename T, typename SFINAE = void> struct is_copy_constructible : std
 // so, copy constructability depends on whether the value_type is copy constructible.
 template <typename Container> struct is_copy_constructible<Container, enable_if_t<all_of<
         std::is_copy_constructible<Container>,
-        std::is_same<typename Container::value_type &, typename Container::reference>
+        std::is_same<typename Container::value_type &, typename Container::reference>,
+        // Avoid infinite recursion
+        negation<std::is_same<Container, typename Container::value_type>>
     >::value>> : is_copy_constructible<typename Container::value_type> {};
 
-#if !defined(PYBIND11_CPP17)
-// Likewise for std::pair before C++17 (which mandates that the copy constructor not exist when the
-// two types aren't themselves copy constructible).
+// Likewise for std::pair
+// (after C++17 it is mandatory that the copy constructor not exist when the two types aren't themselves
+// copy constructible, but this can not be relied upon when T1 or T2 are themselves containers).
 template <typename T1, typename T2> struct is_copy_constructible<std::pair<T1, T2>>
     : all_of<is_copy_constructible<T1>, is_copy_constructible<T2>> {};
-#endif
 
-NAMESPACE_END(detail)
+// The same problems arise with std::is_copy_assignable, so we use the same workaround.
+template <typename T, typename SFINAE = void> struct is_copy_assignable : std::is_copy_assignable<T> {};
+template <typename Container> struct is_copy_assignable<Container, enable_if_t<all_of<
+        std::is_copy_assignable<Container>,
+        std::is_same<typename Container::value_type &, typename Container::reference>
+    >::value>> : is_copy_assignable<typename Container::value_type> {};
+template <typename T1, typename T2> struct is_copy_assignable<std::pair<T1, T2>>
+    : all_of<is_copy_assignable<T1>, is_copy_assignable<T2>> {};
+
+PYBIND11_NAMESPACE_END(detail)
 
 // polymorphic_type_hook<itype>::get(src, tinfo) determines whether the object pointed
 // to by `src` actually is an instance of some class derived from `itype`.
@@ -804,21 +835,27 @@ NAMESPACE_END(detail)
 // You may specialize polymorphic_type_hook yourself for types that want to appear
 // polymorphic to Python but do not use C++ RTTI. (This is a not uncommon pattern
 // in performance-sensitive applications, used most notably in LLVM.)
+//
+// polymorphic_type_hook_base allows users to specialize polymorphic_type_hook with
+// std::enable_if. User provided specializations will always have higher priority than
+// the default implementation and specialization provided in polymorphic_type_hook_base.
 template <typename itype, typename SFINAE = void>
-struct polymorphic_type_hook
+struct polymorphic_type_hook_base
 {
     static const void *get(const itype *src, const std::type_info*&) { return src; }
 };
 template <typename itype>
-struct polymorphic_type_hook<itype, detail::enable_if_t<std::is_polymorphic<itype>::value>>
+struct polymorphic_type_hook_base<itype, detail::enable_if_t<std::is_polymorphic<itype>::value>>
 {
     static const void *get(const itype *src, const std::type_info*& type) {
         type = src ? &typeid(*src) : nullptr;
         return dynamic_cast<const void*>(src);
     }
 };
+template <typename itype, typename SFINAE = void>
+struct polymorphic_type_hook : public polymorphic_type_hook_base<itype> {};
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 /// Generic type caster for objects stored on the heap
 template <typename type> class type_caster_base : public type_caster_generic {
@@ -961,6 +998,9 @@ public:
 
 template <typename CharT> using is_std_char_type = any_of<
     std::is_same<CharT, char>, /* std::string */
+#if defined(PYBIND11_HAS_U8STRING)
+    std::is_same<CharT, char8_t>, /* std::u8string */
+#endif
     std::is_same<CharT, char16_t>, /* std::u16string */
     std::is_same<CharT, char32_t>, /* std::u32string */
     std::is_same<CharT, wchar_t> /* std::wstring */
@@ -995,9 +1035,11 @@ public:
         }
 
         bool py_err = py_value == (py_type) -1 && PyErr_Occurred();
+
+        // Protect std::numeric_limits::min/max with parentheses
         if (py_err || (std::is_integral<T>::value && sizeof(py_type) != sizeof(T) &&
-                       (py_value < (py_type) std::numeric_limits<T>::min() ||
-                        py_value > (py_type) std::numeric_limits<T>::max()))) {
+                       (py_value < (py_type) (std::numeric_limits<T>::min)() ||
+                        py_value > (py_type) (std::numeric_limits<T>::max)()))) {
             bool type_error = py_err && PyErr_ExceptionMatches(
 #if PY_VERSION_HEX < 0x03000000 && !defined(PYPY_VERSION)
                 PyExc_SystemError
@@ -1143,6 +1185,8 @@ public:
             if (res == 0 || res == 1) {
                 value = (bool) res;
                 return true;
+            } else {
+                PyErr_Clear();
             }
         }
         return false;
@@ -1160,6 +1204,9 @@ template <typename StringType, bool IsView = false> struct string_caster {
     // Simplify life by being able to assume standard char sizes (the standard only guarantees
     // minimums, but Python requires exact sizes)
     static_assert(!std::is_same<CharT, char>::value || sizeof(CharT) == 1, "Unsupported char size != 1");
+#if defined(PYBIND11_HAS_U8STRING)
+    static_assert(!std::is_same<CharT, char8_t>::value || sizeof(CharT) == 1, "Unsupported char8_t size != 1");
+#endif
     static_assert(!std::is_same<CharT, char16_t>::value || sizeof(CharT) == 2, "Unsupported char16_t size != 2");
     static_assert(!std::is_same<CharT, char32_t>::value || sizeof(CharT) == 4, "Unsupported char32_t size != 4");
     // wchar_t can be either 16 bits (Windows) or 32 (everywhere else)
@@ -1178,7 +1225,7 @@ template <typename StringType, bool IsView = false> struct string_caster {
 #if PY_MAJOR_VERSION >= 3
             return load_bytes(load_src);
 #else
-            if (sizeof(CharT) == 1) {
+            if (std::is_same<CharT, char>::value) {
                 return load_bytes(load_src);
             }
 
@@ -1238,7 +1285,7 @@ private:
     // without any encoding/decoding attempt).  For other C++ char sizes this is a no-op.
     // which supports loading a unicode from a str, doesn't take this path.
     template <typename C = CharT>
-    bool load_bytes(enable_if_t<sizeof(C) == 1, handle> src) {
+    bool load_bytes(enable_if_t<std::is_same<C, char>::value, handle> src) {
         if (PYBIND11_BYTES_CHECK(src.ptr())) {
             // We were passed a Python 3 raw bytes; accept it into a std::string or char*
             // without any encoding attempt.
@@ -1253,7 +1300,7 @@ private:
     }
 
     template <typename C = CharT>
-    bool load_bytes(enable_if_t<sizeof(C) != 1, handle>) { return false; }
+    bool load_bytes(enable_if_t<!std::is_same<C, char>::value, handle>) { return false; }
 };
 
 template <typename CharT, class Traits, class Allocator>
@@ -1391,9 +1438,14 @@ protected:
 
     template <size_t... Is>
     bool load_impl(const sequence &seq, bool convert, index_sequence<Is...>) {
+#ifdef __cpp_fold_expressions
+        if ((... || !std::get<Is>(subcasters).load(seq[Is], convert)))
+            return false;
+#else
         for (bool r : {std::get<Is>(subcasters).load(seq[Is], convert)...})
             if (!r)
                 return false;
+#endif
         return true;
     }
 
@@ -1446,7 +1498,9 @@ public:
     }
 
     explicit operator type*() { return this->value; }
-    explicit operator type&() { return *(this->value); }
+    // static_cast works around compiler error with MSVC 17 and CUDA 10.2
+    // see issue #2180
+    explicit operator type&() { return *(static_cast<type *>(this->value)); }
     explicit operator holder_type*() { return std::addressof(holder); }
 
     // Workaround for Intel compiler bug
@@ -1552,6 +1606,10 @@ template <typename base, typename deleter> struct is_holder_type<base, std::uniq
 
 template <typename T> struct handle_type_name { static constexpr auto name = _<T>(); };
 template <> struct handle_type_name<bytes> { static constexpr auto name = _(PYBIND11_BYTES_NAME); };
+template <> struct handle_type_name<int_> { static constexpr auto name = _("int"); };
+template <> struct handle_type_name<iterable> { static constexpr auto name = _("Iterable"); };
+template <> struct handle_type_name<iterator> { static constexpr auto name = _("Iterator"); };
+template <> struct handle_type_name<none> { static constexpr auto name = _("None"); };
 template <> struct handle_type_name<args> { static constexpr auto name = _("*args"); };
 template <> struct handle_type_name<kwargs> { static constexpr auto name = _("**kwargs"); };
 
@@ -1650,7 +1708,7 @@ template <typename T> make_caster<T> load_type(const handle &handle) {
     return conv;
 }
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 // pytype -> C++ type
 template <typename T, detail::enable_if_t<!detail::is_pyobject<T>::value, int> = 0>
@@ -1667,13 +1725,16 @@ T cast(const handle &handle) { return T(reinterpret_borrow<object>(handle)); }
 
 // C++ type -> py::object
 template <typename T, detail::enable_if_t<!detail::is_pyobject<T>::value, int> = 0>
-object cast(const T &value, return_value_policy policy = return_value_policy::automatic_reference,
+object cast(T &&value, return_value_policy policy = return_value_policy::automatic_reference,
             handle parent = handle()) {
+    using no_ref_T = typename std::remove_reference<T>::type;
     if (policy == return_value_policy::automatic)
-        policy = std::is_pointer<T>::value ? return_value_policy::take_ownership : return_value_policy::copy;
+        policy = std::is_pointer<no_ref_T>::value ? return_value_policy::take_ownership :
+                 std::is_lvalue_reference<T>::value ? return_value_policy::copy : return_value_policy::move;
     else if (policy == return_value_policy::automatic_reference)
-        policy = std::is_pointer<T>::value ? return_value_policy::reference : return_value_policy::copy;
-    return reinterpret_steal<object>(detail::make_caster<T>::cast(value, policy, parent));
+        policy = std::is_pointer<no_ref_T>::value ? return_value_policy::reference :
+                 std::is_lvalue_reference<T>::value ? return_value_policy::copy : return_value_policy::move;
+    return reinterpret_steal<object>(detail::make_caster<T>::cast(std::forward<T>(value), policy, parent));
 }
 
 template <typename T> T handle::cast() const { return pybind11::cast<T>(*this); }
@@ -1718,7 +1779,7 @@ template <typename T> T object::cast() && { return pybind11::cast<T>(std::move(*
 template <> inline void object::cast() const & { return; }
 template <> inline void object::cast() && { return; }
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 // Declared in pytypes.h:
 template <typename T, enable_if_t<!is_pyobject<T>::value, int>>
@@ -1745,7 +1806,7 @@ template <typename T> enable_if_t<cast_is_temporary_value_reference<T>::value, T
     pybind11_fail("Internal error: cast_safe fallback invoked"); }
 template <> inline void cast_safe<void>(object &&) {}
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 template <return_value_policy policy = return_value_policy::automatic_reference>
 tuple make_tuple() { return tuple(0); }
@@ -1835,6 +1896,11 @@ public:
 #endif
 };
 
+/// \ingroup annotations
+/// Annotation indicating that all following arguments are keyword-only; the is the equivalent of an
+/// unnamed '*' argument (in Python 3)
+struct kwonly {};
+
 template <typename T>
 arg_v arg::operator=(T &&value) const { return {std::move(*this), std::forward<T>(value)}; }
 
@@ -1848,7 +1914,7 @@ inline namespace literals {
 constexpr arg operator"" _a(const char *name, size_t) { return arg(name); }
 }
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 // forward declaration (definition in attr.h)
 struct function_record;
@@ -1920,14 +1986,19 @@ private:
 
     template <size_t... Is>
     bool load_impl_sequence(function_call &call, index_sequence<Is...>) {
+#ifdef __cpp_fold_expressions
+        if ((... || !std::get<Is>(argcasters).load(call.args[Is], call.args_convert[Is])))
+            return false;
+#else
         for (bool r : {std::get<Is>(argcasters).load(call.args[Is], call.args_convert[Is])...})
             if (!r)
                 return false;
+#endif
         return true;
     }
 
     template <typename Return, typename Func, size_t... Is, typename Guard>
-    Return call_impl(Func &&f, index_sequence<Is...>, Guard &&) {
+    Return call_impl(Func &&f, index_sequence<Is...>, Guard &&) && {
         return std::forward<Func>(f)(cast_op<Args>(std::move(std::get<Is>(argcasters)))...);
     }
 
@@ -2114,7 +2185,7 @@ object object_api<Derived>::call(Args &&...args) const {
     return operator()<policy>(std::forward<Args>(args)...);
 }
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 #define PYBIND11_MAKE_OPAQUE(...) \
     namespace pybind11 { namespace detail { \
@@ -2125,4 +2196,4 @@ NAMESPACE_END(detail)
 /// typedef, e.g.: `PYBIND11_OVERLOAD(PYBIND11_TYPE(ReturnType<A, B>), PYBIND11_TYPE(Parent<C, D>), f, arg)`
 #define PYBIND11_TYPE(...) __VA_ARGS__
 
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/chrono.h b/wrap/pybind11/include/pybind11/chrono.h
similarity index 82%
rename from wrap/python/pybind11/include/pybind11/chrono.h
rename to wrap/pybind11/include/pybind11/chrono.h
index 2ace2329d..6b9ab9b82 100644
--- a/wrap/python/pybind11/include/pybind11/chrono.h
+++ b/wrap/pybind11/include/pybind11/chrono.h
@@ -27,8 +27,8 @@
 #define PyDateTime_DELTA_GET_MICROSECONDS(o) (((PyDateTime_Delta*)o)->microseconds)
 #endif
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 template <typename type> class duration_caster {
 public:
@@ -106,8 +106,11 @@ public:
         if (!PyDateTimeAPI) { PyDateTime_IMPORT; }
 
         if (!src) return false;
+
+        std::tm cal;
+        microseconds msecs;
+
         if (PyDateTime_Check(src.ptr())) {
-            std::tm cal;
             cal.tm_sec   = PyDateTime_DATE_GET_SECOND(src.ptr());
             cal.tm_min   = PyDateTime_DATE_GET_MINUTE(src.ptr());
             cal.tm_hour  = PyDateTime_DATE_GET_HOUR(src.ptr());
@@ -115,11 +118,30 @@ public:
             cal.tm_mon   = PyDateTime_GET_MONTH(src.ptr()) - 1;
             cal.tm_year  = PyDateTime_GET_YEAR(src.ptr()) - 1900;
             cal.tm_isdst = -1;
-
-            value = system_clock::from_time_t(std::mktime(&cal)) + microseconds(PyDateTime_DATE_GET_MICROSECOND(src.ptr()));
-            return true;
+            msecs        = microseconds(PyDateTime_DATE_GET_MICROSECOND(src.ptr()));
+        } else if (PyDate_Check(src.ptr())) {
+            cal.tm_sec   = 0;
+            cal.tm_min   = 0;
+            cal.tm_hour  = 0;
+            cal.tm_mday  = PyDateTime_GET_DAY(src.ptr());
+            cal.tm_mon   = PyDateTime_GET_MONTH(src.ptr()) - 1;
+            cal.tm_year  = PyDateTime_GET_YEAR(src.ptr()) - 1900;
+            cal.tm_isdst = -1;
+            msecs        = microseconds(0);
+        } else if (PyTime_Check(src.ptr())) {
+            cal.tm_sec   = PyDateTime_TIME_GET_SECOND(src.ptr());
+            cal.tm_min   = PyDateTime_TIME_GET_MINUTE(src.ptr());
+            cal.tm_hour  = PyDateTime_TIME_GET_HOUR(src.ptr());
+            cal.tm_mday  = 1;   // This date (day, month, year) = (1, 0, 70)
+            cal.tm_mon   = 0;   // represents 1-Jan-1970, which is the first
+            cal.tm_year  = 70;  // earliest available date for Python's datetime
+            cal.tm_isdst = -1;
+            msecs        = microseconds(PyDateTime_TIME_GET_MICROSECOND(src.ptr()));
         }
         else return false;
+
+        value = system_clock::from_time_t(std::mktime(&cal)) + msecs;
+        return true;
     }
 
     static handle cast(const std::chrono::time_point<std::chrono::system_clock, Duration> &src, return_value_policy /* policy */, handle /* parent */) {
@@ -158,5 +180,5 @@ template <typename Rep, typename Period> class type_caster<std::chrono::duration
 : public duration_caster<std::chrono::duration<Rep, Period>> {
 };
 
-NAMESPACE_END(detail)
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/common.h b/wrap/pybind11/include/pybind11/common.h
similarity index 100%
rename from wrap/python/pybind11/include/pybind11/common.h
rename to wrap/pybind11/include/pybind11/common.h
diff --git a/wrap/python/pybind11/include/pybind11/complex.h b/wrap/pybind11/include/pybind11/complex.h
similarity index 92%
rename from wrap/python/pybind11/include/pybind11/complex.h
rename to wrap/pybind11/include/pybind11/complex.h
index 3f8963857..f8327eb37 100644
--- a/wrap/python/pybind11/include/pybind11/complex.h
+++ b/wrap/pybind11/include/pybind11/complex.h
@@ -17,7 +17,7 @@
 #  undef I
 #endif
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
 
 template <typename T> struct format_descriptor<std::complex<T>, detail::enable_if_t<std::is_floating_point<T>::value>> {
     static constexpr const char c = format_descriptor<T>::c;
@@ -32,7 +32,7 @@ template <typename T> constexpr const char format_descriptor<
 
 #endif
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 template <typename T> struct is_fmt_numeric<std::complex<T>, detail::enable_if_t<std::is_floating_point<T>::value>> {
     static constexpr bool value = true;
@@ -61,5 +61,5 @@ public:
 
     PYBIND11_TYPE_CASTER(std::complex<T>, _("complex"));
 };
-NAMESPACE_END(detail)
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/detail/class.h b/wrap/pybind11/include/pybind11/detail/class.h
similarity index 92%
rename from wrap/python/pybind11/include/pybind11/detail/class.h
rename to wrap/pybind11/include/pybind11/detail/class.h
index b1916fcd0..d58f04e61 100644
--- a/wrap/python/pybind11/include/pybind11/detail/class.h
+++ b/wrap/pybind11/include/pybind11/detail/class.h
@@ -12,10 +12,10 @@
 #include "../attr.h"
 #include "../options.h"
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
-#if PY_VERSION_HEX >= 0x03030000
+#if PY_VERSION_HEX >= 0x03030000 && !defined(PYPY_VERSION)
 #  define PYBIND11_BUILTIN_QUALNAME
 #  define PYBIND11_SET_OLDPY_QUALNAME(obj, nameobj)
 #else
@@ -156,6 +156,31 @@ extern "C" inline PyObject *pybind11_meta_getattro(PyObject *obj, PyObject *name
 }
 #endif
 
+/// metaclass `__call__` function that is used to create all pybind11 objects.
+extern "C" inline PyObject *pybind11_meta_call(PyObject *type, PyObject *args, PyObject *kwargs) {
+
+    // use the default metaclass call to create/initialize the object
+    PyObject *self = PyType_Type.tp_call(type, args, kwargs);
+    if (self == nullptr) {
+        return nullptr;
+    }
+
+    // This must be a pybind11 instance
+    auto instance = reinterpret_cast<detail::instance *>(self);
+
+    // Ensure that the base __init__ function(s) were called
+    for (auto vh : values_and_holders(instance)) {
+        if (!vh.holder_constructed()) {
+            PyErr_Format(PyExc_TypeError, "%.200s.__init__() must be called when overriding __init__",
+                         vh.type->type->tp_name);
+            Py_DECREF(self);
+            return nullptr;
+        }
+    }
+
+    return self;
+}
+
 /** This metaclass is assigned by default to all pybind11 types and is required in order
     for static properties to function correctly. Users may override this using `py::metaclass`.
     Return value: New reference. */
@@ -181,6 +206,8 @@ inline PyTypeObject* make_default_metaclass() {
     type->tp_base = type_incref(&PyType_Type);
     type->tp_flags = Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE | Py_TPFLAGS_HEAPTYPE;
 
+    type->tp_call = pybind11_meta_call;
+
     type->tp_setattro = pybind11_meta_setattro;
 #if PY_MAJOR_VERSION >= 3
     type->tp_getattro = pybind11_meta_getattro;
@@ -350,6 +377,7 @@ extern "C" inline void pybind11_object_dealloc(PyObject *self) {
     auto type = Py_TYPE(self);
     type->tp_free(self);
 
+#if PY_VERSION_HEX < 0x03080000
     // `type->tp_dealloc != pybind11_object_dealloc` means that we're being called
     // as part of a derived type's dealloc, in which case we're not allowed to decref
     // the type here. For cross-module compatibility, we shouldn't compare directly
@@ -357,6 +385,11 @@ extern "C" inline void pybind11_object_dealloc(PyObject *self) {
     auto pybind11_object_type = (PyTypeObject *) get_internals().instance_base;
     if (type->tp_dealloc == pybind11_object_type->tp_dealloc)
         Py_DECREF(type);
+#else
+    // This was not needed before Python 3.8 (Python issue 35810)
+    // https://github.com/pybind/pybind11/issues/1946
+    Py_DECREF(type);
+#endif
 }
 
 /** Create the type which can be used as a common base for all classes.  This is
@@ -442,7 +475,7 @@ extern "C" inline int pybind11_clear(PyObject *self) {
 /// Give instances of this type a `__dict__` and opt into garbage collection.
 inline void enable_dynamic_attributes(PyHeapTypeObject *heap_type) {
     auto type = &heap_type->ht_type;
-#if defined(PYPY_VERSION)
+#if defined(PYPY_VERSION) && (PYPY_VERSION_NUM < 0x06000000)
     pybind11_fail(std::string(type->tp_name) + ": dynamic attributes are "
                                                "currently not supported in "
                                                "conjunction with PyPy!");
@@ -485,6 +518,13 @@ extern "C" inline int pybind11_getbuffer(PyObject *obj, Py_buffer *view, int fla
     view->len = view->itemsize;
     for (auto s : info->shape)
         view->len *= s;
+    view->readonly = info->readonly;
+    if ((flags & PyBUF_WRITABLE) == PyBUF_WRITABLE && info->readonly) {
+        if (view)
+            view->obj = nullptr;
+        PyErr_SetString(PyExc_BufferError, "Writable buffer requested for readonly storage");
+        return -1;
+    }
     if ((flags & PyBUF_FORMAT) == PyBUF_FORMAT)
         view->format = const_cast<char *>(info->format.c_str());
     if ((flags & PyBUF_STRIDES) == PyBUF_STRIDES) {
@@ -586,12 +626,17 @@ inline PyObject* make_new_python_type(const type_record &rec) {
     type->tp_as_number = &heap_type->as_number;
     type->tp_as_sequence = &heap_type->as_sequence;
     type->tp_as_mapping = &heap_type->as_mapping;
+#if PY_VERSION_HEX >= 0x03050000
+    type->tp_as_async = &heap_type->as_async;
+#endif
 
     /* Flags */
-    type->tp_flags |= Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE | Py_TPFLAGS_HEAPTYPE;
+    type->tp_flags |= Py_TPFLAGS_DEFAULT | Py_TPFLAGS_HEAPTYPE;
 #if PY_MAJOR_VERSION < 3
     type->tp_flags |= Py_TPFLAGS_CHECKTYPES;
 #endif
+    if (!rec.is_final)
+        type->tp_flags |= Py_TPFLAGS_BASETYPE;
 
     if (rec.dynamic_attr)
         enable_dynamic_attributes(heap_type);
@@ -619,5 +664,5 @@ inline PyObject* make_new_python_type(const type_record &rec) {
     return (PyObject *) type;
 }
 
-NAMESPACE_END(detail)
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/detail/common.h b/wrap/pybind11/include/pybind11/detail/common.h
similarity index 92%
rename from wrap/python/pybind11/include/pybind11/detail/common.h
rename to wrap/pybind11/include/pybind11/detail/common.h
index 300c2b23d..33805e0f9 100644
--- a/wrap/python/pybind11/include/pybind11/detail/common.h
+++ b/wrap/pybind11/include/pybind11/detail/common.h
@@ -9,12 +9,8 @@
 
 #pragma once
 
-#if !defined(NAMESPACE_BEGIN)
-#  define NAMESPACE_BEGIN(name) namespace name {
-#endif
-#if !defined(NAMESPACE_END)
-#  define NAMESPACE_END(name) }
-#endif
+#define PYBIND11_NAMESPACE_BEGIN(name) namespace name {
+#define PYBIND11_NAMESPACE_END(name) }
 
 // Robust support for some features and loading modules compiled against different pybind versions
 // requires forcing hidden visibility on pybind code, so we enforce this by setting the attribute on
@@ -92,10 +88,24 @@
 #  define PYBIND11_DEPRECATED(reason) __attribute__((deprecated(reason)))
 #endif
 
+#if defined(PYBIND11_CPP17)
+#  define PYBIND11_MAYBE_UNUSED [[maybe_unused]]
+#elif defined(_MSC_VER) && !defined(__clang__)
+#  define PYBIND11_MAYBE_UNUSED
+#else
+#  define PYBIND11_MAYBE_UNUSED __attribute__ ((__unused__))
+#endif
+
 #define PYBIND11_VERSION_MAJOR 2
-#define PYBIND11_VERSION_MINOR 3
+#define PYBIND11_VERSION_MINOR 5
 #define PYBIND11_VERSION_PATCH dev1
 
+/* Don't let Python.h #define (v)snprintf as macro because they are implemented
+   properly in Visual Studio since 2015. */
+#if defined(_MSC_VER) && _MSC_VER >= 1900
+#  define HAVE_SNPRINTF 1
+#endif
+
 /// Include Python header, disable linking to pythonX_d.lib on Windows in debug mode
 #if defined(_MSC_VER)
 #  if (PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION < 4)
@@ -103,7 +113,7 @@
 #  endif
 #  pragma warning(push)
 #  pragma warning(disable: 4510 4610 4512 4005)
-#  if defined(_DEBUG)
+#  if defined(_DEBUG) && !defined(Py_DEBUG)
 #    define PYBIND11_DEBUG_MARKER
 #    undef _DEBUG
 #  endif
@@ -113,10 +123,9 @@
 #include <frameobject.h>
 #include <pythread.h>
 
-#if defined(_WIN32) && (defined(min) || defined(max))
-#  error Macro clash with min and max -- define NOMINMAX when compiling your program on Windows
-#endif
-
+/* Python #defines overrides on all sorts of core functions, which
+   tends to weak havok in C++ codebases that expect these to work
+   like regular functions (potentially with several overloads) */
 #if defined(isalnum)
 #  undef isalnum
 #  undef isalpha
@@ -127,6 +136,10 @@
 #  undef toupper
 #endif
 
+#if defined(copysign)
+#  undef copysign
+#endif
+
 #if defined(_MSC_VER)
 #  if defined(PYBIND11_DEBUG_MARKER)
 #    define _DEBUG
@@ -168,7 +181,10 @@
 #define PYBIND11_STR_TYPE ::pybind11::str
 #define PYBIND11_BOOL_ATTR "__bool__"
 #define PYBIND11_NB_BOOL(ptr) ((ptr)->nb_bool)
+// Providing a separate declaration to make Clang's -Wmissing-prototypes happy.
+// See comment for PYBIND11_MODULE below for why this is marked "maybe unused".
 #define PYBIND11_PLUGIN_IMPL(name) \
+    extern "C" PYBIND11_MAYBE_UNUSED PYBIND11_EXPORT PyObject *PyInit_##name(); \
     extern "C" PYBIND11_EXPORT PyObject *PyInit_##name()
 
 #else
@@ -192,11 +208,14 @@
 #define PYBIND11_STR_TYPE ::pybind11::bytes
 #define PYBIND11_BOOL_ATTR "__nonzero__"
 #define PYBIND11_NB_BOOL(ptr) ((ptr)->nb_nonzero)
+// Providing a separate PyInit decl to make Clang's -Wmissing-prototypes happy.
+// See comment for PYBIND11_MODULE below for why this is marked "maybe unused".
 #define PYBIND11_PLUGIN_IMPL(name) \
-    static PyObject *pybind11_init_wrapper();               \
-    extern "C" PYBIND11_EXPORT void init##name() {          \
-        (void)pybind11_init_wrapper();                      \
-    }                                                       \
+    static PyObject *pybind11_init_wrapper();                           \
+    extern "C" PYBIND11_MAYBE_UNUSED PYBIND11_EXPORT void init##name(); \
+    extern "C" PYBIND11_EXPORT void init##name() {                      \
+        (void)pybind11_init_wrapper();                                  \
+    }                                                                   \
     PyObject *pybind11_init_wrapper()
 #endif
 
@@ -211,6 +230,8 @@ extern "C" {
 #define PYBIND11_STRINGIFY(x) #x
 #define PYBIND11_TOSTRING(x) PYBIND11_STRINGIFY(x)
 #define PYBIND11_CONCAT(first, second) first##second
+#define PYBIND11_ENSURE_INTERNALS_READY \
+    pybind11::detail::get_internals();
 
 #define PYBIND11_CHECK_PYTHON_VERSION \
     {                                                                          \
@@ -257,6 +278,7 @@ extern "C" {
     static PyObject *pybind11_init();                                          \
     PYBIND11_PLUGIN_IMPL(name) {                                               \
         PYBIND11_CHECK_PYTHON_VERSION                                          \
+        PYBIND11_ENSURE_INTERNALS_READY                                        \
         try {                                                                  \
             return pybind11_init();                                            \
         } PYBIND11_CATCH_INIT_EXCEPTIONS                                       \
@@ -269,6 +291,10 @@ extern "C" {
     should not be in quotes. The second macro argument defines a variable of type
     `py::module` which can be used to initialize the module.
 
+    The entry point is marked as "maybe unused" to aid dead-code detection analysis:
+    since the entry point is typically only looked up at runtime and not referenced
+    during translation, it would otherwise appear as unused ("dead") code.
+
     .. code-block:: cpp
 
         PYBIND11_MODULE(example, m) {
@@ -281,9 +307,11 @@ extern "C" {
         }
 \endrst */
 #define PYBIND11_MODULE(name, variable)                                        \
+    PYBIND11_MAYBE_UNUSED                                                      \
     static void PYBIND11_CONCAT(pybind11_init_, name)(pybind11::module &);     \
     PYBIND11_PLUGIN_IMPL(name) {                                               \
         PYBIND11_CHECK_PYTHON_VERSION                                          \
+        PYBIND11_ENSURE_INTERNALS_READY                                        \
         auto m = pybind11::module(PYBIND11_TOSTRING(name));                    \
         try {                                                                  \
             PYBIND11_CONCAT(pybind11_init_, name)(m);                          \
@@ -293,7 +321,7 @@ extern "C" {
     void PYBIND11_CONCAT(pybind11_init_, name)(pybind11::module &variable)
 
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
 
 using ssize_t = Py_ssize_t;
 using size_t  = std::size_t;
@@ -350,7 +378,7 @@ enum class return_value_policy : uint8_t {
     reference_internal
 };
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 inline static constexpr int log2(size_t n, int k = 0) { return (n <= 1) ? k : log2(n >> 1, k + 1); }
 
@@ -528,7 +556,7 @@ template <typename T, typename... Ts>
 constexpr size_t constexpr_sum(T n, Ts... ns) { return size_t{n} + constexpr_sum(ns...); }
 #endif
 
-NAMESPACE_BEGIN(constexpr_impl)
+PYBIND11_NAMESPACE_BEGIN(constexpr_impl)
 /// Implementation details for constexpr functions
 constexpr int first(int i) { return i; }
 template <typename T, typename... Ts>
@@ -537,7 +565,7 @@ constexpr int first(int i, T v, Ts... vs) { return v ? i : first(i + 1, vs...);
 constexpr int last(int /*i*/, int result) { return result; }
 template <typename T, typename... Ts>
 constexpr int last(int i, int result, T v, Ts... vs) { return last(i + 1, v ? i : result, vs...); }
-NAMESPACE_END(constexpr_impl)
+PYBIND11_NAMESPACE_END(constexpr_impl)
 
 /// Return the index of the first type in Ts which satisfies Predicate<T>.  Returns sizeof...(Ts) if
 /// none match.
@@ -648,10 +676,10 @@ inline void ignore_unused(const int *) { }
 #define PYBIND11_EXPAND_SIDE_EFFECTS(PATTERN) (((PATTERN), void()), ...)
 #else
 using expand_side_effects = bool[];
-#define PYBIND11_EXPAND_SIDE_EFFECTS(PATTERN) pybind11::detail::expand_side_effects{ ((PATTERN), void(), false)..., false }
+#define PYBIND11_EXPAND_SIDE_EFFECTS(PATTERN) (void)pybind11::detail::expand_side_effects{ ((PATTERN), void(), false)..., false }
 #endif
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 /// C++ bindings of builtin Python exceptions
 class builtin_exception : public std::runtime_error {
@@ -673,6 +701,8 @@ PYBIND11_RUNTIME_EXCEPTION(index_error, PyExc_IndexError)
 PYBIND11_RUNTIME_EXCEPTION(key_error, PyExc_KeyError)
 PYBIND11_RUNTIME_EXCEPTION(value_error, PyExc_ValueError)
 PYBIND11_RUNTIME_EXCEPTION(type_error, PyExc_TypeError)
+PYBIND11_RUNTIME_EXCEPTION(buffer_error, PyExc_BufferError)
+PYBIND11_RUNTIME_EXCEPTION(import_error, PyExc_ImportError)
 PYBIND11_RUNTIME_EXCEPTION(cast_error, PyExc_RuntimeError) /// Thrown when pybind11::cast or handle::call fail due to a type casting error
 PYBIND11_RUNTIME_EXCEPTION(reference_cast_error, PyExc_RuntimeError) /// Used internally
 
@@ -681,7 +711,7 @@ PYBIND11_RUNTIME_EXCEPTION(reference_cast_error, PyExc_RuntimeError) /// Used in
 
 template <typename T, typename SFINAE = void> struct format_descriptor { };
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 // Returns the index of the given type in the type char array below, and in the list in numpy.h
 // The order here is: bool; 8 ints ((signed,unsigned)x(8,16,32,64)bits); float,double,long double;
 // complex float,double,long double.  Note that the long double types only participate when long
@@ -694,7 +724,7 @@ template <typename T> struct is_fmt_numeric<T, enable_if_t<std::is_arithmetic<T>
         std::is_integral<T>::value ? detail::log2(sizeof(T))*2 + std::is_unsigned<T>::value : 8 + (
         std::is_same<T, double>::value ? 1 : std::is_same<T, long double>::value ? 2 : 0));
 };
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 template <typename T> struct format_descriptor<T, detail::enable_if_t<std::is_arithmetic<T>::value>> {
     static constexpr const char c = "?bBhHiIqQfdg"[detail::is_fmt_numeric<T>::index];
@@ -719,11 +749,7 @@ struct error_scope {
 /// Dummy destructor wrapper that can be used to expose classes with a private destructor
 struct nodelete { template <typename T> void operator()(T*) { } };
 
-// overload_cast requires variable templates: C++14
-#if defined(PYBIND11_CPP14)
-#define PYBIND11_OVERLOAD_CAST 1
-
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 template <typename... Args>
 struct overload_cast_impl {
     constexpr overload_cast_impl() {} // MSVC 2015 needs this
@@ -740,28 +766,32 @@ struct overload_cast_impl {
     constexpr auto operator()(Return (Class::*pmf)(Args...) const, std::true_type) const noexcept
                               -> decltype(pmf) { return pmf; }
 };
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
+// overload_cast requires variable templates: C++14
+#if defined(PYBIND11_CPP14)
+#define PYBIND11_OVERLOAD_CAST 1
 /// Syntax sugar for resolving overloaded function pointers:
 ///  - regular: static_cast<Return (Class::*)(Arg0, Arg1, Arg2)>(&Class::func)
 ///  - sweet:   overload_cast<Arg0, Arg1, Arg2>(&Class::func)
 template <typename... Args>
 static constexpr detail::overload_cast_impl<Args...> overload_cast = {};
 // MSVC 2015 only accepts this particular initialization syntax for this variable template.
+#endif
 
 /// Const member function selector for overload_cast
 ///  - regular: static_cast<Return (Class::*)(Arg) const>(&Class::func)
 ///  - sweet:   overload_cast<Arg>(&Class::func, const_)
 static constexpr auto const_ = std::true_type{};
 
-#else // no overload_cast: providing something that static_assert-fails:
+#if !defined(PYBIND11_CPP14) // no overload_cast: providing something that static_assert-fails:
 template <typename... Args> struct overload_cast {
     static_assert(detail::deferred_t<std::false_type, Args...>::value,
                   "pybind11::overload_cast<...> requires compiling in C++14 mode");
 };
 #endif // overload_cast
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 // Adaptor for converting arbitrary container arguments into a vector; implicitly convertible from
 // any standard container (or C-style array) supporting std::begin/std::end, any singleton
@@ -800,8 +830,8 @@ public:
     const std::vector<T> *operator->() const { return &v; }
 };
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 
 
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/detail/descr.h b/wrap/pybind11/include/pybind11/detail/descr.h
similarity index 95%
rename from wrap/python/pybind11/include/pybind11/detail/descr.h
rename to wrap/pybind11/include/pybind11/detail/descr.h
index 8d404e534..92720cd56 100644
--- a/wrap/python/pybind11/include/pybind11/detail/descr.h
+++ b/wrap/pybind11/include/pybind11/detail/descr.h
@@ -11,8 +11,8 @@
 
 #include "common.h"
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 #if !defined(_MSC_VER)
 #  define PYBIND11_DESCR_CONSTEXPR static constexpr
@@ -96,5 +96,5 @@ constexpr descr<N + 2, Ts...> type_descr(const descr<N, Ts...> &descr) {
     return _("{") + descr + _("}");
 }
 
-NAMESPACE_END(detail)
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/detail/init.h b/wrap/pybind11/include/pybind11/detail/init.h
similarity index 98%
rename from wrap/python/pybind11/include/pybind11/detail/init.h
rename to wrap/pybind11/include/pybind11/detail/init.h
index acfe00bdb..35b95bcfa 100644
--- a/wrap/python/pybind11/include/pybind11/detail/init.h
+++ b/wrap/pybind11/include/pybind11/detail/init.h
@@ -11,8 +11,8 @@
 
 #include "class.h"
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 template <>
 class type_caster<value_and_holder> {
@@ -30,7 +30,7 @@ private:
     value_and_holder *value = nullptr;
 };
 
-NAMESPACE_BEGIN(initimpl)
+PYBIND11_NAMESPACE_BEGIN(initimpl)
 
 inline void no_nullptr(void *ptr) {
     if (!ptr) throw type_error("pybind11::init(): factory function returned nullptr");
@@ -330,6 +330,6 @@ struct pickle_factory<Get, Set, RetState(Self), NewInstance(ArgState)> {
     }
 };
 
-NAMESPACE_END(initimpl)
-NAMESPACE_END(detail)
-NAMESPACE_END(pybind11)
+PYBIND11_NAMESPACE_END(initimpl)
+PYBIND11_NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(pybind11)
diff --git a/wrap/python/pybind11/include/pybind11/detail/internals.h b/wrap/pybind11/include/pybind11/detail/internals.h
similarity index 72%
rename from wrap/python/pybind11/include/pybind11/detail/internals.h
rename to wrap/pybind11/include/pybind11/detail/internals.h
index f1dd38764..cf40e9fe9 100644
--- a/wrap/python/pybind11/include/pybind11/detail/internals.h
+++ b/wrap/pybind11/include/pybind11/detail/internals.h
@@ -11,8 +11,8 @@
 
 #include "../pytypes.h"
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(detail)
 // Forward declarations
 inline PyTypeObject *make_static_property_type();
 inline PyTypeObject *make_default_metaclass();
@@ -25,6 +25,7 @@ inline PyObject *make_object_base_type(PyTypeObject *metaclass);
 #    define PYBIND11_TLS_GET_VALUE(key) PyThread_tss_get((key))
 #    define PYBIND11_TLS_REPLACE_VALUE(key, value) PyThread_tss_set((key), (value))
 #    define PYBIND11_TLS_DELETE_VALUE(key) PyThread_tss_set((key), nullptr)
+#    define PYBIND11_TLS_FREE(key) PyThread_tss_free(key)
 #else
     // Usually an int but a long on Cygwin64 with Python 3.x
 #    define PYBIND11_TLS_KEY_INIT(var) decltype(PyThread_create_key()) var = 0
@@ -43,6 +44,7 @@ inline PyObject *make_object_base_type(PyTypeObject *metaclass);
 #        define PYBIND11_TLS_REPLACE_VALUE(key, value)                       \
              PyThread_set_key_value((key), (value))
 #    endif
+#    define PYBIND11_TLS_FREE(key) (void)key
 #endif
 
 // Python loads modules by default with dlopen with the RTLD_LOCAL flag; under libc++ and possibly
@@ -108,6 +110,16 @@ struct internals {
 #if defined(WITH_THREAD)
     PYBIND11_TLS_KEY_INIT(tstate);
     PyInterpreterState *istate = nullptr;
+    ~internals() {
+        // This destructor is called *after* Py_Finalize() in finalize_interpreter().
+        // That *SHOULD BE* fine. The following details what happens whe PyThread_tss_free is called.
+        // PYBIND11_TLS_FREE is PyThread_tss_free on python 3.7+. On older python, it does nothing.
+        // PyThread_tss_free calls PyThread_tss_delete and PyMem_RawFree.
+        // PyThread_tss_delete just calls TlsFree (on Windows) or pthread_key_delete (on *NIX). Neither
+        // of those have anything to do with CPython internals.
+        // PyMem_RawFree *requires* that the `tstate` be allocated with the CPython allocator.
+        PYBIND11_TLS_FREE(tstate);
+    }
 #endif
 };
 
@@ -138,14 +150,49 @@ struct type_info {
 };
 
 /// Tracks the `internals` and `type_info` ABI version independent of the main library version
-#define PYBIND11_INTERNALS_VERSION 3
+#define PYBIND11_INTERNALS_VERSION 4
 
-#if defined(_DEBUG)
+/// On MSVC, debug and release builds are not ABI-compatible!
+#if defined(_MSC_VER) && defined(_DEBUG)
 #   define PYBIND11_BUILD_TYPE "_debug"
 #else
 #   define PYBIND11_BUILD_TYPE ""
 #endif
 
+/// Let's assume that different compilers are ABI-incompatible.
+#if defined(_MSC_VER)
+#   define PYBIND11_COMPILER_TYPE "_msvc"
+#elif defined(__INTEL_COMPILER)
+#   define PYBIND11_COMPILER_TYPE "_icc"
+#elif defined(__clang__)
+#   define PYBIND11_COMPILER_TYPE "_clang"
+#elif defined(__PGI)
+#   define PYBIND11_COMPILER_TYPE "_pgi"
+#elif defined(__MINGW32__)
+#   define PYBIND11_COMPILER_TYPE "_mingw"
+#elif defined(__CYGWIN__)
+#   define PYBIND11_COMPILER_TYPE "_gcc_cygwin"
+#elif defined(__GNUC__)
+#   define PYBIND11_COMPILER_TYPE "_gcc"
+#else
+#   define PYBIND11_COMPILER_TYPE "_unknown"
+#endif
+
+#if defined(_LIBCPP_VERSION)
+#  define PYBIND11_STDLIB "_libcpp"
+#elif defined(__GLIBCXX__) || defined(__GLIBCPP__)
+#  define PYBIND11_STDLIB "_libstdcpp"
+#else
+#  define PYBIND11_STDLIB ""
+#endif
+
+/// On Linux/OSX, changes in __GXX_ABI_VERSION__ indicate ABI incompatibility.
+#if defined(__GXX_ABI_VERSION)
+#  define PYBIND11_BUILD_ABI "_cxxabi" PYBIND11_TOSTRING(__GXX_ABI_VERSION)
+#else
+#  define PYBIND11_BUILD_ABI ""
+#endif
+
 #if defined(WITH_THREAD)
 #  define PYBIND11_INTERNALS_KIND ""
 #else
@@ -153,10 +200,10 @@ struct type_info {
 #endif
 
 #define PYBIND11_INTERNALS_ID "__pybind11_internals_v" \
-    PYBIND11_TOSTRING(PYBIND11_INTERNALS_VERSION) PYBIND11_INTERNALS_KIND PYBIND11_BUILD_TYPE "__"
+    PYBIND11_TOSTRING(PYBIND11_INTERNALS_VERSION) PYBIND11_INTERNALS_KIND PYBIND11_COMPILER_TYPE PYBIND11_STDLIB PYBIND11_BUILD_ABI PYBIND11_BUILD_TYPE "__"
 
 #define PYBIND11_MODULE_LOCAL_ID "__pybind11_module_local_v" \
-    PYBIND11_TOSTRING(PYBIND11_INTERNALS_VERSION) PYBIND11_INTERNALS_KIND PYBIND11_BUILD_TYPE "__"
+    PYBIND11_TOSTRING(PYBIND11_INTERNALS_VERSION) PYBIND11_INTERNALS_KIND PYBIND11_COMPILER_TYPE PYBIND11_STDLIB PYBIND11_BUILD_ABI PYBIND11_BUILD_TYPE "__"
 
 /// Each module locally stores a pointer to the `internals` data. The data
 /// itself is shared among modules with the same `PYBIND11_INTERNALS_ID`.
@@ -165,12 +212,49 @@ inline internals **&get_internals_pp() {
     return internals_pp;
 }
 
+inline void translate_exception(std::exception_ptr p) {
+    try {
+        if (p) std::rethrow_exception(p);
+    } catch (error_already_set &e)           { e.restore();                                    return;
+    } catch (const builtin_exception &e)     { e.set_error();                                  return;
+    } catch (const std::bad_alloc &e)        { PyErr_SetString(PyExc_MemoryError,   e.what()); return;
+    } catch (const std::domain_error &e)     { PyErr_SetString(PyExc_ValueError,    e.what()); return;
+    } catch (const std::invalid_argument &e) { PyErr_SetString(PyExc_ValueError,    e.what()); return;
+    } catch (const std::length_error &e)     { PyErr_SetString(PyExc_ValueError,    e.what()); return;
+    } catch (const std::out_of_range &e)     { PyErr_SetString(PyExc_IndexError,    e.what()); return;
+    } catch (const std::range_error &e)      { PyErr_SetString(PyExc_ValueError,    e.what()); return;
+    } catch (const std::overflow_error &e)   { PyErr_SetString(PyExc_OverflowError, e.what()); return;
+    } catch (const std::exception &e)        { PyErr_SetString(PyExc_RuntimeError,  e.what()); return;
+    } catch (...) {
+        PyErr_SetString(PyExc_RuntimeError, "Caught an unknown exception!");
+        return;
+    }
+}
+
+#if !defined(__GLIBCXX__)
+inline void translate_local_exception(std::exception_ptr p) {
+    try {
+        if (p) std::rethrow_exception(p);
+    } catch (error_already_set &e)       { e.restore();   return;
+    } catch (const builtin_exception &e) { e.set_error(); return;
+    }
+}
+#endif
+
 /// Return a reference to the current `internals` data
 PYBIND11_NOINLINE inline internals &get_internals() {
     auto **&internals_pp = get_internals_pp();
     if (internals_pp && *internals_pp)
         return **internals_pp;
 
+    // Ensure that the GIL is held since we will need to make Python calls.
+    // Cannot use py::gil_scoped_acquire here since that constructor calls get_internals.
+    struct gil_scoped_acquire_local {
+        gil_scoped_acquire_local() : state (PyGILState_Ensure()) {}
+        ~gil_scoped_acquire_local() { PyGILState_Release(state); }
+        const PyGILState_STATE state;
+    } gil;
+
     constexpr auto *id = PYBIND11_INTERNALS_ID;
     auto builtins = handle(PyEval_GetBuiltins());
     if (builtins.contains(id) && isinstance<capsule>(builtins[id])) {
@@ -182,22 +266,17 @@ PYBIND11_NOINLINE inline internals &get_internals() {
         //
         // libstdc++ doesn't require this (types there are identified only by name)
 #if !defined(__GLIBCXX__)
-        (*internals_pp)->registered_exception_translators.push_front(
-            [](std::exception_ptr p) -> void {
-                try {
-                    if (p) std::rethrow_exception(p);
-                } catch (error_already_set &e)       { e.restore();   return;
-                } catch (const builtin_exception &e) { e.set_error(); return;
-                }
-            }
-        );
+        (*internals_pp)->registered_exception_translators.push_front(&translate_local_exception);
 #endif
     } else {
         if (!internals_pp) internals_pp = new internals*();
         auto *&internals_ptr = *internals_pp;
         internals_ptr = new internals();
 #if defined(WITH_THREAD)
-        PyEval_InitThreads();
+
+        #if PY_VERSION_HEX < 0x03090000
+                PyEval_InitThreads();
+        #endif
         PyThreadState *tstate = PyThreadState_Get();
         #if PY_VERSION_HEX >= 0x03070000
             internals_ptr->tstate = PyThread_tss_alloc();
@@ -213,25 +292,7 @@ PYBIND11_NOINLINE inline internals &get_internals() {
         internals_ptr->istate = tstate->interp;
 #endif
         builtins[id] = capsule(internals_pp);
-        internals_ptr->registered_exception_translators.push_front(
-            [](std::exception_ptr p) -> void {
-                try {
-                    if (p) std::rethrow_exception(p);
-                } catch (error_already_set &e)           { e.restore();                                    return;
-                } catch (const builtin_exception &e)     { e.set_error();                                  return;
-                } catch (const std::bad_alloc &e)        { PyErr_SetString(PyExc_MemoryError,   e.what()); return;
-                } catch (const std::domain_error &e)     { PyErr_SetString(PyExc_ValueError,    e.what()); return;
-                } catch (const std::invalid_argument &e) { PyErr_SetString(PyExc_ValueError,    e.what()); return;
-                } catch (const std::length_error &e)     { PyErr_SetString(PyExc_ValueError,    e.what()); return;
-                } catch (const std::out_of_range &e)     { PyErr_SetString(PyExc_IndexError,    e.what()); return;
-                } catch (const std::range_error &e)      { PyErr_SetString(PyExc_ValueError,    e.what()); return;
-                } catch (const std::exception &e)        { PyErr_SetString(PyExc_RuntimeError,  e.what()); return;
-                } catch (...) {
-                    PyErr_SetString(PyExc_RuntimeError, "Caught an unknown exception!");
-                    return;
-                }
-            }
-        );
+        internals_ptr->registered_exception_translators.push_front(&translate_exception);
         internals_ptr->static_property_type = make_static_property_type();
         internals_ptr->default_metaclass = make_default_metaclass();
         internals_ptr->instance_base = make_object_base_type(internals_ptr->default_metaclass);
@@ -256,7 +317,7 @@ const char *c_str(Args &&...args) {
     return strings.front().c_str();
 }
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 /// Returns a named pointer that is shared among all extension modules (using the same
 /// pybind11 version) running in the current interpreter. Names starting with underscores
@@ -288,4 +349,4 @@ T &get_or_create_shared_data(const std::string &name) {
     return *ptr;
 }
 
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/detail/typeid.h b/wrap/pybind11/include/pybind11/detail/typeid.h
similarity index 89%
rename from wrap/python/pybind11/include/pybind11/detail/typeid.h
rename to wrap/pybind11/include/pybind11/detail/typeid.h
index 9c8a4fc69..148889ffe 100644
--- a/wrap/python/pybind11/include/pybind11/detail/typeid.h
+++ b/wrap/pybind11/include/pybind11/detail/typeid.h
@@ -18,8 +18,8 @@
 
 #include "common.h"
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(detail)
 /// Erase all occurrences of a substring
 inline void erase_all(std::string &string, const std::string &search) {
     for (size_t pos = 0;;) {
@@ -43,7 +43,7 @@ PYBIND11_NOINLINE inline void clean_type_id(std::string &name) {
 #endif
     detail::erase_all(name, "pybind11::");
 }
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 /// Return a string representation of a C++ type
 template <typename T> static std::string type_id() {
@@ -52,4 +52,4 @@ template <typename T> static std::string type_id() {
     return name;
 }
 
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/eigen.h b/wrap/pybind11/include/pybind11/eigen.h
similarity index 99%
rename from wrap/python/pybind11/include/pybind11/eigen.h
rename to wrap/pybind11/include/pybind11/eigen.h
index d963d9650..22139def6 100644
--- a/wrap/python/pybind11/include/pybind11/eigen.h
+++ b/wrap/pybind11/include/pybind11/eigen.h
@@ -41,14 +41,14 @@
 // of matrices seems highly undesirable.
 static_assert(EIGEN_VERSION_AT_LEAST(3,2,7), "Eigen support in pybind11 requires Eigen >= 3.2.7");
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
 
 // Provide a convenience alias for easier pass-by-ref usage with fully dynamic strides:
 using EigenDStride = Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>;
 template <typename MatrixType> using EigenDRef = Eigen::Ref<MatrixType, 0, EigenDStride>;
 template <typename MatrixType> using EigenDMap = Eigen::Map<MatrixType, 0, EigenDStride>;
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 #if EIGEN_VERSION_AT_LEAST(3,3,0)
 using EigenIndex = Eigen::Index;
@@ -597,8 +597,8 @@ struct type_caster<Type, enable_if_t<is_eigen_sparse<Type>::value>> {
             + npy_format_descriptor<Scalar>::name + _("]"));
 };
 
-NAMESPACE_END(detail)
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
 
 #if defined(__GNUG__) || defined(__clang__)
 #  pragma GCC diagnostic pop
diff --git a/wrap/python/pybind11/include/pybind11/embed.h b/wrap/pybind11/include/pybind11/embed.h
similarity index 94%
rename from wrap/python/pybind11/include/pybind11/embed.h
rename to wrap/pybind11/include/pybind11/embed.h
index 72655885e..eae86c714 100644
--- a/wrap/python/pybind11/include/pybind11/embed.h
+++ b/wrap/pybind11/include/pybind11/embed.h
@@ -18,11 +18,13 @@
 
 #if PY_MAJOR_VERSION >= 3
 #  define PYBIND11_EMBEDDED_MODULE_IMPL(name)            \
+      extern "C" PyObject *pybind11_init_impl_##name();  \
       extern "C" PyObject *pybind11_init_impl_##name() { \
           return pybind11_init_wrapper_##name();         \
       }
 #else
 #  define PYBIND11_EMBEDDED_MODULE_IMPL(name)            \
+      extern "C" void pybind11_init_impl_##name();       \
       extern "C" void pybind11_init_impl_##name() {      \
           pybind11_init_wrapper_##name();                \
       }
@@ -59,13 +61,14 @@
         }                                                                     \
     }                                                                         \
     PYBIND11_EMBEDDED_MODULE_IMPL(name)                                       \
-    pybind11::detail::embedded_module name(PYBIND11_TOSTRING(name),           \
+    pybind11::detail::embedded_module PYBIND11_CONCAT(pybind11_module_, name) \
+                              (PYBIND11_TOSTRING(name),             \
                                PYBIND11_CONCAT(pybind11_init_impl_, name));   \
     void PYBIND11_CONCAT(pybind11_init_, name)(pybind11::module &variable)
 
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 /// Python 2.7/3.x compatible version of `PyImport_AppendInittab` and error checks.
 struct embedded_module {
@@ -84,7 +87,7 @@ struct embedded_module {
     }
 };
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 /** \rst
     Initialize the Python interpreter. No other pybind11 or CPython API functions can be
@@ -197,4 +200,4 @@ private:
     bool is_valid = true;
 };
 
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/eval.h b/wrap/pybind11/include/pybind11/eval.h
similarity index 86%
rename from wrap/python/pybind11/include/pybind11/eval.h
rename to wrap/pybind11/include/pybind11/eval.h
index ea85ba1db..ba82cf42a 100644
--- a/wrap/python/pybind11/include/pybind11/eval.h
+++ b/wrap/pybind11/include/pybind11/eval.h
@@ -13,7 +13,7 @@
 
 #include "pybind11.h"
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
 
 enum eval_mode {
     /// Evaluate a string containing an isolated expression
@@ -66,6 +66,20 @@ void exec(const char (&s)[N], object global = globals(), object local = object()
     eval<eval_statements>(s, global, local);
 }
 
+#if defined(PYPY_VERSION) && PY_VERSION_HEX >= 0x3000000
+template <eval_mode mode = eval_statements>
+object eval_file(str, object, object) {
+    pybind11_fail("eval_file not supported in PyPy3. Use eval");
+}
+template <eval_mode mode = eval_statements>
+object eval_file(str, object) {
+    pybind11_fail("eval_file not supported in PyPy3. Use eval");
+}
+template <eval_mode mode = eval_statements>
+object eval_file(str) {
+    pybind11_fail("eval_file not supported in PyPy3. Use eval");
+}
+#else
 template <eval_mode mode = eval_statements>
 object eval_file(str fname, object global = globals(), object local = object()) {
     if (!local)
@@ -113,5 +127,6 @@ object eval_file(str fname, object global = globals(), object local = object())
         throw error_already_set();
     return reinterpret_steal<object>(result);
 }
+#endif
 
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/functional.h b/wrap/pybind11/include/pybind11/functional.h
similarity index 79%
rename from wrap/python/pybind11/include/pybind11/functional.h
rename to wrap/pybind11/include/pybind11/functional.h
index 7a0988ab0..57b6cd210 100644
--- a/wrap/python/pybind11/include/pybind11/functional.h
+++ b/wrap/pybind11/include/pybind11/functional.h
@@ -12,8 +12,8 @@
 #include "pybind11.h"
 #include <functional>
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 template <typename Return, typename... Args>
 struct type_caster<std::function<Return(Args...)>> {
@@ -65,12 +65,19 @@ public:
             }
         };
 
-        value = [hfunc = func_handle(std::move(func))](Args... args) -> Return {
-            gil_scoped_acquire acq;
-            object retval(hfunc.f(std::forward<Args>(args)...));
-            /* Visual studio 2015 parser issue: need parentheses around this expression */
-            return (retval.template cast<Return>());
+        // to emulate 'move initialization capture' in C++11
+        struct func_wrapper {
+            func_handle hfunc;
+            func_wrapper(func_handle&& hf): hfunc(std::move(hf)) {}
+            Return operator()(Args... args) const {
+                gil_scoped_acquire acq;
+                object retval(hfunc.f(std::forward<Args>(args)...));
+                /* Visual studio 2015 parser issue: need parentheses around this expression */
+                return (retval.template cast<Return>());
+            }
         };
+
+        value = func_wrapper(func_handle(std::move(func)));
         return true;
     }
 
@@ -90,5 +97,5 @@ public:
                                + make_caster<retval_type>::name + _("]"));
 };
 
-NAMESPACE_END(detail)
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/iostream.h b/wrap/pybind11/include/pybind11/iostream.h
similarity index 95%
rename from wrap/python/pybind11/include/pybind11/iostream.h
rename to wrap/pybind11/include/pybind11/iostream.h
index 72baef8fd..eaf92dfa4 100644
--- a/wrap/python/pybind11/include/pybind11/iostream.h
+++ b/wrap/pybind11/include/pybind11/iostream.h
@@ -17,8 +17,8 @@
 #include <memory>
 #include <iostream>
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 // Buffer that writes to Python instead of C++
 class pythonbuf : public std::streambuf {
@@ -64,13 +64,15 @@ public:
         setp(d_buffer.get(), d_buffer.get() + buf_size - 1);
     }
 
+    pythonbuf(pythonbuf&&) = default;
+
     /// Sync before destroy
     ~pythonbuf() {
         sync();
     }
 };
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 
 /** \rst
@@ -142,7 +144,7 @@ public:
 };
 
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 // Class to redirect output as a context manager. C++ backend.
 class OstreamRedirect {
@@ -168,7 +170,7 @@ public:
     }
 };
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 /** \rst
     This is a helper function to add a C++ redirect context manager to Python
@@ -204,4 +206,4 @@ inline class_<detail::OstreamRedirect> add_ostream_redirect(module m, std::strin
         .def("__exit__", [](detail::OstreamRedirect &self_, args) { self_.exit(); });
 }
 
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/numpy.h b/wrap/pybind11/include/pybind11/numpy.h
similarity index 95%
rename from wrap/python/pybind11/include/pybind11/numpy.h
rename to wrap/pybind11/include/pybind11/numpy.h
index b2a02e024..8c390c531 100644
--- a/wrap/python/pybind11/include/pybind11/numpy.h
+++ b/wrap/pybind11/include/pybind11/numpy.h
@@ -14,6 +14,7 @@
 #include <numeric>
 #include <algorithm>
 #include <array>
+#include <cstdint>
 #include <cstdlib>
 #include <cstring>
 #include <sstream>
@@ -34,11 +35,14 @@
    upon the library user. */
 static_assert(sizeof(ssize_t) == sizeof(Py_intptr_t), "ssize_t != Py_intptr_t");
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
 
 class array; // Forward declaration
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
+
+template <> struct handle_type_name<array> { static constexpr auto name = _("numpy.ndarray"); };
+
 template <typename type, typename SFINAE = void> struct npy_format_descriptor;
 
 struct PyArrayDescr_Proxy {
@@ -108,6 +112,18 @@ inline numpy_internals& get_numpy_internals() {
     return *ptr;
 }
 
+template <typename T> struct same_size {
+    template <typename U> using as = bool_constant<sizeof(T) == sizeof(U)>;
+};
+
+template <typename Concrete> constexpr int platform_lookup() { return -1; }
+
+// Lookup a type according to its size, and return a value corresponding to the NumPy typenum.
+template <typename Concrete, typename T, typename... Ts, typename... Ints>
+constexpr int platform_lookup(int I, Ints... Is) {
+    return sizeof(Concrete) == sizeof(T) ? I : platform_lookup<Concrete, Ts...>(Is...);
+}
+
 struct npy_api {
     enum constants {
         NPY_ARRAY_C_CONTIGUOUS_ = 0x0001,
@@ -126,7 +142,23 @@ struct npy_api {
         NPY_FLOAT_, NPY_DOUBLE_, NPY_LONGDOUBLE_,
         NPY_CFLOAT_, NPY_CDOUBLE_, NPY_CLONGDOUBLE_,
         NPY_OBJECT_ = 17,
-        NPY_STRING_, NPY_UNICODE_, NPY_VOID_
+        NPY_STRING_, NPY_UNICODE_, NPY_VOID_,
+        // Platform-dependent normalization
+        NPY_INT8_ = NPY_BYTE_,
+        NPY_UINT8_ = NPY_UBYTE_,
+        NPY_INT16_ = NPY_SHORT_,
+        NPY_UINT16_ = NPY_USHORT_,
+        // `npy_common.h` defines the integer aliases. In order, it checks:
+        // NPY_BITSOF_LONG, NPY_BITSOF_LONGLONG, NPY_BITSOF_INT, NPY_BITSOF_SHORT, NPY_BITSOF_CHAR
+        // and assigns the alias to the first matching size, so we should check in this order.
+        NPY_INT32_ = platform_lookup<std::int32_t, long, int, short>(
+            NPY_LONG_, NPY_INT_, NPY_SHORT_),
+        NPY_UINT32_ = platform_lookup<std::uint32_t, unsigned long, unsigned int, unsigned short>(
+            NPY_ULONG_, NPY_UINT_, NPY_USHORT_),
+        NPY_INT64_ = platform_lookup<std::int64_t, long, long long, int>(
+            NPY_LONG_, NPY_LONGLONG_, NPY_INT_),
+        NPY_UINT64_ = platform_lookup<std::uint64_t, unsigned long, unsigned long long, unsigned int>(
+            NPY_ULONG_, NPY_ULONGLONG_, NPY_UINT_),
     };
 
     typedef struct {
@@ -149,8 +181,9 @@ struct npy_api {
     unsigned int (*PyArray_GetNDArrayCFeatureVersion_)();
     PyObject *(*PyArray_DescrFromType_)(int);
     PyObject *(*PyArray_NewFromDescr_)
-        (PyTypeObject *, PyObject *, int, Py_intptr_t *,
-         Py_intptr_t *, void *, int, PyObject *);
+        (PyTypeObject *, PyObject *, int, Py_intptr_t const *,
+         Py_intptr_t const *, void *, int, PyObject *);
+    // Unused. Not removed because that affects ABI of the class.
     PyObject *(*PyArray_DescrNewFromType_)(int);
     int (*PyArray_CopyInto_)(PyObject *, PyObject *);
     PyObject *(*PyArray_NewCopy_)(PyObject *, int);
@@ -161,9 +194,10 @@ struct npy_api {
     PyObject *(*PyArray_FromAny_) (PyObject *, PyObject *, int, int, int, PyObject *);
     int (*PyArray_DescrConverter_) (PyObject *, PyObject **);
     bool (*PyArray_EquivTypes_) (PyObject *, PyObject *);
-    int (*PyArray_GetArrayParamsFromObject_)(PyObject *, PyObject *, char, PyObject **, int *,
-                                             Py_ssize_t *, PyObject **, PyObject *);
+    int (*PyArray_GetArrayParamsFromObject_)(PyObject *, PyObject *, unsigned char, PyObject **, int *,
+                                             Py_intptr_t *, PyObject **, PyObject *);
     PyObject *(*PyArray_Squeeze_)(PyObject *);
+    // Unused. Not removed because that affects ABI of the class.
     int (*PyArray_SetBaseObject_)(PyObject *, PyObject *);
     PyObject* (*PyArray_Resize_)(PyObject*, PyArray_Dims*, int, int);
 private:
@@ -179,7 +213,7 @@ private:
         API_PyArray_CopyInto = 82,
         API_PyArray_NewCopy = 85,
         API_PyArray_NewFromDescr = 94,
-        API_PyArray_DescrNewFromType = 9,
+        API_PyArray_DescrNewFromType = 96,
         API_PyArray_DescrConverter = 174,
         API_PyArray_EquivTypes = 182,
         API_PyArray_GetArrayParamsFromObject = 278,
@@ -410,7 +444,7 @@ struct type_caster<unchecked_reference<T, Dim>> {
 template <typename T, ssize_t Dim>
 struct type_caster<unchecked_mutable_reference<T, Dim>> : type_caster<unchecked_reference<T, Dim>> {};
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 class dtype : public object {
 public:
@@ -834,7 +868,7 @@ public:
                 ExtraFlags & f_style ? f_strides(*shape, itemsize()) : c_strides(*shape, itemsize()),
                 ptr, base) { }
 
-    explicit array_t(size_t count, const T *ptr = nullptr, handle base = handle())
+    explicit array_t(ssize_t count, const T *ptr = nullptr, handle base = handle())
         : array({count}, {}, ptr, base) { }
 
     constexpr ssize_t itemsize() const {
@@ -947,7 +981,7 @@ struct format_descriptor<T, detail::enable_if_t<detail::array_info<T>::is_array>
     }
 };
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 template <typename T, int ExtraFlags>
 struct pyobject_caster<array_t<T, ExtraFlags>> {
     using type = array_t<T, ExtraFlags>;
@@ -978,14 +1012,14 @@ struct npy_format_descriptor_name;
 template <typename T>
 struct npy_format_descriptor_name<T, enable_if_t<std::is_integral<T>::value>> {
     static constexpr auto name = _<std::is_same<T, bool>::value>(
-        _("bool"), _<std::is_signed<T>::value>("int", "uint") + _<sizeof(T)*8>()
+        _("bool"), _<std::is_signed<T>::value>("numpy.int", "numpy.uint") + _<sizeof(T)*8>()
     );
 };
 
 template <typename T>
 struct npy_format_descriptor_name<T, enable_if_t<std::is_floating_point<T>::value>> {
     static constexpr auto name = _<std::is_same<T, float>::value || std::is_same<T, double>::value>(
-        _("float") + _<sizeof(T)*8>(), _("longdouble")
+        _("numpy.float") + _<sizeof(T)*8>(), _("numpy.longdouble")
     );
 };
 
@@ -993,7 +1027,7 @@ template <typename T>
 struct npy_format_descriptor_name<T, enable_if_t<is_complex<T>::value>> {
     static constexpr auto name = _<std::is_same<typename T::value_type, float>::value
                                    || std::is_same<typename T::value_type, double>::value>(
-        _("complex") + _<sizeof(typename T::value_type)*16>(), _("longcomplex")
+        _("numpy.complex") + _<sizeof(typename T::value_type)*16>(), _("numpy.longcomplex")
     );
 };
 
@@ -1004,8 +1038,8 @@ private:
     // NB: the order here must match the one in common.h
     constexpr static const int values[15] = {
         npy_api::NPY_BOOL_,
-        npy_api::NPY_BYTE_,   npy_api::NPY_UBYTE_,   npy_api::NPY_SHORT_,    npy_api::NPY_USHORT_,
-        npy_api::NPY_INT_,    npy_api::NPY_UINT_,    npy_api::NPY_LONGLONG_, npy_api::NPY_ULONGLONG_,
+        npy_api::NPY_BYTE_,   npy_api::NPY_UBYTE_,   npy_api::NPY_INT16_,    npy_api::NPY_UINT16_,
+        npy_api::NPY_INT32_,  npy_api::NPY_UINT32_,  npy_api::NPY_INT64_,    npy_api::NPY_UINT64_,
         npy_api::NPY_FLOAT_,  npy_api::NPY_DOUBLE_,  npy_api::NPY_LONGDOUBLE_,
         npy_api::NPY_CFLOAT_, npy_api::NPY_CDOUBLE_, npy_api::NPY_CLONGDOUBLE_
     };
@@ -1015,7 +1049,7 @@ public:
 
     static pybind11::dtype dtype() {
         if (auto ptr = npy_api::get().PyArray_DescrFromType_(value))
-            return reinterpret_borrow<pybind11::dtype>(ptr);
+            return reinterpret_steal<pybind11::dtype>(ptr);
         pybind11_fail("Unsupported buffer format!");
     }
 };
@@ -1066,8 +1100,14 @@ inline PYBIND11_NOINLINE void register_structured_dtype(
     if (numpy_internals.get_type_info(tinfo, false))
         pybind11_fail("NumPy: dtype is already registered");
 
+    // Use ordered fields because order matters as of NumPy 1.14:
+    // https://docs.scipy.org/doc/numpy/release.html#multiple-field-indexing-assignment-of-structured-arrays
+    std::vector<field_descriptor> ordered_fields(std::move(fields));
+    std::sort(ordered_fields.begin(), ordered_fields.end(),
+        [](const field_descriptor &a, const field_descriptor &b) { return a.offset < b.offset; });
+
     list names, formats, offsets;
-    for (auto field : *fields) {
+    for (auto& field : ordered_fields) {
         if (!field.descr)
             pybind11_fail(std::string("NumPy: unsupported field dtype: `") +
                             field.name + "` @ " + tinfo.name());
@@ -1084,9 +1124,6 @@ inline PYBIND11_NOINLINE void register_structured_dtype(
     // - https://github.com/numpy/numpy/pull/7798
     // Because of this, we won't use numpy's logic to generate buffer format
     // strings and will just do it ourselves.
-    std::vector<field_descriptor> ordered_fields(std::move(fields));
-    std::sort(ordered_fields.begin(), ordered_fields.end(),
-        [](const field_descriptor &a, const field_descriptor &b) { return a.offset < b.offset; });
     ssize_t offset = 0;
     std::ostringstream oss;
     // mark the structure as unaligned with '^', because numpy and C++ don't
@@ -1186,7 +1223,7 @@ private:
 #define PYBIND11_MAP_NEXT0(test, next, ...) next PYBIND11_MAP_OUT
 #define PYBIND11_MAP_NEXT1(test, next) PYBIND11_MAP_NEXT0 (test, next, 0)
 #define PYBIND11_MAP_NEXT(test, next)  PYBIND11_MAP_NEXT1 (PYBIND11_MAP_GET_END test, next)
-#ifdef _MSC_VER // MSVC is not as eager to expand macros, hence this workaround
+#if defined(_MSC_VER) && !defined(__clang__) // MSVC is not as eager to expand macros, hence this workaround
 #define PYBIND11_MAP_LIST_NEXT1(test, next) \
     PYBIND11_EVAL0 (PYBIND11_MAP_NEXT0 (test, PYBIND11_MAP_COMMA next, 0))
 #else
@@ -1208,7 +1245,7 @@ private:
         (::std::vector<::pybind11::detail::field_descriptor> \
          {PYBIND11_MAP_LIST (PYBIND11_FIELD_DESCRIPTOR, Type, __VA_ARGS__)})
 
-#ifdef _MSC_VER
+#if defined(_MSC_VER) && !defined(__clang__)
 #define PYBIND11_MAP2_LIST_NEXT1(test, next) \
     PYBIND11_EVAL0 (PYBIND11_MAP_NEXT0 (test, PYBIND11_MAP_COMMA next, 0))
 #else
@@ -1573,7 +1610,7 @@ template <typename T, int Flags> struct handle_type_name<array_t<T, Flags>> {
     static constexpr auto name = _("numpy.ndarray[") + npy_format_descriptor<T>::name + _("]");
 };
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 // Vanilla pointer vectorizer:
 template <typename Return, typename... Args>
@@ -1603,7 +1640,7 @@ Helper vectorize(Return (Class::*f)(Args...) const) {
     return Helper(std::mem_fn(f));
 }
 
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
 
 #if defined(_MSC_VER)
 #pragma warning(pop)
diff --git a/wrap/python/pybind11/include/pybind11/operators.h b/wrap/pybind11/include/pybind11/operators.h
similarity index 95%
rename from wrap/python/pybind11/include/pybind11/operators.h
rename to wrap/pybind11/include/pybind11/operators.h
index b3dd62c3b..086cb4cfd 100644
--- a/wrap/python/pybind11/include/pybind11/operators.h
+++ b/wrap/pybind11/include/pybind11/operators.h
@@ -18,8 +18,8 @@
 #  pragma warning(disable: 4127) // warning C4127: Conditional expression is constant
 #endif
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 /// Enumeration with all supported operator types
 enum op_id : int {
@@ -147,6 +147,9 @@ PYBIND11_INPLACE_OPERATOR(ixor,     operator^=,   l ^= r)
 PYBIND11_INPLACE_OPERATOR(ior,      operator|=,   l |= r)
 PYBIND11_UNARY_OPERATOR(neg,        operator-,    -l)
 PYBIND11_UNARY_OPERATOR(pos,        operator+,    +l)
+// WARNING: This usage of `abs` should only be done for existing STL overloads.
+// Adding overloads directly in to the `std::` namespace is advised against:
+// https://en.cppreference.com/w/cpp/language/extending_std
 PYBIND11_UNARY_OPERATOR(abs,        abs,          std::abs(l))
 PYBIND11_UNARY_OPERATOR(hash,       hash,         std::hash<L>()(l))
 PYBIND11_UNARY_OPERATOR(invert,     operator~,    (~l))
@@ -157,11 +160,13 @@ PYBIND11_UNARY_OPERATOR(float,      float_,       (double) l)
 #undef PYBIND11_BINARY_OPERATOR
 #undef PYBIND11_INPLACE_OPERATOR
 #undef PYBIND11_UNARY_OPERATOR
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 using detail::self;
+// Add named operators so that they are accessible via `py::`.
+using detail::hash;
 
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
 
 #if defined(_MSC_VER)
 #  pragma warning(pop)
diff --git a/wrap/python/pybind11/include/pybind11/options.h b/wrap/pybind11/include/pybind11/options.h
similarity index 95%
rename from wrap/python/pybind11/include/pybind11/options.h
rename to wrap/pybind11/include/pybind11/options.h
index cc1e1f6f0..d74db1c68 100644
--- a/wrap/python/pybind11/include/pybind11/options.h
+++ b/wrap/pybind11/include/pybind11/options.h
@@ -11,7 +11,7 @@
 
 #include "detail/common.h"
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
 
 class options {
 public:
@@ -62,4 +62,4 @@ private:
     state previous_state;
 };
 
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/pybind11.h b/wrap/pybind11/include/pybind11/pybind11.h
similarity index 94%
rename from wrap/python/pybind11/include/pybind11/pybind11.h
rename to wrap/pybind11/include/pybind11/pybind11.h
index f1d91c788..ee2870524 100644
--- a/wrap/python/pybind11/include/pybind11/pybind11.h
+++ b/wrap/pybind11/include/pybind11/pybind11.h
@@ -41,17 +41,16 @@
 #  endif
 #endif
 
-#if defined(__GNUG__) && !defined(__clang__)
- #include <cxxabi.h>
-#endif
-
-
 #include "attr.h"
 #include "options.h"
 #include "detail/class.h"
 #include "detail/init.h"
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+#if defined(__GNUG__) && !defined(__clang__)
+#  include <cxxabi.h>
+#endif
+
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
 
 /// Wraps an arbitrary C++ function/method/lambda function/.. into a callable Python object
 class cpp_function : public function {
@@ -73,16 +72,34 @@ public:
                    (detail::function_signature_t<Func> *) nullptr, extra...);
     }
 
-    /// Construct a cpp_function from a class method (non-const)
+    /// Construct a cpp_function from a class method (non-const, no ref-qualifier)
     template <typename Return, typename Class, typename... Arg, typename... Extra>
     cpp_function(Return (Class::*f)(Arg...), const Extra&... extra) {
+        initialize([f](Class *c, Arg... args) -> Return { return (c->*f)(std::forward<Arg>(args)...); },
+                   (Return (*) (Class *, Arg...)) nullptr, extra...);
+    }
+
+    /// Construct a cpp_function from a class method (non-const, lvalue ref-qualifier)
+    /// A copy of the overload for non-const functions without explicit ref-qualifier
+    /// but with an added `&`.
+    template <typename Return, typename Class, typename... Arg, typename... Extra>
+    cpp_function(Return (Class::*f)(Arg...)&, const Extra&... extra) {
         initialize([f](Class *c, Arg... args) -> Return { return (c->*f)(args...); },
                    (Return (*) (Class *, Arg...)) nullptr, extra...);
     }
 
-    /// Construct a cpp_function from a class method (const)
+    /// Construct a cpp_function from a class method (const, no ref-qualifier)
     template <typename Return, typename Class, typename... Arg, typename... Extra>
     cpp_function(Return (Class::*f)(Arg...) const, const Extra&... extra) {
+        initialize([f](const Class *c, Arg... args) -> Return { return (c->*f)(std::forward<Arg>(args)...); },
+                   (Return (*)(const Class *, Arg ...)) nullptr, extra...);
+    }
+
+    /// Construct a cpp_function from a class method (const, lvalue ref-qualifier)
+    /// A copy of the overload for const functions without explicit ref-qualifier
+    /// but with an added `&`.
+    template <typename Return, typename Class, typename... Arg, typename... Extra>
+    cpp_function(Return (Class::*f)(Arg...) const&, const Extra&... extra) {
         initialize([f](const Class *c, Arg... args) -> Return { return (c->*f)(args...); },
                    (Return (*)(const Class *, Arg ...)) nullptr, extra...);
     }
@@ -169,6 +186,14 @@ protected:
         /* Process any user-provided function attributes */
         process_attributes<Extra...>::init(extra..., rec);
 
+        {
+            constexpr bool has_kwonly_args = any_of<std::is_same<kwonly, Extra>...>::value,
+                           has_args = any_of<std::is_same<args, Args>...>::value,
+                           has_arg_annotations = any_of<is_keyword<Extra>...>::value;
+            static_assert(has_arg_annotations || !has_kwonly_args, "py::kwonly requires the use of argument annotations");
+            static_assert(!(has_args && has_kwonly_args), "py::kwonly cannot be combined with a py::args argument");
+        }
+
         /* Generate a readable signature describing the function's arguments and return value types */
         static constexpr auto signature = _("(") + cast_in::arg_names + _(") -> ") + cast_out::name;
         PYBIND11_DESCR_CONSTEXPR auto types = decltype(signature)::types();
@@ -484,19 +509,20 @@ protected:
                  */
 
                 const function_record &func = *it;
-                size_t pos_args = func.nargs;    // Number of positional arguments that we need
-                if (func.has_args) --pos_args;   // (but don't count py::args
-                if (func.has_kwargs) --pos_args; //  or py::kwargs)
+                size_t num_args = func.nargs;    // Number of positional arguments that we need
+                if (func.has_args) --num_args;   // (but don't count py::args
+                if (func.has_kwargs) --num_args; //  or py::kwargs)
+                size_t pos_args = num_args - func.nargs_kwonly;
 
                 if (!func.has_args && n_args_in > pos_args)
-                    continue; // Too many arguments for this overload
+                    continue; // Too many positional arguments for this overload
 
                 if (n_args_in < pos_args && func.args.size() < pos_args)
-                    continue; // Not enough arguments given, and not enough defaults to fill in the blanks
+                    continue; // Not enough positional arguments given, and not enough defaults to fill in the blanks
 
                 function_call call(func, parent);
 
-                size_t args_to_copy = std::min(pos_args, n_args_in);
+                size_t args_to_copy = (std::min)(pos_args, n_args_in); // Protect std::min with parentheses
                 size_t args_copied = 0;
 
                 // 0. Inject new-style `self` argument
@@ -536,10 +562,10 @@ protected:
                 dict kwargs = reinterpret_borrow<dict>(kwargs_in);
 
                 // 2. Check kwargs and, failing that, defaults that may help complete the list
-                if (args_copied < pos_args) {
+                if (args_copied < num_args) {
                     bool copied_kwargs = false;
 
-                    for (; args_copied < pos_args; ++args_copied) {
+                    for (; args_copied < num_args; ++args_copied) {
                         const auto &arg = func.args[args_copied];
 
                         handle value;
@@ -565,7 +591,7 @@ protected:
                             break;
                     }
 
-                    if (args_copied < pos_args)
+                    if (args_copied < num_args)
                         continue; // Not enough arguments, defaults, or kwargs to fill the positional arguments
                 }
 
@@ -883,7 +909,7 @@ inline dict globals() {
     return reinterpret_borrow<dict>(p ? p : module::import("__main__").attr("__dict__").ptr());
 }
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 /// Generic support for creating new Python heap types
 class generic_type : public object {
     template <typename...> friend class class_;
@@ -1004,17 +1030,24 @@ void call_operator_delete(T *p, size_t s, size_t) { T::operator delete(p, s); }
 
 inline void call_operator_delete(void *p, size_t s, size_t a) {
     (void)s; (void)a;
-#if defined(PYBIND11_CPP17)
-    if (a > __STDCPP_DEFAULT_NEW_ALIGNMENT__)
-        ::operator delete(p, s, std::align_val_t(a));
-    else
+    #if defined(__cpp_aligned_new) && (!defined(_MSC_VER) || _MSC_VER >= 1912)
+        if (a > __STDCPP_DEFAULT_NEW_ALIGNMENT__) {
+            #ifdef __cpp_sized_deallocation
+                ::operator delete(p, s, std::align_val_t(a));
+            #else
+                ::operator delete(p, std::align_val_t(a));
+            #endif
+            return;
+        }
+    #endif
+    #ifdef __cpp_sized_deallocation
         ::operator delete(p, s);
-#else
-    ::operator delete(p);
-#endif
+    #else
+        ::operator delete(p);
+    #endif
 }
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 /// Given a pointer to a member function, cast it to its `Derived` version.
 /// Forward everything else unchanged.
@@ -1392,7 +1425,7 @@ detail::initimpl::pickle_factory<GetState, SetState> pickle(GetState &&g, SetSta
     return {std::forward<GetState>(g), std::forward<SetState>(s)};
 }
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 struct enum_base {
     enum_base(handle base, handle parent) : m_base(base), m_parent(parent) { }
 
@@ -1412,7 +1445,7 @@ struct enum_base {
                         return pybind11::str("{}.{}").format(type_name, kv.first);
                 }
                 return pybind11::str("{}.???").format(type_name);
-            }, is_method(m_base)
+            }, name("__repr__"), is_method(m_base)
         );
 
         m_base.attr("name") = property(cpp_function(
@@ -1423,7 +1456,7 @@ struct enum_base {
                         return pybind11::str(kv.first);
                 }
                 return "???";
-            }, is_method(m_base)
+            }, name("name"), is_method(m_base)
         ));
 
         m_base.attr("__doc__") = static_property(cpp_function(
@@ -1441,7 +1474,7 @@ struct enum_base {
                         docstring += " : " + (std::string) pybind11::str(comment);
                 }
                 return docstring;
-            }
+            }, name("__doc__")
         ), none(), none(), "");
 
         m_base.attr("__members__") = static_property(cpp_function(
@@ -1450,7 +1483,7 @@ struct enum_base {
                 for (const auto &kv : entries)
                     m[kv.first] = kv.second[int_(0)];
                 return m;
-            }), none(), none(), ""
+            }, name("__members__")), none(), none(), ""
         );
 
         #define PYBIND11_ENUM_OP_STRICT(op, expr, strict_behavior)                     \
@@ -1460,7 +1493,7 @@ struct enum_base {
                         strict_behavior;                                               \
                     return expr;                                                       \
                 },                                                                     \
-                is_method(m_base))
+                name(op), is_method(m_base))
 
         #define PYBIND11_ENUM_OP_CONV(op, expr)                                        \
             m_base.attr(op) = cpp_function(                                            \
@@ -1468,11 +1501,19 @@ struct enum_base {
                     int_ a(a_), b(b_);                                                 \
                     return expr;                                                       \
                 },                                                                     \
-                is_method(m_base))
+                name(op), is_method(m_base))
+
+        #define PYBIND11_ENUM_OP_CONV_LHS(op, expr)                                    \
+            m_base.attr(op) = cpp_function(                                            \
+                [](object a_, object b) {                                              \
+                    int_ a(a_);                                                        \
+                    return expr;                                                       \
+                },                                                                     \
+                name(op), is_method(m_base))
 
         if (is_convertible) {
-            PYBIND11_ENUM_OP_CONV("__eq__", !b.is_none() &&  a.equal(b));
-            PYBIND11_ENUM_OP_CONV("__ne__",  b.is_none() || !a.equal(b));
+            PYBIND11_ENUM_OP_CONV_LHS("__eq__", !b.is_none() &&  a.equal(b));
+            PYBIND11_ENUM_OP_CONV_LHS("__ne__",  b.is_none() || !a.equal(b));
 
             if (is_arithmetic) {
                 PYBIND11_ENUM_OP_CONV("__lt__",   a <  b);
@@ -1485,6 +1526,8 @@ struct enum_base {
                 PYBIND11_ENUM_OP_CONV("__ror__",  a |  b);
                 PYBIND11_ENUM_OP_CONV("__xor__",  a ^  b);
                 PYBIND11_ENUM_OP_CONV("__rxor__", a ^  b);
+                m_base.attr("__invert__") = cpp_function(
+                    [](object arg) { return ~(int_(arg)); }, name("__invert__"), is_method(m_base));
             }
         } else {
             PYBIND11_ENUM_OP_STRICT("__eq__",  int_(a).equal(int_(b)), return false);
@@ -1500,14 +1543,15 @@ struct enum_base {
             }
         }
 
+        #undef PYBIND11_ENUM_OP_CONV_LHS
         #undef PYBIND11_ENUM_OP_CONV
         #undef PYBIND11_ENUM_OP_STRICT
 
-        object getstate = cpp_function(
-            [](object arg) { return int_(arg); }, is_method(m_base));
+        m_base.attr("__getstate__") = cpp_function(
+            [](object arg) { return int_(arg); }, name("__getstate__"), is_method(m_base));
 
-        m_base.attr("__getstate__") = getstate;
-        m_base.attr("__hash__") = getstate;
+        m_base.attr("__hash__") = cpp_function(
+            [](object arg) { return int_(arg); }, name("__hash__"), is_method(m_base));
     }
 
     PYBIND11_NOINLINE void value(char const* name_, object value, const char *doc = nullptr) {
@@ -1532,7 +1576,7 @@ struct enum_base {
     handle m_parent;
 };
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 /// Binds C++ enumerations and enumeration classes to Python
 template <typename Type> class enum_ : public class_<Type> {
@@ -1556,10 +1600,16 @@ public:
         #if PY_MAJOR_VERSION < 3
             def("__long__", [](Type value) { return (Scalar) value; });
         #endif
-        cpp_function setstate(
-            [](Type &value, Scalar arg) { value = static_cast<Type>(arg); },
-            is_method(*this));
-        attr("__setstate__") = setstate;
+        #if PY_MAJOR_VERSION > 3 || (PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION >= 8)
+            def("__index__", [](Type value) { return (Scalar) value; });
+        #endif
+
+        attr("__setstate__") = cpp_function(
+            [](detail::value_and_holder &v_h, Scalar arg) {
+                detail::initimpl::setstate<Base>(v_h, static_cast<Type>(arg),
+                        Py_TYPE(v_h.inst) != v_h.type->type); },
+            detail::is_new_style_constructor(),
+            pybind11::name("__setstate__"), is_method(*this));
     }
 
     /// Export enumeration entries into the parent scope
@@ -1578,7 +1628,7 @@ private:
     detail::enum_base m_base;
 };
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 
 inline void keep_alive_impl(handle nurse, handle patient) {
@@ -1648,7 +1698,7 @@ struct iterator_state {
     bool first_or_done;
 };
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 /// Makes a python iterator from a first and past-the-end C++ InputIterator.
 template <return_value_policy Policy = return_value_policy::reference_internal,
@@ -1781,13 +1831,13 @@ public:
     }
 };
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 // Returns a reference to a function-local static exception object used in the simple
 // register_exception approach below.  (It would be simpler to have the static local variable
 // directly in register_exception, but that makes clang <3.5 segfault - issue #1349).
 template <typename CppException>
 exception<CppException> &get_exception_object() { static exception<CppException> ex; return ex; }
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 /**
  * Registers a Python exception in `m` of the given `name` and installs an exception translator to
@@ -1813,7 +1863,7 @@ exception<CppException> &register_exception(handle scope,
     return ex;
 }
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 PYBIND11_NOINLINE inline void print(tuple args, dict kwargs) {
     auto strings = tuple(args.size());
     for (size_t i = 0; i < args.size(); ++i) {
@@ -1844,7 +1894,7 @@ PYBIND11_NOINLINE inline void print(tuple args, dict kwargs) {
     if (kwargs.contains("flush") && kwargs["flush"].cast<bool>())
         file.attr("flush")();
 }
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 template <return_value_policy policy = return_value_policy::automatic_reference, typename... Args>
 void print(Args &&...args) {
@@ -1999,8 +2049,8 @@ class gil_scoped_release { };
 
 error_already_set::~error_already_set() {
     if (m_type) {
-        error_scope scope;
         gil_scoped_acquire gil;
+        error_scope scope;
         m_type.release().dec_ref();
         m_value.release().dec_ref();
         m_trace.release().dec_ref();
@@ -2102,8 +2152,8 @@ template <class T> function get_overload(const T *this_ptr, const char *name) {
         PYBIND11_OVERLOAD_NAME(
             std::string, // Return type (ret_type)
             Animal,      // Parent class (cname)
-            toString,    // Name of function in C++ (name)
-            "__str__",   // Name of method in Python (fn)
+            "__str__",   // Name of method in Python (name)
+            toString,    // Name of function in C++ (fn)
         );
       }
 \endrst */
@@ -2153,7 +2203,7 @@ template <class T> function get_overload(const T *this_ptr, const char *name) {
 #define PYBIND11_OVERLOAD_PURE(ret_type, cname, fn, ...) \
     PYBIND11_OVERLOAD_PURE_NAME(PYBIND11_TYPE(ret_type), PYBIND11_TYPE(cname), #fn, fn, __VA_ARGS__)
 
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
 
 #if defined(_MSC_VER) && !defined(__INTEL_COMPILER)
 #  pragma warning(pop)
diff --git a/wrap/python/pybind11/include/pybind11/pytypes.h b/wrap/pybind11/include/pybind11/pytypes.h
similarity index 89%
rename from wrap/python/pybind11/include/pybind11/pytypes.h
rename to wrap/pybind11/include/pybind11/pytypes.h
index 2d573dfad..5152f6804 100644
--- a/wrap/python/pybind11/include/pybind11/pytypes.h
+++ b/wrap/pybind11/include/pybind11/pytypes.h
@@ -14,14 +14,14 @@
 #include <utility>
 #include <type_traits>
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
 
 /* A few forward declarations */
 class handle; class object;
 class str; class iterator;
 struct arg; struct arg_v;
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 class args_proxy;
 inline bool isinstance_generic(handle obj, const std::type_info &tp);
 
@@ -158,7 +158,7 @@ private:
     bool rich_compare(object_api const &other, int value) const;
 };
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 /** \rst
     Holds a reference to a Python object (no reference counting)
@@ -311,9 +311,9 @@ template <typename T> T reinterpret_borrow(handle h) { return {h, object::borrow
 \endrst */
 template <typename T> T reinterpret_steal(handle h) { return {h, object::stolen_t{}}; }
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 inline std::string error_string();
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 /// Fetch and hold an error which was already set in Python.  An instance of this is typically
 /// thrown to propagate python-side errors back through C++ which can either be caught manually or
@@ -446,7 +446,7 @@ inline ssize_t hash(handle obj) {
 
 /// @} python_builtins
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 inline handle get_function(handle value) {
     if (value) {
 #if PY_MAJOR_VERSION >= 3
@@ -520,7 +520,7 @@ private:
     mutable object cache;
 };
 
-NAMESPACE_BEGIN(accessor_policies)
+PYBIND11_NAMESPACE_BEGIN(accessor_policies)
 struct obj_attr {
     using key_type = object;
     static object get(handle obj, handle key) { return getattr(obj, key); }
@@ -597,7 +597,7 @@ struct tuple_item {
         }
     }
 };
-NAMESPACE_END(accessor_policies)
+PYBIND11_NAMESPACE_END(accessor_policies)
 
 /// STL iterator template used for tuple, list, sequence and dict
 template <typename Policy>
@@ -638,7 +638,7 @@ public:
     friend bool operator<=(const It &a, const It &b) { return !(a > b); }
 };
 
-NAMESPACE_BEGIN(iterator_policies)
+PYBIND11_NAMESPACE_BEGIN(iterator_policies)
 /// Quick proxy class needed to implement ``operator->`` for iterators which can't return pointers
 template <typename T>
 struct arrow_proxy {
@@ -711,7 +711,7 @@ private:
     PyObject *key = nullptr, *value = nullptr;
     ssize_t pos = -1;
 };
-NAMESPACE_END(iterator_policies)
+PYBIND11_NAMESPACE_END(iterator_policies)
 
 #if !defined(PYPY_VERSION)
 using tuple_iterator = generic_iterator<iterator_policies::sequence_fast_readonly>;
@@ -770,7 +770,7 @@ class simple_collector;
 template <return_value_policy policy = return_value_policy::automatic_reference>
 class unpacking_collector;
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 // TODO: After the deprecated constructors are removed, this macro can be simplified by
 //       inheriting ctors: `using Parent::Parent`. It's not an option right now because
@@ -933,8 +933,8 @@ private:
     /// Return string representation -- always returns a new reference, even if already a str
     static PyObject *raw_str(PyObject *op) {
         PyObject *str_value = PyObject_Str(op);
-#if PY_MAJOR_VERSION < 3
         if (!str_value) throw error_already_set();
+#if PY_MAJOR_VERSION < 3
         PyObject *unicode = PyUnicode_FromEncodedObject(str_value, "utf-8", nullptr);
         Py_XDECREF(str_value); str_value = unicode;
 #endif
@@ -980,6 +980,9 @@ public:
         return std::string(buffer, (size_t) length);
     }
 };
+// Note: breathe >= 4.17.0 will fail to build docs if the below two constructors
+// are included in the doxygen group; close here and reopen after as a workaround
+/// @} pytypes
 
 inline bytes::bytes(const pybind11::str &s) {
     object temp = s;
@@ -1009,6 +1012,8 @@ inline str::str(const bytes& b) {
     m_ptr = obj.release().ptr();
 }
 
+/// \addtogroup pytypes
+/// @{
 class none : public object {
 public:
     PYBIND11_OBJECT(none, object, detail::PyNone_Check)
@@ -1040,7 +1045,7 @@ private:
     }
 };
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 // Converts a value to the given unsigned type.  If an error occurs, you get back (Unsigned) -1;
 // otherwise you get back the unsigned long or unsigned long long value cast to (Unsigned).
 // (The distinction is critically important when casting a returned -1 error value to some other
@@ -1060,7 +1065,7 @@ Unsigned as_unsigned(PyObject *o) {
         return v == (unsigned long long) -1 && PyErr_Occurred() ? (Unsigned) -1 : (Unsigned) v;
     }
 }
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 class int_ : public object {
 public:
@@ -1202,6 +1207,7 @@ public:
         if (!m_ptr) pybind11_fail("Could not allocate tuple object!");
     }
     size_t size() const { return (size_t) PyTuple_Size(m_ptr); }
+    bool empty() const { return size() == 0; }
     detail::tuple_accessor operator[](size_t index) const { return {*this, index}; }
     detail::item_accessor operator[](handle h) const { return object::operator[](h); }
     detail::tuple_iterator begin() const { return {*this, 0}; }
@@ -1221,11 +1227,13 @@ public:
     explicit dict(Args &&...args) : dict(collector(std::forward<Args>(args)...).kwargs()) { }
 
     size_t size() const { return (size_t) PyDict_Size(m_ptr); }
+    bool empty() const { return size() == 0; }
     detail::dict_iterator begin() const { return {*this, 0}; }
     detail::dict_iterator end() const { return {}; }
     void clear() const { PyDict_Clear(ptr()); }
-    bool contains(handle key) const { return PyDict_Contains(ptr(), key.ptr()) == 1; }
-    bool contains(const char *key) const { return PyDict_Contains(ptr(), pybind11::str(key).ptr()) == 1; }
+    template <typename T> bool contains(T &&key) const {
+        return PyDict_Contains(m_ptr, detail::object_or_cast(std::forward<T>(key)).ptr()) == 1;
+    }
 
 private:
     /// Call the `dict` Python type -- always returns a new reference
@@ -1239,7 +1247,13 @@ private:
 class sequence : public object {
 public:
     PYBIND11_OBJECT_DEFAULT(sequence, object, PySequence_Check)
-    size_t size() const { return (size_t) PySequence_Size(m_ptr); }
+    size_t size() const {
+        ssize_t result = PySequence_Size(m_ptr);
+        if (result == -1)
+            throw error_already_set();
+        return (size_t) result;
+    }
+    bool empty() const { return size() == 0; }
     detail::sequence_accessor operator[](size_t index) const { return {*this, index}; }
     detail::item_accessor operator[](handle h) const { return object::operator[](h); }
     detail::sequence_iterator begin() const { return {*this, 0}; }
@@ -1253,6 +1267,7 @@ public:
         if (!m_ptr) pybind11_fail("Could not allocate list object!");
     }
     size_t size() const { return (size_t) PyList_Size(m_ptr); }
+    bool empty() const { return size() == 0; }
     detail::list_accessor operator[](size_t index) const { return {*this, index}; }
     detail::item_accessor operator[](handle h) const { return object::operator[](h); }
     detail::list_iterator begin() const { return {*this, 0}; }
@@ -1260,6 +1275,10 @@ public:
     template <typename T> void append(T &&val) const {
         PyList_Append(m_ptr, detail::object_or_cast(std::forward<T>(val)).ptr());
     }
+    template <typename T> void insert(size_t index, T &&val) const {
+        PyList_Insert(m_ptr, static_cast<ssize_t>(index),
+            detail::object_or_cast(std::forward<T>(val)).ptr());
+    }
 };
 
 class args : public tuple { PYBIND11_OBJECT_DEFAULT(args, tuple, PyTuple_Check) };
@@ -1272,10 +1291,14 @@ public:
         if (!m_ptr) pybind11_fail("Could not allocate set object!");
     }
     size_t size() const { return (size_t) PySet_Size(m_ptr); }
+    bool empty() const { return size() == 0; }
     template <typename T> bool add(T &&val) const {
         return PySet_Add(m_ptr, detail::object_or_cast(std::forward<T>(val)).ptr()) == 0;
     }
     void clear() const { PySet_Clear(m_ptr); }
+    template <typename T> bool contains(T &&val) const {
+        return PySet_Contains(m_ptr, detail::object_or_cast(std::forward<T>(val)).ptr()) == 1;
+    }
 };
 
 class function : public object {
@@ -1299,7 +1322,7 @@ class buffer : public object {
 public:
     PYBIND11_OBJECT_DEFAULT(buffer, object, PyObject_CheckBuffer)
 
-    buffer_info request(bool writable = false) {
+    buffer_info request(bool writable = false) const {
         int flags = PyBUF_STRIDES | PyBUF_FORMAT;
         if (writable) flags |= PyBUF_WRITABLE;
         Py_buffer *view = new Py_buffer();
@@ -1313,35 +1336,139 @@ public:
 
 class memoryview : public object {
 public:
-    explicit memoryview(const buffer_info& info) {
-        static Py_buffer buf { };
-        // Py_buffer uses signed sizes, strides and shape!..
-        static std::vector<Py_ssize_t> py_strides { };
-        static std::vector<Py_ssize_t> py_shape { };
-        buf.buf = info.ptr;
-        buf.itemsize = info.itemsize;
-        buf.format = const_cast<char *>(info.format.c_str());
-        buf.ndim = (int) info.ndim;
-        buf.len = info.size;
-        py_strides.clear();
-        py_shape.clear();
-        for (size_t i = 0; i < (size_t) info.ndim; ++i) {
-            py_strides.push_back(info.strides[i]);
-            py_shape.push_back(info.shape[i]);
-        }
-        buf.strides = py_strides.data();
-        buf.shape = py_shape.data();
-        buf.suboffsets = nullptr;
-        buf.readonly = false;
-        buf.internal = nullptr;
+    PYBIND11_OBJECT_CVT(memoryview, object, PyMemoryView_Check, PyMemoryView_FromObject)
 
-        m_ptr = PyMemoryView_FromBuffer(&buf);
+    /** \rst
+        Creates ``memoryview`` from ``buffer_info``.
+
+        ``buffer_info`` must be created from ``buffer::request()``. Otherwise
+        throws an exception.
+
+        For creating a ``memoryview`` from objects that support buffer protocol,
+        use ``memoryview(const object& obj)`` instead of this constructor.
+     \endrst */
+    explicit memoryview(const buffer_info& info) {
+        if (!info.view())
+            pybind11_fail("Prohibited to create memoryview without Py_buffer");
+        // Note: PyMemoryView_FromBuffer never increments obj reference.
+        m_ptr = (info.view()->obj) ?
+            PyMemoryView_FromObject(info.view()->obj) :
+            PyMemoryView_FromBuffer(info.view());
         if (!m_ptr)
             pybind11_fail("Unable to create memoryview from buffer descriptor");
     }
 
-    PYBIND11_OBJECT_CVT(memoryview, object, PyMemoryView_Check, PyMemoryView_FromObject)
+    /** \rst
+        Creates ``memoryview`` from static buffer.
+
+        This method is meant for providing a ``memoryview`` for C/C++ buffer not
+        managed by Python. The caller is responsible for managing the lifetime
+        of ``ptr`` and ``format``, which MUST outlive the memoryview constructed
+        here.
+
+        See also: Python C API documentation for `PyMemoryView_FromBuffer`_.
+
+        .. _PyMemoryView_FromBuffer: https://docs.python.org/c-api/memoryview.html#c.PyMemoryView_FromBuffer
+
+        :param ptr: Pointer to the buffer.
+        :param itemsize: Byte size of an element.
+        :param format: Pointer to the null-terminated format string. For
+            homogeneous Buffers, this should be set to
+            ``format_descriptor<T>::value``.
+        :param shape: Shape of the tensor (1 entry per dimension).
+        :param strides: Number of bytes between adjacent entries (for each
+            per dimension).
+        :param readonly: Flag to indicate if the underlying storage may be
+            written to.
+     \endrst */
+    static memoryview from_buffer(
+        void *ptr, ssize_t itemsize, const char *format,
+        detail::any_container<ssize_t> shape,
+        detail::any_container<ssize_t> strides, bool readonly = false);
+
+    static memoryview from_buffer(
+        const void *ptr, ssize_t itemsize, const char *format,
+        detail::any_container<ssize_t> shape,
+        detail::any_container<ssize_t> strides) {
+        return memoryview::from_buffer(
+            const_cast<void*>(ptr), itemsize, format, shape, strides, true);
+    }
+
+    template<typename T>
+    static memoryview from_buffer(
+        T *ptr, detail::any_container<ssize_t> shape,
+        detail::any_container<ssize_t> strides, bool readonly = false) {
+        return memoryview::from_buffer(
+            reinterpret_cast<void*>(ptr), sizeof(T),
+            format_descriptor<T>::value, shape, strides, readonly);
+    }
+
+    template<typename T>
+    static memoryview from_buffer(
+        const T *ptr, detail::any_container<ssize_t> shape,
+        detail::any_container<ssize_t> strides) {
+        return memoryview::from_buffer(
+            const_cast<T*>(ptr), shape, strides, true);
+    }
+
+#if PY_MAJOR_VERSION >= 3
+    /** \rst
+        Creates ``memoryview`` from static memory.
+
+        This method is meant for providing a ``memoryview`` for C/C++ buffer not
+        managed by Python. The caller is responsible for managing the lifetime
+        of ``mem``, which MUST outlive the memoryview constructed here.
+
+        This method is not available in Python 2.
+
+        See also: Python C API documentation for `PyMemoryView_FromBuffer`_.
+
+        .. _PyMemoryView_FromMemory: https://docs.python.org/c-api/memoryview.html#c.PyMemoryView_FromMemory
+     \endrst */
+    static memoryview from_memory(void *mem, ssize_t size, bool readonly = false) {
+        PyObject* ptr = PyMemoryView_FromMemory(
+            reinterpret_cast<char*>(mem), size,
+            (readonly) ? PyBUF_READ : PyBUF_WRITE);
+        if (!ptr)
+            pybind11_fail("Could not allocate memoryview object!");
+        return memoryview(object(ptr, stolen_t{}));
+    }
+
+    static memoryview from_memory(const void *mem, ssize_t size) {
+        return memoryview::from_memory(const_cast<void*>(mem), size, true);
+    }
+#endif
 };
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+inline memoryview memoryview::from_buffer(
+    void *ptr, ssize_t itemsize, const char* format,
+    detail::any_container<ssize_t> shape,
+    detail::any_container<ssize_t> strides, bool readonly) {
+    size_t ndim = shape->size();
+    if (ndim != strides->size())
+        pybind11_fail("memoryview: shape length doesn't match strides length");
+    ssize_t size = ndim ? 1 : 0;
+    for (size_t i = 0; i < ndim; ++i)
+        size *= (*shape)[i];
+    Py_buffer view;
+    view.buf = ptr;
+    view.obj = nullptr;
+    view.len = size * itemsize;
+    view.readonly = static_cast<int>(readonly);
+    view.itemsize = itemsize;
+    view.format = const_cast<char*>(format);
+    view.ndim = static_cast<int>(ndim);
+    view.shape = shape->data();
+    view.strides = strides->data();
+    view.suboffsets = nullptr;
+    view.internal = nullptr;
+    PyObject* obj = PyMemoryView_FromBuffer(&view);
+    if (!obj)
+        throw error_already_set();
+    return memoryview(object(obj, stolen_t{}));
+}
+#endif  // DOXYGEN_SHOULD_SKIP_THIS
 /// @} pytypes
 
 /// \addtogroup python_builtins
@@ -1386,7 +1513,7 @@ inline iterator iter(handle obj) {
 }
 /// @} python_builtins
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 template <typename D> iterator object_api<D>::begin() const { return iter(derived()); }
 template <typename D> iterator object_api<D>::end() const { return iterator::sentinel(); }
 template <typename D> item_accessor object_api<D>::operator[](handle key) const {
@@ -1467,5 +1594,5 @@ PYBIND11_MATH_OPERATOR_BINARY(operator>>=, PyNumber_InPlaceRshift)
 #undef PYBIND11_MATH_OPERATOR_UNARY
 #undef PYBIND11_MATH_OPERATOR_BINARY
 
-NAMESPACE_END(detail)
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/python/pybind11/include/pybind11/stl.h b/wrap/pybind11/include/pybind11/stl.h
similarity index 97%
rename from wrap/python/pybind11/include/pybind11/stl.h
rename to wrap/pybind11/include/pybind11/stl.h
index 32f8d294a..6c2bebda8 100644
--- a/wrap/python/pybind11/include/pybind11/stl.h
+++ b/wrap/pybind11/include/pybind11/stl.h
@@ -48,8 +48,8 @@
 #  define PYBIND11_HAS_VARIANT 1
 #endif
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 /// Extracts an const lvalue reference or rvalue reference for U based on the type of T (e.g. for
 /// forwarding a container element).  Typically used indirect via forwarded_type(), below.
@@ -266,7 +266,9 @@ template<typename T> struct optional_caster {
     static handle cast(T_ &&src, return_value_policy policy, handle parent) {
         if (!src)
             return none().inc_ref();
-        policy = return_value_policy_override<typename T::value_type>::policy(policy);
+        if (!std::is_lvalue_reference<T>::value) {
+            policy = return_value_policy_override<T>::policy(policy);
+        }
         return value_conv::cast(*std::forward<T_>(src), policy, parent);
     }
 
@@ -372,14 +374,14 @@ template <typename... Ts>
 struct type_caster<std::variant<Ts...>> : variant_caster<std::variant<Ts...>> { };
 #endif
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 inline std::ostream &operator<<(std::ostream &os, const handle &obj) {
     os << (std::string) str(obj);
     return os;
 }
 
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
 
 #if defined(_MSC_VER)
 #pragma warning(pop)
diff --git a/wrap/python/pybind11/include/pybind11/stl_bind.h b/wrap/pybind11/include/pybind11/stl_bind.h
similarity index 91%
rename from wrap/python/pybind11/include/pybind11/stl_bind.h
rename to wrap/pybind11/include/pybind11/stl_bind.h
index 1f8725260..61b94b622 100644
--- a/wrap/python/pybind11/include/pybind11/stl_bind.h
+++ b/wrap/pybind11/include/pybind11/stl_bind.h
@@ -15,8 +15,8 @@
 #include <algorithm>
 #include <sstream>
 
-NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 /* SFINAE helper class used by 'is_comparable */
 template <typename T>  struct container_traits {
@@ -115,6 +115,14 @@ void vector_modifiers(enable_if_t<is_copy_constructible<typename Vector::value_t
     using SizeType = typename Vector::size_type;
     using DiffType = typename Vector::difference_type;
 
+    auto wrap_i = [](DiffType i, SizeType n) {
+        if (i < 0)
+            i += n;
+        if (i < 0 || (SizeType)i >= n)
+            throw index_error();
+        return i;
+    };
+
     cl.def("append",
            [](Vector &v, const T &value) { v.push_back(value); },
            arg("x"),
@@ -128,6 +136,13 @@ void vector_modifiers(enable_if_t<is_copy_constructible<typename Vector::value_t
         return v.release();
     }));
 
+    cl.def("clear",
+        [](Vector &v) {
+            v.clear();
+        },
+        "Clear the contents"
+    );
+
     cl.def("extend",
        [](Vector &v, const Vector &src) {
            v.insert(v.end(), src.begin(), src.end());
@@ -159,10 +174,13 @@ void vector_modifiers(enable_if_t<is_copy_constructible<typename Vector::value_t
     );
 
     cl.def("insert",
-        [](Vector &v, SizeType i, const T &x) {
-            if (i > v.size())
+        [](Vector &v, DiffType i, const T &x) {
+            // Can't use wrap_i; i == v.size() is OK
+            if (i < 0)
+                i += v.size();
+            if (i < 0 || (SizeType)i > v.size())
                 throw index_error();
-            v.insert(v.begin() + (DiffType) i, x);
+            v.insert(v.begin() + i, x);
         },
         arg("i") , arg("x"),
         "Insert an item at a given position."
@@ -180,11 +198,10 @@ void vector_modifiers(enable_if_t<is_copy_constructible<typename Vector::value_t
     );
 
     cl.def("pop",
-        [](Vector &v, SizeType i) {
-            if (i >= v.size())
-                throw index_error();
-            T t = v[i];
-            v.erase(v.begin() + (DiffType) i);
+        [wrap_i](Vector &v, DiffType i) {
+            i = wrap_i(i, v.size());
+            T t = v[(SizeType) i];
+            v.erase(v.begin() + i);
             return t;
         },
         arg("i"),
@@ -192,10 +209,9 @@ void vector_modifiers(enable_if_t<is_copy_constructible<typename Vector::value_t
     );
 
     cl.def("__setitem__",
-        [](Vector &v, SizeType i, const T &t) {
-            if (i >= v.size())
-                throw index_error();
-            v[i] = t;
+        [wrap_i](Vector &v, DiffType i, const T &t) {
+            i = wrap_i(i, v.size());
+            v[(SizeType)i] = t;
         }
     );
 
@@ -238,10 +254,9 @@ void vector_modifiers(enable_if_t<is_copy_constructible<typename Vector::value_t
     );
 
     cl.def("__delitem__",
-        [](Vector &v, SizeType i) {
-            if (i >= v.size())
-                throw index_error();
-            v.erase(v.begin() + DiffType(i));
+        [wrap_i](Vector &v, DiffType i) {
+            i = wrap_i(i, v.size());
+            v.erase(v.begin() + i);
         },
         "Delete the list elements at index ``i``"
     );
@@ -277,13 +292,21 @@ template <typename Vector, typename Class_>
 void vector_accessor(enable_if_t<!vector_needs_copy<Vector>::value, Class_> &cl) {
     using T = typename Vector::value_type;
     using SizeType = typename Vector::size_type;
+    using DiffType = typename Vector::difference_type;
     using ItType   = typename Vector::iterator;
 
+    auto wrap_i = [](DiffType i, SizeType n) {
+        if (i < 0)
+            i += n;
+        if (i < 0 || (SizeType)i >= n)
+            throw index_error();
+        return i;
+    };
+
     cl.def("__getitem__",
-        [](Vector &v, SizeType i) -> T & {
-            if (i >= v.size())
-                throw index_error();
-            return v[i];
+        [wrap_i](Vector &v, DiffType i) -> T & {
+            i = wrap_i(i, v.size());
+            return v[(SizeType)i];
         },
         return_value_policy::reference_internal // ref + keepalive
     );
@@ -303,12 +326,15 @@ template <typename Vector, typename Class_>
 void vector_accessor(enable_if_t<vector_needs_copy<Vector>::value, Class_> &cl) {
     using T = typename Vector::value_type;
     using SizeType = typename Vector::size_type;
+    using DiffType = typename Vector::difference_type;
     using ItType   = typename Vector::iterator;
     cl.def("__getitem__",
-        [](const Vector &v, SizeType i) -> T {
-            if (i >= v.size())
+        [](const Vector &v, DiffType i) -> T {
+            if (i < 0 && (i += v.size()) < 0)
                 throw index_error();
-            return v[i];
+            if ((SizeType)i >= v.size())
+                throw index_error();
+            return v[(SizeType)i];
         }
     );
 
@@ -387,7 +413,7 @@ vector_buffer(Class_& cl) {
 template <typename Vector, typename Class_, typename... Args>
 enable_if_t<!detail::any_of<std::is_same<Args, buffer_protocol>...>::value> vector_buffer(Class_&) {}
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 //
 // std::vector
@@ -485,7 +511,7 @@ class_<Vector, holder_type> bind_vector(handle scope, std::string const &name, A
 // std::map, std::unordered_map
 //
 
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(detail)
 
 /* Fallback functions */
 template <typename, typename, typename... Args> void map_if_insertion_operator(const Args &...) { }
@@ -493,7 +519,7 @@ template <typename, typename, typename... Args> void map_assignment(const Args &
 
 // Map assignment when copy-assignable: just copy the value
 template <typename Map, typename Class_>
-void map_assignment(enable_if_t<std::is_copy_assignable<typename Map::mapped_type>::value, Class_> &cl) {
+void map_assignment(enable_if_t<is_copy_assignable<typename Map::mapped_type>::value, Class_> &cl) {
     using KeyType = typename Map::key_type;
     using MappedType = typename Map::mapped_type;
 
@@ -509,7 +535,7 @@ void map_assignment(enable_if_t<std::is_copy_assignable<typename Map::mapped_typ
 // Not copy-assignable, but still copy-constructible: we can update the value by erasing and reinserting
 template<typename Map, typename Class_>
 void map_assignment(enable_if_t<
-        !std::is_copy_assignable<typename Map::mapped_type>::value &&
+        !is_copy_assignable<typename Map::mapped_type>::value &&
         is_copy_constructible<typename Map::mapped_type>::value,
         Class_> &cl) {
     using KeyType = typename Map::key_type;
@@ -551,7 +577,7 @@ template <typename Map, typename Class_> auto map_if_insertion_operator(Class_ &
 }
 
 
-NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(detail)
 
 template <typename Map, typename holder_type = std::unique_ptr<Map>, typename... Args>
 class_<Map, holder_type> bind_map(handle scope, const std::string &name, Args&&... args) {
@@ -627,4 +653,4 @@ class_<Map, holder_type> bind_map(handle scope, const std::string &name, Args&&.
     return cl;
 }
 
-NAMESPACE_END(PYBIND11_NAMESPACE)
+PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE)
diff --git a/wrap/pybind11/pybind11/__init__.py b/wrap/pybind11/pybind11/__init__.py
new file mode 100644
index 000000000..5b2f83d5c
--- /dev/null
+++ b/wrap/pybind11/pybind11/__init__.py
@@ -0,0 +1,13 @@
+# -*- coding: utf-8 -*-
+from ._version import version_info, __version__  # noqa: F401 imported but unused
+
+
+def get_include(user=False):
+    import os
+    d = os.path.dirname(__file__)
+    if os.path.exists(os.path.join(d, "include")):
+        # Package is installed
+        return os.path.join(d, "include")
+    else:
+        # Package is from a source directory
+        return os.path.join(os.path.dirname(d), "include")
diff --git a/wrap/python/pybind11/pybind11/__main__.py b/wrap/pybind11/pybind11/__main__.py
similarity index 93%
rename from wrap/python/pybind11/pybind11/__main__.py
rename to wrap/pybind11/pybind11/__main__.py
index 9ef837802..5e393cc8f 100644
--- a/wrap/python/pybind11/pybind11/__main__.py
+++ b/wrap/pybind11/pybind11/__main__.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 from __future__ import print_function
 
 import argparse
@@ -10,8 +11,7 @@ from . import get_include
 def print_includes():
     dirs = [sysconfig.get_path('include'),
             sysconfig.get_path('platinclude'),
-            get_include(),
-            get_include(True)]
+            get_include()]
 
     # Make unique but preserve order
     unique_dirs = []
diff --git a/wrap/pybind11/pybind11/_version.py b/wrap/pybind11/pybind11/_version.py
new file mode 100644
index 000000000..1f2f254ce
--- /dev/null
+++ b/wrap/pybind11/pybind11/_version.py
@@ -0,0 +1,3 @@
+# -*- coding: utf-8 -*-
+version_info = (2, 5, 'dev1')
+__version__ = '.'.join(map(str, version_info))
diff --git a/wrap/python/pybind11/setup.cfg b/wrap/pybind11/setup.cfg
similarity index 100%
rename from wrap/python/pybind11/setup.cfg
rename to wrap/pybind11/setup.cfg
diff --git a/wrap/python/pybind11/setup.py b/wrap/pybind11/setup.py
similarity index 66%
rename from wrap/python/pybind11/setup.py
rename to wrap/pybind11/setup.py
index f677f2af4..577a6b6c3 100644
--- a/wrap/python/pybind11/setup.py
+++ b/wrap/pybind11/setup.py
@@ -1,43 +1,47 @@
 #!/usr/bin/env python
+# -*- coding: utf-8 -*-
 
 # Setup script for PyPI; use CMakeFile.txt to build extension modules
 
 from setuptools import setup
 from distutils.command.install_headers import install_headers
+from distutils.command.build_py import build_py
 from pybind11 import __version__
 import os
 
+package_data = [
+    'include/pybind11/detail/class.h',
+    'include/pybind11/detail/common.h',
+    'include/pybind11/detail/descr.h',
+    'include/pybind11/detail/init.h',
+    'include/pybind11/detail/internals.h',
+    'include/pybind11/detail/typeid.h',
+    'include/pybind11/attr.h',
+    'include/pybind11/buffer_info.h',
+    'include/pybind11/cast.h',
+    'include/pybind11/chrono.h',
+    'include/pybind11/common.h',
+    'include/pybind11/complex.h',
+    'include/pybind11/eigen.h',
+    'include/pybind11/embed.h',
+    'include/pybind11/eval.h',
+    'include/pybind11/functional.h',
+    'include/pybind11/iostream.h',
+    'include/pybind11/numpy.h',
+    'include/pybind11/operators.h',
+    'include/pybind11/options.h',
+    'include/pybind11/pybind11.h',
+    'include/pybind11/pytypes.h',
+    'include/pybind11/stl.h',
+    'include/pybind11/stl_bind.h',
+]
+
 # Prevent installation of pybind11 headers by setting
 # PYBIND11_USE_CMAKE.
 if os.environ.get('PYBIND11_USE_CMAKE'):
     headers = []
 else:
-    headers = [
-        'include/pybind11/detail/class.h',
-        'include/pybind11/detail/common.h',
-        'include/pybind11/detail/descr.h',
-        'include/pybind11/detail/init.h',
-        'include/pybind11/detail/internals.h',
-        'include/pybind11/detail/typeid.h',
-        'include/pybind11/attr.h',
-        'include/pybind11/buffer_info.h',
-        'include/pybind11/cast.h',
-        'include/pybind11/chrono.h',
-        'include/pybind11/common.h',
-        'include/pybind11/complex.h',
-        'include/pybind11/eigen.h',
-        'include/pybind11/embed.h',
-        'include/pybind11/eval.h',
-        'include/pybind11/functional.h',
-        'include/pybind11/iostream.h',
-        'include/pybind11/numpy.h',
-        'include/pybind11/operators.h',
-        'include/pybind11/options.h',
-        'include/pybind11/pybind11.h',
-        'include/pybind11/pytypes.h',
-        'include/pybind11/stl.h',
-        'include/pybind11/stl_bind.h',
-    ]
+    headers = package_data
 
 
 class InstallHeaders(install_headers):
@@ -55,6 +59,23 @@ class InstallHeaders(install_headers):
             self.outfiles.append(out)
 
 
+# Install the headers inside the package as well
+class BuildPy(build_py):
+    def build_package_data(self):
+        build_py.build_package_data(self)
+        for header in package_data:
+            target = os.path.join(self.build_lib, 'pybind11', header)
+            self.mkpath(os.path.dirname(target))
+            self.copy_file(header, target, preserve_mode=False)
+
+    def get_outputs(self, include_bytecode=1):
+        outputs = build_py.get_outputs(self, include_bytecode=include_bytecode)
+        for header in package_data:
+            target = os.path.join(self.build_lib, 'pybind11', header)
+            outputs.append(target)
+        return outputs
+
+
 setup(
     name='pybind11',
     version=__version__,
@@ -66,7 +87,8 @@ setup(
     packages=['pybind11'],
     license='BSD',
     headers=headers,
-    cmdclass=dict(install_headers=InstallHeaders),
+    zip_safe=False,
+    cmdclass=dict(install_headers=InstallHeaders, build_py=BuildPy),
     classifiers=[
         'Development Status :: 5 - Production/Stable',
         'Intended Audience :: Developers',
diff --git a/wrap/python/pybind11/tests/CMakeLists.txt b/wrap/pybind11/tests/CMakeLists.txt
similarity index 91%
rename from wrap/python/pybind11/tests/CMakeLists.txt
rename to wrap/pybind11/tests/CMakeLists.txt
index 9a701108c..e4aba7567 100644
--- a/wrap/python/pybind11/tests/CMakeLists.txt
+++ b/wrap/pybind11/tests/CMakeLists.txt
@@ -8,6 +8,7 @@
 cmake_minimum_required(VERSION 2.8.12)
 
 option(PYBIND11_WERROR  "Report all warnings as errors"  OFF)
+set(PYBIND11_TEST_OVERRIDE "" CACHE STRING "Tests from ;-separated list of *.cpp files will be built instead of all tests")
 
 if (CMAKE_CURRENT_SOURCE_DIR STREQUAL CMAKE_SOURCE_DIR)
     # We're being loaded directly, i.e. not via add_subdirectory, so make this
@@ -26,6 +27,7 @@ endif()
 
 # Full set of test files (you can override these; see below)
 set(PYBIND11_TEST_FILES
+  test_async.cpp
   test_buffers.cpp
   test_builtin_casters.cpp
   test_call_policies.cpp
@@ -34,6 +36,7 @@ set(PYBIND11_TEST_FILES
   test_class.cpp
   test_constants_and_functions.cpp
   test_copy_move.cpp
+  test_custom_type_casters.cpp
   test_docstring_options.cpp
   test_eigen.cpp
   test_enum.cpp
@@ -71,6 +74,13 @@ if (PYBIND11_TEST_OVERRIDE)
   set(PYBIND11_TEST_FILES ${PYBIND11_TEST_OVERRIDE})
 endif()
 
+# Skip test_async for Python < 3.5
+list(FIND PYBIND11_TEST_FILES test_async.cpp PYBIND11_TEST_FILES_ASYNC_I)
+if((PYBIND11_TEST_FILES_ASYNC_I GREATER -1) AND ("${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}" VERSION_LESS 3.5))
+  message(STATUS "Skipping test_async because Python version ${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR} < 3.5")
+  list(REMOVE_AT PYBIND11_TEST_FILES ${PYBIND11_TEST_FILES_ASYNC_I})
+endif()
+
 string(REPLACE ".cpp" ".py" PYBIND11_PYTEST_FILES "${PYBIND11_TEST_FILES}")
 
 # Contains the set of test files that require pybind11_cross_module_tests to be
@@ -83,6 +93,10 @@ set(PYBIND11_CROSS_MODULE_TESTS
   test_stl_binders.py
 )
 
+set(PYBIND11_CROSS_MODULE_GIL_TESTS
+  test_gil_scoped.py
+)
+
 # Check if Eigen is available; if not, remove from PYBIND11_TEST_FILES (but
 # keep it in PYBIND11_PYTEST_FILES, so that we get the "eigen is not installed"
 # skip message).
@@ -150,6 +164,14 @@ foreach(t ${PYBIND11_CROSS_MODULE_TESTS})
   endif()
 endforeach()
 
+foreach(t ${PYBIND11_CROSS_MODULE_GIL_TESTS})
+  list(FIND PYBIND11_PYTEST_FILES ${t} i)
+  if (i GREATER -1)
+    list(APPEND test_targets cross_module_gil_utils)
+    break()
+  endif()
+endforeach()
+
 set(testdir ${CMAKE_CURRENT_SOURCE_DIR})
 foreach(target ${test_targets})
   set(test_files ${PYBIND11_TEST_FILES})
diff --git a/wrap/python/pybind11/tests/conftest.py b/wrap/pybind11/tests/conftest.py
similarity index 92%
rename from wrap/python/pybind11/tests/conftest.py
rename to wrap/pybind11/tests/conftest.py
index 55d9d0d53..d317c49db 100644
--- a/wrap/python/pybind11/tests/conftest.py
+++ b/wrap/pybind11/tests/conftest.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 """pytest configuration
 
 Extends output capture as needed by pybind11: ignore constructors, optional unordered lines.
@@ -17,6 +18,11 @@ _unicode_marker = re.compile(r'u(\'[^\']*\')')
 _long_marker = re.compile(r'([0-9])L')
 _hexadecimal = re.compile(r'0x[0-9a-fA-F]+')
 
+# test_async.py requires support for async and await
+collect_ignore = []
+if sys.version_info[:2] < (3, 5):
+    collect_ignore.append("test_async.py")
+
 
 def _strip_and_dedent(s):
     """For triple-quote strings"""
@@ -210,6 +216,11 @@ def pytest_configure():
     pytest.requires_eigen_and_scipy = skipif(
         not have_eigen or not scipy, reason="eigen and/or scipy are not installed")
     pytest.unsupported_on_pypy = skipif(pypy, reason="unsupported on PyPy")
+    pytest.bug_in_pypy = pytest.mark.xfail(pypy, reason="bug in PyPy")
+    pytest.unsupported_on_pypy3 = skipif(pypy and sys.version_info.major >= 3,
+                                         reason="unsupported on PyPy3")
+    pytest.unsupported_on_pypy_lt_6 = skipif(pypy and sys.pypy_version_info[0] < 6,
+                                             reason="unsupported on PyPy<6")
     pytest.unsupported_on_py2 = skipif(sys.version_info.major < 3,
                                        reason="unsupported on Python 2.x")
     pytest.gc_collect = gc_collect
diff --git a/wrap/python/pybind11/tests/constructor_stats.h b/wrap/pybind11/tests/constructor_stats.h
similarity index 99%
rename from wrap/python/pybind11/tests/constructor_stats.h
rename to wrap/pybind11/tests/constructor_stats.h
index f026e70f9..abfaf9161 100644
--- a/wrap/python/pybind11/tests/constructor_stats.h
+++ b/wrap/pybind11/tests/constructor_stats.h
@@ -180,7 +180,7 @@ public:
                 }
             }
         }
-        catch (const std::out_of_range &) {}
+        catch (const std::out_of_range&) {}
         if (!t1) throw std::runtime_error("Unknown class passed to ConstructorStats::get()");
         auto &cs1 = get(*t1);
         // If we have both a t1 and t2 match, one is probably the trampoline class; return whichever
@@ -273,4 +273,3 @@ template <class T, typename... Values> void print_values(T *inst, Values &&...va
     print_constr_details(inst, ":", values...);
     track_values(inst, values...);
 }
-
diff --git a/wrap/pybind11/tests/cross_module_gil_utils.cpp b/wrap/pybind11/tests/cross_module_gil_utils.cpp
new file mode 100644
index 000000000..07db9f6e4
--- /dev/null
+++ b/wrap/pybind11/tests/cross_module_gil_utils.cpp
@@ -0,0 +1,73 @@
+/*
+    tests/cross_module_gil_utils.cpp -- tools for acquiring GIL from a different module
+
+    Copyright (c) 2019 Google LLC
+
+    All rights reserved. Use of this source code is governed by a
+    BSD-style license that can be found in the LICENSE file.
+*/
+#include <pybind11/pybind11.h>
+#include <cstdint>
+
+// This file mimics a DSO that makes pybind11 calls but does not define a
+// PYBIND11_MODULE. The purpose is to test that such a DSO can create a
+// py::gil_scoped_acquire when the running thread is in a GIL-released state.
+//
+// Note that we define a Python module here for convenience, but in general
+// this need not be the case. The typical scenario would be a DSO that implements
+// shared logic used internally by multiple pybind11 modules.
+
+namespace {
+
+namespace py = pybind11;
+void gil_acquire() { py::gil_scoped_acquire gil; }
+
+constexpr char kModuleName[] = "cross_module_gil_utils";
+
+#if PY_MAJOR_VERSION >= 3
+struct PyModuleDef moduledef = {
+    PyModuleDef_HEAD_INIT,
+    kModuleName,
+    NULL,
+    0,
+    NULL,
+    NULL,
+    NULL,
+    NULL,
+    NULL
+};
+#else
+PyMethodDef module_methods[] = {
+    {NULL, NULL, 0, NULL}
+};
+#endif
+
+}  // namespace
+
+extern "C" PYBIND11_EXPORT
+#if PY_MAJOR_VERSION >= 3
+PyObject* PyInit_cross_module_gil_utils()
+#else
+void initcross_module_gil_utils()
+#endif
+{
+
+    PyObject* m =
+#if PY_MAJOR_VERSION >= 3
+        PyModule_Create(&moduledef);
+#else
+        Py_InitModule(kModuleName, module_methods);
+#endif
+
+    if (m != NULL) {
+        static_assert(
+            sizeof(&gil_acquire) == sizeof(void*),
+            "Function pointer must have the same size as void*");
+        PyModule_AddObject(m, "gil_acquire_funcaddr",
+                           PyLong_FromVoidPtr(reinterpret_cast<void*>(&gil_acquire)));
+    }
+
+#if PY_MAJOR_VERSION >= 3
+    return m;
+#endif
+}
diff --git a/wrap/python/pybind11/tests/local_bindings.h b/wrap/pybind11/tests/local_bindings.h
similarity index 100%
rename from wrap/python/pybind11/tests/local_bindings.h
rename to wrap/pybind11/tests/local_bindings.h
diff --git a/wrap/python/pybind11/tests/object.h b/wrap/pybind11/tests/object.h
similarity index 100%
rename from wrap/python/pybind11/tests/object.h
rename to wrap/pybind11/tests/object.h
diff --git a/wrap/python/pybind11/tests/pybind11_cross_module_tests.cpp b/wrap/pybind11/tests/pybind11_cross_module_tests.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/pybind11_cross_module_tests.cpp
rename to wrap/pybind11/tests/pybind11_cross_module_tests.cpp
diff --git a/wrap/python/pybind11/tests/pybind11_tests.cpp b/wrap/pybind11/tests/pybind11_tests.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/pybind11_tests.cpp
rename to wrap/pybind11/tests/pybind11_tests.cpp
diff --git a/wrap/python/pybind11/tests/pybind11_tests.h b/wrap/pybind11/tests/pybind11_tests.h
similarity index 93%
rename from wrap/python/pybind11/tests/pybind11_tests.h
rename to wrap/pybind11/tests/pybind11_tests.h
index 90963a5de..1e4741627 100644
--- a/wrap/python/pybind11/tests/pybind11_tests.h
+++ b/wrap/pybind11/tests/pybind11_tests.h
@@ -53,13 +53,13 @@ public:
 /// Custom cast-only type that casts to a string "rvalue" or "lvalue" depending on the cast context.
 /// Used to test recursive casters (e.g. std::tuple, stl containers).
 struct RValueCaster {};
-NAMESPACE_BEGIN(pybind11)
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(pybind11)
+PYBIND11_NAMESPACE_BEGIN(detail)
 template<> class type_caster<RValueCaster> {
 public:
     PYBIND11_TYPE_CASTER(RValueCaster, _("RValueCaster"));
     static handle cast(RValueCaster &&, return_value_policy, handle) { return py::str("rvalue").release(); }
     static handle cast(const RValueCaster &, return_value_policy, handle) { return py::str("lvalue").release(); }
 };
-NAMESPACE_END(detail)
-NAMESPACE_END(pybind11)
+PYBIND11_NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(pybind11)
diff --git a/wrap/python/pybind11/tests/pytest.ini b/wrap/pybind11/tests/pytest.ini
similarity index 100%
rename from wrap/python/pybind11/tests/pytest.ini
rename to wrap/pybind11/tests/pytest.ini
diff --git a/wrap/pybind11/tests/test_async.cpp b/wrap/pybind11/tests/test_async.cpp
new file mode 100644
index 000000000..f0ad0d535
--- /dev/null
+++ b/wrap/pybind11/tests/test_async.cpp
@@ -0,0 +1,26 @@
+/*
+    tests/test_async.cpp -- __await__ support
+
+    Copyright (c) 2019 Google Inc.
+
+    All rights reserved. Use of this source code is governed by a
+    BSD-style license that can be found in the LICENSE file.
+*/
+
+#include "pybind11_tests.h"
+
+TEST_SUBMODULE(async_module, m) {
+    struct DoesNotSupportAsync {};
+    py::class_<DoesNotSupportAsync>(m, "DoesNotSupportAsync")
+        .def(py::init<>());
+    struct SupportsAsync {};
+    py::class_<SupportsAsync>(m, "SupportsAsync")
+        .def(py::init<>())
+        .def("__await__", [](const SupportsAsync& self) -> py::object {
+            static_cast<void>(self);
+            py::object loop = py::module::import("asyncio.events").attr("get_event_loop")();
+            py::object f = loop.attr("create_future")();
+            f.attr("set_result")(5);
+            return f.attr("__await__")();
+        });
+}
diff --git a/wrap/pybind11/tests/test_async.py b/wrap/pybind11/tests/test_async.py
new file mode 100644
index 000000000..e9292c9d9
--- /dev/null
+++ b/wrap/pybind11/tests/test_async.py
@@ -0,0 +1,24 @@
+# -*- coding: utf-8 -*-
+import asyncio
+import pytest
+from pybind11_tests import async_module as m
+
+
+@pytest.fixture
+def event_loop():
+    loop = asyncio.new_event_loop()
+    yield loop
+    loop.close()
+
+
+async def get_await_result(x):
+    return await x
+
+
+def test_await(event_loop):
+    assert 5 == event_loop.run_until_complete(get_await_result(m.SupportsAsync()))
+
+
+def test_await_missing(event_loop):
+    with pytest.raises(TypeError):
+        event_loop.run_until_complete(get_await_result(m.DoesNotSupportAsync()))
diff --git a/wrap/python/pybind11/tests/test_buffers.cpp b/wrap/pybind11/tests/test_buffers.cpp
similarity index 86%
rename from wrap/python/pybind11/tests/test_buffers.cpp
rename to wrap/pybind11/tests/test_buffers.cpp
index 5199cf646..1bc67ff7b 100644
--- a/wrap/python/pybind11/tests/test_buffers.cpp
+++ b/wrap/pybind11/tests/test_buffers.cpp
@@ -78,7 +78,7 @@ TEST_SUBMODULE(buffers, m) {
     py::class_<Matrix>(m, "Matrix", py::buffer_protocol())
         .def(py::init<ssize_t, ssize_t>())
         /// Construct from a buffer
-        .def(py::init([](py::buffer b) {
+        .def(py::init([](py::buffer const b) {
             py::buffer_info info = b.request();
             if (info.format != py::format_descriptor<float>::format() || info.ndim != 2)
                 throw std::runtime_error("Incompatible buffer format!");
@@ -166,4 +166,30 @@ TEST_SUBMODULE(buffers, m) {
         .def_readwrite("value", (int32_t DerivedBuffer::*) &DerivedBuffer::value)
         .def_buffer(&DerivedBuffer::get_buffer_info);
 
+    struct BufferReadOnly {
+        const uint8_t value = 0;
+        BufferReadOnly(uint8_t value): value(value) {}
+
+        py::buffer_info get_buffer_info() {
+            return py::buffer_info(&value, 1);
+        }
+    };
+    py::class_<BufferReadOnly>(m, "BufferReadOnly", py::buffer_protocol())
+        .def(py::init<uint8_t>())
+        .def_buffer(&BufferReadOnly::get_buffer_info);
+
+    struct BufferReadOnlySelect {
+        uint8_t value = 0;
+        bool readonly = false;
+
+        py::buffer_info get_buffer_info() {
+            return py::buffer_info(&value, 1, readonly);
+        }
+    };
+    py::class_<BufferReadOnlySelect>(m, "BufferReadOnlySelect", py::buffer_protocol())
+        .def(py::init<>())
+        .def_readwrite("value", &BufferReadOnlySelect::value)
+        .def_readwrite("readonly", &BufferReadOnlySelect::readonly)
+        .def_buffer(&BufferReadOnlySelect::get_buffer_info);
+
 }
diff --git a/wrap/python/pybind11/tests/test_buffers.py b/wrap/pybind11/tests/test_buffers.py
similarity index 78%
rename from wrap/python/pybind11/tests/test_buffers.py
rename to wrap/pybind11/tests/test_buffers.py
index f006552bf..e264311d7 100644
--- a/wrap/python/pybind11/tests/test_buffers.py
+++ b/wrap/pybind11/tests/test_buffers.py
@@ -1,8 +1,15 @@
+# -*- coding: utf-8 -*-
+import io
 import struct
+import sys
+
 import pytest
+
 from pybind11_tests import buffers as m
 from pybind11_tests import ConstructorStats
 
+PY3 = sys.version_info[0] >= 3
+
 pytestmark = pytest.requires_numpy
 
 with pytest.suppress(ImportError):
@@ -85,3 +92,28 @@ def test_pointer_to_member_fn():
         buf.value = 0x12345678
         value = struct.unpack('i', bytearray(buf))[0]
         assert value == 0x12345678
+
+
+@pytest.unsupported_on_pypy
+def test_readonly_buffer():
+    buf = m.BufferReadOnly(0x64)
+    view = memoryview(buf)
+    assert view[0] == 0x64 if PY3 else b'd'
+    assert view.readonly
+
+
+@pytest.unsupported_on_pypy
+def test_selective_readonly_buffer():
+    buf = m.BufferReadOnlySelect()
+
+    memoryview(buf)[0] = 0x64 if PY3 else b'd'
+    assert buf.value == 0x64
+
+    io.BytesIO(b'A').readinto(buf)
+    assert buf.value == ord(b'A')
+
+    buf.readonly = True
+    with pytest.raises(TypeError):
+        memoryview(buf)[0] = 0 if PY3 else b'\0'
+    with pytest.raises(TypeError):
+        io.BytesIO(b'1').readinto(buf)
diff --git a/wrap/python/pybind11/tests/test_builtin_casters.cpp b/wrap/pybind11/tests/test_builtin_casters.cpp
similarity index 88%
rename from wrap/python/pybind11/tests/test_builtin_casters.cpp
rename to wrap/pybind11/tests/test_builtin_casters.cpp
index e026127f8..acb244691 100644
--- a/wrap/python/pybind11/tests/test_builtin_casters.cpp
+++ b/wrap/pybind11/tests/test_builtin_casters.cpp
@@ -30,7 +30,7 @@ TEST_SUBMODULE(builtin_casters, m) {
     else { wstr.push_back((wchar_t) mathbfA32); } // 𝐀, utf32
     wstr.push_back(0x7a); // z
 
-    m.def("good_utf8_string", []() { return std::string(u8"Say utf8\u203d \U0001f382 \U0001d400"); }); // Say utf8‽ 🎂 𝐀
+    m.def("good_utf8_string", []() { return std::string((const char*)u8"Say utf8\u203d \U0001f382 \U0001d400"); }); // Say utf8‽ 🎂 𝐀
     m.def("good_utf16_string", [=]() { return std::u16string({ b16, ib16, cake16_1, cake16_2, mathbfA16_1, mathbfA16_2, z16 }); }); // b‽🎂𝐀z
     m.def("good_utf32_string", [=]() { return std::u32string({ a32, mathbfA32, cake32, ib32, z32 }); }); // a𝐀🎂‽z
     m.def("good_wchar_string", [=]() { return wstr; }); // a‽𝐀z
@@ -60,6 +60,18 @@ TEST_SUBMODULE(builtin_casters, m) {
     m.def("strlen", [](char *s) { return strlen(s); });
     m.def("string_length", [](std::string s) { return s.length(); });
 
+#ifdef PYBIND11_HAS_U8STRING
+    m.attr("has_u8string") = true;
+    m.def("good_utf8_u8string", []() { return std::u8string(u8"Say utf8\u203d \U0001f382 \U0001d400"); }); // Say utf8‽ 🎂 𝐀
+    m.def("bad_utf8_u8string", []()  { return std::u8string((const char8_t*)"abc\xd0" "def"); });
+
+    m.def("u8_char8_Z", []() -> char8_t { return u8'Z'; });
+
+    // test_single_char_arguments
+    m.def("ord_char8", [](char8_t c) -> int { return static_cast<unsigned char>(c); });
+    m.def("ord_char8_lv", [](char8_t &c) -> int { return static_cast<unsigned char>(c); });
+#endif
+
     // test_string_view
 #ifdef PYBIND11_HAS_STRING_VIEW
     m.attr("has_string_view") = true;
@@ -69,9 +81,15 @@ TEST_SUBMODULE(builtin_casters, m) {
     m.def("string_view_chars",   [](std::string_view s)    { py::list l; for (auto c : s) l.append((std::uint8_t) c); return l; });
     m.def("string_view16_chars", [](std::u16string_view s) { py::list l; for (auto c : s) l.append((int) c); return l; });
     m.def("string_view32_chars", [](std::u32string_view s) { py::list l; for (auto c : s) l.append((int) c); return l; });
-    m.def("string_view_return",   []() { return std::string_view(u8"utf8 secret \U0001f382"); });
+    m.def("string_view_return",   []() { return std::string_view((const char*)u8"utf8 secret \U0001f382"); });
     m.def("string_view16_return", []() { return std::u16string_view(u"utf16 secret \U0001f382"); });
     m.def("string_view32_return", []() { return std::u32string_view(U"utf32 secret \U0001f382"); });
+
+#   ifdef PYBIND11_HAS_U8STRING
+    m.def("string_view8_print",  [](std::u8string_view s) { py::print(s, s.size()); });
+    m.def("string_view8_chars",  [](std::u8string_view s) { py::list l; for (auto c : s) l.append((std::uint8_t) c); return l; });
+    m.def("string_view8_return", []() { return std::u8string_view(u8"utf8 secret \U0001f382"); });
+#   endif
 #endif
 
     // test_integer_casting
diff --git a/wrap/python/pybind11/tests/test_builtin_casters.py b/wrap/pybind11/tests/test_builtin_casters.py
similarity index 83%
rename from wrap/python/pybind11/tests/test_builtin_casters.py
rename to wrap/pybind11/tests/test_builtin_casters.py
index 73cc465f5..a13acd5b9 100644
--- a/wrap/python/pybind11/tests/test_builtin_casters.py
+++ b/wrap/pybind11/tests/test_builtin_casters.py
@@ -1,4 +1,4 @@
-# Python < 3 needs this: coding=utf-8
+# -*- coding: utf-8 -*-
 import pytest
 
 from pybind11_tests import builtin_casters as m
@@ -15,6 +15,8 @@ def test_unicode_conversion():
     assert m.good_utf16_string() == u"b‽🎂𝐀z"
     assert m.good_utf32_string() == u"a𝐀🎂‽z"
     assert m.good_wchar_string() == u"a⸘𝐀z"
+    if hasattr(m, "has_u8string"):
+        assert m.good_utf8_u8string() == u"Say utf8‽ 🎂 𝐀"
 
     with pytest.raises(UnicodeDecodeError):
         m.bad_utf8_string()
@@ -29,12 +31,17 @@ def test_unicode_conversion():
     if hasattr(m, "bad_wchar_string"):
         with pytest.raises(UnicodeDecodeError):
             m.bad_wchar_string()
+    if hasattr(m, "has_u8string"):
+        with pytest.raises(UnicodeDecodeError):
+            m.bad_utf8_u8string()
 
     assert m.u8_Z() == 'Z'
     assert m.u8_eacute() == u'é'
     assert m.u16_ibang() == u'‽'
     assert m.u32_mathbfA() == u'𝐀'
     assert m.wchar_heart() == u'♥'
+    if hasattr(m, "has_u8string"):
+        assert m.u8_char8_Z() == 'Z'
 
 
 def test_single_char_arguments():
@@ -92,6 +99,17 @@ def test_single_char_arguments():
         assert m.ord_wchar(u'aa')
     assert str(excinfo.value) == toolong_message
 
+    if hasattr(m, "has_u8string"):
+        assert m.ord_char8(u'a') == 0x61  # simple ASCII
+        assert m.ord_char8_lv(u'b') == 0x62
+        assert m.ord_char8(u'é') == 0xE9  # requires 2 bytes in utf-8, but can be stuffed in a char
+        with pytest.raises(ValueError) as excinfo:
+            assert m.ord_char8(u'Ā') == 0x100  # requires 2 bytes, doesn't fit in a char
+        assert str(excinfo.value) == toobig_message(0x100)
+        with pytest.raises(ValueError) as excinfo:
+            assert m.ord_char8(u'ab')
+        assert str(excinfo.value) == toolong_message
+
 
 def test_bytes_to_string():
     """Tests the ability to pass bytes to C++ string-accepting functions.  Note that this is
@@ -114,36 +132,57 @@ def test_string_view(capture):
     """Tests support for C++17 string_view arguments and return values"""
     assert m.string_view_chars("Hi") == [72, 105]
     assert m.string_view_chars("Hi 🎂") == [72, 105, 32, 0xf0, 0x9f, 0x8e, 0x82]
-    assert m.string_view16_chars("Hi 🎂") == [72, 105, 32, 0xd83c, 0xdf82]
-    assert m.string_view32_chars("Hi 🎂") == [72, 105, 32, 127874]
+    assert m.string_view16_chars(u"Hi 🎂") == [72, 105, 32, 0xd83c, 0xdf82]
+    assert m.string_view32_chars(u"Hi 🎂") == [72, 105, 32, 127874]
+    if hasattr(m, "has_u8string"):
+        assert m.string_view8_chars("Hi") == [72, 105]
+        assert m.string_view8_chars(u"Hi 🎂") == [72, 105, 32, 0xf0, 0x9f, 0x8e, 0x82]
 
-    assert m.string_view_return() == "utf8 secret 🎂"
-    assert m.string_view16_return() == "utf16 secret 🎂"
-    assert m.string_view32_return() == "utf32 secret 🎂"
+    assert m.string_view_return() == u"utf8 secret 🎂"
+    assert m.string_view16_return() == u"utf16 secret 🎂"
+    assert m.string_view32_return() == u"utf32 secret 🎂"
+    if hasattr(m, "has_u8string"):
+        assert m.string_view8_return() == u"utf8 secret 🎂"
 
     with capture:
         m.string_view_print("Hi")
         m.string_view_print("utf8 🎂")
-        m.string_view16_print("utf16 🎂")
-        m.string_view32_print("utf32 🎂")
-    assert capture == """
+        m.string_view16_print(u"utf16 🎂")
+        m.string_view32_print(u"utf32 🎂")
+    assert capture == u"""
         Hi 2
         utf8 🎂 9
         utf16 🎂 8
         utf32 🎂 7
     """
+    if hasattr(m, "has_u8string"):
+        with capture:
+            m.string_view8_print("Hi")
+            m.string_view8_print(u"utf8 🎂")
+        assert capture == u"""
+            Hi 2
+            utf8 🎂 9
+        """
 
     with capture:
         m.string_view_print("Hi, ascii")
         m.string_view_print("Hi, utf8 🎂")
-        m.string_view16_print("Hi, utf16 🎂")
-        m.string_view32_print("Hi, utf32 🎂")
-    assert capture == """
+        m.string_view16_print(u"Hi, utf16 🎂")
+        m.string_view32_print(u"Hi, utf32 🎂")
+    assert capture == u"""
         Hi, ascii 9
         Hi, utf8 🎂 13
         Hi, utf16 🎂 12
         Hi, utf32 🎂 11
     """
+    if hasattr(m, "has_u8string"):
+        with capture:
+            m.string_view8_print("Hi, ascii")
+            m.string_view8_print(u"Hi, utf8 🎂")
+        assert capture == u"""
+            Hi, ascii 9
+            Hi, utf8 🎂 13
+        """
 
 
 def test_integer_casting():
@@ -318,11 +357,15 @@ def test_numpy_bool():
     import numpy as np
     convert, noconvert = m.bool_passthrough, m.bool_passthrough_noconvert
 
+    def cant_convert(v):
+        pytest.raises(TypeError, convert, v)
+
     # np.bool_ is not considered implicit
     assert convert(np.bool_(True)) is True
     assert convert(np.bool_(False)) is False
     assert noconvert(np.bool_(True)) is True
     assert noconvert(np.bool_(False)) is False
+    cant_convert(np.zeros(2, dtype='int'))
 
 
 def test_int_long():
diff --git a/wrap/python/pybind11/tests/test_call_policies.cpp b/wrap/pybind11/tests/test_call_policies.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_call_policies.cpp
rename to wrap/pybind11/tests/test_call_policies.cpp
diff --git a/wrap/python/pybind11/tests/test_call_policies.py b/wrap/pybind11/tests/test_call_policies.py
similarity index 99%
rename from wrap/python/pybind11/tests/test_call_policies.py
rename to wrap/pybind11/tests/test_call_policies.py
index 7c835599c..0e3230c57 100644
--- a/wrap/python/pybind11/tests/test_call_policies.py
+++ b/wrap/pybind11/tests/test_call_policies.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import call_policies as m
 from pybind11_tests import ConstructorStats
diff --git a/wrap/python/pybind11/tests/test_callbacks.cpp b/wrap/pybind11/tests/test_callbacks.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_callbacks.cpp
rename to wrap/pybind11/tests/test_callbacks.cpp
diff --git a/wrap/python/pybind11/tests/test_callbacks.py b/wrap/pybind11/tests/test_callbacks.py
similarity index 99%
rename from wrap/python/pybind11/tests/test_callbacks.py
rename to wrap/pybind11/tests/test_callbacks.py
index 6439c8e72..d5d0e045d 100644
--- a/wrap/python/pybind11/tests/test_callbacks.py
+++ b/wrap/pybind11/tests/test_callbacks.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import callbacks as m
 from threading import Thread
diff --git a/wrap/python/pybind11/tests/test_chrono.cpp b/wrap/pybind11/tests/test_chrono.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_chrono.cpp
rename to wrap/pybind11/tests/test_chrono.cpp
diff --git a/wrap/python/pybind11/tests/test_chrono.py b/wrap/pybind11/tests/test_chrono.py
similarity index 59%
rename from wrap/python/pybind11/tests/test_chrono.py
rename to wrap/pybind11/tests/test_chrono.py
index f308de977..f1817e44f 100644
--- a/wrap/python/pybind11/tests/test_chrono.py
+++ b/wrap/pybind11/tests/test_chrono.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 from pybind11_tests import chrono as m
 import datetime
 
@@ -40,6 +41,62 @@ def test_chrono_system_clock_roundtrip():
     assert diff.microseconds == 0
 
 
+def test_chrono_system_clock_roundtrip_date():
+    date1 = datetime.date.today()
+
+    # Roundtrip the time
+    datetime2 = m.test_chrono2(date1)
+    date2 = datetime2.date()
+    time2 = datetime2.time()
+
+    # The returned value should be a datetime
+    assert isinstance(datetime2, datetime.datetime)
+    assert isinstance(date2, datetime.date)
+    assert isinstance(time2, datetime.time)
+
+    # They should be identical (no information lost on roundtrip)
+    diff = abs(date1 - date2)
+    assert diff.days == 0
+    assert diff.seconds == 0
+    assert diff.microseconds == 0
+
+    # Year, Month & Day should be the same after the round trip
+    assert date1.year == date2.year
+    assert date1.month == date2.month
+    assert date1.day == date2.day
+
+    # There should be no time information
+    assert time2.hour == 0
+    assert time2.minute == 0
+    assert time2.second == 0
+    assert time2.microsecond == 0
+
+
+def test_chrono_system_clock_roundtrip_time():
+    time1 = datetime.datetime.today().time()
+
+    # Roundtrip the time
+    datetime2 = m.test_chrono2(time1)
+    date2 = datetime2.date()
+    time2 = datetime2.time()
+
+    # The returned value should be a datetime
+    assert isinstance(datetime2, datetime.datetime)
+    assert isinstance(date2, datetime.date)
+    assert isinstance(time2, datetime.time)
+
+    # Hour, Minute, Second & Microsecond should be the same after the round trip
+    assert time1.hour == time2.hour
+    assert time1.minute == time2.minute
+    assert time1.second == time2.second
+    assert time1.microsecond == time2.microsecond
+
+    # There should be no date information (i.e. date = python base date)
+    assert date2.year == 1970
+    assert date2.month == 1
+    assert date2.day == 1
+
+
 def test_chrono_duration_roundtrip():
 
     # Get the difference between two times (a timedelta)
@@ -70,6 +127,19 @@ def test_chrono_duration_subtraction_equivalence():
     assert cpp_diff.microseconds == diff.microseconds
 
 
+def test_chrono_duration_subtraction_equivalence_date():
+
+    date1 = datetime.date.today()
+    date2 = datetime.date.today()
+
+    diff = date2 - date1
+    cpp_diff = m.test_chrono4(date2, date1)
+
+    assert cpp_diff.days == diff.days
+    assert cpp_diff.seconds == diff.seconds
+    assert cpp_diff.microseconds == diff.microseconds
+
+
 def test_chrono_steady_clock():
     time1 = m.test_chrono5()
     assert isinstance(time1, datetime.timedelta)
diff --git a/wrap/python/pybind11/tests/test_class.cpp b/wrap/pybind11/tests/test_class.cpp
similarity index 98%
rename from wrap/python/pybind11/tests/test_class.cpp
rename to wrap/pybind11/tests/test_class.cpp
index 499d0cc51..128bc39e9 100644
--- a/wrap/python/pybind11/tests/test_class.cpp
+++ b/wrap/pybind11/tests/test_class.cpp
@@ -367,6 +367,14 @@ TEST_SUBMODULE(class_, m) {
             .def(py::init<>())
             .def("ptr", &Aligned::ptr);
     #endif
+
+    // test_final
+    struct IsFinal final {};
+    py::class_<IsFinal>(m, "IsFinal", py::is_final());
+
+    // test_non_final_final
+    struct IsNonFinalFinal {};
+    py::class_<IsNonFinalFinal>(m, "IsNonFinalFinal", py::is_final());
 }
 
 template <int N> class BreaksBase { public: virtual ~BreaksBase() = default; };
diff --git a/wrap/python/pybind11/tests/test_class.py b/wrap/pybind11/tests/test_class.py
similarity index 85%
rename from wrap/python/pybind11/tests/test_class.py
rename to wrap/pybind11/tests/test_class.py
index ed63ca853..c38a5e8ce 100644
--- a/wrap/python/pybind11/tests/test_class.py
+++ b/wrap/pybind11/tests/test_class.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 
 from pybind11_tests import class_ as m
@@ -101,6 +102,31 @@ def test_inheritance(msg):
     assert "No constructor defined!" in str(excinfo.value)
 
 
+def test_inheritance_init(msg):
+
+    # Single base
+    class Python(m.Pet):
+        def __init__(self):
+            pass
+    with pytest.raises(TypeError) as exc_info:
+        Python()
+    expected = ["m.class_.Pet.__init__() must be called when overriding __init__",
+                "Pet.__init__() must be called when overriding __init__"]  # PyPy?
+    # TODO: fix PyPy error message wrt. tp_name/__qualname__?
+    assert msg(exc_info.value) in expected
+
+    # Multiple bases
+    class RabbitHamster(m.Rabbit, m.Hamster):
+        def __init__(self):
+            m.Rabbit.__init__(self, "RabbitHamster")
+
+    with pytest.raises(TypeError) as exc_info:
+        RabbitHamster()
+    expected = ["m.class_.Hamster.__init__() must be called when overriding __init__",
+                "Hamster.__init__() must be called when overriding __init__"]  # PyPy
+    assert msg(exc_info.value) in expected
+
+
 def test_automatic_upcasting():
     assert type(m.return_class_1()).__name__ == "DerivedClass1"
     assert type(m.return_class_2()).__name__ == "DerivedClass2"
@@ -279,3 +305,21 @@ def test_aligned():
     if hasattr(m, "Aligned"):
         p = m.Aligned().ptr()
         assert p % 1024 == 0
+
+
+# https://bitbucket.org/pypy/pypy/issues/2742
+@pytest.unsupported_on_pypy
+def test_final():
+    with pytest.raises(TypeError) as exc_info:
+        class PyFinalChild(m.IsFinal):
+            pass
+    assert str(exc_info.value).endswith("is not an acceptable base type")
+
+
+# https://bitbucket.org/pypy/pypy/issues/2742
+@pytest.unsupported_on_pypy
+def test_non_final_final():
+    with pytest.raises(TypeError) as exc_info:
+        class PyNonFinalFinalChild(m.IsNonFinalFinal):
+            pass
+    assert str(exc_info.value).endswith("is not an acceptable base type")
diff --git a/wrap/python/pybind11/tests/test_cmake_build/CMakeLists.txt b/wrap/pybind11/tests/test_cmake_build/CMakeLists.txt
similarity index 100%
rename from wrap/python/pybind11/tests/test_cmake_build/CMakeLists.txt
rename to wrap/pybind11/tests/test_cmake_build/CMakeLists.txt
diff --git a/wrap/python/pybind11/tests/test_cmake_build/embed.cpp b/wrap/pybind11/tests/test_cmake_build/embed.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_cmake_build/embed.cpp
rename to wrap/pybind11/tests/test_cmake_build/embed.cpp
diff --git a/wrap/python/pybind11/tests/test_cmake_build/installed_embed/CMakeLists.txt b/wrap/pybind11/tests/test_cmake_build/installed_embed/CMakeLists.txt
similarity index 100%
rename from wrap/python/pybind11/tests/test_cmake_build/installed_embed/CMakeLists.txt
rename to wrap/pybind11/tests/test_cmake_build/installed_embed/CMakeLists.txt
diff --git a/wrap/python/pybind11/tests/test_cmake_build/installed_function/CMakeLists.txt b/wrap/pybind11/tests/test_cmake_build/installed_function/CMakeLists.txt
similarity index 100%
rename from wrap/python/pybind11/tests/test_cmake_build/installed_function/CMakeLists.txt
rename to wrap/pybind11/tests/test_cmake_build/installed_function/CMakeLists.txt
diff --git a/wrap/python/pybind11/tests/test_cmake_build/installed_target/CMakeLists.txt b/wrap/pybind11/tests/test_cmake_build/installed_target/CMakeLists.txt
similarity index 100%
rename from wrap/python/pybind11/tests/test_cmake_build/installed_target/CMakeLists.txt
rename to wrap/pybind11/tests/test_cmake_build/installed_target/CMakeLists.txt
diff --git a/wrap/python/pybind11/tests/test_cmake_build/main.cpp b/wrap/pybind11/tests/test_cmake_build/main.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_cmake_build/main.cpp
rename to wrap/pybind11/tests/test_cmake_build/main.cpp
diff --git a/wrap/python/pybind11/tests/test_cmake_build/subdirectory_embed/CMakeLists.txt b/wrap/pybind11/tests/test_cmake_build/subdirectory_embed/CMakeLists.txt
similarity index 100%
rename from wrap/python/pybind11/tests/test_cmake_build/subdirectory_embed/CMakeLists.txt
rename to wrap/pybind11/tests/test_cmake_build/subdirectory_embed/CMakeLists.txt
diff --git a/wrap/python/pybind11/tests/test_cmake_build/subdirectory_function/CMakeLists.txt b/wrap/pybind11/tests/test_cmake_build/subdirectory_function/CMakeLists.txt
similarity index 100%
rename from wrap/python/pybind11/tests/test_cmake_build/subdirectory_function/CMakeLists.txt
rename to wrap/pybind11/tests/test_cmake_build/subdirectory_function/CMakeLists.txt
diff --git a/wrap/python/pybind11/tests/test_cmake_build/subdirectory_target/CMakeLists.txt b/wrap/pybind11/tests/test_cmake_build/subdirectory_target/CMakeLists.txt
similarity index 100%
rename from wrap/python/pybind11/tests/test_cmake_build/subdirectory_target/CMakeLists.txt
rename to wrap/pybind11/tests/test_cmake_build/subdirectory_target/CMakeLists.txt
diff --git a/wrap/python/pybind11/tests/test_cmake_build/test.py b/wrap/pybind11/tests/test_cmake_build/test.py
similarity index 85%
rename from wrap/python/pybind11/tests/test_cmake_build/test.py
rename to wrap/pybind11/tests/test_cmake_build/test.py
index 1467a61dc..87ed5135f 100644
--- a/wrap/python/pybind11/tests/test_cmake_build/test.py
+++ b/wrap/pybind11/tests/test_cmake_build/test.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import sys
 import test_cmake_build
 
diff --git a/wrap/python/pybind11/tests/test_constants_and_functions.cpp b/wrap/pybind11/tests/test_constants_and_functions.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_constants_and_functions.cpp
rename to wrap/pybind11/tests/test_constants_and_functions.cpp
diff --git a/wrap/python/pybind11/tests/test_constants_and_functions.py b/wrap/pybind11/tests/test_constants_and_functions.py
similarity index 97%
rename from wrap/python/pybind11/tests/test_constants_and_functions.py
rename to wrap/pybind11/tests/test_constants_and_functions.py
index 472682d61..36b1aa64b 100644
--- a/wrap/python/pybind11/tests/test_constants_and_functions.py
+++ b/wrap/pybind11/tests/test_constants_and_functions.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 from pybind11_tests import constants_and_functions as m
 
 
diff --git a/wrap/python/pybind11/tests/test_copy_move.cpp b/wrap/pybind11/tests/test_copy_move.cpp
similarity index 98%
rename from wrap/python/pybind11/tests/test_copy_move.cpp
rename to wrap/pybind11/tests/test_copy_move.cpp
index 98d5e0a0b..0f698bdf0 100644
--- a/wrap/python/pybind11/tests/test_copy_move.cpp
+++ b/wrap/pybind11/tests/test_copy_move.cpp
@@ -68,8 +68,8 @@ public:
 
     int value;
 };
-NAMESPACE_BEGIN(pybind11)
-NAMESPACE_BEGIN(detail)
+PYBIND11_NAMESPACE_BEGIN(pybind11)
+PYBIND11_NAMESPACE_BEGIN(detail)
 template <> struct type_caster<MoveOnlyInt> {
     PYBIND11_TYPE_CASTER(MoveOnlyInt, _("MoveOnlyInt"));
     bool load(handle src, bool) { value = MoveOnlyInt(src.cast<int>()); return true; }
@@ -97,8 +97,8 @@ public:
     operator CopyOnlyInt&() { return value; }
     template <typename T> using cast_op_type = pybind11::detail::cast_op_type<T>;
 };
-NAMESPACE_END(detail)
-NAMESPACE_END(pybind11)
+PYBIND11_NAMESPACE_END(detail)
+PYBIND11_NAMESPACE_END(pybind11)
 
 TEST_SUBMODULE(copy_move_policies, m) {
     // test_lacking_copy_ctor
diff --git a/wrap/python/pybind11/tests/test_copy_move.py b/wrap/pybind11/tests/test_copy_move.py
similarity index 95%
rename from wrap/python/pybind11/tests/test_copy_move.py
rename to wrap/pybind11/tests/test_copy_move.py
index aff2d99f2..6b53993a9 100644
--- a/wrap/python/pybind11/tests/test_copy_move.py
+++ b/wrap/pybind11/tests/test_copy_move.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import copy_move_policies as m
 
@@ -5,13 +6,13 @@ from pybind11_tests import copy_move_policies as m
 def test_lacking_copy_ctor():
     with pytest.raises(RuntimeError) as excinfo:
         m.lacking_copy_ctor.get_one()
-    assert "the object is non-copyable!" in str(excinfo.value)
+    assert "is non-copyable!" in str(excinfo.value)
 
 
 def test_lacking_move_ctor():
     with pytest.raises(RuntimeError) as excinfo:
         m.lacking_move_ctor.get_one()
-    assert "the object is neither movable nor copyable!" in str(excinfo.value)
+    assert "is neither movable nor copyable!" in str(excinfo.value)
 
 
 def test_move_and_copy_casts():
@@ -98,7 +99,7 @@ def test_private_op_new():
 
     with pytest.raises(RuntimeError) as excinfo:
         m.private_op_new_value()
-    assert "the object is neither movable nor copyable" in str(excinfo.value)
+    assert "is neither movable nor copyable" in str(excinfo.value)
 
     assert m.private_op_new_reference().value == 1
 
diff --git a/wrap/pybind11/tests/test_custom_type_casters.cpp b/wrap/pybind11/tests/test_custom_type_casters.cpp
new file mode 100644
index 000000000..9485d3cdb
--- /dev/null
+++ b/wrap/pybind11/tests/test_custom_type_casters.cpp
@@ -0,0 +1,125 @@
+/*
+    tests/test_custom_type_casters.cpp -- tests type_caster<T>
+
+    Copyright (c) 2016 Wenzel Jakob <wenzel.jakob@epfl.ch>
+
+    All rights reserved. Use of this source code is governed by a
+    BSD-style license that can be found in the LICENSE file.
+*/
+
+#include "pybind11_tests.h"
+#include "constructor_stats.h"
+
+
+// py::arg/py::arg_v testing: these arguments just record their argument when invoked
+class ArgInspector1 { public: std::string arg = "(default arg inspector 1)"; };
+class ArgInspector2 { public: std::string arg = "(default arg inspector 2)"; };
+class ArgAlwaysConverts { };
+namespace pybind11 { namespace detail {
+template <> struct type_caster<ArgInspector1> {
+public:
+    PYBIND11_TYPE_CASTER(ArgInspector1, _("ArgInspector1"));
+
+    bool load(handle src, bool convert) {
+        value.arg = "loading ArgInspector1 argument " +
+            std::string(convert ? "WITH" : "WITHOUT") + " conversion allowed.  "
+            "Argument value = " + (std::string) str(src);
+        return true;
+    }
+
+    static handle cast(const ArgInspector1 &src, return_value_policy, handle) {
+        return str(src.arg).release();
+    }
+};
+template <> struct type_caster<ArgInspector2> {
+public:
+    PYBIND11_TYPE_CASTER(ArgInspector2, _("ArgInspector2"));
+
+    bool load(handle src, bool convert) {
+        value.arg = "loading ArgInspector2 argument " +
+            std::string(convert ? "WITH" : "WITHOUT") + " conversion allowed.  "
+            "Argument value = " + (std::string) str(src);
+        return true;
+    }
+
+    static handle cast(const ArgInspector2 &src, return_value_policy, handle) {
+        return str(src.arg).release();
+    }
+};
+template <> struct type_caster<ArgAlwaysConverts> {
+public:
+    PYBIND11_TYPE_CASTER(ArgAlwaysConverts, _("ArgAlwaysConverts"));
+
+    bool load(handle, bool convert) {
+        return convert;
+    }
+
+    static handle cast(const ArgAlwaysConverts &, return_value_policy, handle) {
+        return py::none().release();
+    }
+};
+}}
+
+// test_custom_caster_destruction
+class DestructionTester {
+public:
+    DestructionTester() { print_default_created(this); }
+    ~DestructionTester() { print_destroyed(this); }
+    DestructionTester(const DestructionTester &) { print_copy_created(this); }
+    DestructionTester(DestructionTester &&) { print_move_created(this); }
+    DestructionTester &operator=(const DestructionTester &) { print_copy_assigned(this); return *this; }
+    DestructionTester &operator=(DestructionTester &&) { print_move_assigned(this); return *this; }
+};
+namespace pybind11 { namespace detail {
+template <> struct type_caster<DestructionTester> {
+    PYBIND11_TYPE_CASTER(DestructionTester, _("DestructionTester"));
+    bool load(handle, bool) { return true; }
+
+    static handle cast(const DestructionTester &, return_value_policy, handle) {
+        return py::bool_(true).release();
+    }
+};
+}}
+
+TEST_SUBMODULE(custom_type_casters, m) {
+    // test_custom_type_casters
+
+    // test_noconvert_args
+    //
+    // Test converting.  The ArgAlwaysConverts is just there to make the first no-conversion pass
+    // fail so that our call always ends up happening via the second dispatch (the one that allows
+    // some conversion).
+    class ArgInspector {
+    public:
+        ArgInspector1 f(ArgInspector1 a, ArgAlwaysConverts) { return a; }
+        std::string g(ArgInspector1 a, const ArgInspector1 &b, int c, ArgInspector2 *d, ArgAlwaysConverts) {
+            return a.arg + "\n" + b.arg + "\n" + std::to_string(c) + "\n" + d->arg;
+        }
+        static ArgInspector2 h(ArgInspector2 a, ArgAlwaysConverts) { return a; }
+    };
+    py::class_<ArgInspector>(m, "ArgInspector")
+        .def(py::init<>())
+        .def("f", &ArgInspector::f, py::arg(), py::arg() = ArgAlwaysConverts())
+        .def("g", &ArgInspector::g, "a"_a.noconvert(), "b"_a, "c"_a.noconvert()=13, "d"_a=ArgInspector2(), py::arg() = ArgAlwaysConverts())
+        .def_static("h", &ArgInspector::h, py::arg().noconvert(), py::arg() = ArgAlwaysConverts())
+        ;
+    m.def("arg_inspect_func", [](ArgInspector2 a, ArgInspector1 b, ArgAlwaysConverts) { return a.arg + "\n" + b.arg; },
+            py::arg().noconvert(false), py::arg_v(nullptr, ArgInspector1()).noconvert(true), py::arg() = ArgAlwaysConverts());
+
+    m.def("floats_preferred", [](double f) { return 0.5 * f; }, py::arg("f"));
+    m.def("floats_only", [](double f) { return 0.5 * f; }, py::arg("f").noconvert());
+    m.def("ints_preferred", [](int i) { return i / 2; }, py::arg("i"));
+    m.def("ints_only", [](int i) { return i / 2; }, py::arg("i").noconvert());
+
+    // test_custom_caster_destruction
+    // Test that `take_ownership` works on types with a custom type caster when given a pointer
+
+    // default policy: don't take ownership:
+    m.def("custom_caster_no_destroy", []() { static auto *dt = new DestructionTester(); return dt; });
+
+    m.def("custom_caster_destroy", []() { return new DestructionTester(); },
+            py::return_value_policy::take_ownership); // Takes ownership: destroy when finished
+    m.def("custom_caster_destroy_const", []() -> const DestructionTester * { return new DestructionTester(); },
+            py::return_value_policy::take_ownership); // Likewise (const doesn't inhibit destruction)
+    m.def("destruction_tester_cstats", &ConstructorStats::get<DestructionTester>, py::return_value_policy::reference);
+}
diff --git a/wrap/pybind11/tests/test_custom_type_casters.py b/wrap/pybind11/tests/test_custom_type_casters.py
new file mode 100644
index 000000000..9475c4516
--- /dev/null
+++ b/wrap/pybind11/tests/test_custom_type_casters.py
@@ -0,0 +1,90 @@
+# -*- coding: utf-8 -*-
+import pytest
+from pybind11_tests import custom_type_casters as m
+
+
+def test_noconvert_args(msg):
+    a = m.ArgInspector()
+    assert msg(a.f("hi")) == """
+        loading ArgInspector1 argument WITH conversion allowed.  Argument value = hi
+    """
+    assert msg(a.g("this is a", "this is b")) == """
+        loading ArgInspector1 argument WITHOUT conversion allowed.  Argument value = this is a
+        loading ArgInspector1 argument WITH conversion allowed.  Argument value = this is b
+        13
+        loading ArgInspector2 argument WITH conversion allowed.  Argument value = (default arg inspector 2)
+    """  # noqa: E501 line too long
+    assert msg(a.g("this is a", "this is b", 42)) == """
+        loading ArgInspector1 argument WITHOUT conversion allowed.  Argument value = this is a
+        loading ArgInspector1 argument WITH conversion allowed.  Argument value = this is b
+        42
+        loading ArgInspector2 argument WITH conversion allowed.  Argument value = (default arg inspector 2)
+    """  # noqa: E501 line too long
+    assert msg(a.g("this is a", "this is b", 42, "this is d")) == """
+        loading ArgInspector1 argument WITHOUT conversion allowed.  Argument value = this is a
+        loading ArgInspector1 argument WITH conversion allowed.  Argument value = this is b
+        42
+        loading ArgInspector2 argument WITH conversion allowed.  Argument value = this is d
+    """
+    assert (a.h("arg 1") ==
+            "loading ArgInspector2 argument WITHOUT conversion allowed.  Argument value = arg 1")
+    assert msg(m.arg_inspect_func("A1", "A2")) == """
+        loading ArgInspector2 argument WITH conversion allowed.  Argument value = A1
+        loading ArgInspector1 argument WITHOUT conversion allowed.  Argument value = A2
+    """
+
+    assert m.floats_preferred(4) == 2.0
+    assert m.floats_only(4.0) == 2.0
+    with pytest.raises(TypeError) as excinfo:
+        m.floats_only(4)
+    assert msg(excinfo.value) == """
+        floats_only(): incompatible function arguments. The following argument types are supported:
+            1. (f: float) -> float
+
+        Invoked with: 4
+    """
+
+    assert m.ints_preferred(4) == 2
+    assert m.ints_preferred(True) == 0
+    with pytest.raises(TypeError) as excinfo:
+        m.ints_preferred(4.0)
+    assert msg(excinfo.value) == """
+        ints_preferred(): incompatible function arguments. The following argument types are supported:
+            1. (i: int) -> int
+
+        Invoked with: 4.0
+    """  # noqa: E501 line too long
+
+    assert m.ints_only(4) == 2
+    with pytest.raises(TypeError) as excinfo:
+        m.ints_only(4.0)
+    assert msg(excinfo.value) == """
+        ints_only(): incompatible function arguments. The following argument types are supported:
+            1. (i: int) -> int
+
+        Invoked with: 4.0
+    """
+
+
+def test_custom_caster_destruction():
+    """Tests that returning a pointer to a type that gets converted with a custom type caster gets
+    destroyed when the function has py::return_value_policy::take_ownership policy applied."""
+
+    cstats = m.destruction_tester_cstats()
+    # This one *doesn't* have take_ownership: the pointer should be used but not destroyed:
+    z = m.custom_caster_no_destroy()
+    assert cstats.alive() == 1 and cstats.default_constructions == 1
+    assert z
+
+    # take_ownership applied: this constructs a new object, casts it, then destroys it:
+    z = m.custom_caster_destroy()
+    assert z
+    assert cstats.default_constructions == 2
+
+    # Same, but with a const pointer return (which should *not* inhibit destruction):
+    z = m.custom_caster_destroy_const()
+    assert z
+    assert cstats.default_constructions == 3
+
+    # Make sure we still only have the original object (from ..._no_destroy()) alive:
+    assert cstats.alive() == 1
diff --git a/wrap/python/pybind11/tests/test_docstring_options.cpp b/wrap/pybind11/tests/test_docstring_options.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_docstring_options.cpp
rename to wrap/pybind11/tests/test_docstring_options.cpp
diff --git a/wrap/python/pybind11/tests/test_docstring_options.py b/wrap/pybind11/tests/test_docstring_options.py
similarity index 98%
rename from wrap/python/pybind11/tests/test_docstring_options.py
rename to wrap/pybind11/tests/test_docstring_options.py
index 0dbca609e..80ade0f15 100644
--- a/wrap/python/pybind11/tests/test_docstring_options.py
+++ b/wrap/pybind11/tests/test_docstring_options.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 from pybind11_tests import docstring_options as m
 
 
diff --git a/wrap/python/pybind11/tests/test_eigen.cpp b/wrap/pybind11/tests/test_eigen.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_eigen.cpp
rename to wrap/pybind11/tests/test_eigen.cpp
diff --git a/wrap/python/pybind11/tests/test_eigen.py b/wrap/pybind11/tests/test_eigen.py
similarity index 94%
rename from wrap/python/pybind11/tests/test_eigen.py
rename to wrap/pybind11/tests/test_eigen.py
index 45f64ca94..ae868da51 100644
--- a/wrap/python/pybind11/tests/test_eigen.py
+++ b/wrap/pybind11/tests/test_eigen.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import ConstructorStats
 
@@ -79,15 +80,17 @@ def test_mutator_descriptors():
     m.fixed_mutator_a(zc)
     with pytest.raises(TypeError) as excinfo:
         m.fixed_mutator_r(zc)
-    assert ('(arg0: numpy.ndarray[float32[5, 6], flags.writeable, flags.c_contiguous]) -> None'
+    assert ('(arg0: numpy.ndarray[numpy.float32[5, 6],'
+            ' flags.writeable, flags.c_contiguous]) -> None'
             in str(excinfo.value))
     with pytest.raises(TypeError) as excinfo:
         m.fixed_mutator_c(zr)
-    assert ('(arg0: numpy.ndarray[float32[5, 6], flags.writeable, flags.f_contiguous]) -> None'
+    assert ('(arg0: numpy.ndarray[numpy.float32[5, 6],'
+            ' flags.writeable, flags.f_contiguous]) -> None'
             in str(excinfo.value))
     with pytest.raises(TypeError) as excinfo:
         m.fixed_mutator_a(np.array([[1, 2], [3, 4]], dtype='float32'))
-    assert ('(arg0: numpy.ndarray[float32[5, 6], flags.writeable]) -> None'
+    assert ('(arg0: numpy.ndarray[numpy.float32[5, 6], flags.writeable]) -> None'
             in str(excinfo.value))
     zr.flags.writeable = False
     with pytest.raises(TypeError):
@@ -141,7 +144,7 @@ def test_nonunit_stride_from_python():
 
     counting_3d = np.arange(27.0, dtype=np.float32).reshape((3, 3, 3))
     slices = [counting_3d[0, :, :], counting_3d[:, 0, :], counting_3d[:, :, 0]]
-    for slice_idx, ref_mat in enumerate(slices):
+    for ref_mat in slices:
         np.testing.assert_array_equal(m.double_mat_cm(ref_mat), 2.0 * ref_mat)
         np.testing.assert_array_equal(m.double_mat_rm(ref_mat), 2.0 * ref_mat)
 
@@ -170,7 +173,7 @@ def test_negative_stride_from_python(msg):
     counting_3d = np.arange(27.0, dtype=np.float32).reshape((3, 3, 3))
     counting_3d = counting_3d[::-1, ::-1, ::-1]
     slices = [counting_3d[0, :, :], counting_3d[:, 0, :], counting_3d[:, :, 0]]
-    for slice_idx, ref_mat in enumerate(slices):
+    for ref_mat in slices:
         np.testing.assert_array_equal(m.double_mat_cm(ref_mat), 2.0 * ref_mat)
         np.testing.assert_array_equal(m.double_mat_rm(ref_mat), 2.0 * ref_mat)
 
@@ -179,7 +182,7 @@ def test_negative_stride_from_python(msg):
         m.double_threer(second_row)
     assert msg(excinfo.value) == """
         double_threer(): incompatible function arguments. The following argument types are supported:
-            1. (arg0: numpy.ndarray[float32[1, 3], flags.writeable]) -> None
+            1. (arg0: numpy.ndarray[numpy.float32[1, 3], flags.writeable]) -> None
 
         Invoked with: """ + repr(np.array([ 5.,  4.,  3.], dtype='float32'))  # noqa: E501 line too long
 
@@ -187,7 +190,7 @@ def test_negative_stride_from_python(msg):
         m.double_threec(second_col)
     assert msg(excinfo.value) == """
         double_threec(): incompatible function arguments. The following argument types are supported:
-            1. (arg0: numpy.ndarray[float32[3, 1], flags.writeable]) -> None
+            1. (arg0: numpy.ndarray[numpy.float32[3, 1], flags.writeable]) -> None
 
         Invoked with: """ + repr(np.array([ 7.,  4.,  1.], dtype='float32'))  # noqa: E501 line too long
 
@@ -607,17 +610,19 @@ def test_special_matrix_objects():
 
 def test_dense_signature(doc):
     assert doc(m.double_col) == """
-        double_col(arg0: numpy.ndarray[float32[m, 1]]) -> numpy.ndarray[float32[m, 1]]
+        double_col(arg0: numpy.ndarray[numpy.float32[m, 1]]) -> numpy.ndarray[numpy.float32[m, 1]]
     """
     assert doc(m.double_row) == """
-        double_row(arg0: numpy.ndarray[float32[1, n]]) -> numpy.ndarray[float32[1, n]]
-    """
-    assert doc(m.double_complex) == """
-        double_complex(arg0: numpy.ndarray[complex64[m, 1]]) -> numpy.ndarray[complex64[m, 1]]
-    """
-    assert doc(m.double_mat_rm) == """
-        double_mat_rm(arg0: numpy.ndarray[float32[m, n]]) -> numpy.ndarray[float32[m, n]]
+        double_row(arg0: numpy.ndarray[numpy.float32[1, n]]) -> numpy.ndarray[numpy.float32[1, n]]
     """
+    assert doc(m.double_complex) == ("""
+        double_complex(arg0: numpy.ndarray[numpy.complex64[m, 1]])"""
+                                     """ -> numpy.ndarray[numpy.complex64[m, 1]]
+    """)
+    assert doc(m.double_mat_rm) == ("""
+        double_mat_rm(arg0: numpy.ndarray[numpy.float32[m, n]])"""
+                                    """ -> numpy.ndarray[numpy.float32[m, n]]
+    """)
 
 
 def test_named_arguments():
@@ -654,10 +659,10 @@ def test_sparse():
 @pytest.requires_eigen_and_scipy
 def test_sparse_signature(doc):
     assert doc(m.sparse_copy_r) == """
-        sparse_copy_r(arg0: scipy.sparse.csr_matrix[float32]) -> scipy.sparse.csr_matrix[float32]
+        sparse_copy_r(arg0: scipy.sparse.csr_matrix[numpy.float32]) -> scipy.sparse.csr_matrix[numpy.float32]
     """  # noqa: E501 line too long
     assert doc(m.sparse_copy_c) == """
-        sparse_copy_c(arg0: scipy.sparse.csc_matrix[float32]) -> scipy.sparse.csc_matrix[float32]
+        sparse_copy_c(arg0: scipy.sparse.csc_matrix[numpy.float32]) -> scipy.sparse.csc_matrix[numpy.float32]
     """  # noqa: E501 line too long
 
 
@@ -679,10 +684,10 @@ def test_issue1105():
     # These should still fail (incompatible dimensions):
     with pytest.raises(TypeError) as excinfo:
         m.iss1105_row(np.ones((7, 1)))
-    assert "incompatible function arguments" in str(excinfo)
+    assert "incompatible function arguments" in str(excinfo.value)
     with pytest.raises(TypeError) as excinfo:
         m.iss1105_col(np.ones((1, 7)))
-    assert "incompatible function arguments" in str(excinfo)
+    assert "incompatible function arguments" in str(excinfo.value)
 
 
 def test_custom_operator_new():
diff --git a/wrap/python/pybind11/tests/test_embed/CMakeLists.txt b/wrap/pybind11/tests/test_embed/CMakeLists.txt
similarity index 100%
rename from wrap/python/pybind11/tests/test_embed/CMakeLists.txt
rename to wrap/pybind11/tests/test_embed/CMakeLists.txt
diff --git a/wrap/python/pybind11/tests/test_embed/catch.cpp b/wrap/pybind11/tests/test_embed/catch.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_embed/catch.cpp
rename to wrap/pybind11/tests/test_embed/catch.cpp
diff --git a/wrap/python/pybind11/tests/test_embed/external_module.cpp b/wrap/pybind11/tests/test_embed/external_module.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_embed/external_module.cpp
rename to wrap/pybind11/tests/test_embed/external_module.cpp
diff --git a/wrap/python/pybind11/tests/test_embed/test_interpreter.cpp b/wrap/pybind11/tests/test_embed/test_interpreter.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_embed/test_interpreter.cpp
rename to wrap/pybind11/tests/test_embed/test_interpreter.cpp
diff --git a/wrap/python/pybind11/tests/test_embed/test_interpreter.py b/wrap/pybind11/tests/test_embed/test_interpreter.py
similarity index 89%
rename from wrap/python/pybind11/tests/test_embed/test_interpreter.py
rename to wrap/pybind11/tests/test_embed/test_interpreter.py
index 26a047921..6174ede44 100644
--- a/wrap/python/pybind11/tests/test_embed/test_interpreter.py
+++ b/wrap/pybind11/tests/test_embed/test_interpreter.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 from widget_module import Widget
 
 
diff --git a/wrap/python/pybind11/tests/test_enum.cpp b/wrap/pybind11/tests/test_enum.cpp
similarity index 96%
rename from wrap/python/pybind11/tests/test_enum.cpp
rename to wrap/pybind11/tests/test_enum.cpp
index 498a00e16..315308920 100644
--- a/wrap/python/pybind11/tests/test_enum.cpp
+++ b/wrap/pybind11/tests/test_enum.cpp
@@ -13,11 +13,13 @@ TEST_SUBMODULE(enums, m) {
     // test_unscoped_enum
     enum UnscopedEnum {
         EOne = 1,
-        ETwo
+        ETwo,
+        EThree
     };
     py::enum_<UnscopedEnum>(m, "UnscopedEnum", py::arithmetic(), "An unscoped enumeration")
         .value("EOne", EOne, "Docstring for EOne")
         .value("ETwo", ETwo, "Docstring for ETwo")
+        .value("EThree", EThree, "Docstring for EThree")
         .export_values();
 
     // test_scoped_enum
diff --git a/wrap/python/pybind11/tests/test_enum.py b/wrap/pybind11/tests/test_enum.py
similarity index 77%
rename from wrap/python/pybind11/tests/test_enum.py
rename to wrap/pybind11/tests/test_enum.py
index d0989adcd..bfaa193e9 100644
--- a/wrap/python/pybind11/tests/test_enum.py
+++ b/wrap/pybind11/tests/test_enum.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import enums as m
 
@@ -21,7 +22,7 @@ def test_unscoped_enum():
 
     # __members__ property
     assert m.UnscopedEnum.__members__ == \
-        {"EOne": m.UnscopedEnum.EOne, "ETwo": m.UnscopedEnum.ETwo}
+        {"EOne": m.UnscopedEnum.EOne, "ETwo": m.UnscopedEnum.ETwo, "EThree": m.UnscopedEnum.EThree}
     # __members__ readonly
     with pytest.raises(AttributeError):
         m.UnscopedEnum.__members__ = {}
@@ -29,23 +30,18 @@ def test_unscoped_enum():
     foo = m.UnscopedEnum.__members__
     foo["bar"] = "baz"
     assert m.UnscopedEnum.__members__ == \
-        {"EOne": m.UnscopedEnum.EOne, "ETwo": m.UnscopedEnum.ETwo}
+        {"EOne": m.UnscopedEnum.EOne, "ETwo": m.UnscopedEnum.ETwo, "EThree": m.UnscopedEnum.EThree}
 
-    assert m.UnscopedEnum.__doc__ == \
-        '''An unscoped enumeration
+    for docstring_line in '''An unscoped enumeration
 
 Members:
 
   EOne : Docstring for EOne
 
-  ETwo : Docstring for ETwo''' or m.UnscopedEnum.__doc__ == \
-        '''An unscoped enumeration
-
-Members:
-
   ETwo : Docstring for ETwo
 
-  EOne : Docstring for EOne'''
+  EThree : Docstring for EThree'''.split('\n'):
+        assert docstring_line in m.UnscopedEnum.__doc__
 
     # Unscoped enums will accept ==/!= int comparisons
     y = m.UnscopedEnum.ETwo
@@ -53,6 +49,38 @@ Members:
     assert 2 == y
     assert y != 3
     assert 3 != y
+    # Compare with None
+    assert (y != None)  # noqa: E711
+    assert not (y == None)  # noqa: E711
+    # Compare with an object
+    assert (y != object())
+    assert not (y == object())
+    # Compare with string
+    assert y != "2"
+    assert "2" != y
+    assert not ("2" == y)
+    assert not (y == "2")
+
+    with pytest.raises(TypeError):
+        y < object()
+
+    with pytest.raises(TypeError):
+        y <= object()
+
+    with pytest.raises(TypeError):
+        y > object()
+
+    with pytest.raises(TypeError):
+        y >= object()
+
+    with pytest.raises(TypeError):
+        y | object()
+
+    with pytest.raises(TypeError):
+        y & object()
+
+    with pytest.raises(TypeError):
+        y ^ object()
 
     assert int(m.UnscopedEnum.ETwo) == 2
     assert str(m.UnscopedEnum(2)) == "UnscopedEnum.ETwo"
@@ -71,6 +99,11 @@ Members:
     assert not (m.UnscopedEnum.ETwo < m.UnscopedEnum.EOne)
     assert not (2 < m.UnscopedEnum.EOne)
 
+    # arithmetic
+    assert m.UnscopedEnum.EOne & m.UnscopedEnum.EThree == m.UnscopedEnum.EOne
+    assert m.UnscopedEnum.EOne | m.UnscopedEnum.ETwo == m.UnscopedEnum.EThree
+    assert m.UnscopedEnum.EOne ^ m.UnscopedEnum.EThree == m.UnscopedEnum.ETwo
+
 
 def test_scoped_enum():
     assert m.test_scoped_enum(m.ScopedEnum.Three) == "ScopedEnum::Three"
@@ -82,6 +115,12 @@ def test_scoped_enum():
     assert not 3 == z
     assert z != 3
     assert 3 != z
+    # Compare with None
+    assert (z != None)  # noqa: E711
+    assert not (z == None)  # noqa: E711
+    # Compare with an object
+    assert (z != object())
+    assert not (z == object())
     # Scoped enums will *NOT* accept >, <, >= and <= int comparisons (Will throw exceptions)
     with pytest.raises(TypeError):
         z > 3
@@ -140,6 +179,7 @@ def test_binary_operators():
     assert int(m.Flags.Read | m.Flags.Execute) == 5
     assert int(m.Flags.Write | m.Flags.Execute) == 3
     assert int(m.Flags.Write | 1) == 3
+    assert ~m.Flags.Write == -3
 
     state = m.Flags.Read | m.Flags.Write
     assert (state & m.Flags.Read) != 0
diff --git a/wrap/python/pybind11/tests/test_eval.cpp b/wrap/pybind11/tests/test_eval.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_eval.cpp
rename to wrap/pybind11/tests/test_eval.cpp
diff --git a/wrap/python/pybind11/tests/test_eval.py b/wrap/pybind11/tests/test_eval.py
similarity index 82%
rename from wrap/python/pybind11/tests/test_eval.py
rename to wrap/pybind11/tests/test_eval.py
index bda4ef6bf..66bec55f8 100644
--- a/wrap/python/pybind11/tests/test_eval.py
+++ b/wrap/pybind11/tests/test_eval.py
@@ -1,4 +1,6 @@
+# -*- coding: utf-8 -*-
 import os
+import pytest
 from pybind11_tests import eval_ as m
 
 
@@ -10,8 +12,12 @@ def test_evals(capture):
     assert m.test_eval()
     assert m.test_eval_single_statement()
 
+    assert m.test_eval_failure()
+
+
+@pytest.unsupported_on_pypy3
+def test_eval_file():
     filename = os.path.join(os.path.dirname(__file__), "test_eval_call.py")
     assert m.test_eval_file(filename)
 
-    assert m.test_eval_failure()
     assert m.test_eval_file_failure()
diff --git a/wrap/python/pybind11/tests/test_eval_call.py b/wrap/pybind11/tests/test_eval_call.py
similarity index 83%
rename from wrap/python/pybind11/tests/test_eval_call.py
rename to wrap/pybind11/tests/test_eval_call.py
index 53c7e721f..d42a0a6d3 100644
--- a/wrap/python/pybind11/tests/test_eval_call.py
+++ b/wrap/pybind11/tests/test_eval_call.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 # This file is called from 'test_eval.py'
 
 if 'call_test2' in locals():
diff --git a/wrap/python/pybind11/tests/test_exceptions.cpp b/wrap/pybind11/tests/test_exceptions.cpp
similarity index 98%
rename from wrap/python/pybind11/tests/test_exceptions.cpp
rename to wrap/pybind11/tests/test_exceptions.cpp
index d30139037..56cd9bc48 100644
--- a/wrap/python/pybind11/tests/test_exceptions.cpp
+++ b/wrap/pybind11/tests/test_exceptions.cpp
@@ -116,6 +116,7 @@ TEST_SUBMODULE(exceptions, m) {
     m.def("throws5", []() { throw MyException5("this is a helper-defined translated exception"); });
     m.def("throws5_1", []() { throw MyException5_1("MyException5 subclass"); });
     m.def("throws_logic_error", []() { throw std::logic_error("this error should fall through to the standard handler"); });
+    m.def("throws_overflow_error", []() {throw std::overflow_error(""); });
     m.def("exception_matches", []() {
         py::dict foo;
         try {
diff --git a/wrap/python/pybind11/tests/test_exceptions.py b/wrap/pybind11/tests/test_exceptions.py
similarity index 97%
rename from wrap/python/pybind11/tests/test_exceptions.py
rename to wrap/pybind11/tests/test_exceptions.py
index 6edff9fe4..053e7d4a2 100644
--- a/wrap/python/pybind11/tests/test_exceptions.py
+++ b/wrap/pybind11/tests/test_exceptions.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 
 from pybind11_tests import exceptions as m
@@ -79,6 +80,10 @@ def test_custom(msg):
         m.throws_logic_error()
     assert msg(excinfo.value) == "this error should fall through to the standard handler"
 
+    # OverFlow error translation.
+    with pytest.raises(OverflowError) as excinfo:
+        m.throws_overflow_error()
+
     # Can we handle a helper-declared exception?
     with pytest.raises(m.MyException5) as excinfo:
         m.throws5()
diff --git a/wrap/python/pybind11/tests/test_factory_constructors.cpp b/wrap/pybind11/tests/test_factory_constructors.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_factory_constructors.cpp
rename to wrap/pybind11/tests/test_factory_constructors.cpp
diff --git a/wrap/python/pybind11/tests/test_factory_constructors.py b/wrap/pybind11/tests/test_factory_constructors.py
similarity index 99%
rename from wrap/python/pybind11/tests/test_factory_constructors.py
rename to wrap/pybind11/tests/test_factory_constructors.py
index 78a3910ad..49e6f4f33 100644
--- a/wrap/python/pybind11/tests/test_factory_constructors.py
+++ b/wrap/pybind11/tests/test_factory_constructors.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 import re
 
diff --git a/wrap/python/pybind11/tests/test_gil_scoped.cpp b/wrap/pybind11/tests/test_gil_scoped.cpp
similarity index 78%
rename from wrap/python/pybind11/tests/test_gil_scoped.cpp
rename to wrap/pybind11/tests/test_gil_scoped.cpp
index cb0010ee6..76c17fdc7 100644
--- a/wrap/python/pybind11/tests/test_gil_scoped.cpp
+++ b/wrap/pybind11/tests/test_gil_scoped.cpp
@@ -41,4 +41,12 @@ TEST_SUBMODULE(gil_scoped, m) {
           [](VirtClass &virt) { virt.virtual_func(); });
     m.def("test_callback_pure_virtual_func",
           [](VirtClass &virt) { virt.pure_virtual_func(); });
+    m.def("test_cross_module_gil",
+          []() {
+              auto cm = py::module::import("cross_module_gil_utils");
+              auto gil_acquire = reinterpret_cast<void (*)()>(
+                  PyLong_AsVoidPtr(cm.attr("gil_acquire_funcaddr").ptr()));
+              py::gil_scoped_release gil_release;
+              gil_acquire();
+          });
 }
diff --git a/wrap/python/pybind11/tests/test_gil_scoped.py b/wrap/pybind11/tests/test_gil_scoped.py
similarity index 92%
rename from wrap/python/pybind11/tests/test_gil_scoped.py
rename to wrap/pybind11/tests/test_gil_scoped.py
index 2c72fc6d6..1307712ad 100644
--- a/wrap/python/pybind11/tests/test_gil_scoped.py
+++ b/wrap/pybind11/tests/test_gil_scoped.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import multiprocessing
 import threading
 from pybind11_tests import gil_scoped as m
@@ -78,3 +79,8 @@ def test_python_to_cpp_to_python_from_process():
     This test is for completion, but it was never an issue.
     """
     assert _run_in_process(_python_to_cpp_to_python) == 0
+
+
+def test_cross_module_gil():
+    """Makes sure that the GIL can be acquired by another module from a GIL-released state."""
+    m.test_cross_module_gil()  # Should not raise a SIGSEGV
diff --git a/wrap/python/pybind11/tests/test_iostream.cpp b/wrap/pybind11/tests/test_iostream.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_iostream.cpp
rename to wrap/pybind11/tests/test_iostream.cpp
diff --git a/wrap/python/pybind11/tests/test_iostream.py b/wrap/pybind11/tests/test_iostream.py
similarity index 99%
rename from wrap/python/pybind11/tests/test_iostream.py
rename to wrap/pybind11/tests/test_iostream.py
index 27095b270..7ac4fcece 100644
--- a/wrap/python/pybind11/tests/test_iostream.py
+++ b/wrap/pybind11/tests/test_iostream.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 from pybind11_tests import iostream as m
 import sys
 
diff --git a/wrap/python/pybind11/tests/test_kwargs_and_defaults.cpp b/wrap/pybind11/tests/test_kwargs_and_defaults.cpp
similarity index 75%
rename from wrap/python/pybind11/tests/test_kwargs_and_defaults.cpp
rename to wrap/pybind11/tests/test_kwargs_and_defaults.cpp
index 6563fb9ad..8f095fe4a 100644
--- a/wrap/python/pybind11/tests/test_kwargs_and_defaults.cpp
+++ b/wrap/pybind11/tests/test_kwargs_and_defaults.cpp
@@ -94,6 +94,30 @@ TEST_SUBMODULE(kwargs_and_defaults, m) {
 //    m.def("bad_args6", [](py::args, py::args) {});
 //    m.def("bad_args7", [](py::kwargs, py::kwargs) {});
 
+    // test_keyword_only_args
+    m.def("kwonly_all", [](int i, int j) { return py::make_tuple(i, j); },
+            py::kwonly(), py::arg("i"), py::arg("j"));
+    m.def("kwonly_some", [](int i, int j, int k) { return py::make_tuple(i, j, k); },
+            py::arg(), py::kwonly(), py::arg("j"), py::arg("k"));
+    m.def("kwonly_with_defaults", [](int i, int j, int k, int z) { return py::make_tuple(i, j, k, z); },
+            py::arg() = 3, "j"_a = 4, py::kwonly(), "k"_a = 5, "z"_a);
+    m.def("kwonly_mixed", [](int i, int j) { return py::make_tuple(i, j); },
+            "i"_a, py::kwonly(), "j"_a);
+    m.def("kwonly_plus_more", [](int i, int j, int k, py::kwargs kwargs) {
+            return py::make_tuple(i, j, k, kwargs); },
+            py::arg() /* positional */, py::arg("j") = -1 /* both */, py::kwonly(), py::arg("k") /* kw-only */);
+
+    m.def("register_invalid_kwonly", [](py::module m) {
+        m.def("bad_kwonly", [](int i, int j) { return py::make_tuple(i, j); },
+                py::kwonly(), py::arg() /* invalid unnamed argument */, "j"_a);
+    });
+
+    // These should fail to compile:
+    // argument annotations are required when using kwonly
+//    m.def("bad_kwonly1", [](int) {}, py::kwonly());
+    // can't specify both `py::kwonly` and a `py::args` argument
+//    m.def("bad_kwonly2", [](int i, py::args) {}, py::kwonly(), "i"_a);
+
     // test_function_signatures (along with most of the above)
     struct KWClass { void foo(int, float) {} };
     py::class_<KWClass>(m, "KWClass")
diff --git a/wrap/python/pybind11/tests/test_kwargs_and_defaults.py b/wrap/pybind11/tests/test_kwargs_and_defaults.py
similarity index 80%
rename from wrap/python/pybind11/tests/test_kwargs_and_defaults.py
rename to wrap/pybind11/tests/test_kwargs_and_defaults.py
index 27a05a024..6c72a9368 100644
--- a/wrap/python/pybind11/tests/test_kwargs_and_defaults.py
+++ b/wrap/pybind11/tests/test_kwargs_and_defaults.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import kwargs_and_defaults as m
 
@@ -107,6 +108,44 @@ def test_mixed_args_and_kwargs(msg):
     """  # noqa: E501 line too long
 
 
+def test_keyword_only_args(msg):
+    assert m.kwonly_all(i=1, j=2) == (1, 2)
+    assert m.kwonly_all(j=1, i=2) == (2, 1)
+
+    with pytest.raises(TypeError) as excinfo:
+        assert m.kwonly_all(i=1) == (1,)
+    assert "incompatible function arguments" in str(excinfo.value)
+
+    with pytest.raises(TypeError) as excinfo:
+        assert m.kwonly_all(1, 2) == (1, 2)
+    assert "incompatible function arguments" in str(excinfo.value)
+
+    assert m.kwonly_some(1, k=3, j=2) == (1, 2, 3)
+
+    assert m.kwonly_with_defaults(z=8) == (3, 4, 5, 8)
+    assert m.kwonly_with_defaults(2, z=8) == (2, 4, 5, 8)
+    assert m.kwonly_with_defaults(2, j=7, k=8, z=9) == (2, 7, 8, 9)
+    assert m.kwonly_with_defaults(2, 7, z=9, k=8) == (2, 7, 8, 9)
+
+    assert m.kwonly_mixed(1, j=2) == (1, 2)
+    assert m.kwonly_mixed(j=2, i=3) == (3, 2)
+    assert m.kwonly_mixed(i=2, j=3) == (2, 3)
+
+    assert m.kwonly_plus_more(4, 5, k=6, extra=7) == (4, 5, 6, {'extra': 7})
+    assert m.kwonly_plus_more(3, k=5, j=4, extra=6) == (3, 4, 5, {'extra': 6})
+    assert m.kwonly_plus_more(2, k=3, extra=4) == (2, -1, 3, {'extra': 4})
+
+    with pytest.raises(TypeError) as excinfo:
+        assert m.kwonly_mixed(i=1) == (1,)
+    assert "incompatible function arguments" in str(excinfo.value)
+
+    with pytest.raises(RuntimeError) as excinfo:
+        m.register_invalid_kwonly(m)
+    assert msg(excinfo.value) == """
+        arg(): cannot specify an unnamed argument after an kwonly() annotation
+    """
+
+
 def test_args_refcount():
     """Issue/PR #1216 - py::args elements get double-inc_ref()ed when combined with regular
     arguments"""
diff --git a/wrap/python/pybind11/tests/test_local_bindings.cpp b/wrap/pybind11/tests/test_local_bindings.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_local_bindings.cpp
rename to wrap/pybind11/tests/test_local_bindings.cpp
diff --git a/wrap/python/pybind11/tests/test_local_bindings.py b/wrap/pybind11/tests/test_local_bindings.py
similarity index 97%
rename from wrap/python/pybind11/tests/test_local_bindings.py
rename to wrap/pybind11/tests/test_local_bindings.py
index b3dc3619c..913cf0ee5 100644
--- a/wrap/python/pybind11/tests/test_local_bindings.py
+++ b/wrap/pybind11/tests/test_local_bindings.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 
 from pybind11_tests import local_bindings as m
@@ -152,6 +153,7 @@ def test_internal_locals_differ():
     assert m.local_cpp_types_addr() != cm.local_cpp_types_addr()
 
 
+@pytest.bug_in_pypy
 def test_stl_caster_vs_stl_bind(msg):
     """One module uses a generic vector caster from `<pybind11/stl.h>` while the other
     exports `std::vector<int>` via `py:bind_vector` and `py::module_local`"""
@@ -220,7 +222,7 @@ def test_cross_module_calls():
     c, d = m.MixGL2(3), cm.MixGL2(4)
     with pytest.raises(TypeError) as excinfo:
         m.get_gl_value(c)
-    assert "incompatible function arguments" in str(excinfo)
+    assert "incompatible function arguments" in str(excinfo.value)
     with pytest.raises(TypeError) as excinfo:
         m.get_gl_value(d)
-    assert "incompatible function arguments" in str(excinfo)
+    assert "incompatible function arguments" in str(excinfo.value)
diff --git a/wrap/python/pybind11/tests/test_methods_and_attributes.cpp b/wrap/pybind11/tests/test_methods_and_attributes.cpp
similarity index 75%
rename from wrap/python/pybind11/tests/test_methods_and_attributes.cpp
rename to wrap/pybind11/tests/test_methods_and_attributes.cpp
index fde152b9f..901f2046c 100644
--- a/wrap/python/pybind11/tests/test_methods_and_attributes.cpp
+++ b/wrap/pybind11/tests/test_methods_and_attributes.cpp
@@ -11,11 +11,17 @@
 #include "pybind11_tests.h"
 #include "constructor_stats.h"
 
+#if !defined(PYBIND11_OVERLOAD_CAST)
+template <typename... Args>
+using overload_cast_ = pybind11::detail::overload_cast_impl<Args...>;
+#endif
+
 class ExampleMandA {
 public:
     ExampleMandA() { print_default_created(this); }
     ExampleMandA(int value) : value(value) { print_created(this, value); }
     ExampleMandA(const ExampleMandA &e) : value(e.value) { print_copy_created(this); }
+    ExampleMandA(std::string&&) {}
     ExampleMandA(ExampleMandA &&e) : value(e.value) { print_move_created(this); }
     ~ExampleMandA() { print_destroyed(this); }
 
@@ -38,6 +44,8 @@ public:
     void add9(int *other) { value += *other; }                      // passing by pointer
     void add10(const int *other) { value += *other; }               // passing by const pointer
 
+    void consume_str(std::string&&) {}
+
     ExampleMandA self1() { return *this; }                          // return by value
     ExampleMandA &self2() { return *this; }                         // return by reference
     const ExampleMandA &self3() { return *this; }                   // return by const reference
@@ -100,76 +108,6 @@ struct TestPropRVP {
 UserType TestPropRVP::sv1(1);
 UserType TestPropRVP::sv2(1);
 
-// py::arg/py::arg_v testing: these arguments just record their argument when invoked
-class ArgInspector1 { public: std::string arg = "(default arg inspector 1)"; };
-class ArgInspector2 { public: std::string arg = "(default arg inspector 2)"; };
-class ArgAlwaysConverts { };
-namespace pybind11 { namespace detail {
-template <> struct type_caster<ArgInspector1> {
-public:
-    PYBIND11_TYPE_CASTER(ArgInspector1, _("ArgInspector1"));
-
-    bool load(handle src, bool convert) {
-        value.arg = "loading ArgInspector1 argument " +
-            std::string(convert ? "WITH" : "WITHOUT") + " conversion allowed.  "
-            "Argument value = " + (std::string) str(src);
-        return true;
-    }
-
-    static handle cast(const ArgInspector1 &src, return_value_policy, handle) {
-        return str(src.arg).release();
-    }
-};
-template <> struct type_caster<ArgInspector2> {
-public:
-    PYBIND11_TYPE_CASTER(ArgInspector2, _("ArgInspector2"));
-
-    bool load(handle src, bool convert) {
-        value.arg = "loading ArgInspector2 argument " +
-            std::string(convert ? "WITH" : "WITHOUT") + " conversion allowed.  "
-            "Argument value = " + (std::string) str(src);
-        return true;
-    }
-
-    static handle cast(const ArgInspector2 &src, return_value_policy, handle) {
-        return str(src.arg).release();
-    }
-};
-template <> struct type_caster<ArgAlwaysConverts> {
-public:
-    PYBIND11_TYPE_CASTER(ArgAlwaysConverts, _("ArgAlwaysConverts"));
-
-    bool load(handle, bool convert) {
-        return convert;
-    }
-
-    static handle cast(const ArgAlwaysConverts &, return_value_policy, handle) {
-        return py::none().release();
-    }
-};
-}}
-
-// test_custom_caster_destruction
-class DestructionTester {
-public:
-    DestructionTester() { print_default_created(this); }
-    ~DestructionTester() { print_destroyed(this); }
-    DestructionTester(const DestructionTester &) { print_copy_created(this); }
-    DestructionTester(DestructionTester &&) { print_move_created(this); }
-    DestructionTester &operator=(const DestructionTester &) { print_copy_assigned(this); return *this; }
-    DestructionTester &operator=(DestructionTester &&) { print_move_assigned(this); return *this; }
-};
-namespace pybind11 { namespace detail {
-template <> struct type_caster<DestructionTester> {
-    PYBIND11_TYPE_CASTER(DestructionTester, _("DestructionTester"));
-    bool load(handle, bool) { return true; }
-
-    static handle cast(const DestructionTester &, return_value_policy, handle) {
-        return py::bool_(true).release();
-    }
-};
-}}
-
 // Test None-allowed py::arg argument policy
 class NoneTester { public: int answer = 42; };
 int none1(const NoneTester &obj) { return obj.answer; }
@@ -202,11 +140,20 @@ public:
     double sum() const { return rw_value + ro_value; }
 };
 
+// Test explicit lvalue ref-qualification
+struct RefQualified {
+    int value = 0;
+
+    void refQualified(int other) & { value += other; }
+    int constRefQualified(int other) const & { return value + other; }
+};
+
 TEST_SUBMODULE(methods_and_attributes, m) {
     // test_methods_and_attributes
     py::class_<ExampleMandA> emna(m, "ExampleMandA");
     emna.def(py::init<>())
         .def(py::init<int>())
+        .def(py::init<std::string&&>())
         .def(py::init<const ExampleMandA&>())
         .def("add1", &ExampleMandA::add1)
         .def("add2", &ExampleMandA::add2)
@@ -218,6 +165,7 @@ TEST_SUBMODULE(methods_and_attributes, m) {
         .def("add8", &ExampleMandA::add8)
         .def("add9", &ExampleMandA::add9)
         .def("add10", &ExampleMandA::add10)
+        .def("consume_str", &ExampleMandA::consume_str)
         .def("self1", &ExampleMandA::self1)
         .def("self2", &ExampleMandA::self2)
         .def("self3", &ExampleMandA::self3)
@@ -242,15 +190,16 @@ TEST_SUBMODULE(methods_and_attributes, m) {
         .def("overloaded_const", py::overload_cast<int,     int>(&ExampleMandA::overloaded, py::const_))
         .def("overloaded_const", py::overload_cast<float, float>(&ExampleMandA::overloaded, py::const_))
 #else
-        .def("overloaded", static_cast<py::str (ExampleMandA::*)()>(&ExampleMandA::overloaded))
-        .def("overloaded", static_cast<py::str (ExampleMandA::*)(int)>(&ExampleMandA::overloaded))
-        .def("overloaded", static_cast<py::str (ExampleMandA::*)(int,   float)>(&ExampleMandA::overloaded))
+        // Use both the traditional static_cast method and the C++11 compatible overload_cast_
+        .def("overloaded", overload_cast_<>()(&ExampleMandA::overloaded))
+        .def("overloaded", overload_cast_<int>()(&ExampleMandA::overloaded))
+        .def("overloaded", overload_cast_<int,   float>()(&ExampleMandA::overloaded))
         .def("overloaded", static_cast<py::str (ExampleMandA::*)(float,   int)>(&ExampleMandA::overloaded))
         .def("overloaded", static_cast<py::str (ExampleMandA::*)(int,     int)>(&ExampleMandA::overloaded))
         .def("overloaded", static_cast<py::str (ExampleMandA::*)(float, float)>(&ExampleMandA::overloaded))
-        .def("overloaded_float", static_cast<py::str (ExampleMandA::*)(float, float)>(&ExampleMandA::overloaded))
-        .def("overloaded_const", static_cast<py::str (ExampleMandA::*)(int         ) const>(&ExampleMandA::overloaded))
-        .def("overloaded_const", static_cast<py::str (ExampleMandA::*)(int,   float) const>(&ExampleMandA::overloaded))
+        .def("overloaded_float", overload_cast_<float, float>()(&ExampleMandA::overloaded))
+        .def("overloaded_const", overload_cast_<int         >()(&ExampleMandA::overloaded, py::const_))
+        .def("overloaded_const", overload_cast_<int,   float>()(&ExampleMandA::overloaded, py::const_))
         .def("overloaded_const", static_cast<py::str (ExampleMandA::*)(float,   int) const>(&ExampleMandA::overloaded))
         .def("overloaded_const", static_cast<py::str (ExampleMandA::*)(int,     int) const>(&ExampleMandA::overloaded))
         .def("overloaded_const", static_cast<py::str (ExampleMandA::*)(float, float) const>(&ExampleMandA::overloaded))
@@ -350,33 +299,6 @@ TEST_SUBMODULE(methods_and_attributes, m) {
         .def(py::init());
 #endif
 
-    // test_noconvert_args
-    //
-    // Test converting.  The ArgAlwaysConverts is just there to make the first no-conversion pass
-    // fail so that our call always ends up happening via the second dispatch (the one that allows
-    // some conversion).
-    class ArgInspector {
-    public:
-        ArgInspector1 f(ArgInspector1 a, ArgAlwaysConverts) { return a; }
-        std::string g(ArgInspector1 a, const ArgInspector1 &b, int c, ArgInspector2 *d, ArgAlwaysConverts) {
-            return a.arg + "\n" + b.arg + "\n" + std::to_string(c) + "\n" + d->arg;
-        }
-        static ArgInspector2 h(ArgInspector2 a, ArgAlwaysConverts) { return a; }
-    };
-    py::class_<ArgInspector>(m, "ArgInspector")
-        .def(py::init<>())
-        .def("f", &ArgInspector::f, py::arg(), py::arg() = ArgAlwaysConverts())
-        .def("g", &ArgInspector::g, "a"_a.noconvert(), "b"_a, "c"_a.noconvert()=13, "d"_a=ArgInspector2(), py::arg() = ArgAlwaysConverts())
-        .def_static("h", &ArgInspector::h, py::arg().noconvert(), py::arg() = ArgAlwaysConverts())
-        ;
-    m.def("arg_inspect_func", [](ArgInspector2 a, ArgInspector1 b, ArgAlwaysConverts) { return a.arg + "\n" + b.arg; },
-            py::arg().noconvert(false), py::arg_v(nullptr, ArgInspector1()).noconvert(true), py::arg() = ArgAlwaysConverts());
-
-    m.def("floats_preferred", [](double f) { return 0.5 * f; }, py::arg("f"));
-    m.def("floats_only", [](double f) { return 0.5 * f; }, py::arg("f").noconvert());
-    m.def("ints_preferred", [](int i) { return i / 2; }, py::arg("i"));
-    m.def("ints_only", [](int i) { return i / 2; }, py::arg("i").noconvert());
-
     // test_bad_arg_default
     // Issue/PR #648: bad arg default debugging output
 #if !defined(NDEBUG)
@@ -440,15 +362,10 @@ TEST_SUBMODULE(methods_and_attributes, m) {
     using Adapted = decltype(py::method_adaptor<RegisteredDerived>(&RegisteredDerived::do_nothing));
     static_assert(std::is_same<Adapted, void (RegisteredDerived::*)() const>::value, "");
 
-    // test_custom_caster_destruction
-    // Test that `take_ownership` works on types with a custom type caster when given a pointer
-
-    // default policy: don't take ownership:
-    m.def("custom_caster_no_destroy", []() { static auto *dt = new DestructionTester(); return dt; });
-
-    m.def("custom_caster_destroy", []() { return new DestructionTester(); },
-            py::return_value_policy::take_ownership); // Takes ownership: destroy when finished
-    m.def("custom_caster_destroy_const", []() -> const DestructionTester * { return new DestructionTester(); },
-            py::return_value_policy::take_ownership); // Likewise (const doesn't inhibit destruction)
-    m.def("destruction_tester_cstats", &ConstructorStats::get<DestructionTester>, py::return_value_policy::reference);
+    // test_methods_and_attributes
+    py::class_<RefQualified>(m, "RefQualified")
+        .def(py::init<>())
+        .def_readonly("value", &RefQualified::value)
+        .def("refQualified", &RefQualified::refQualified)
+        .def("constRefQualified", &RefQualified::constRefQualified);
 }
diff --git a/wrap/python/pybind11/tests/test_methods_and_attributes.py b/wrap/pybind11/tests/test_methods_and_attributes.py
similarity index 77%
rename from wrap/python/pybind11/tests/test_methods_and_attributes.py
rename to wrap/pybind11/tests/test_methods_and_attributes.py
index 86b2c3b4b..25a01c718 100644
--- a/wrap/python/pybind11/tests/test_methods_and_attributes.py
+++ b/wrap/pybind11/tests/test_methods_and_attributes.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import methods_and_attributes as m
 from pybind11_tests import ConstructorStats
@@ -58,8 +59,8 @@ def test_methods_and_attributes():
     assert cstats.alive() == 0
     assert cstats.values() == ["32"]
     assert cstats.default_constructions == 1
-    assert cstats.copy_constructions == 3
-    assert cstats.move_constructions >= 1
+    assert cstats.copy_constructions == 2
+    assert cstats.move_constructions >= 2
     assert cstats.copy_assignments == 0
     assert cstats.move_assignments == 0
 
@@ -100,32 +101,32 @@ def test_properties():
 
     with pytest.raises(AttributeError) as excinfo:
         dummy = instance.def_property_writeonly  # noqa: F841 unused var
-    assert "unreadable attribute" in str(excinfo)
+    assert "unreadable attribute" in str(excinfo.value)
 
     instance.def_property_writeonly = 4
     assert instance.def_property_readonly == 4
 
     with pytest.raises(AttributeError) as excinfo:
         dummy = instance.def_property_impossible  # noqa: F841 unused var
-    assert "unreadable attribute" in str(excinfo)
+    assert "unreadable attribute" in str(excinfo.value)
 
     with pytest.raises(AttributeError) as excinfo:
         instance.def_property_impossible = 5
-    assert "can't set attribute" in str(excinfo)
+    assert "can't set attribute" in str(excinfo.value)
 
 
 def test_static_properties():
     assert m.TestProperties.def_readonly_static == 1
     with pytest.raises(AttributeError) as excinfo:
         m.TestProperties.def_readonly_static = 2
-    assert "can't set attribute" in str(excinfo)
+    assert "can't set attribute" in str(excinfo.value)
 
     m.TestProperties.def_readwrite_static = 2
     assert m.TestProperties.def_readwrite_static == 2
 
     with pytest.raises(AttributeError) as excinfo:
         dummy = m.TestProperties.def_writeonly_static  # noqa: F841 unused var
-    assert "unreadable attribute" in str(excinfo)
+    assert "unreadable attribute" in str(excinfo.value)
 
     m.TestProperties.def_writeonly_static = 3
     assert m.TestProperties.def_readonly_static == 3
@@ -133,14 +134,14 @@ def test_static_properties():
     assert m.TestProperties.def_property_readonly_static == 3
     with pytest.raises(AttributeError) as excinfo:
         m.TestProperties.def_property_readonly_static = 99
-    assert "can't set attribute" in str(excinfo)
+    assert "can't set attribute" in str(excinfo.value)
 
     m.TestProperties.def_property_static = 4
     assert m.TestProperties.def_property_static == 4
 
     with pytest.raises(AttributeError) as excinfo:
         dummy = m.TestProperties.def_property_writeonly_static
-    assert "unreadable attribute" in str(excinfo)
+    assert "unreadable attribute" in str(excinfo.value)
 
     m.TestProperties.def_property_writeonly_static = 5
     assert m.TestProperties.def_property_static == 5
@@ -158,7 +159,7 @@ def test_static_properties():
 
     with pytest.raises(AttributeError) as excinfo:
         dummy = instance.def_property_writeonly_static  # noqa: F841 unused var
-    assert "unreadable attribute" in str(excinfo)
+    assert "unreadable attribute" in str(excinfo.value)
 
     instance.def_property_writeonly_static = 4
     assert instance.def_property_static == 4
@@ -321,69 +322,6 @@ def test_cyclic_gc():
     assert cstats.alive() == 0
 
 
-def test_noconvert_args(msg):
-    a = m.ArgInspector()
-    assert msg(a.f("hi")) == """
-        loading ArgInspector1 argument WITH conversion allowed.  Argument value = hi
-    """
-    assert msg(a.g("this is a", "this is b")) == """
-        loading ArgInspector1 argument WITHOUT conversion allowed.  Argument value = this is a
-        loading ArgInspector1 argument WITH conversion allowed.  Argument value = this is b
-        13
-        loading ArgInspector2 argument WITH conversion allowed.  Argument value = (default arg inspector 2)
-    """  # noqa: E501 line too long
-    assert msg(a.g("this is a", "this is b", 42)) == """
-        loading ArgInspector1 argument WITHOUT conversion allowed.  Argument value = this is a
-        loading ArgInspector1 argument WITH conversion allowed.  Argument value = this is b
-        42
-        loading ArgInspector2 argument WITH conversion allowed.  Argument value = (default arg inspector 2)
-    """  # noqa: E501 line too long
-    assert msg(a.g("this is a", "this is b", 42, "this is d")) == """
-        loading ArgInspector1 argument WITHOUT conversion allowed.  Argument value = this is a
-        loading ArgInspector1 argument WITH conversion allowed.  Argument value = this is b
-        42
-        loading ArgInspector2 argument WITH conversion allowed.  Argument value = this is d
-    """
-    assert (a.h("arg 1") ==
-            "loading ArgInspector2 argument WITHOUT conversion allowed.  Argument value = arg 1")
-    assert msg(m.arg_inspect_func("A1", "A2")) == """
-        loading ArgInspector2 argument WITH conversion allowed.  Argument value = A1
-        loading ArgInspector1 argument WITHOUT conversion allowed.  Argument value = A2
-    """
-
-    assert m.floats_preferred(4) == 2.0
-    assert m.floats_only(4.0) == 2.0
-    with pytest.raises(TypeError) as excinfo:
-        m.floats_only(4)
-    assert msg(excinfo.value) == """
-        floats_only(): incompatible function arguments. The following argument types are supported:
-            1. (f: float) -> float
-
-        Invoked with: 4
-    """
-
-    assert m.ints_preferred(4) == 2
-    assert m.ints_preferred(True) == 0
-    with pytest.raises(TypeError) as excinfo:
-        m.ints_preferred(4.0)
-    assert msg(excinfo.value) == """
-        ints_preferred(): incompatible function arguments. The following argument types are supported:
-            1. (i: int) -> int
-
-        Invoked with: 4.0
-    """  # noqa: E501 line too long
-
-    assert m.ints_only(4) == 2
-    with pytest.raises(TypeError) as excinfo:
-        m.ints_only(4.0)
-    assert msg(excinfo.value) == """
-        ints_only(): incompatible function arguments. The following argument types are supported:
-            1. (i: int) -> int
-
-        Invoked with: 4.0
-    """
-
-
 def test_bad_arg_default(msg):
     from pybind11_tests import debug_enabled
 
@@ -488,25 +426,12 @@ def test_unregistered_base_implementations():
     assert a.ro_value_prop == 1.75
 
 
-def test_custom_caster_destruction():
-    """Tests that returning a pointer to a type that gets converted with a custom type caster gets
-    destroyed when the function has py::return_value_policy::take_ownership policy applied."""
+def test_ref_qualified():
+    """Tests that explicit lvalue ref-qualified methods can be called just like their
+    non ref-qualified counterparts."""
 
-    cstats = m.destruction_tester_cstats()
-    # This one *doesn't* have take_ownership: the pointer should be used but not destroyed:
-    z = m.custom_caster_no_destroy()
-    assert cstats.alive() == 1 and cstats.default_constructions == 1
-    assert z
-
-    # take_ownership applied: this constructs a new object, casts it, then destroys it:
-    z = m.custom_caster_destroy()
-    assert z
-    assert cstats.default_constructions == 2
-
-    # Same, but with a const pointer return (which should *not* inhibit destruction):
-    z = m.custom_caster_destroy_const()
-    assert z
-    assert cstats.default_constructions == 3
-
-    # Make sure we still only have the original object (from ..._no_destroy()) alive:
-    assert cstats.alive() == 1
+    r = m.RefQualified()
+    assert r.value == 0
+    r.refQualified(17)
+    assert r.value == 17
+    assert r.constRefQualified(23) == 40
diff --git a/wrap/python/pybind11/tests/test_modules.cpp b/wrap/pybind11/tests/test_modules.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_modules.cpp
rename to wrap/pybind11/tests/test_modules.cpp
diff --git a/wrap/python/pybind11/tests/test_modules.py b/wrap/pybind11/tests/test_modules.py
similarity index 99%
rename from wrap/python/pybind11/tests/test_modules.py
rename to wrap/pybind11/tests/test_modules.py
index 2552838c2..7e2100524 100644
--- a/wrap/python/pybind11/tests/test_modules.py
+++ b/wrap/pybind11/tests/test_modules.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 from pybind11_tests import modules as m
 from pybind11_tests.modules import subsubmodule as ms
 from pybind11_tests import ConstructorStats
diff --git a/wrap/python/pybind11/tests/test_multiple_inheritance.cpp b/wrap/pybind11/tests/test_multiple_inheritance.cpp
similarity index 99%
rename from wrap/python/pybind11/tests/test_multiple_inheritance.cpp
rename to wrap/pybind11/tests/test_multiple_inheritance.cpp
index ba1674fb2..70e341785 100644
--- a/wrap/python/pybind11/tests/test_multiple_inheritance.cpp
+++ b/wrap/pybind11/tests/test_multiple_inheritance.cpp
@@ -193,7 +193,7 @@ TEST_SUBMODULE(multiple_inheritance, m) {
         .def_readwrite_static("static_value", &VanillaStaticMix2::static_value);
 
 
-#if !defined(PYPY_VERSION)
+#if !(defined(PYPY_VERSION) && (PYPY_VERSION_NUM < 0x06000000))
     struct WithDict { };
     struct VanillaDictMix1 : Vanilla, WithDict { };
     struct VanillaDictMix2 : WithDict, Vanilla { };
diff --git a/wrap/python/pybind11/tests/test_multiple_inheritance.py b/wrap/pybind11/tests/test_multiple_inheritance.py
similarity index 98%
rename from wrap/python/pybind11/tests/test_multiple_inheritance.py
rename to wrap/pybind11/tests/test_multiple_inheritance.py
index 475dd3b3d..bb602f84b 100644
--- a/wrap/python/pybind11/tests/test_multiple_inheritance.py
+++ b/wrap/pybind11/tests/test_multiple_inheritance.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import ConstructorStats
 from pybind11_tests import multiple_inheritance as m
@@ -10,6 +11,7 @@ def test_multiple_inheritance_cpp():
     assert mt.bar() == 4
 
 
+@pytest.bug_in_pypy
 def test_multiple_inheritance_mix1():
     class Base1:
         def __init__(self, i):
@@ -49,6 +51,7 @@ def test_multiple_inheritance_mix2():
     assert mt.bar() == 4
 
 
+@pytest.bug_in_pypy
 def test_multiple_inheritance_python():
 
     class MI1(m.Base1, m.Base2):
@@ -253,7 +256,7 @@ def test_mi_static_properties():
         assert d.static_value == 0
 
 
-@pytest.unsupported_on_pypy
+@pytest.unsupported_on_pypy_lt_6
 def test_mi_dynamic_attributes():
     """Mixing bases with and without dynamic attribute support"""
 
diff --git a/wrap/python/pybind11/tests/test_numpy_array.cpp b/wrap/pybind11/tests/test_numpy_array.cpp
similarity index 82%
rename from wrap/python/pybind11/tests/test_numpy_array.cpp
rename to wrap/pybind11/tests/test_numpy_array.cpp
index cdf0b82df..156a3bfa8 100644
--- a/wrap/python/pybind11/tests/test_numpy_array.cpp
+++ b/wrap/pybind11/tests/test_numpy_array.cpp
@@ -14,6 +14,67 @@
 
 #include <cstdint>
 
+// Size / dtype checks.
+struct DtypeCheck {
+    py::dtype numpy{};
+    py::dtype pybind11{};
+};
+
+template <typename T>
+DtypeCheck get_dtype_check(const char* name) {
+    py::module np = py::module::import("numpy");
+    DtypeCheck check{};
+    check.numpy = np.attr("dtype")(np.attr(name));
+    check.pybind11 = py::dtype::of<T>();
+    return check;
+}
+
+std::vector<DtypeCheck> get_concrete_dtype_checks() {
+    return {
+        // Normalization
+        get_dtype_check<std::int8_t>("int8"),
+        get_dtype_check<std::uint8_t>("uint8"),
+        get_dtype_check<std::int16_t>("int16"),
+        get_dtype_check<std::uint16_t>("uint16"),
+        get_dtype_check<std::int32_t>("int32"),
+        get_dtype_check<std::uint32_t>("uint32"),
+        get_dtype_check<std::int64_t>("int64"),
+        get_dtype_check<std::uint64_t>("uint64")
+    };
+}
+
+struct DtypeSizeCheck {
+    std::string name{};
+    int size_cpp{};
+    int size_numpy{};
+    // For debugging.
+    py::dtype dtype{};
+};
+
+template <typename T>
+DtypeSizeCheck get_dtype_size_check() {
+    DtypeSizeCheck check{};
+    check.name = py::type_id<T>();
+    check.size_cpp = sizeof(T);
+    check.dtype = py::dtype::of<T>();
+    check.size_numpy = check.dtype.attr("itemsize").template cast<int>();
+    return check;
+}
+
+std::vector<DtypeSizeCheck> get_platform_dtype_size_checks() {
+    return {
+        get_dtype_size_check<short>(),
+        get_dtype_size_check<unsigned short>(),
+        get_dtype_size_check<int>(),
+        get_dtype_size_check<unsigned int>(),
+        get_dtype_size_check<long>(),
+        get_dtype_size_check<unsigned long>(),
+        get_dtype_size_check<long long>(),
+        get_dtype_size_check<unsigned long long>(),
+    };
+}
+
+// Arrays.
 using arr = py::array;
 using arr_t = py::array_t<uint16_t, 0>;
 static_assert(std::is_same<arr_t::value_type, uint16_t>::value, "");
@@ -75,6 +136,26 @@ TEST_SUBMODULE(numpy_array, sm) {
     try { py::module::import("numpy"); }
     catch (...) { return; }
 
+    // test_dtypes
+    py::class_<DtypeCheck>(sm, "DtypeCheck")
+        .def_readonly("numpy", &DtypeCheck::numpy)
+        .def_readonly("pybind11", &DtypeCheck::pybind11)
+        .def("__repr__", [](const DtypeCheck& self) {
+            return py::str("<DtypeCheck numpy={} pybind11={}>").format(
+                self.numpy, self.pybind11);
+        });
+    sm.def("get_concrete_dtype_checks", &get_concrete_dtype_checks);
+
+    py::class_<DtypeSizeCheck>(sm, "DtypeSizeCheck")
+        .def_readonly("name", &DtypeSizeCheck::name)
+        .def_readonly("size_cpp", &DtypeSizeCheck::size_cpp)
+        .def_readonly("size_numpy", &DtypeSizeCheck::size_numpy)
+        .def("__repr__", [](const DtypeSizeCheck& self) {
+            return py::str("<DtypeSizeCheck name='{}' size_cpp={} size_numpy={} dtype={}>").format(
+                self.name, self.size_cpp, self.size_numpy, self.dtype);
+        });
+    sm.def("get_platform_dtype_size_checks", &get_platform_dtype_size_checks);
+
     // test_array_attributes
     sm.def("ndim", [](const arr& a) { return a.ndim(); });
     sm.def("shape", [](const arr& a) { return arr(a.ndim(), a.shape()); });
diff --git a/wrap/python/pybind11/tests/test_numpy_array.py b/wrap/pybind11/tests/test_numpy_array.py
similarity index 91%
rename from wrap/python/pybind11/tests/test_numpy_array.py
rename to wrap/pybind11/tests/test_numpy_array.py
index 8bacb7f6d..2c977cd62 100644
--- a/wrap/python/pybind11/tests/test_numpy_array.py
+++ b/wrap/pybind11/tests/test_numpy_array.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import numpy_array as m
 
@@ -7,6 +8,21 @@ with pytest.suppress(ImportError):
     import numpy as np
 
 
+def test_dtypes():
+    # See issue #1328.
+    # - Platform-dependent sizes.
+    for size_check in m.get_platform_dtype_size_checks():
+        print(size_check)
+        assert size_check.size_cpp == size_check.size_numpy, size_check
+    # - Concrete sizes.
+    for check in m.get_concrete_dtype_checks():
+        print(check)
+        assert check.numpy == check.pybind11, check
+        if check.numpy.num != check.pybind11.num:
+            print("NOTE: typenum mismatch for {}: {} != {}".format(
+                check, check.numpy.num, check.pybind11.num))
+
+
 @pytest.fixture(scope='function')
 def arr():
     return np.array([[1, 2, 3], [4, 5, 6]], '=u2')
@@ -271,13 +287,13 @@ def test_overload_resolution(msg):
         m.overloaded("not an array")
     assert msg(excinfo.value) == """
         overloaded(): incompatible function arguments. The following argument types are supported:
-            1. (arg0: numpy.ndarray[float64]) -> str
-            2. (arg0: numpy.ndarray[float32]) -> str
-            3. (arg0: numpy.ndarray[int32]) -> str
-            4. (arg0: numpy.ndarray[uint16]) -> str
-            5. (arg0: numpy.ndarray[int64]) -> str
-            6. (arg0: numpy.ndarray[complex128]) -> str
-            7. (arg0: numpy.ndarray[complex64]) -> str
+            1. (arg0: numpy.ndarray[numpy.float64]) -> str
+            2. (arg0: numpy.ndarray[numpy.float32]) -> str
+            3. (arg0: numpy.ndarray[numpy.int32]) -> str
+            4. (arg0: numpy.ndarray[numpy.uint16]) -> str
+            5. (arg0: numpy.ndarray[numpy.int64]) -> str
+            6. (arg0: numpy.ndarray[numpy.complex128]) -> str
+            7. (arg0: numpy.ndarray[numpy.complex64]) -> str
 
         Invoked with: 'not an array'
     """
@@ -292,8 +308,8 @@ def test_overload_resolution(msg):
     assert m.overloaded3(np.array([1], dtype='intc')) == 'int'
     expected_exc = """
         overloaded3(): incompatible function arguments. The following argument types are supported:
-            1. (arg0: numpy.ndarray[int32]) -> str
-            2. (arg0: numpy.ndarray[float64]) -> str
+            1. (arg0: numpy.ndarray[numpy.int32]) -> str
+            2. (arg0: numpy.ndarray[numpy.float64]) -> str
 
         Invoked with: """
 
@@ -419,3 +435,14 @@ def test_array_create_and_resize(msg):
 def test_index_using_ellipsis():
     a = m.index_using_ellipsis(np.zeros((5, 6, 7)))
     assert a.shape == (6,)
+
+
+@pytest.unsupported_on_pypy
+def test_dtype_refcount_leak():
+    from sys import getrefcount
+    dtype = np.dtype(np.float_)
+    a = np.array([1], dtype=dtype)
+    before = getrefcount(dtype)
+    m.ndim(a)
+    after = getrefcount(dtype)
+    assert after == before
diff --git a/wrap/python/pybind11/tests/test_numpy_dtypes.cpp b/wrap/pybind11/tests/test_numpy_dtypes.cpp
similarity index 98%
rename from wrap/python/pybind11/tests/test_numpy_dtypes.cpp
rename to wrap/pybind11/tests/test_numpy_dtypes.cpp
index 6e3dc6ba2..467e0253f 100644
--- a/wrap/python/pybind11/tests/test_numpy_dtypes.cpp
+++ b/wrap/pybind11/tests/test_numpy_dtypes.cpp
@@ -29,6 +29,13 @@ std::ostream& operator<<(std::ostream& os, const SimpleStruct& v) {
     return os << "s:" << v.bool_ << "," << v.uint_ << "," << v.float_ << "," << v.ldbl_;
 }
 
+struct SimpleStructReordered {
+    bool bool_;
+    float float_;
+    uint32_t uint_;
+    long double ldbl_;
+};
+
 PYBIND11_PACKED(struct PackedStruct {
     bool bool_;
     uint32_t uint_;
@@ -255,6 +262,7 @@ TEST_SUBMODULE(numpy_dtypes, m) {
     py::class_<SimpleStruct>(m, "SimpleStruct");
 
     PYBIND11_NUMPY_DTYPE(SimpleStruct, bool_, uint_, float_, ldbl_);
+    PYBIND11_NUMPY_DTYPE(SimpleStructReordered, bool_, uint_, float_, ldbl_);
     PYBIND11_NUMPY_DTYPE(PackedStruct, bool_, uint_, float_, ldbl_);
     PYBIND11_NUMPY_DTYPE(NestedStruct, a, b);
     PYBIND11_NUMPY_DTYPE(PartialStruct, bool_, uint_, float_, ldbl_);
diff --git a/wrap/python/pybind11/tests/test_numpy_dtypes.py b/wrap/pybind11/tests/test_numpy_dtypes.py
similarity index 99%
rename from wrap/python/pybind11/tests/test_numpy_dtypes.py
rename to wrap/pybind11/tests/test_numpy_dtypes.py
index 2e6388517..d173435fe 100644
--- a/wrap/python/pybind11/tests/test_numpy_dtypes.py
+++ b/wrap/pybind11/tests/test_numpy_dtypes.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import re
 import pytest
 from pybind11_tests import numpy_dtypes as m
diff --git a/wrap/python/pybind11/tests/test_numpy_vectorize.cpp b/wrap/pybind11/tests/test_numpy_vectorize.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_numpy_vectorize.cpp
rename to wrap/pybind11/tests/test_numpy_vectorize.cpp
diff --git a/wrap/python/pybind11/tests/test_numpy_vectorize.py b/wrap/pybind11/tests/test_numpy_vectorize.py
similarity index 95%
rename from wrap/python/pybind11/tests/test_numpy_vectorize.py
rename to wrap/pybind11/tests/test_numpy_vectorize.py
index 0e9c88397..bd3c01347 100644
--- a/wrap/python/pybind11/tests/test_numpy_vectorize.py
+++ b/wrap/pybind11/tests/test_numpy_vectorize.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import numpy_vectorize as m
 
@@ -109,7 +110,7 @@ def test_type_selection():
 
 def test_docs(doc):
     assert doc(m.vectorized_func) == """
-        vectorized_func(arg0: numpy.ndarray[int32], arg1: numpy.ndarray[float32], arg2: numpy.ndarray[float64]) -> object
+        vectorized_func(arg0: numpy.ndarray[numpy.int32], arg1: numpy.ndarray[numpy.float32], arg2: numpy.ndarray[numpy.float64]) -> object
     """  # noqa: E501 line too long
 
 
@@ -160,12 +161,12 @@ def test_passthrough_arguments(doc):
     assert doc(m.vec_passthrough) == (
         "vec_passthrough(" + ", ".join([
             "arg0: float",
-            "arg1: numpy.ndarray[float64]",
-            "arg2: numpy.ndarray[float64]",
-            "arg3: numpy.ndarray[int32]",
+            "arg1: numpy.ndarray[numpy.float64]",
+            "arg2: numpy.ndarray[numpy.float64]",
+            "arg3: numpy.ndarray[numpy.int32]",
             "arg4: int",
             "arg5: m.numpy_vectorize.NonPODClass",
-            "arg6: numpy.ndarray[float64]"]) + ") -> object")
+            "arg6: numpy.ndarray[numpy.float64]"]) + ") -> object")
 
     b = np.array([[10, 20, 30]], dtype='float64')
     c = np.array([100, 200])  # NOT a vectorized argument
diff --git a/wrap/python/pybind11/tests/test_opaque_types.cpp b/wrap/pybind11/tests/test_opaque_types.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_opaque_types.cpp
rename to wrap/pybind11/tests/test_opaque_types.cpp
diff --git a/wrap/python/pybind11/tests/test_opaque_types.py b/wrap/pybind11/tests/test_opaque_types.py
similarity index 98%
rename from wrap/python/pybind11/tests/test_opaque_types.py
rename to wrap/pybind11/tests/test_opaque_types.py
index 6b3802fdb..3f2392775 100644
--- a/wrap/python/pybind11/tests/test_opaque_types.py
+++ b/wrap/pybind11/tests/test_opaque_types.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import opaque_types as m
 from pybind11_tests import ConstructorStats, UserType
diff --git a/wrap/python/pybind11/tests/test_operator_overloading.cpp b/wrap/pybind11/tests/test_operator_overloading.cpp
similarity index 86%
rename from wrap/python/pybind11/tests/test_operator_overloading.cpp
rename to wrap/pybind11/tests/test_operator_overloading.cpp
index 8ca7d8bcf..52fcd3383 100644
--- a/wrap/python/pybind11/tests/test_operator_overloading.cpp
+++ b/wrap/pybind11/tests/test_operator_overloading.cpp
@@ -23,6 +23,7 @@ public:
 
     std::string toString() const { return "[" + std::to_string(x) + ", " + std::to_string(y) + "]"; }
 
+    Vector2 operator-() const { return Vector2(-x, -y); }
     Vector2 operator+(const Vector2 &v) const { return Vector2(x + v.x, y + v.y); }
     Vector2 operator-(const Vector2 &v) const { return Vector2(x - v.x, y - v.y); }
     Vector2 operator-(float value) const { return Vector2(x - value, y - value); }
@@ -42,6 +43,13 @@ public:
     friend Vector2 operator-(float f, const Vector2 &v) { return Vector2(f - v.x, f - v.y); }
     friend Vector2 operator*(float f, const Vector2 &v) { return Vector2(f * v.x, f * v.y); }
     friend Vector2 operator/(float f, const Vector2 &v) { return Vector2(f / v.x, f / v.y); }
+
+    bool operator==(const Vector2 &v) const {
+        return x == v.x && y == v.y;
+    }
+    bool operator!=(const Vector2 &v) const {
+        return x != v.x || y != v.y;
+    }
 private:
     float x, y;
 };
@@ -54,6 +62,11 @@ int operator+(const C2 &, const C2 &) { return 22; }
 int operator+(const C2 &, const C1 &) { return 21; }
 int operator+(const C1 &, const C2 &) { return 12; }
 
+// Note: Specializing explicit within `namespace std { ... }` is done due to a
+// bug in GCC<7. If you are supporting compilers later than this, consider
+// specializing `using template<> struct std::hash<...>` in the global
+// namespace instead, per this recommendation:
+// https://en.cppreference.com/w/cpp/language/extending_std#Adding_template_specializations
 namespace std {
     template<>
     struct hash<Vector2> {
@@ -62,6 +75,11 @@ namespace std {
     };
 }
 
+// Not a good abs function, but easy to test.
+std::string abs(const Vector2&) {
+    return "abs(Vector2)";
+}
+
 // MSVC warns about unknown pragmas, and warnings are errors.
 #ifndef _MSC_VER
   #pragma GCC diagnostic push
@@ -104,8 +122,15 @@ TEST_SUBMODULE(operators, m) {
         .def(float() - py::self)
         .def(float() * py::self)
         .def(float() / py::self)
+        .def(-py::self)
         .def("__str__", &Vector2::toString)
-        .def(hash(py::self))
+        .def("__repr__", &Vector2::toString)
+        .def(py::self == py::self)
+        .def(py::self != py::self)
+        .def(py::hash(py::self))
+        // N.B. See warning about usage of `py::detail::abs(py::self)` in
+        // `operators.h`.
+        .def("__abs__", [](const Vector2& v) { return abs(v); })
         ;
 
     m.attr("Vector") = m.attr("Vector2");
diff --git a/wrap/python/pybind11/tests/test_operator_overloading.py b/wrap/pybind11/tests/test_operator_overloading.py
similarity index 74%
rename from wrap/python/pybind11/tests/test_operator_overloading.py
rename to wrap/pybind11/tests/test_operator_overloading.py
index 86827d2ba..6c927228c 100644
--- a/wrap/python/pybind11/tests/test_operator_overloading.py
+++ b/wrap/pybind11/tests/test_operator_overloading.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import operators as m
 from pybind11_tests import ConstructorStats
@@ -6,9 +7,14 @@ from pybind11_tests import ConstructorStats
 def test_operator_overloading():
     v1 = m.Vector2(1, 2)
     v2 = m.Vector(3, -1)
+    v3 = m.Vector2(1, 2)  # Same value as v1, but different instance.
+    assert v1 is not v3
+
     assert str(v1) == "[1.000000, 2.000000]"
     assert str(v2) == "[3.000000, -1.000000]"
 
+    assert str(-v2) == "[-3.000000, 1.000000]"
+
     assert str(v1 + v2) == "[4.000000, 1.000000]"
     assert str(v1 - v2) == "[-2.000000, 3.000000]"
     assert str(v1 - 8) == "[-7.000000, -6.000000]"
@@ -22,6 +28,12 @@ def test_operator_overloading():
     assert str(v1 * v2) == "[3.000000, -2.000000]"
     assert str(v2 / v1) == "[3.000000, -0.500000]"
 
+    assert v1 == v3
+    assert v1 != v2
+    assert hash(v1) == 4
+    # TODO(eric.cousineau): Make this work.
+    # assert abs(v1) == "abs(Vector2)"
+
     v1 += 2 * v2
     assert str(v1) == "[7.000000, 0.000000]"
     v1 -= v2
@@ -35,22 +47,33 @@ def test_operator_overloading():
     v2 /= v1
     assert str(v2) == "[2.000000, 8.000000]"
 
-    assert hash(v1) == 4
-
     cstats = ConstructorStats.get(m.Vector2)
-    assert cstats.alive() == 2
+    assert cstats.alive() == 3
     del v1
-    assert cstats.alive() == 1
+    assert cstats.alive() == 2
     del v2
+    assert cstats.alive() == 1
+    del v3
     assert cstats.alive() == 0
-    assert cstats.values() == ['[1.000000, 2.000000]', '[3.000000, -1.000000]',
-                               '[4.000000, 1.000000]', '[-2.000000, 3.000000]',
-                               '[-7.000000, -6.000000]', '[9.000000, 10.000000]',
-                               '[8.000000, 16.000000]', '[0.125000, 0.250000]',
-                               '[7.000000, 6.000000]', '[9.000000, 10.000000]',
-                               '[8.000000, 16.000000]', '[8.000000, 4.000000]',
-                               '[3.000000, -2.000000]', '[3.000000, -0.500000]',
-                               '[6.000000, -2.000000]']
+    assert cstats.values() == [
+        '[1.000000, 2.000000]',
+        '[3.000000, -1.000000]',
+        '[1.000000, 2.000000]',
+        '[-3.000000, 1.000000]',
+        '[4.000000, 1.000000]',
+        '[-2.000000, 3.000000]',
+        '[-7.000000, -6.000000]',
+        '[9.000000, 10.000000]',
+        '[8.000000, 16.000000]',
+        '[0.125000, 0.250000]',
+        '[7.000000, 6.000000]',
+        '[9.000000, 10.000000]',
+        '[8.000000, 16.000000]',
+        '[8.000000, 4.000000]',
+        '[3.000000, -2.000000]',
+        '[3.000000, -0.500000]',
+        '[6.000000, -2.000000]',
+    ]
     assert cstats.default_constructions == 0
     assert cstats.copy_constructions == 0
     assert cstats.move_constructions >= 10
diff --git a/wrap/python/pybind11/tests/test_pickling.cpp b/wrap/pybind11/tests/test_pickling.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_pickling.cpp
rename to wrap/pybind11/tests/test_pickling.cpp
diff --git a/wrap/python/pybind11/tests/test_pickling.py b/wrap/pybind11/tests/test_pickling.py
similarity index 97%
rename from wrap/python/pybind11/tests/test_pickling.py
rename to wrap/pybind11/tests/test_pickling.py
index 5ae05aaa0..58d67a633 100644
--- a/wrap/python/pybind11/tests/test_pickling.py
+++ b/wrap/pybind11/tests/test_pickling.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import pickling as m
 
diff --git a/wrap/python/pybind11/tests/test_pytypes.cpp b/wrap/pybind11/tests/test_pytypes.cpp
similarity index 78%
rename from wrap/python/pybind11/tests/test_pytypes.cpp
rename to wrap/pybind11/tests/test_pytypes.cpp
index e6c955ff9..9f7bc37dc 100644
--- a/wrap/python/pybind11/tests/test_pytypes.cpp
+++ b/wrap/pybind11/tests/test_pytypes.cpp
@@ -11,12 +11,20 @@
 
 
 TEST_SUBMODULE(pytypes, m) {
+    // test_int
+    m.def("get_int", []{return py::int_(0);});
+    // test_iterator
+    m.def("get_iterator", []{return py::iterator();});
+    // test_iterable
+    m.def("get_iterable", []{return py::iterable();});
     // test_list
     m.def("get_list", []() {
         py::list list;
         list.append("value");
         py::print("Entry at position 0:", list[0]);
         list[0] = py::str("overwritten");
+        list.insert(0, "inserted-0");
+        list.insert(2, "inserted-2");
         return list;
     });
     m.def("print_list", [](py::list list) {
@@ -24,6 +32,11 @@ TEST_SUBMODULE(pytypes, m) {
         for (auto item : list)
             py::print("list item {}: {}"_s.format(index++, item));
     });
+    // test_none
+    m.def("get_none", []{return py::none();});
+    m.def("print_none", [](py::none none) {
+        py::print("none: {}"_s.format(none));
+    });
 
     // test_set
     m.def("get_set", []() {
@@ -37,6 +50,12 @@ TEST_SUBMODULE(pytypes, m) {
         for (auto item : set)
             py::print("key:", item);
     });
+    m.def("set_contains", [](py::set set, py::object key) {
+        return set.contains(key);
+    });
+    m.def("set_contains", [](py::set set, const char* key) {
+        return set.contains(key);
+    });
 
     // test_dict
     m.def("get_dict", []() { return py::dict("key"_a="value"); });
@@ -49,6 +68,12 @@ TEST_SUBMODULE(pytypes, m) {
         auto d2 = py::dict("z"_a=3, **d1);
         return d2;
     });
+    m.def("dict_contains", [](py::dict dict, py::object val) {
+        return dict.contains(val);
+    });
+    m.def("dict_contains", [](py::dict dict, const char* val) {
+        return dict.contains(val);
+    });
 
     // test_str
     m.def("str_from_string", []() { return py::str(std::string("baz")); });
@@ -293,4 +318,53 @@ TEST_SUBMODULE(pytypes, m) {
     m.def("test_list_slicing", [](py::list a) {
         return a[py::slice(0, -1, 2)];
     });
+
+    m.def("test_memoryview_object", [](py::buffer b) {
+        return py::memoryview(b);
+    });
+
+    m.def("test_memoryview_buffer_info", [](py::buffer b) {
+        return py::memoryview(b.request());
+    });
+
+    m.def("test_memoryview_from_buffer", [](bool is_unsigned) {
+        static const int16_t si16[] = { 3, 1, 4, 1, 5 };
+        static const uint16_t ui16[] = { 2, 7, 1, 8 };
+        if (is_unsigned)
+            return py::memoryview::from_buffer(
+                ui16, { 4 }, { sizeof(uint16_t) });
+        else
+            return py::memoryview::from_buffer(
+                si16, { 5 }, { sizeof(int16_t) });
+    });
+
+    m.def("test_memoryview_from_buffer_nativeformat", []() {
+        static const char* format = "@i";
+        static const int32_t arr[] = { 4, 7, 5 };
+        return py::memoryview::from_buffer(
+            arr, sizeof(int32_t), format, { 3 }, { sizeof(int32_t) });
+    });
+
+    m.def("test_memoryview_from_buffer_empty_shape", []() {
+        static const char* buf = "";
+        return py::memoryview::from_buffer(buf, 1, "B", { }, { });
+    });
+
+    m.def("test_memoryview_from_buffer_invalid_strides", []() {
+        static const char* buf = "\x02\x03\x04";
+        return py::memoryview::from_buffer(buf, 1, "B", { 3 }, { });
+    });
+
+    m.def("test_memoryview_from_buffer_nullptr", []() {
+        return py::memoryview::from_buffer(
+            static_cast<void*>(nullptr), 1, "B", { }, { });
+    });
+
+#if PY_MAJOR_VERSION >= 3
+    m.def("test_memoryview_from_memory", []() {
+        const char* buf = "\xff\xe1\xab\x37";
+        return py::memoryview::from_memory(
+            buf, static_cast<ssize_t>(strlen(buf)));
+    });
+#endif
 }
diff --git a/wrap/python/pybind11/tests/test_pytypes.py b/wrap/pybind11/tests/test_pytypes.py
similarity index 68%
rename from wrap/python/pybind11/tests/test_pytypes.py
rename to wrap/pybind11/tests/test_pytypes.py
index 0116d4ef2..5d2ccf8fd 100644
--- a/wrap/python/pybind11/tests/test_pytypes.py
+++ b/wrap/pybind11/tests/test_pytypes.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 from __future__ import division
 import pytest
 import sys
@@ -6,23 +7,42 @@ from pybind11_tests import pytypes as m
 from pybind11_tests import debug_enabled
 
 
+def test_int(doc):
+    assert doc(m.get_int) == "get_int() -> int"
+
+
+def test_iterator(doc):
+    assert doc(m.get_iterator) == "get_iterator() -> Iterator"
+
+
+def test_iterable(doc):
+    assert doc(m.get_iterable) == "get_iterable() -> Iterable"
+
+
 def test_list(capture, doc):
     with capture:
         lst = m.get_list()
-        assert lst == ["overwritten"]
+        assert lst == ["inserted-0", "overwritten", "inserted-2"]
 
         lst.append("value2")
         m.print_list(lst)
     assert capture.unordered == """
         Entry at position 0: value
-        list item 0: overwritten
-        list item 1: value2
+        list item 0: inserted-0
+        list item 1: overwritten
+        list item 2: inserted-2
+        list item 3: value2
     """
 
     assert doc(m.get_list) == "get_list() -> list"
     assert doc(m.print_list) == "print_list(arg0: list) -> None"
 
 
+def test_none(capture, doc):
+    assert doc(m.get_none) == "get_none() -> None"
+    assert doc(m.print_none) == "print_none(arg0: None) -> None"
+
+
 def test_set(capture, doc):
     s = m.get_set()
     assert s == {"key1", "key2", "key3"}
@@ -37,6 +57,10 @@ def test_set(capture, doc):
         key: key4
     """
 
+    assert not m.set_contains(set([]), 42)
+    assert m.set_contains({42}, 42)
+    assert m.set_contains({"foo"}, "foo")
+
     assert doc(m.get_list) == "get_list() -> list"
     assert doc(m.print_list) == "print_list(arg0: list) -> None"
 
@@ -53,6 +77,10 @@ def test_dict(capture, doc):
         key: key2, value=value2
     """
 
+    assert not m.dict_contains({}, 42)
+    assert m.dict_contains({42: None}, 42)
+    assert m.dict_contains({"foo": None}, "foo")
+
     assert doc(m.get_dict) == "get_dict() -> dict"
     assert doc(m.print_dict) == "print_dict(arg0: dict) -> None"
 
@@ -251,3 +279,70 @@ def test_number_protocol():
 def test_list_slicing():
     li = list(range(100))
     assert li[::2] == m.test_list_slicing(li)
+
+
+@pytest.mark.parametrize('method, args, fmt, expected_view', [
+    (m.test_memoryview_object, (b'red',), 'B', b'red'),
+    (m.test_memoryview_buffer_info, (b'green',), 'B', b'green'),
+    (m.test_memoryview_from_buffer, (False,), 'h', [3, 1, 4, 1, 5]),
+    (m.test_memoryview_from_buffer, (True,), 'H', [2, 7, 1, 8]),
+    (m.test_memoryview_from_buffer_nativeformat, (), '@i', [4, 7, 5]),
+])
+def test_memoryview(method, args, fmt, expected_view):
+    view = method(*args)
+    assert isinstance(view, memoryview)
+    assert view.format == fmt
+    if isinstance(expected_view, bytes) or sys.version_info[0] >= 3:
+        view_as_list = list(view)
+    else:
+        # Using max to pick non-zero byte (big-endian vs little-endian).
+        view_as_list = [max([ord(c) for c in s]) for s in view]
+    assert view_as_list == list(expected_view)
+
+
+@pytest.mark.skipif(
+    not hasattr(sys, 'getrefcount'),
+    reason='getrefcount is not available')
+@pytest.mark.parametrize('method', [
+    m.test_memoryview_object,
+    m.test_memoryview_buffer_info,
+])
+def test_memoryview_refcount(method):
+    buf = b'\x0a\x0b\x0c\x0d'
+    ref_before = sys.getrefcount(buf)
+    view = method(buf)
+    ref_after = sys.getrefcount(buf)
+    assert ref_before < ref_after
+    assert list(view) == list(buf)
+
+
+def test_memoryview_from_buffer_empty_shape():
+    view = m.test_memoryview_from_buffer_empty_shape()
+    assert isinstance(view, memoryview)
+    assert view.format == 'B'
+    if sys.version_info.major < 3:
+        # Python 2 behavior is weird, but Python 3 (the future) is fine.
+        assert bytes(view).startswith(b'<memory at ')
+    else:
+        assert bytes(view) == b''
+
+
+def test_test_memoryview_from_buffer_invalid_strides():
+    with pytest.raises(RuntimeError):
+        m.test_memoryview_from_buffer_invalid_strides()
+
+
+def test_test_memoryview_from_buffer_nullptr():
+    if sys.version_info.major < 3:
+        m.test_memoryview_from_buffer_nullptr()
+    else:
+        with pytest.raises(ValueError):
+            m.test_memoryview_from_buffer_nullptr()
+
+
+@pytest.mark.skipif(sys.version_info.major < 3, reason='API not available')
+def test_memoryview_from_memory():
+    view = m.test_memoryview_from_memory()
+    assert isinstance(view, memoryview)
+    assert view.format == 'B'
+    assert bytes(view) == b'\xff\xe1\xab\x37'
diff --git a/wrap/python/pybind11/tests/test_sequences_and_iterators.cpp b/wrap/pybind11/tests/test_sequences_and_iterators.cpp
similarity index 98%
rename from wrap/python/pybind11/tests/test_sequences_and_iterators.cpp
rename to wrap/pybind11/tests/test_sequences_and_iterators.cpp
index 87ccf99d6..05f999bb3 100644
--- a/wrap/python/pybind11/tests/test_sequences_and_iterators.cpp
+++ b/wrap/pybind11/tests/test_sequences_and_iterators.cpp
@@ -319,6 +319,9 @@ TEST_SUBMODULE(sequences_and_iterators, m) {
         return l;
     });
 
+    // test_sequence_length: check that Python sequences can be converted to py::sequence.
+    m.def("sequence_length", [](py::sequence seq) { return seq.size(); });
+
     // Make sure that py::iterator works with std algorithms
     m.def("count_none", [](py::object o) {
         return std::count_if(o.begin(), o.end(), [](py::handle h) { return h.is_none(); });
diff --git a/wrap/python/pybind11/tests/test_sequences_and_iterators.py b/wrap/pybind11/tests/test_sequences_and_iterators.py
similarity index 92%
rename from wrap/python/pybind11/tests/test_sequences_and_iterators.py
rename to wrap/pybind11/tests/test_sequences_and_iterators.py
index 6bd160640..8f6c0c4bb 100644
--- a/wrap/python/pybind11/tests/test_sequences_and_iterators.py
+++ b/wrap/pybind11/tests/test_sequences_and_iterators.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import sequences_and_iterators as m
 from pybind11_tests import ConstructorStats
@@ -100,6 +101,25 @@ def test_sequence():
     assert cstats.move_assignments == 0
 
 
+def test_sequence_length():
+    """#2076: Exception raised by len(arg) should be propagated """
+    class BadLen(RuntimeError):
+        pass
+
+    class SequenceLike():
+        def __getitem__(self, i):
+            return None
+
+        def __len__(self):
+            raise BadLen()
+
+    with pytest.raises(BadLen):
+        m.sequence_length(SequenceLike())
+
+    assert m.sequence_length([1, 2, 3]) == 3
+    assert m.sequence_length("hello") == 5
+
+
 def test_map_iterator():
     sm = m.StringMap({'hi': 'bye', 'black': 'white'})
     assert sm['hi'] == 'bye'
diff --git a/wrap/python/pybind11/tests/test_smart_ptr.cpp b/wrap/pybind11/tests/test_smart_ptr.cpp
similarity index 99%
rename from wrap/python/pybind11/tests/test_smart_ptr.cpp
rename to wrap/pybind11/tests/test_smart_ptr.cpp
index 87c9be8c2..6f8f3821a 100644
--- a/wrap/python/pybind11/tests/test_smart_ptr.cpp
+++ b/wrap/pybind11/tests/test_smart_ptr.cpp
@@ -291,7 +291,8 @@ TEST_SUBMODULE(smart_ptr, m) {
         ~C() { print_destroyed(this); }
     };
     py::class_<C, custom_unique_ptr<C>>(m, "TypeWithMoveOnlyHolder")
-        .def_static("make", []() { return custom_unique_ptr<C>(new C); });
+        .def_static("make", []() { return custom_unique_ptr<C>(new C); })
+        .def_static("make_as_object", []() { return py::cast(custom_unique_ptr<C>(new C)); });
 
     // test_holder_with_addressof_operator
     struct TypeForHolderWithAddressOf {
diff --git a/wrap/python/pybind11/tests/test_smart_ptr.py b/wrap/pybind11/tests/test_smart_ptr.py
similarity index 97%
rename from wrap/python/pybind11/tests/test_smart_ptr.py
rename to wrap/pybind11/tests/test_smart_ptr.py
index 0a3bb58ee..c9267f687 100644
--- a/wrap/python/pybind11/tests/test_smart_ptr.py
+++ b/wrap/pybind11/tests/test_smart_ptr.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 from pybind11_tests import smart_ptr as m
 from pybind11_tests import ConstructorStats
@@ -218,7 +219,10 @@ def test_shared_ptr_from_this_and_references():
 
 def test_move_only_holder():
     a = m.TypeWithMoveOnlyHolder.make()
+    b = m.TypeWithMoveOnlyHolder.make_as_object()
     stats = ConstructorStats.get(m.TypeWithMoveOnlyHolder)
+    assert stats.alive() == 2
+    del b
     assert stats.alive() == 1
     del a
     assert stats.alive() == 0
@@ -272,7 +276,8 @@ def test_smart_ptr_from_default():
     instance = m.HeldByDefaultHolder()
     with pytest.raises(RuntimeError) as excinfo:
         m.HeldByDefaultHolder.load_shared_ptr(instance)
-    assert "Unable to load a custom holder type from a default-holder instance" in str(excinfo)
+    assert "Unable to load a custom holder type from a " \
+           "default-holder instance" in str(excinfo.value)
 
 
 def test_shared_ptr_gc():
diff --git a/wrap/python/pybind11/tests/test_stl.cpp b/wrap/pybind11/tests/test_stl.cpp
similarity index 88%
rename from wrap/python/pybind11/tests/test_stl.cpp
rename to wrap/pybind11/tests/test_stl.cpp
index 207c9fb2b..928635788 100644
--- a/wrap/python/pybind11/tests/test_stl.cpp
+++ b/wrap/pybind11/tests/test_stl.cpp
@@ -50,6 +50,17 @@ namespace std {
 }
 
 
+template <template <typename> class OptionalImpl, typename T>
+struct OptionalHolder
+{
+    OptionalHolder() = default;
+    bool member_initialized() const {
+        return member && member->initialized;
+    }
+    OptionalImpl<T> member = T{};
+};
+
+
 TEST_SUBMODULE(stl, m) {
     // test_vector
     m.def("cast_vector", []() { return std::vector<int>{1}; });
@@ -154,6 +165,23 @@ TEST_SUBMODULE(stl, m) {
         .def(py::init<>())
         .def(py::init<int>());
 
+
+    struct MoveOutDetector
+    {
+        MoveOutDetector() = default;
+        MoveOutDetector(const MoveOutDetector&) = default;
+        MoveOutDetector(MoveOutDetector&& other) noexcept
+         : initialized(other.initialized) {
+            // steal underlying resource
+            other.initialized = false;
+        }
+        bool initialized = true;
+    };
+    py::class_<MoveOutDetector>(m, "MoveOutDetector", "Class with move tracking")
+        .def(py::init<>())
+        .def_readonly("initialized", &MoveOutDetector::initialized);
+
+
 #ifdef PYBIND11_HAS_OPTIONAL
     // test_optional
     m.attr("has_optional") = true;
@@ -175,6 +203,12 @@ TEST_SUBMODULE(stl, m) {
 
     m.def("nodefer_none_optional", [](std::optional<int>) { return true; });
     m.def("nodefer_none_optional", [](py::none) { return false; });
+
+    using opt_holder = OptionalHolder<std::optional, MoveOutDetector>;
+    py::class_<opt_holder>(m, "OptionalHolder", "Class with optional member")
+        .def(py::init<>())
+        .def_readonly("member", &opt_holder::member)
+        .def("member_initialized", &opt_holder::member_initialized);
 #endif
 
 #ifdef PYBIND11_HAS_EXP_OPTIONAL
@@ -195,6 +229,12 @@ TEST_SUBMODULE(stl, m) {
     m.def("test_no_assign_exp", [](const exp_opt_no_assign &x) {
         return x ? x->value : 42;
     }, py::arg_v("x", std::experimental::nullopt, "None"));
+
+    using opt_exp_holder = OptionalHolder<std::experimental::optional, MoveOutDetector>;
+    py::class_<opt_exp_holder>(m, "OptionalExpHolder", "Class with optional member")
+        .def(py::init<>())
+        .def_readonly("member", &opt_exp_holder::member)
+        .def("member_initialized", &opt_exp_holder::member_initialized);
 #endif
 
 #ifdef PYBIND11_HAS_VARIANT
diff --git a/wrap/python/pybind11/tests/test_stl.py b/wrap/pybind11/tests/test_stl.py
similarity index 96%
rename from wrap/python/pybind11/tests/test_stl.py
rename to wrap/pybind11/tests/test_stl.py
index 2335cb9fd..141b3e849 100644
--- a/wrap/python/pybind11/tests/test_stl.py
+++ b/wrap/pybind11/tests/test_stl.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 
 from pybind11_tests import stl as m
@@ -127,6 +128,11 @@ def test_optional():
 
     assert m.nodefer_none_optional(None)
 
+    holder = m.OptionalHolder()
+    mvalue = holder.member
+    assert mvalue.initialized
+    assert holder.member_initialized()
+
 
 @pytest.mark.skipif(not hasattr(m, "has_exp_optional"), reason='no <experimental/optional>')
 def test_exp_optional():
@@ -148,6 +154,11 @@ def test_exp_optional():
     assert m.test_no_assign_exp(m.NoAssign(43)) == 43
     pytest.raises(TypeError, m.test_no_assign_exp, 43)
 
+    holder = m.OptionalExpHolder()
+    mvalue = holder.member
+    assert mvalue.initialized
+    assert holder.member_initialized()
+
 
 @pytest.mark.skipif(not hasattr(m, "load_variant"), reason='no <variant>')
 def test_variant(doc):
diff --git a/wrap/python/pybind11/tests/test_stl_binders.cpp b/wrap/pybind11/tests/test_stl_binders.cpp
similarity index 76%
rename from wrap/python/pybind11/tests/test_stl_binders.cpp
rename to wrap/pybind11/tests/test_stl_binders.cpp
index a88b589e1..868887409 100644
--- a/wrap/python/pybind11/tests/test_stl_binders.cpp
+++ b/wrap/pybind11/tests/test_stl_binders.cpp
@@ -54,6 +54,14 @@ template <class Map> Map *times_ten(int n) {
     return m;
 }
 
+template <class NestMap> NestMap *times_hundred(int n) {
+    auto m = new NestMap();
+    for (int i = 1; i <= n; i++)
+        for (int j = 1; j <= n; j++)
+            (*m)[i].emplace(int(j*10), E_nc(100*j));
+    return m;
+}
+
 TEST_SUBMODULE(stl_binders, m) {
     // test_vector_int
     py::bind_vector<std::vector<unsigned int>>(m, "VectorInt", py::buffer_protocol());
@@ -85,6 +93,20 @@ TEST_SUBMODULE(stl_binders, m) {
     m.def("get_mnc", &times_ten<std::map<int, E_nc>>, py::return_value_policy::reference);
     py::bind_map<std::unordered_map<int, E_nc>>(m, "UmapENC");
     m.def("get_umnc", &times_ten<std::unordered_map<int, E_nc>>, py::return_value_policy::reference);
+    // Issue #1885: binding nested std::map<X, Container<E>> with E non-copyable
+    py::bind_map<std::map<int, std::vector<E_nc>>>(m, "MapVecENC");
+    m.def("get_nvnc", [](int n)
+        {
+            auto m = new std::map<int, std::vector<E_nc>>();
+            for (int i = 1; i <= n; i++)
+                for (int j = 1; j <= n; j++)
+                    (*m)[i].emplace_back(j);
+            return m;
+        }, py::return_value_policy::reference);
+    py::bind_map<std::map<int, std::map<int, E_nc>>>(m, "MapMapENC");
+    m.def("get_nmnc", &times_hundred<std::map<int, std::map<int, E_nc>>>, py::return_value_policy::reference);
+    py::bind_map<std::unordered_map<int, std::unordered_map<int, E_nc>>>(m, "UmapUmapENC");
+    m.def("get_numnc", &times_hundred<std::unordered_map<int, std::unordered_map<int, E_nc>>>, py::return_value_policy::reference);
 
     // test_vector_buffer
     py::bind_vector<std::vector<unsigned char>>(m, "VectorUChar", py::buffer_protocol());
diff --git a/wrap/python/pybind11/tests/test_stl_binders.py b/wrap/pybind11/tests/test_stl_binders.py
similarity index 79%
rename from wrap/python/pybind11/tests/test_stl_binders.py
rename to wrap/pybind11/tests/test_stl_binders.py
index 52c8ac0c4..27b326f07 100644
--- a/wrap/python/pybind11/tests/test_stl_binders.py
+++ b/wrap/pybind11/tests/test_stl_binders.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 import sys
 from pybind11_tests import stl_binders as m
@@ -53,6 +54,20 @@ def test_vector_int():
     v_int2.extend(x for x in range(5))
     assert v_int2 == m.VectorInt([0, 99, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4])
 
+    # test negative indexing
+    assert v_int2[-1] == 4
+
+    # insert with negative index
+    v_int2.insert(-1, 88)
+    assert v_int2 == m.VectorInt([0, 99, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 88, 4])
+
+    # delete negative index
+    del v_int2[-1]
+    assert v_int2 == m.VectorInt([0, 99, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 88])
+
+    v_int2.clear()
+    assert len(v_int2) == 0
+
 
 # related to the PyPy's buffer protocol.
 @pytest.unsupported_on_pypy
@@ -202,6 +217,45 @@ def test_noncopyable_containers():
 
     assert vsum == 150
 
+    # nested std::map<std::vector>
+    nvnc = m.get_nvnc(5)
+    for i in range(1, 6):
+        for j in range(0, 5):
+            assert nvnc[i][j].value == j + 1
+
+    # Note: maps do not have .values()
+    for _, v in nvnc.items():
+        for i, j in enumerate(v, start=1):
+            assert j.value == i
+
+    # nested std::map<std::map>
+    nmnc = m.get_nmnc(5)
+    for i in range(1, 6):
+        for j in range(10, 60, 10):
+            assert nmnc[i][j].value == 10 * j
+
+    vsum = 0
+    for _, v_o in nmnc.items():
+        for k_i, v_i in v_o.items():
+            assert v_i.value == 10 * k_i
+            vsum += v_i.value
+
+    assert vsum == 7500
+
+    # nested std::unordered_map<std::unordered_map>
+    numnc = m.get_numnc(5)
+    for i in range(1, 6):
+        for j in range(10, 60, 10):
+            assert numnc[i][j].value == 10 * j
+
+    vsum = 0
+    for _, v_o in numnc.items():
+        for k_i, v_i in v_o.items():
+            assert v_i.value == 10 * k_i
+            vsum += v_i.value
+
+    assert vsum == 7500
+
 
 def test_map_delitem():
     mm = m.MapStringDouble()
diff --git a/wrap/python/pybind11/tests/test_tagbased_polymorphic.cpp b/wrap/pybind11/tests/test_tagbased_polymorphic.cpp
similarity index 93%
rename from wrap/python/pybind11/tests/test_tagbased_polymorphic.cpp
rename to wrap/pybind11/tests/test_tagbased_polymorphic.cpp
index 272e460c9..dcc005126 100644
--- a/wrap/python/pybind11/tests/test_tagbased_polymorphic.cpp
+++ b/wrap/pybind11/tests/test_tagbased_polymorphic.cpp
@@ -12,6 +12,12 @@
 
 struct Animal
 {
+    // Make this type also a "standard" polymorphic type, to confirm that
+    // specializing polymorphic_type_hook using enable_if_t still works
+    // (https://github.com/pybind/pybind11/pull/2016/).
+    virtual ~Animal() = default;
+
+    // Enum for tag-based polymorphism.
     enum class Kind {
         Unknown = 0,
         Dog = 100, Labrador, Chihuahua, LastDog = 199,
diff --git a/wrap/python/pybind11/tests/test_tagbased_polymorphic.py b/wrap/pybind11/tests/test_tagbased_polymorphic.py
similarity index 96%
rename from wrap/python/pybind11/tests/test_tagbased_polymorphic.py
rename to wrap/pybind11/tests/test_tagbased_polymorphic.py
index 2574d7de7..94f374da9 100644
--- a/wrap/python/pybind11/tests/test_tagbased_polymorphic.py
+++ b/wrap/pybind11/tests/test_tagbased_polymorphic.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 from pybind11_tests import tagbased_polymorphic as m
 
 
diff --git a/wrap/python/pybind11/tests/test_union.cpp b/wrap/pybind11/tests/test_union.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_union.cpp
rename to wrap/pybind11/tests/test_union.cpp
diff --git a/wrap/python/pybind11/tests/test_union.py b/wrap/pybind11/tests/test_union.py
similarity index 86%
rename from wrap/python/pybind11/tests/test_union.py
rename to wrap/pybind11/tests/test_union.py
index e1866e701..2a2c12fb4 100644
--- a/wrap/python/pybind11/tests/test_union.py
+++ b/wrap/pybind11/tests/test_union.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 from pybind11_tests import union_ as m
 
 
diff --git a/wrap/python/pybind11/tests/test_virtual_functions.cpp b/wrap/pybind11/tests/test_virtual_functions.cpp
similarity index 100%
rename from wrap/python/pybind11/tests/test_virtual_functions.cpp
rename to wrap/pybind11/tests/test_virtual_functions.cpp
diff --git a/wrap/python/pybind11/tests/test_virtual_functions.py b/wrap/pybind11/tests/test_virtual_functions.py
similarity index 99%
rename from wrap/python/pybind11/tests/test_virtual_functions.py
rename to wrap/pybind11/tests/test_virtual_functions.py
index 5ce9abd35..0f2d85fce 100644
--- a/wrap/python/pybind11/tests/test_virtual_functions.py
+++ b/wrap/pybind11/tests/test_virtual_functions.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 import pytest
 
 from pybind11_tests import virtual_functions as m
diff --git a/wrap/python/pybind11/tools/FindCatch.cmake b/wrap/pybind11/tools/FindCatch.cmake
similarity index 100%
rename from wrap/python/pybind11/tools/FindCatch.cmake
rename to wrap/pybind11/tools/FindCatch.cmake
diff --git a/wrap/python/pybind11/tools/FindEigen3.cmake b/wrap/pybind11/tools/FindEigen3.cmake
similarity index 99%
rename from wrap/python/pybind11/tools/FindEigen3.cmake
rename to wrap/pybind11/tools/FindEigen3.cmake
index 9c546a05d..66ffe8e1e 100644
--- a/wrap/python/pybind11/tools/FindEigen3.cmake
+++ b/wrap/pybind11/tools/FindEigen3.cmake
@@ -78,4 +78,3 @@ else (EIGEN3_INCLUDE_DIR)
   mark_as_advanced(EIGEN3_INCLUDE_DIR)
 
 endif(EIGEN3_INCLUDE_DIR)
-
diff --git a/wrap/python/pybind11/tools/FindPythonLibsNew.cmake b/wrap/pybind11/tools/FindPythonLibsNew.cmake
similarity index 86%
rename from wrap/python/pybind11/tools/FindPythonLibsNew.cmake
rename to wrap/pybind11/tools/FindPythonLibsNew.cmake
index 7d85af4a5..994bb1e03 100644
--- a/wrap/python/pybind11/tools/FindPythonLibsNew.cmake
+++ b/wrap/pybind11/tools/FindPythonLibsNew.cmake
@@ -56,6 +56,9 @@ if(PYTHONLIBS_FOUND AND PYTHON_MODULE_EXTENSION)
 endif()
 
 # Use the Python interpreter to find the libs.
+if(NOT PythonLibsNew_FIND_VERSION)
+    set(PythonLibsNew_FIND_VERSION "")
+endif()
 if(PythonLibsNew_FIND_REQUIRED)
     find_package(PythonInterp ${PythonLibsNew_FIND_VERSION} REQUIRED)
 else()
@@ -140,20 +143,34 @@ list(GET _PYTHON_VERSION_LIST 1 PYTHON_VERSION_MINOR)
 list(GET _PYTHON_VERSION_LIST 2 PYTHON_VERSION_PATCH)
 
 # Make sure all directory separators are '/'
-string(REGEX REPLACE "\\\\" "/" PYTHON_PREFIX ${PYTHON_PREFIX})
-string(REGEX REPLACE "\\\\" "/" PYTHON_INCLUDE_DIR ${PYTHON_INCLUDE_DIR})
-string(REGEX REPLACE "\\\\" "/" PYTHON_SITE_PACKAGES ${PYTHON_SITE_PACKAGES})
+string(REGEX REPLACE "\\\\" "/" PYTHON_PREFIX "${PYTHON_PREFIX}")
+string(REGEX REPLACE "\\\\" "/" PYTHON_INCLUDE_DIR "${PYTHON_INCLUDE_DIR}")
+string(REGEX REPLACE "\\\\" "/" PYTHON_SITE_PACKAGES "${PYTHON_SITE_PACKAGES}")
 
 if(CMAKE_HOST_WIN32)
     set(PYTHON_LIBRARY
-        "${PYTHON_PREFIX}/libs/Python${PYTHON_LIBRARY_SUFFIX}.lib")
+        "${PYTHON_PREFIX}/libs/python${PYTHON_LIBRARY_SUFFIX}.lib")
 
     # when run in a venv, PYTHON_PREFIX points to it. But the libraries remain in the
     # original python installation. They may be found relative to PYTHON_INCLUDE_DIR.
     if(NOT EXISTS "${PYTHON_LIBRARY}")
         get_filename_component(_PYTHON_ROOT ${PYTHON_INCLUDE_DIR} DIRECTORY)
         set(PYTHON_LIBRARY
-            "${_PYTHON_ROOT}/libs/Python${PYTHON_LIBRARY_SUFFIX}.lib")
+            "${_PYTHON_ROOT}/libs/python${PYTHON_LIBRARY_SUFFIX}.lib")
+    endif()
+
+    # if we are in MSYS & MINGW, and we didn't find windows python lib, look for system python lib
+    if(DEFINED ENV{MSYSTEM} AND MINGW AND NOT EXISTS "${PYTHON_LIBRARY}")
+        if(PYTHON_MULTIARCH)
+            set(_PYTHON_LIBS_SEARCH "${PYTHON_LIBDIR}/${PYTHON_MULTIARCH}" "${PYTHON_LIBDIR}")
+        else()
+            set(_PYTHON_LIBS_SEARCH "${PYTHON_LIBDIR}")
+        endif()
+        unset(PYTHON_LIBRARY)
+        find_library(PYTHON_LIBRARY
+            NAMES "python${PYTHON_LIBRARY_SUFFIX}"
+            PATHS ${_PYTHON_LIBS_SEARCH}
+            NO_DEFAULT_PATH)
     endif()
 
     # raise an error if the python libs are still not found.
@@ -192,11 +209,18 @@ MARK_AS_ADVANCED(
 # module.
 SET(PYTHON_INCLUDE_DIRS "${PYTHON_INCLUDE_DIR}")
 SET(PYTHON_LIBRARIES "${PYTHON_LIBRARY}")
+if(NOT PYTHON_DEBUG_LIBRARY)
+    SET(PYTHON_DEBUG_LIBRARY "")
+endif()
 SET(PYTHON_DEBUG_LIBRARIES "${PYTHON_DEBUG_LIBRARY}")
 
 find_package_message(PYTHON
     "Found PythonLibs: ${PYTHON_LIBRARY}"
-    "${PYTHON_EXECUTABLE}${PYTHON_VERSION}")
+    "${PYTHON_EXECUTABLE}${PYTHON_VERSION_STRING}")
 
 set(PYTHONLIBS_FOUND TRUE)
 set(PythonLibsNew_FOUND TRUE)
+
+if(NOT PYTHON_MODULE_PREFIX)
+    SET(PYTHON_MODULE_PREFIX "")
+endif()
diff --git a/wrap/python/pybind11/tools/check-style.sh b/wrap/pybind11/tools/check-style.sh
similarity index 100%
rename from wrap/python/pybind11/tools/check-style.sh
rename to wrap/pybind11/tools/check-style.sh
diff --git a/wrap/pybind11/tools/clang b/wrap/pybind11/tools/clang
new file mode 160000
index 000000000..6a00cbc4a
--- /dev/null
+++ b/wrap/pybind11/tools/clang
@@ -0,0 +1 @@
+Subproject commit 6a00cbc4a9b8e68b71caf7f774b3f9c753ae84d5
diff --git a/wrap/python/pybind11/tools/libsize.py b/wrap/pybind11/tools/libsize.py
similarity index 97%
rename from wrap/python/pybind11/tools/libsize.py
rename to wrap/pybind11/tools/libsize.py
index 5dcb8b0d0..50f88bdb3 100644
--- a/wrap/python/pybind11/tools/libsize.py
+++ b/wrap/pybind11/tools/libsize.py
@@ -1,3 +1,4 @@
+# -*- coding: utf-8 -*-
 from __future__ import print_function, division
 import os
 import sys
@@ -35,4 +36,3 @@ else:
 
 with open(save, 'w') as sf:
     sf.write(str(libsize))
-
diff --git a/wrap/python/pybind11/tools/mkdoc.py b/wrap/pybind11/tools/mkdoc.py
similarity index 95%
rename from wrap/python/pybind11/tools/mkdoc.py
rename to wrap/pybind11/tools/mkdoc.py
index 44164af3d..a22aacdef 100755
--- a/wrap/python/pybind11/tools/mkdoc.py
+++ b/wrap/pybind11/tools/mkdoc.py
@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+# -*- coding: utf-8 -*-
 #
 #  Syntax: mkdoc.py [-I<path> ..] [.. a list of header files ..]
 #
@@ -106,8 +107,8 @@ def process_comment(comment):
         result = result2
 
     # Doxygen tags
-    cpp_group = '([\w:]+)'
-    param_group = '([\[\w:\]]+)'
+    cpp_group = r'([\w:]+)'
+    param_group = r'([\[\w:\]]+)'
 
     s = result
     s = re.sub(r'\\c\s+%s' % cpp_group, r'``\1``', s)
@@ -254,6 +255,13 @@ def read_args(args):
             parameters.append('-isysroot')
             parameters.append(sysroot_dir)
     elif platform.system() == 'Linux':
+        # cython.util.find_library does not find `libclang` for all clang
+        # versions and distributions. LLVM switched to a monolithical setup
+        # that includes everything under /usr/lib/llvm{version_number}/
+        # We therefore glob for the library and select the highest version
+        library_file = sorted(glob("/usr/lib/llvm-*/lib/libclang.so"), reverse=True)[0]
+        cindex.Config.set_library_file(library_file)
+
         # clang doesn't find its own base includes by default on Linux,
         # but different distros install them in different paths.
         # Try to autodetect, preferring the highest numbered version.
diff --git a/wrap/python/pybind11/tools/pybind11Config.cmake.in b/wrap/pybind11/tools/pybind11Config.cmake.in
similarity index 97%
rename from wrap/python/pybind11/tools/pybind11Config.cmake.in
rename to wrap/pybind11/tools/pybind11Config.cmake.in
index 8a7272ff9..58426887a 100644
--- a/wrap/python/pybind11/tools/pybind11Config.cmake.in
+++ b/wrap/pybind11/tools/pybind11Config.cmake.in
@@ -28,8 +28,8 @@
 #
 # Python headers, libraries (as needed by platform), and the C++ standard
 # are attached to the target. Set PythonLibsNew variables to influence
-# python detection and PYBIND11_CPP_STANDARD (-std=c++11 or -std=c++14) to
-# influence standard setting. ::
+# python detection and CMAKE_CXX_STANDARD (11 or 14) to influence standard
+# setting. ::
 #
 #   find_package(pybind11 CONFIG REQUIRED)
 #   message(STATUS "Found pybind11 v${pybind11_VERSION}: ${pybind11_INCLUDE_DIRS}")
diff --git a/wrap/python/pybind11/tools/pybind11Tools.cmake b/wrap/pybind11/tools/pybind11Tools.cmake
similarity index 84%
rename from wrap/python/pybind11/tools/pybind11Tools.cmake
rename to wrap/pybind11/tools/pybind11Tools.cmake
index e3ec572b5..a3603abe9 100644
--- a/wrap/python/pybind11/tools/pybind11Tools.cmake
+++ b/wrap/pybind11/tools/pybind11Tools.cmake
@@ -12,30 +12,46 @@ if(NOT PYBIND11_PYTHON_VERSION)
   set(PYBIND11_PYTHON_VERSION "" CACHE STRING "Python version to use for compiling modules")
 endif()
 
-set(Python_ADDITIONAL_VERSIONS 3.7 3.6 3.5 3.4)
+set(Python_ADDITIONAL_VERSIONS 3.9 3.8 3.7 3.6 3.5 3.4)
 find_package(PythonLibsNew ${PYBIND11_PYTHON_VERSION} REQUIRED)
 
 include(CheckCXXCompilerFlag)
 include(CMakeParseArguments)
 
-if(NOT PYBIND11_CPP_STANDARD AND NOT CMAKE_CXX_STANDARD)
-  if(NOT MSVC)
-    check_cxx_compiler_flag("-std=c++14" HAS_CPP14_FLAG)
+# Use the language standards abstraction if CMake supports it with the current compiler
+if(NOT CMAKE_VERSION VERSION_LESS 3.1)
+  if(NOT CMAKE_CXX_STANDARD)
+    if(CMAKE_CXX14_STANDARD_COMPILE_OPTION)
+      set(CMAKE_CXX_STANDARD 14)
+    elseif(CMAKE_CXX11_STANDARD_COMPILE_OPTION)
+      set(CMAKE_CXX_STANDARD 11)
+    endif()
+  endif()
+  if(CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_EXTENSIONS OFF)
+    set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  endif()
+endif()
 
-    if (HAS_CPP14_FLAG)
+# Fall back to heuristics
+if(NOT PYBIND11_CPP_STANDARD AND NOT CMAKE_CXX_STANDARD)
+  if(MSVC)
+    set(PYBIND11_CPP_STANDARD /std:c++14)
+  else()
+    check_cxx_compiler_flag("-std=c++14" HAS_CPP14_FLAG)
+    if(HAS_CPP14_FLAG)
       set(PYBIND11_CPP_STANDARD -std=c++14)
     else()
       check_cxx_compiler_flag("-std=c++11" HAS_CPP11_FLAG)
-      if (HAS_CPP11_FLAG)
+      if(HAS_CPP11_FLAG)
         set(PYBIND11_CPP_STANDARD -std=c++11)
-      else()
-        message(FATAL_ERROR "Unsupported compiler -- pybind11 requires C++11 support!")
       endif()
     endif()
-  elseif(MSVC)
-    set(PYBIND11_CPP_STANDARD /std:c++14)
   endif()
 
+  if(NOT PYBIND11_CPP_STANDARD)
+    message(FATAL_ERROR "Unsupported compiler -- pybind11 requires C++11 support!")
+  endif()
   set(PYBIND11_CPP_STANDARD ${PYBIND11_CPP_STANDARD} CACHE STRING
       "C++ standard flag, e.g. -std=c++11, -std=c++14, /std:c++14.  Defaults to C++14 mode." FORCE)
 endif()
@@ -126,17 +142,32 @@ function(pybind11_add_module target_name)
 
   if(ARG_EXCLUDE_FROM_ALL)
     set(exclude_from_all EXCLUDE_FROM_ALL)
+  else()
+    set(exclude_from_all "")
   endif()
 
   add_library(${target_name} ${lib_type} ${exclude_from_all} ${ARG_UNPARSED_ARGUMENTS})
 
   if(ARG_SYSTEM)
     set(inc_isystem SYSTEM)
+  else()
+    set(inc_isystem "")
+  endif()
+
+  set(PYBIND11_INCLUDE_DIR_SELECTED "")
+  if(PYBIND11_INCLUDE_DIR)
+    # from project CMakeLists.txt
+    set(PYBIND11_INCLUDE_DIR_SELECTED ${PYBIND11_INCLUDE_DIR})
+  elseif(pybind11_INCLUDE_DIR)
+    # from pybind11Config
+    set(PYBIND11_INCLUDE_DIR_SELECTED ${pybind11_INCLUDE_DIR})
+  else()
+    message(FATAL "No pybind11_INCLUDE_DIR available. Use "
+      "find_package(pybind11) before calling pybind11_add_module.")
   endif()
 
   target_include_directories(${target_name} ${inc_isystem}
-    PRIVATE ${PYBIND11_INCLUDE_DIR}  # from project CMakeLists.txt
-    PRIVATE ${pybind11_INCLUDE_DIR}  # from pybind11Config
+    PRIVATE ${PYBIND11_INCLUDE_DIR_SELECTED}
     PRIVATE ${PYTHON_INCLUDE_DIRS})
 
   # Python debug libraries expose slightly different objects
@@ -185,10 +216,12 @@ function(pybind11_add_module target_name)
   endif()
 
   # Make sure C++11/14 are enabled
-  if(CMAKE_VERSION VERSION_LESS 3.3)
-    target_compile_options(${target_name} PUBLIC ${PYBIND11_CPP_STANDARD})
-  else()
-    target_compile_options(${target_name} PUBLIC $<$<COMPILE_LANGUAGE:CXX>:${PYBIND11_CPP_STANDARD}>)
+  if(PYBIND11_CPP_STANDARD)
+    if(CMAKE_VERSION VERSION_LESS 3.3)
+      target_compile_options(${target_name} PUBLIC ${PYBIND11_CPP_STANDARD})
+    else()
+      target_compile_options(${target_name} PUBLIC $<$<COMPILE_LANGUAGE:CXX>:${PYBIND11_CPP_STANDARD}>)
+    endif()
   endif()
 
   if(ARG_NO_EXTRAS)
@@ -197,7 +230,7 @@ function(pybind11_add_module target_name)
 
   _pybind11_add_lto_flags(${target_name} ${ARG_THIN_LTO})
 
-  if (NOT MSVC AND NOT ${CMAKE_BUILD_TYPE} MATCHES Debug)
+  if (NOT MSVC AND NOT ${CMAKE_BUILD_TYPE} MATCHES Debug|RelWithDebInfo)
     # Strip unnecessary sections of the binary on Linux/Mac OS
     if(CMAKE_STRIP)
       if(APPLE)
diff --git a/wrap/pybind_wrapper.py b/wrap/pybind_wrapper.py
new file mode 100755
index 000000000..3624d06df
--- /dev/null
+++ b/wrap/pybind_wrapper.py
@@ -0,0 +1,394 @@
+#!/usr/bin/env python3
+"""
+GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Code generator for wrapping a C++ module with Pybind11
+Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar and Frank Dellaert
+"""
+import argparse
+import re
+import textwrap
+
+import interface_parser as parser
+import template_instantiator as instantiator
+
+
+class PybindWrapper(object):
+    def __init__(self,
+                 module,
+                 module_name,
+                 top_module_namespaces='',
+                 use_boost=False,
+                 ignore_classes=[],
+                 module_template=""):
+        self.module = module
+        self.module_name = module_name
+        self.top_module_namespaces = top_module_namespaces
+        self.use_boost = use_boost
+        self.ignore_classes = ignore_classes
+        self._serializing_classes = list()
+        self.module_template = module_template
+        self.python_keywords = ['print', 'lambda']
+
+    def _py_args_names(self, args_list):
+        names = args_list.args_names()
+        if names:
+            py_args = ['py::arg("{}")'.format(name) for name in names]
+            return ", " + ", ".join(py_args)
+        else:
+            return ''
+
+    def _method_args_signature_with_names(self, args_list):
+        cpp_types = args_list.to_cpp(self.use_boost)
+        names = args_list.args_names()
+        types_names = ["{} {}".format(ctype, name) for ctype, name in zip(cpp_types, names)]
+
+        return ','.join(types_names)
+
+    def wrap_ctors(self, my_class):
+        res = ""
+        for ctor in my_class.ctors:
+            res += ('\n' + ' ' * 8 + '.def(py::init<{args_cpp_types}>()'
+                    '{py_args_names})'.format(
+                        args_cpp_types=", ".join(ctor.args.to_cpp(self.use_boost)),
+                        py_args_names=self._py_args_names(ctor.args),
+                    ))
+        return res
+
+    def _wrap_method(self, method, cpp_class, prefix, suffix, method_suffix=""):
+        py_method = method.name + method_suffix
+        cpp_method = method.to_cpp()
+
+        if cpp_method in ["serialize", "serializable"]:
+            if not cpp_class in self._serializing_classes:
+                self._serializing_classes.append(cpp_class)
+            return textwrap.dedent('''
+                    .def("serialize",
+                        []({class_inst} self){{
+                            return gtsam::serialize(self);
+                        }}
+                    )
+                    .def("deserialize",
+                        []({class_inst} self, string serialized){{
+                            return gtsam::deserialize(serialized, self);
+                        }})
+                    '''.format(class_inst=cpp_class + '*'))
+
+        is_method = isinstance(method, instantiator.InstantiatedMethod)
+        is_static = isinstance(method, parser.StaticMethod)
+        return_void = method.return_type.is_void()
+        args_names = method.args.args_names()
+        py_args_names = self._py_args_names(method.args)
+        args_signature_with_names = self._method_args_signature_with_names(method.args)
+
+        caller = cpp_class + "::" if not is_method else "self->"
+        function_call = ('{opt_return} {caller}{function_name}'
+                         '({args_names});'.format(
+                             opt_return='return' if not return_void else '',
+                             caller=caller,
+                             function_name=cpp_method,
+                             args_names=', '.join(args_names),
+                         ))
+
+        ret = ('{prefix}.{cdef}("{py_method}",'
+               '[]({opt_self}{opt_comma}{args_signature_with_names}){{'
+               '{function_call}'
+               '}}'
+               '{py_args_names}){suffix}'.format(
+                   prefix=prefix,
+                   cdef="def_static" if is_static else "def",
+                   py_method=py_method if not py_method in self.python_keywords else py_method + "_",
+                   opt_self="{cpp_class}* self".format(cpp_class=cpp_class) if is_method else "",
+                   cpp_class=cpp_class,
+                   cpp_method=cpp_method,
+                   opt_comma=',' if is_method and args_names else '',
+                   args_signature_with_names=args_signature_with_names,
+                   function_call=function_call,
+                   py_args_names=py_args_names,
+                   suffix=suffix,
+               ))
+        if method.name == 'print':
+            type_list = method.args.to_cpp(self.use_boost)
+            if len(type_list) > 0 and type_list[0].strip() == 'string':
+                ret += '''{prefix}.def("__repr__",
+                    [](const {cpp_class} &a) {{
+                        gtsam::RedirectCout redirect;
+                        a.print("");
+                        return redirect.str();
+                    }}){suffix}'''.format(
+                    prefix=prefix,
+                    cpp_class=cpp_class,
+                    suffix=suffix,
+                )
+            else:
+                ret += '''{prefix}.def("__repr__",
+                    [](const {cpp_class} &a) {{
+                        gtsam::RedirectCout redirect;
+                        a.print();
+                        return redirect.str();
+                    }}){suffix}'''.format(
+                    prefix=prefix,
+                    cpp_class=cpp_class,
+                    suffix=suffix,
+                )
+        return ret
+
+    def wrap_methods(self, methods, cpp_class, prefix='\n' + ' ' * 8, suffix=''):
+        res = ""
+        for method in methods:
+
+            # To avoid type confusion for insert, currently unused
+            if method.name == 'insert' and cpp_class == 'gtsam::Values':
+                name_list = method.args.args_names()
+                type_list = method.args.to_cpp(self.use_boost)
+                if type_list[0].strip() == 'size_t':  # inserting non-wrapped value types
+                    method_suffix = '_' + name_list[1].strip()
+                    res += self._wrap_method(method=method,
+                                             cpp_class=cpp_class,
+                                             prefix=prefix,
+                                             suffix=suffix,
+                                             method_suffix=method_suffix)
+
+            res += self._wrap_method(
+                method=method,
+                cpp_class=cpp_class,
+                prefix=prefix,
+                suffix=suffix,
+            )
+        return res
+
+    def wrap_properties(self, properties, cpp_class, prefix='\n' + ' ' * 8):
+        res = ""
+        for prop in properties:
+            res += ('{prefix}.def_{property}("{property_name}", '
+                    '&{cpp_class}::{property_name})'.format(
+                        prefix=prefix,
+                        property="readonly" if prop.ctype.is_const else "readwrite",
+                        cpp_class=cpp_class,
+                        property_name=prop.name,
+                    ))
+        return res
+
+    def wrap_instantiated_class(self, instantiated_class):
+        module_var = self._gen_module_var(instantiated_class.namespaces())
+        cpp_class = instantiated_class.cpp_class()
+        if cpp_class in self.ignore_classes:
+            return ""
+        return ('\n    py::class_<{cpp_class}, {class_parent}'
+                '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")'
+                '{wrapped_ctors}'
+                '{wrapped_methods}'
+                '{wrapped_static_methods}'
+                '{wrapped_properties};\n'.format(
+                    shared_ptr_type=('boost' if self.use_boost else 'std'),
+                    cpp_class=cpp_class,
+                    class_name=instantiated_class.name,
+                    class_parent=str(instantiated_class.parent_class) +
+                    (', ' if instantiated_class.parent_class else ''),
+                    module_var=module_var,
+                    wrapped_ctors=self.wrap_ctors(instantiated_class),
+                    wrapped_methods=self.wrap_methods(instantiated_class.methods, cpp_class),
+                    wrapped_static_methods=self.wrap_methods(instantiated_class.static_methods, cpp_class),
+                    wrapped_properties=self.wrap_properties(instantiated_class.properties, cpp_class),
+                ))
+
+    def wrap_stl_class(self, stl_class):
+        module_var = self._gen_module_var(stl_class.namespaces())
+        cpp_class = stl_class.cpp_class()
+        if cpp_class in self.ignore_classes:
+            return ""
+
+        return ('\n    py::class_<{cpp_class}, {class_parent}'
+                '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")'
+                '{wrapped_ctors}'
+                '{wrapped_methods}'
+                '{wrapped_static_methods}'
+                '{wrapped_properties};\n'.format(
+                    shared_ptr_type=('boost' if self.use_boost else 'std'),
+                    cpp_class=cpp_class,
+                    class_name=stl_class.name,
+                    class_parent=str(stl_class.parent_class) + (', ' if stl_class.parent_class else ''),
+                    module_var=module_var,
+                    wrapped_ctors=self.wrap_ctors(stl_class),
+                    wrapped_methods=self.wrap_methods(stl_class.methods, cpp_class),
+                    wrapped_static_methods=self.wrap_methods(stl_class.static_methods, cpp_class),
+                    wrapped_properties=self.wrap_properties(stl_class.properties, cpp_class),
+                ))
+
+    def _partial_match(self, namespaces1, namespaces2):
+        for i in range(min(len(namespaces1), len(namespaces2))):
+            if namespaces1[i] != namespaces2[i]:
+                return False
+        return True
+
+    def _gen_module_var(self, namespaces):
+        sub_module_namespaces = namespaces[len(self.top_module_namespaces):]
+        return "m_{}".format('_'.join(sub_module_namespaces))
+
+    def _add_namespaces(self, name, namespaces):
+        if namespaces:
+            # Ignore the first empty global namespace.
+            idx = 1 if not namespaces[0] else 0
+            return '::'.join(namespaces[idx:] + [name])
+        else:
+            return name
+
+    def wrap_namespace(self, namespace):
+        wrapped = ""
+        includes = ""
+
+        namespaces = namespace.full_namespaces()
+        if not self._partial_match(namespaces, self.top_module_namespaces):
+            return "", ""
+
+        if len(namespaces) < len(self.top_module_namespaces):
+            for element in namespace.content:
+                if isinstance(element, parser.Include):
+                    includes += ("{}\n".format(element).replace('<', '"').replace('>', '"'))
+                if isinstance(element, parser.Namespace):
+                    (
+                        wrapped_namespace,
+                        includes_namespace,
+                    ) = self.wrap_namespace(  # noqa
+                        element)
+                    wrapped += wrapped_namespace
+                    includes += includes_namespace
+        else:
+            module_var = self._gen_module_var(namespaces)
+
+            if len(namespaces) > len(self.top_module_namespaces):
+                wrapped += (' ' * 4 + 'pybind11::module {module_var} = '
+                            '{parent_module_var}.def_submodule("{namespace}", "'
+                            '{namespace} submodule");\n'.format(
+                                module_var=module_var,
+                                namespace=namespace.name,
+                                parent_module_var=self._gen_module_var(namespaces[:-1]),
+                            ))
+
+            for element in namespace.content:
+                if isinstance(element, parser.Include):
+                    includes += ("{}\n".format(element).replace('<', '"').replace('>', '"'))
+                elif isinstance(element, parser.Namespace):
+                    (
+                        wrapped_namespace,
+                        includes_namespace,
+                    ) = self.wrap_namespace(  # noqa
+                        element)
+                    wrapped += wrapped_namespace
+                    includes += includes_namespace
+                elif isinstance(element, instantiator.InstantiatedClass):
+                    wrapped += self.wrap_instantiated_class(element)
+
+            # Global functions.
+            all_funcs = [func for func in namespace.content if isinstance(func, parser.GlobalFunction)]
+            wrapped += self.wrap_methods(
+                all_funcs,
+                self._add_namespaces('', namespaces)[:-2],
+                prefix='\n' + ' ' * 4 + module_var,
+                suffix=';',
+            )
+        return wrapped, includes
+
+    def wrap(self):
+        wrapped_namespace, includes = self.wrap_namespace(self.module)
+
+        # Export classes for serialization.
+        boost_class_export = ""
+        for cpp_class in self._serializing_classes:
+            new_name = cpp_class
+            # The boost's macro doesn't like commas, so we have to typedef.
+            if ',' in cpp_class:
+                new_name = re.sub("[,:<> ]", "", cpp_class)
+                boost_class_export += "typedef {cpp_class} {new_name};\n".format(  # noqa
+                    cpp_class=cpp_class,
+                    new_name=new_name,
+                )
+            boost_class_export += "BOOST_CLASS_EXPORT({new_name})\n".format(new_name=new_name, )
+
+        return self.module_template.format(
+            include_boost="#include <boost/shared_ptr.hpp>" if self.use_boost else "",
+            module_name=self.module_name,
+            includes=includes,
+            hoder_type=
+            "PYBIND11_DECLARE_HOLDER_TYPE(TYPE_PLACEHOLDER_DONOTUSE, {shared_ptr_type}::shared_ptr<TYPE_PLACEHOLDER_DONOTUSE>);"
+            .format(shared_ptr_type=('boost' if self.use_boost else 'std')) if self.use_boost else "",
+            wrapped_namespace=wrapped_namespace,
+            boost_class_export=boost_class_export,
+        )
+
+
+def main():
+    arg_parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+    arg_parser.add_argument("--src", type=str, required=True, help="Input interface .h file")
+    arg_parser.add_argument(
+        "--module_name",
+        type=str,
+        required=True,
+        help="Name of the Python module to be generated and "
+        "used in the Python `import` statement.",
+    )
+    arg_parser.add_argument(
+        "--out",
+        type=str,
+        required=True,
+        help="Name of the output pybind .cc file",
+    )
+    arg_parser.add_argument(
+        "--use-boost",
+        action="store_true",
+        help="using boost's shared_ptr instead of std's",
+    )
+    arg_parser.add_argument(
+        "--top_module_namespaces",
+        type=str,
+        default="",
+        help="C++ namespace for the top module, e.g. `ns1::ns2::ns3`. "
+        "Only the content within this namespace and its sub-namespaces "
+        "will be wrapped. The content of this namespace will be available at "
+        "the top module level, and its sub-namespaces' in the submodules.\n"
+        "For example, `import <module_name>` gives you access to a Python "
+        "`<module_name>.Class` of the corresponding C++ `ns1::ns2::ns3::Class`"
+        "and `from <module_name> import ns4` gives you access to a Python "
+        "`ns4.Class` of the C++ `ns1::ns2::ns3::ns4::Class`. ",
+    )
+    arg_parser.add_argument(
+        "--ignore",
+        nargs='*',
+        type=str,
+        help="A space-separated list of classes to ignore. "
+        "Class names must include their full namespaces.",
+    )
+    arg_parser.add_argument("--template", type=str, help="The module template file")
+    args = arg_parser.parse_args()
+
+    top_module_namespaces = args.top_module_namespaces.split("::")
+    if top_module_namespaces[0]:
+        top_module_namespaces = [''] + top_module_namespaces
+
+    with open(args.src, "r") as f:
+        content = f.read()
+    module = parser.Module.parseString(content)
+    instantiator.instantiate_namespace_inplace(module)
+
+    with open(args.template, "r") as f:
+        template_content = f.read()
+    wrapper = PybindWrapper(
+        module=module,
+        module_name=args.module_name,
+        use_boost=args.use_boost,
+        top_module_namespaces=top_module_namespaces,
+        ignore_classes=args.ignore,
+        module_template=template_content,
+    )
+
+    cc_content = wrapper.wrap()
+    with open(args.out, "w") as f:
+        f.write(cc_content)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/wrap/pybind_wrapper.tpl.example b/wrap/pybind_wrapper.tpl.example
new file mode 100644
index 000000000..1bdd55140
--- /dev/null
+++ b/wrap/pybind_wrapper.tpl.example
@@ -0,0 +1,30 @@
+{include_boost}
+
+#include <pybind11/eigen.h>
+#include <pybind11/stl_bind.h>
+#include <pybind11/pybind11.h>
+#include "gtsam/base/serialization.h"
+#include "gtsam/nonlinear/utilities.h"  // for RedirectCout.
+
+{includes}
+#include <boost/serialization/export.hpp>
+
+{boost_class_export}
+
+{hoder_type}
+
+#include "python/preamble.h"
+
+using namespace std;
+
+namespace py = pybind11;
+
+PYBIND11_MODULE({module_name}, m_) {{
+    m_.doc() = "pybind11 wrapper of {module_name}";
+
+{wrapped_namespace}
+
+#include "python/specializations.h"
+
+}}
+
diff --git a/wrap/python/pybind11/pybind11/__init__.py b/wrap/python/pybind11/pybind11/__init__.py
deleted file mode 100644
index 5782ffea2..000000000
--- a/wrap/python/pybind11/pybind11/__init__.py
+++ /dev/null
@@ -1,28 +0,0 @@
-from ._version import version_info, __version__  # noqa: F401 imported but unused
-
-
-def get_include(user=False):
-    from distutils.dist import Distribution
-    import os
-    import sys
-
-    # Are we running in a virtual environment?
-    virtualenv = hasattr(sys, 'real_prefix') or \
-        sys.prefix != getattr(sys, "base_prefix", sys.prefix)
-
-    if virtualenv:
-        return os.path.join(sys.prefix, 'include', 'site',
-                            'python' + sys.version[:3])
-    else:
-        dist = Distribution({'name': 'pybind11'})
-        dist.parse_config_files()
-
-        dist_cobj = dist.get_command_obj('install', create=True)
-
-        # Search for packages in user's home directory?
-        if user:
-            dist_cobj.user = user
-            dist_cobj.prefix = ""
-        dist_cobj.finalize_options()
-
-        return os.path.dirname(dist_cobj.install_headers)
diff --git a/wrap/python/pybind11/pybind11/_version.py b/wrap/python/pybind11/pybind11/_version.py
deleted file mode 100644
index fef541bdb..000000000
--- a/wrap/python/pybind11/pybind11/_version.py
+++ /dev/null
@@ -1,2 +0,0 @@
-version_info = (2, 3, 'dev1')
-__version__ = '.'.join(map(str, version_info))
diff --git a/wrap/requirements.txt b/wrap/requirements.txt
new file mode 100644
index 000000000..210dfec50
--- /dev/null
+++ b/wrap/requirements.txt
@@ -0,0 +1 @@
+pyparsing
diff --git a/wrap/sphinx/Makefile b/wrap/sphinx/Makefile
new file mode 100644
index 000000000..4c1a0febd
--- /dev/null
+++ b/wrap/sphinx/Makefile
@@ -0,0 +1,216 @@
+# Makefile for Sphinx documentation
+#
+
+# You can set these variables from the command line.
+SPHINXOPTS    =
+SPHINXBUILD   = sphinx-build
+PAPER         =
+BUILDDIR      = _build
+
+# User-friendly check for sphinx-build
+ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1)
+$(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don't have Sphinx installed, grab it from http://sphinx-doc.org/)
+endif
+
+# Internal variables.
+PAPEROPT_a4     = -D latex_paper_size=a4
+PAPEROPT_letter = -D latex_paper_size=letter
+ALLSPHINXOPTS   = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) .
+# the i18n builder cannot share the environment and doctrees with the others
+I18NSPHINXOPTS  = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) .
+
+.PHONY: help
+help:
+	@echo "Please use \`make <target>' where <target> is one of"
+	@echo "  html       to make standalone HTML files"
+	@echo "  dirhtml    to make HTML files named index.html in directories"
+	@echo "  singlehtml to make a single large HTML file"
+	@echo "  pickle     to make pickle files"
+	@echo "  json       to make JSON files"
+	@echo "  htmlhelp   to make HTML files and a HTML help project"
+	@echo "  qthelp     to make HTML files and a qthelp project"
+	@echo "  applehelp  to make an Apple Help Book"
+	@echo "  devhelp    to make HTML files and a Devhelp project"
+	@echo "  epub       to make an epub"
+	@echo "  latex      to make LaTeX files, you can set PAPER=a4 or PAPER=letter"
+	@echo "  latexpdf   to make LaTeX files and run them through pdflatex"
+	@echo "  latexpdfja to make LaTeX files and run them through platex/dvipdfmx"
+	@echo "  text       to make text files"
+	@echo "  man        to make manual pages"
+	@echo "  texinfo    to make Texinfo files"
+	@echo "  info       to make Texinfo files and run them through makeinfo"
+	@echo "  gettext    to make PO message catalogs"
+	@echo "  changes    to make an overview of all changed/added/deprecated items"
+	@echo "  xml        to make Docutils-native XML files"
+	@echo "  pseudoxml  to make pseudoxml-XML files for display purposes"
+	@echo "  linkcheck  to check all external links for integrity"
+	@echo "  doctest    to run all doctests embedded in the documentation (if enabled)"
+	@echo "  coverage   to run coverage check of the documentation (if enabled)"
+
+.PHONY: clean
+clean:
+	rm -rf $(BUILDDIR)/*
+
+.PHONY: html
+html:
+	$(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html
+	@echo
+	@echo "Build finished. The HTML pages are in $(BUILDDIR)/html."
+
+.PHONY: dirhtml
+dirhtml:
+	$(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml
+	@echo
+	@echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml."
+
+.PHONY: singlehtml
+singlehtml:
+	$(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml
+	@echo
+	@echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml."
+
+.PHONY: pickle
+pickle:
+	$(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle
+	@echo
+	@echo "Build finished; now you can process the pickle files."
+
+.PHONY: json
+json:
+	$(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json
+	@echo
+	@echo "Build finished; now you can process the JSON files."
+
+.PHONY: htmlhelp
+htmlhelp:
+	$(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp
+	@echo
+	@echo "Build finished; now you can run HTML Help Workshop with the" \
+	      ".hhp project file in $(BUILDDIR)/htmlhelp."
+
+.PHONY: qthelp
+qthelp:
+	$(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp
+	@echo
+	@echo "Build finished; now you can run "qcollectiongenerator" with the" \
+	      ".qhcp project file in $(BUILDDIR)/qthelp, like this:"
+	@echo "# qcollectiongenerator $(BUILDDIR)/qthelp/python_example.qhcp"
+	@echo "To view the help file:"
+	@echo "# assistant -collectionFile $(BUILDDIR)/qthelp/python_example.qhc"
+
+.PHONY: applehelp
+applehelp:
+	$(SPHINXBUILD) -b applehelp $(ALLSPHINXOPTS) $(BUILDDIR)/applehelp
+	@echo
+	@echo "Build finished. The help book is in $(BUILDDIR)/applehelp."
+	@echo "N.B. You won't be able to view it unless you put it in" \
+	      "~/Library/Documentation/Help or install it in your application" \
+	      "bundle."
+
+.PHONY: devhelp
+devhelp:
+	$(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp
+	@echo
+	@echo "Build finished."
+	@echo "To view the help file:"
+	@echo "# mkdir -p $$HOME/.local/share/devhelp/python_example"
+	@echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/python_example"
+	@echo "# devhelp"
+
+.PHONY: epub
+epub:
+	$(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub
+	@echo
+	@echo "Build finished. The epub file is in $(BUILDDIR)/epub."
+
+.PHONY: latex
+latex:
+	$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex
+	@echo
+	@echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex."
+	@echo "Run \`make' in that directory to run these through (pdf)latex" \
+	      "(use \`make latexpdf' here to do that automatically)."
+
+.PHONY: latexpdf
+latexpdf:
+	$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex
+	@echo "Running LaTeX files through pdflatex..."
+	$(MAKE) -C $(BUILDDIR)/latex all-pdf
+	@echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex."
+
+.PHONY: latexpdfja
+latexpdfja:
+	$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex
+	@echo "Running LaTeX files through platex and dvipdfmx..."
+	$(MAKE) -C $(BUILDDIR)/latex all-pdf-ja
+	@echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex."
+
+.PHONY: text
+text:
+	$(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text
+	@echo
+	@echo "Build finished. The text files are in $(BUILDDIR)/text."
+
+.PHONY: man
+man:
+	$(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man
+	@echo
+	@echo "Build finished. The manual pages are in $(BUILDDIR)/man."
+
+.PHONY: texinfo
+texinfo:
+	$(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo
+	@echo
+	@echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo."
+	@echo "Run \`make' in that directory to run these through makeinfo" \
+	      "(use \`make info' here to do that automatically)."
+
+.PHONY: info
+info:
+	$(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo
+	@echo "Running Texinfo files through makeinfo..."
+	make -C $(BUILDDIR)/texinfo info
+	@echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo."
+
+.PHONY: gettext
+gettext:
+	$(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale
+	@echo
+	@echo "Build finished. The message catalogs are in $(BUILDDIR)/locale."
+
+.PHONY: changes
+changes:
+	$(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes
+	@echo
+	@echo "The overview file is in $(BUILDDIR)/changes."
+
+.PHONY: linkcheck
+linkcheck:
+	$(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck
+	@echo
+	@echo "Link check complete; look for any errors in the above output " \
+	      "or in $(BUILDDIR)/linkcheck/output.txt."
+
+.PHONY: doctest
+doctest:
+	$(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest
+	@echo "Testing of doctests in the sources finished, look at the " \
+	      "results in $(BUILDDIR)/doctest/output.txt."
+
+.PHONY: coverage
+coverage:
+	$(SPHINXBUILD) -b coverage $(ALLSPHINXOPTS) $(BUILDDIR)/coverage
+	@echo "Testing of coverage in the sources finished, look at the " \
+	      "results in $(BUILDDIR)/coverage/python.txt."
+
+.PHONY: xml
+xml:
+	$(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml
+	@echo
+	@echo "Build finished. The XML files are in $(BUILDDIR)/xml."
+
+.PHONY: pseudoxml
+pseudoxml:
+	$(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml
+	@echo
+	@echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml."
diff --git a/wrap/sphinx/conf.py b/wrap/sphinx/conf.py
new file mode 100644
index 000000000..568d50228
--- /dev/null
+++ b/wrap/sphinx/conf.py
@@ -0,0 +1,294 @@
+# -*- coding: utf-8 -*-
+#
+# python_exameple documentation build configuration file, created by
+# sphinx-quickstart on Fri Feb 26 00:29:33 2016.
+#
+# This file is execfile()d with the current directory set to its
+# containing dir.
+#
+# Note that not all possible configuration values are present in this
+# autogenerated file.
+#
+# All configuration values have a default; values that are commented out
+# serve to show the default.
+
+import sys
+import os
+
+# If extensions (or modules to document with autodoc) are in another directory,
+# add these directories to sys.path here. If the directory is relative to the
+# documentation root, use os.path.abspath to make it absolute, like shown here.
+#sys.path.insert(0, os.path.abspath('.'))
+sys.path.insert(0, os.path.abspath('..'))
+
+# -- General configuration ------------------------------------------------
+
+# If your documentation needs a minimal Sphinx version, state it here.
+#needs_sphinx = '1.0'
+
+# Add any Sphinx extension module names here, as strings. They can be
+# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
+# ones.
+extensions = [
+    'sphinx.ext.autodoc',
+    'sphinx.ext.intersphinx',
+    'sphinx.ext.autosummary',
+    'sphinx.ext.napoleon',
+]
+
+autosummary_generate = True
+
+# Add any paths that contain templates here, relative to this directory.
+templates_path = ['_templates']
+
+# The suffix(es) of source filenames.
+# You can specify multiple suffix as a list of string:
+# source_suffix = ['.rst', '.md']
+source_suffix = '.rst'
+
+# The encoding of source files.
+#source_encoding = 'utf-8-sig'
+
+# The master toctree document.
+master_doc = 'index'
+
+# General information about the project.
+project = u'GTSAM Pybind Wrapper'
+author = u'Matthew Sklar'
+
+# The version info for the project you're documenting, acts as replacement for
+# |version| and |release|, also used in various other places throughout the
+# built documents.
+#
+# The short X.Y version.
+version = u'0.0.1'
+# The full version, including alpha/beta/rc tags.
+release = u'0.0.1'
+
+# The language for content autogenerated by Sphinx. Refer to documentation
+# for a list of supported languages.
+#
+# This is also used if you do content translation via gettext catalogs.
+# Usually you set "language" from the command line for these cases.
+language = None
+
+# There are two options for replacing |today|: either, you set today to some
+# non-false value, then it is used:
+#today = ''
+# Else, today_fmt is used as the format for a strftime call.
+#today_fmt = '%B %d, %Y'
+
+# List of patterns, relative to source directory, that match files and
+# directories to ignore when looking for source files.
+exclude_patterns = ['_build']
+
+# The reST default role (used for this markup: `text`) to use for all
+# documents.
+#default_role = None
+
+# If true, '()' will be appended to :func: etc. cross-reference text.
+#add_function_parentheses = True
+
+# If true, the current module name will be prepended to all description
+# unit titles (such as .. function::).
+#add_module_names = True
+
+# If true, sectionauthor and moduleauthor directives will be shown in the
+# output. They are ignored by default.
+#show_authors = False
+
+# The name of the Pygments (syntax highlighting) style to use.
+pygments_style = 'sphinx'
+
+# A list of ignored prefixes for module index sorting.
+#modindex_common_prefix = []
+
+# If true, keep warnings as "system message" paragraphs in the built documents.
+#keep_warnings = False
+
+# If true, `todo` and `todoList` produce output, else they produce nothing.
+todo_include_todos = False
+
+
+# -- Options for HTML output ----------------------------------------------
+
+# The theme to use for HTML and HTML Help pages.  See the documentation for
+# a list of builtin themes.
+html_theme = 'alabaster'
+
+# Theme options are theme-specific and customize the look and feel of a theme
+# further.  For a list of options available for each theme, see the
+# documentation.
+#html_theme_options = {}
+
+# Add any paths that contain custom themes here, relative to this directory.
+#html_theme_path = []
+
+# The name for this set of Sphinx documents.  If None, it defaults to
+# "<project> v<release> documentation".
+#html_title = None
+
+# A shorter title for the navigation bar.  Default is the same as html_title.
+#html_short_title = None
+
+# The name of an image file (relative to this directory) to place at the top
+# of the sidebar.
+#html_logo = None
+
+# The name of an image file (within the static path) to use as favicon of the
+# docs.  This file should be a Windows icon file (.ico) being 16x16 or 32x32
+# pixels large.
+#html_favicon = None
+
+# Add any paths that contain custom static files (such as style sheets) here,
+# relative to this directory. They are copied after the builtin static files,
+# so a file named "default.css" will overwrite the builtin "default.css".
+html_static_path = ['_static']
+
+# Add any extra paths that contain custom files (such as robots.txt or
+# .htaccess) here, relative to this directory. These files are copied
+# directly to the root of the documentation.
+#html_extra_path = []
+
+# If not '', a 'Last updated on:' timestamp is inserted at every page bottom,
+# using the given strftime format.
+#html_last_updated_fmt = '%b %d, %Y'
+
+# If true, SmartyPants will be used to convert quotes and dashes to
+# typographically correct entities.
+#html_use_smartypants = True
+
+# Custom sidebar templates, maps document names to template names.
+#html_sidebars = {}
+
+# Additional templates that should be rendered to pages, maps page names to
+# template names.
+#html_additional_pages = {}
+
+# If false, no module index is generated.
+#html_domain_indices = True
+
+# If false, no index is generated.
+#html_use_index = True
+
+# If true, the index is split into individual pages for each letter.
+#html_split_index = False
+
+# If true, links to the reST sources are added to the pages.
+#html_show_sourcelink = True
+
+# If true, "Created using Sphinx" is shown in the HTML footer. Default is True.
+#html_show_sphinx = True
+
+# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True.
+#html_show_copyright = True
+
+# If true, an OpenSearch description file will be output, and all pages will
+# contain a <link> tag referring to it.  The value of this option must be the
+# base URL from which the finished HTML is served.
+#html_use_opensearch = ''
+
+# This is the file name suffix for HTML files (e.g. ".xhtml").
+#html_file_suffix = None
+
+# Language to be used for generating the HTML full-text search index.
+# Sphinx supports the following languages:
+#   'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja'
+#   'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr'
+#html_search_language = 'en'
+
+# A dictionary with options for the search language support, empty by default.
+# Now only 'ja' uses this config value
+#html_search_options = {'type': 'default'}
+
+# The name of a javascript file (relative to the configuration directory) that
+# implements a search results scorer. If empty, the default will be used.
+#html_search_scorer = 'scorer.js'
+
+# Output file base name for HTML help builder.
+htmlhelp_basename = 'pybind_wrapper_doc'
+
+# -- Options for LaTeX output ---------------------------------------------
+
+latex_elements = {
+# The paper size ('letterpaper' or 'a4paper').
+#'papersize': 'letterpaper',
+
+# The font size ('10pt', '11pt' or '12pt').
+#'pointsize': '10pt',
+
+# Additional stuff for the LaTeX preamble.
+#'preamble': '',
+
+# Latex figure (float) alignment
+#'figure_align': 'htbp',
+}
+
+# Grouping the document tree into LaTeX files. List of tuples
+# (source start file, target name, title,
+#  author, documentclass [howto, manual, or own class]).
+latex_documents = [
+    (master_doc, 'pybind_wrapper.tex', u'pybind_wrapper Documentation',
+     u'Matthew Sklar', 'manual'),
+]
+
+# The name of an image file (relative to this directory) to place at the top of
+# the title page.
+#latex_logo = None
+
+# For "manual" documents, if this is true, then toplevel headings are parts,
+# not chapters.
+#latex_use_parts = False
+
+# If true, show page references after internal links.
+#latex_show_pagerefs = False
+
+# If true, show URL addresses after external links.
+#latex_show_urls = False
+
+# Documents to append as an appendix to all manuals.
+#latex_appendices = []
+
+# If false, no module index is generated.
+#latex_domain_indices = True
+
+
+# -- Options for manual page output ---------------------------------------
+
+# One entry per manual page. List of tuples
+# (source start file, name, description, authors, manual section).
+man_pages = [
+    (master_doc, 'pybind_wrapper', u'pybind_wrapper Documentation',
+     [author], 1)
+]
+
+# If true, show URL addresses after external links.
+#man_show_urls = False
+
+
+# -- Options for Texinfo output -------------------------------------------
+
+# Grouping the document tree into Texinfo files. List of tuples
+# (source start file, target name, title, author,
+#  dir menu entry, description, category)
+texinfo_documents = [
+    (master_doc, 'pybind_wrapper', u'pybind_wrapper Documentation',
+     author, 'pybind_wrapper', 'One line description of project.',
+     'Miscellaneous'),
+]
+
+# Documents to append as an appendix to all manuals.
+#texinfo_appendices = []
+
+# If false, no module index is generated.
+#texinfo_domain_indices = True
+
+# How to display URL addresses: 'footnote', 'no', or 'inline'.
+#texinfo_show_urls = 'footnote'
+
+# If true, do not generate a @detailmenu in the "Top" node's menu.
+#texinfo_no_detailmenu = False
+
+
+# Example configuration for intersphinx: refer to the Python standard library.
+intersphinx_mapping = {'https://docs.python.org/': None}
diff --git a/wrap/sphinx/index.rst b/wrap/sphinx/index.rst
new file mode 100644
index 000000000..32466681d
--- /dev/null
+++ b/wrap/sphinx/index.rst
@@ -0,0 +1,10 @@
+pybind_wrapper Documentation
+============================
+
+Contents:
+
+.. toctree::
+   :maxdepth: 2
+
+   wrap
+
diff --git a/wrap/sphinx/make.bat b/wrap/sphinx/make.bat
new file mode 100755
index 000000000..460bef91c
--- /dev/null
+++ b/wrap/sphinx/make.bat
@@ -0,0 +1,263 @@
+@ECHO OFF
+
+REM Command file for Sphinx documentation
+
+if "%SPHINXBUILD%" == "" (
+	set SPHINXBUILD=sphinx-build
+)
+set BUILDDIR=_build
+set ALLSPHINXOPTS=-d %BUILDDIR%/doctrees %SPHINXOPTS% .
+set I18NSPHINXOPTS=%SPHINXOPTS% .
+if NOT "%PAPER%" == "" (
+	set ALLSPHINXOPTS=-D latex_paper_size=%PAPER% %ALLSPHINXOPTS%
+	set I18NSPHINXOPTS=-D latex_paper_size=%PAPER% %I18NSPHINXOPTS%
+)
+
+if "%1" == "" goto help
+
+if "%1" == "help" (
+	:help
+	echo.Please use `make ^<target^>` where ^<target^> is one of
+	echo.  html       to make standalone HTML files
+	echo.  dirhtml    to make HTML files named index.html in directories
+	echo.  singlehtml to make a single large HTML file
+	echo.  pickle     to make pickle files
+	echo.  json       to make JSON files
+	echo.  htmlhelp   to make HTML files and a HTML help project
+	echo.  qthelp     to make HTML files and a qthelp project
+	echo.  devhelp    to make HTML files and a Devhelp project
+	echo.  epub       to make an epub
+	echo.  latex      to make LaTeX files, you can set PAPER=a4 or PAPER=letter
+	echo.  text       to make text files
+	echo.  man        to make manual pages
+	echo.  texinfo    to make Texinfo files
+	echo.  gettext    to make PO message catalogs
+	echo.  changes    to make an overview over all changed/added/deprecated items
+	echo.  xml        to make Docutils-native XML files
+	echo.  pseudoxml  to make pseudoxml-XML files for display purposes
+	echo.  linkcheck  to check all external links for integrity
+	echo.  doctest    to run all doctests embedded in the documentation if enabled
+	echo.  coverage   to run coverage check of the documentation if enabled
+	goto end
+)
+
+if "%1" == "clean" (
+	for /d %%i in (%BUILDDIR%\*) do rmdir /q /s %%i
+	del /q /s %BUILDDIR%\*
+	goto end
+)
+
+
+REM Check if sphinx-build is available and fallback to Python version if any
+%SPHINXBUILD% 1>NUL 2>NUL
+if errorlevel 9009 goto sphinx_python
+goto sphinx_ok
+
+:sphinx_python
+
+set SPHINXBUILD=python -m sphinx.__init__
+%SPHINXBUILD% 2> nul
+if errorlevel 9009 (
+	echo.
+	echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
+	echo.installed, then set the SPHINXBUILD environment variable to point
+	echo.to the full path of the 'sphinx-build' executable. Alternatively you
+	echo.may add the Sphinx directory to PATH.
+	echo.
+	echo.If you don't have Sphinx installed, grab it from
+	echo.http://sphinx-doc.org/
+	exit /b 1
+)
+
+:sphinx_ok
+
+
+if "%1" == "html" (
+	%SPHINXBUILD% -b html %ALLSPHINXOPTS% %BUILDDIR%/html
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished. The HTML pages are in %BUILDDIR%/html.
+	goto end
+)
+
+if "%1" == "dirhtml" (
+	%SPHINXBUILD% -b dirhtml %ALLSPHINXOPTS% %BUILDDIR%/dirhtml
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished. The HTML pages are in %BUILDDIR%/dirhtml.
+	goto end
+)
+
+if "%1" == "singlehtml" (
+	%SPHINXBUILD% -b singlehtml %ALLSPHINXOPTS% %BUILDDIR%/singlehtml
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished. The HTML pages are in %BUILDDIR%/singlehtml.
+	goto end
+)
+
+if "%1" == "pickle" (
+	%SPHINXBUILD% -b pickle %ALLSPHINXOPTS% %BUILDDIR%/pickle
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished; now you can process the pickle files.
+	goto end
+)
+
+if "%1" == "json" (
+	%SPHINXBUILD% -b json %ALLSPHINXOPTS% %BUILDDIR%/json
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished; now you can process the JSON files.
+	goto end
+)
+
+if "%1" == "htmlhelp" (
+	%SPHINXBUILD% -b htmlhelp %ALLSPHINXOPTS% %BUILDDIR%/htmlhelp
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished; now you can run HTML Help Workshop with the ^
+.hhp project file in %BUILDDIR%/htmlhelp.
+	goto end
+)
+
+if "%1" == "qthelp" (
+	%SPHINXBUILD% -b qthelp %ALLSPHINXOPTS% %BUILDDIR%/qthelp
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished; now you can run "qcollectiongenerator" with the ^
+.qhcp project file in %BUILDDIR%/qthelp, like this:
+	echo.^> qcollectiongenerator %BUILDDIR%\qthelp\python_example.qhcp
+	echo.To view the help file:
+	echo.^> assistant -collectionFile %BUILDDIR%\qthelp\python_example.ghc
+	goto end
+)
+
+if "%1" == "devhelp" (
+	%SPHINXBUILD% -b devhelp %ALLSPHINXOPTS% %BUILDDIR%/devhelp
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished.
+	goto end
+)
+
+if "%1" == "epub" (
+	%SPHINXBUILD% -b epub %ALLSPHINXOPTS% %BUILDDIR%/epub
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished. The epub file is in %BUILDDIR%/epub.
+	goto end
+)
+
+if "%1" == "latex" (
+	%SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished; the LaTeX files are in %BUILDDIR%/latex.
+	goto end
+)
+
+if "%1" == "latexpdf" (
+	%SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex
+	cd %BUILDDIR%/latex
+	make all-pdf
+	cd %~dp0
+	echo.
+	echo.Build finished; the PDF files are in %BUILDDIR%/latex.
+	goto end
+)
+
+if "%1" == "latexpdfja" (
+	%SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex
+	cd %BUILDDIR%/latex
+	make all-pdf-ja
+	cd %~dp0
+	echo.
+	echo.Build finished; the PDF files are in %BUILDDIR%/latex.
+	goto end
+)
+
+if "%1" == "text" (
+	%SPHINXBUILD% -b text %ALLSPHINXOPTS% %BUILDDIR%/text
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished. The text files are in %BUILDDIR%/text.
+	goto end
+)
+
+if "%1" == "man" (
+	%SPHINXBUILD% -b man %ALLSPHINXOPTS% %BUILDDIR%/man
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished. The manual pages are in %BUILDDIR%/man.
+	goto end
+)
+
+if "%1" == "texinfo" (
+	%SPHINXBUILD% -b texinfo %ALLSPHINXOPTS% %BUILDDIR%/texinfo
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished. The Texinfo files are in %BUILDDIR%/texinfo.
+	goto end
+)
+
+if "%1" == "gettext" (
+	%SPHINXBUILD% -b gettext %I18NSPHINXOPTS% %BUILDDIR%/locale
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished. The message catalogs are in %BUILDDIR%/locale.
+	goto end
+)
+
+if "%1" == "changes" (
+	%SPHINXBUILD% -b changes %ALLSPHINXOPTS% %BUILDDIR%/changes
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.The overview file is in %BUILDDIR%/changes.
+	goto end
+)
+
+if "%1" == "linkcheck" (
+	%SPHINXBUILD% -b linkcheck %ALLSPHINXOPTS% %BUILDDIR%/linkcheck
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Link check complete; look for any errors in the above output ^
+or in %BUILDDIR%/linkcheck/output.txt.
+	goto end
+)
+
+if "%1" == "doctest" (
+	%SPHINXBUILD% -b doctest %ALLSPHINXOPTS% %BUILDDIR%/doctest
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Testing of doctests in the sources finished, look at the ^
+results in %BUILDDIR%/doctest/output.txt.
+	goto end
+)
+
+if "%1" == "coverage" (
+	%SPHINXBUILD% -b coverage %ALLSPHINXOPTS% %BUILDDIR%/coverage
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Testing of coverage in the sources finished, look at the ^
+results in %BUILDDIR%/coverage/python.txt.
+	goto end
+)
+
+if "%1" == "xml" (
+	%SPHINXBUILD% -b xml %ALLSPHINXOPTS% %BUILDDIR%/xml
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished. The XML files are in %BUILDDIR%/xml.
+	goto end
+)
+
+if "%1" == "pseudoxml" (
+	%SPHINXBUILD% -b pseudoxml %ALLSPHINXOPTS% %BUILDDIR%/pseudoxml
+	if errorlevel 1 exit /b 1
+	echo.
+	echo.Build finished. The pseudo-XML files are in %BUILDDIR%/pseudoxml.
+	goto end
+)
+
+:end
diff --git a/wrap/sphinx/wrap.rst b/wrap/sphinx/wrap.rst
new file mode 100644
index 000000000..2048d23f7
--- /dev/null
+++ b/wrap/sphinx/wrap.rst
@@ -0,0 +1 @@
+.. automodule:: python_example
diff --git a/wrap/spirit.h b/wrap/spirit.h
deleted file mode 100644
index 202b7e56d..000000000
--- a/wrap/spirit.h
+++ /dev/null
@@ -1,172 +0,0 @@
-/**
- * @file spirit_actors.h
- *
- * @brief Additional utilities and actors for the wrap parser
- *
- * @date Dec 8, 2011
- * @author Alex Cunningham
- * @author Frank Dellaert
- */
-
-#pragma once
-
-#include <boost/spirit/include/classic_core.hpp>
-#include <boost/spirit/include/classic_push_back_actor.hpp>
-#include <boost/spirit/include/classic_clear_actor.hpp>
-#include <boost/spirit/include/classic_assign_actor.hpp>
-#include <boost/spirit/include/classic_confix.hpp>
-#include <boost/spirit/include/classic_increment_actor.hpp>
-
-namespace boost {
-namespace spirit {
-
-BOOST_SPIRIT_CLASSIC_NAMESPACE_BEGIN
-
-///////////////////////////////////////////////////////////////////////////
-//  Summary:
-//  A semantic action policy that calls pop_back method.
-//  (This doc uses convention available in actors.hpp)
-//  Note that this action performs a "safe" pop, that checks the size
-//  before popping to avoid segfaults
-//
-//  Actions (what it does):
-//      ref.pop_back();
-//
-//  Policy name:
-//      pop_action
-//
-//  Policy holder, corresponding helper method:
-//      ref_actor, clear_a( ref );
-//
-//  () operators: both.
-//
-//  See also ref_actor for more details.
-///////////////////////////////////////////////////////////////////////////
-struct pop_action
-{
-  template< typename T>
-  void act(T& ref_) const
-  {
-    if (!ref_.empty())
-    ref_.pop_back();
-  }
-};
-
-///////////////////////////////////////////////////////////////////////////
-// helper method that creates a and_assign_actor.
-///////////////////////////////////////////////////////////////////////////
-template<typename T>
-inline ref_actor<T,pop_action> pop_a(T& ref_)
-{
-  return ref_actor<T,pop_action>(ref_);
-}
-
-///////////////////////////////////////////////////////////////////////////
-//  Summary:
-//
-//  A semantic action policy that appends a set of values to the back of a
-//  container.
-//  (This doc uses convention available in actors.hpp)
-//
-//  Actions (what it does and what ref, value_ref must support):
-//      ref.push_back( values );
-//      ref.push_back( T::value_type(first,last) );
-//      ref.push_back( value_ref );
-//
-//  Policy name:
-//      append_action
-//
-//  Policy holder, corresponding helper method:
-//      ref_value_actor, append_a( ref );
-//      ref_const_ref_actor, append_a( ref, value_ref );
-//
-//  () operators: both
-//
-//  See also ref_value_actor and ref_const_ref_actor for more details.
-///////////////////////////////////////////////////////////////////////////
-
-struct append_action
-{
-  template< typename T, typename ValueT >
-  void act(T& ref_, ValueT const& value_) const
-  {
-    ref_.insert(ref_.begin(), value_.begin(), value_.end());
-  }
-
-  template<typename T,typename IteratorT>
-  void act(
-      T& ref_,
-      IteratorT const& first_,
-      IteratorT const& last_
-  ) const
-  {
-    ref_.insert(ref_.end(), first_, last_);
-  }
-};
-
-template<typename T>
-inline ref_value_actor<T,append_action>
-append_a(T& ref_)
-{
-  return ref_value_actor<T,append_action>(ref_);
-}
-
-template<typename T,typename ValueT>
-inline ref_const_ref_actor<T,ValueT,append_action>
-append_a(
-    T& ref_,
-    ValueT const& value_
-)
-{
-  return ref_const_ref_actor<T,ValueT,append_action>(ref_,value_);
-}
-
-BOOST_SPIRIT_CLASSIC_NAMESPACE_END
-
-}
-}
-
-namespace wrap {
-
-namespace classic = BOOST_SPIRIT_CLASSIC_NS;
-
-/// Some basic rules used by all parsers
-template<typename ScannerT>
-struct BasicRules {
-
-  classic::rule<ScannerT> comments_p, basisType_p, eigenType_p, keywords_p,
-      stlType_p, name_p, className_p, namespace_p;
-
-  BasicRules() {
-
-    using classic::comment_p;
-    using classic::eol_p;
-    using classic::str_p;
-    using classic::alpha_p;
-    using classic::lexeme_d;
-    using classic::upper_p;
-    using classic::lower_p;
-    using classic::alnum_p;
-
-    comments_p = comment_p("/*", "*/") | comment_p("//", eol_p);
-
-    basisType_p = (str_p("string") | "bool" | "size_t" | "int" | "double"
-        | "char" | "unsigned char");
-
-    eigenType_p = (str_p("Vector") | "Matrix");
-
-    keywords_p =
-        (str_p("const") | "static" | "namespace" | "void" | basisType_p);
-
-    stlType_p = (str_p("vector") | "list");
-
-    name_p = lexeme_d[alpha_p >> *(alnum_p | '_')];
-
-    className_p = (lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p
-        - keywords_p) | stlType_p;
-
-    namespace_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p;
-  }
-};
-}
-
diff --git a/wrap/template_instantiator.py b/wrap/template_instantiator.py
new file mode 100644
index 000000000..3d98e9699
--- /dev/null
+++ b/wrap/template_instantiator.py
@@ -0,0 +1,346 @@
+import interface_parser as parser
+
+
+def instantiate_type(ctype, template_typenames, instantiations, cpp_typename, instantiated_class=None):
+    """
+    Instantiate template typename for @p ctype.
+    @return If ctype's name is in the @p template_typenames, return the
+        corresponding type to replace in @p instantiations.
+        If ctype name is `This`, return the new typename @p `cpp_typename`.
+        Otherwise, return the original ctype.
+    """
+    str_arg_typename = str(ctype.typename)
+    if str_arg_typename in template_typenames:
+        idx = template_typenames.index(str_arg_typename)
+        return parser.Type(
+            typename=instantiations[idx],
+            is_const=ctype.is_const,
+            is_ptr=ctype.is_ptr,
+            is_ref=ctype.is_ref,
+            is_basis=ctype.is_basis,
+        )
+    elif str_arg_typename == 'This':
+        # import sys
+        if instantiated_class:
+            name = instantiated_class.original.name
+            namespaces_name = instantiated_class.namespaces()
+            namespaces_name.append(name)
+            # print("INST: {}, {}, CPP: {}, CLS: {}".format(
+            #     ctype, instantiations, cpp_typename, instantiated_class.instantiations
+            # ), file=sys.stderr)
+            cpp_typename = parser.Typename(
+                namespaces_name, instantiations=[inst for inst in instantiated_class.instantiations]
+            )
+        return parser.Type(
+            typename=cpp_typename,
+            is_const=ctype.is_const,
+            is_ptr=ctype.is_ptr,
+            is_ref=ctype.is_ref,
+            is_basis=ctype.is_basis,
+        )
+    else:
+        return ctype
+
+
+def instantiate_args_list(args_list, template_typenames, instantiations,
+                          cpp_typename):
+    """
+    Instantiate template typenames in an argument list.
+    Type with name `This` will be replaced by @p `cpp_typename`.
+
+    @param[in] args_list A list of `parser.Argument` to instantiate.
+    @param[in] template_typenames List of template typenames to instantiate,
+        e.g. ['T', 'U', 'V'].
+    @param[in] instantiations List of specific types to instantiate, each
+        associated with each template typename. Each type is a parser.Typename,
+        including its name and full namespaces.
+    @param[in] cpp_typename Full-namespace cpp class name of this instantiation
+        to replace for arguments of type named `This`.
+    @return A new list of parser.Argument which types are replaced with their
+        instantiations.
+    """
+    instantiated_args = []
+    for arg in args_list:
+        new_type = instantiate_type(
+            arg.ctype, template_typenames, instantiations, cpp_typename)
+        instantiated_args.append(
+            parser.Argument(name=arg.name, ctype=new_type))
+    return instantiated_args
+
+
+def instantiate_return_type(return_type, template_typenames, instantiations,
+                            cpp_typename, instantiated_class=None):
+    new_type1 = instantiate_type(
+        return_type.type1, template_typenames, instantiations, cpp_typename, instantiated_class=instantiated_class)
+    if return_type.type2:
+        new_type2 = instantiate_type(
+            return_type.type2, template_typenames, instantiations,
+            cpp_typename, instantiated_class=instantiated_class)
+    else:
+        new_type2 = ''
+    return parser.ReturnType(new_type1, new_type2)
+
+
+def instantiate_name(original_name, instantiations):
+    """
+    Concatenate instantiated types with an @p original name to form a new
+    instantiated name.
+    TODO(duy): To avoid conflicts, we should include the instantiation's
+    namespaces, but I find that too verbose.
+    """
+    inst_name = ''
+
+    return "{}{}".format(original_name, "".join(
+        [inst.instantiated_name() for inst in instantiations]))
+
+
+class InstantiatedMethod(parser.Method):
+    """
+    We can only instantiate template methods with a single template parameter.
+    """
+
+    def __init__(self, original, instantiation=''):
+        self.original = original
+        self.instantiation = instantiation
+        self.template = ''
+        self.is_const = original.is_const
+        self.parent = original.parent
+
+        if not original.template:
+            self.name = original.name
+            self.return_type = original.return_type
+            self.args = original.args
+        else:
+            if len(self.original.template.typenames) > 1:
+                raise ValueError("Can only instantiate template method with "
+                                 "single template parameter.")
+            self.name = instantiate_name(original.name, [self.instantiation])
+            self.return_type = instantiate_return_type(
+                original.return_type,
+                [self.original.template.typenames[0]],
+                [self.instantiation],
+                # Keyword type name `This` should already be replaced in the
+                # previous class template instantiation round.
+                cpp_typename='',
+            )
+            instantiated_args = instantiate_args_list(
+                original.args.args_list,
+                [self.original.template.typenames[0]],
+                [self.instantiation],
+                # Keyword type name `This` should already be replaced in the
+                # previous class template instantiation round.
+                cpp_typename='',
+            )
+            self.args = parser.ArgumentList(instantiated_args)
+
+    def to_cpp(self):
+        if self.original.template:
+            return "{}<{}>".format(self.original.name, self.instantiation)
+        else:
+            return self.original.name
+
+    def __repr__(self):
+        return "Instantiated {}".format(
+            super(InstantiatedMethod, self).__repr__()
+        )
+
+
+class InstantiatedClass(parser.Class):
+    def __init__(self, original, instantiations=[], new_name=''):
+        """
+        Template <T, U>
+        Instantiations: [T1, U1]
+        """
+        self.original = original
+        self.instantiations = instantiations
+
+        self.template = ''
+        self.is_virtual = original.is_virtual
+        self.parent_class = original.parent_class
+        self.parent = original.parent
+
+        if not original.template:
+            self.name = original.name
+            self.ctors = list(original.ctors)
+            self.static_methods = list(original.static_methods)
+            class_instantiated_methods = list(original.methods)
+            self.properties = list(original.properties)
+        else:
+            # Check conditions.
+            assert len(original.template.typenames) == len(
+                instantiations), "Typenames and instantiations mismatch!"
+
+            self.name = instantiate_name(
+                original.name, instantiations) if not new_name else new_name
+            self.ctors = self.instantiate_ctors()
+            self.static_methods = self.instantiate_static_methods()
+            class_instantiated_methods = \
+                self.instantiate_class_templates_in_methods()
+            self.properties = self.instantiate_properties()
+
+        # Second instantiation round to instantiate template methods.
+        self.methods = []
+        for method in class_instantiated_methods:
+            if not method.template:
+                self.methods.append(InstantiatedMethod(method, ''))
+            else:
+                assert len(
+                    method.template.typenames) == 1, ""\
+                    "Can only instantiate single template methods"
+                for inst in method.template.instantiations[0]:
+                    self.methods.append(InstantiatedMethod(method, inst))
+
+    def __repr__(self):
+        return "{virtual} class {name} [{cpp_class}]: {parent_class}\n"\
+            "{ctors}\n{static_methods}\n{methods}".format(
+               virtual="virtual" if self.is_virtual else '',
+               name=self.name,
+               cpp_class=self.cpp_class(),
+               parent_class=self.parent,
+               ctors="\n".join([ctor.__repr__() for ctor in self.ctors]),
+               methods="\n".join([m.__repr__() for m in self.methods]),
+               static_methods="\n".join([m.__repr__()
+                                         for m in self.static_methods]),
+            )
+
+    def instantiate_ctors(self):
+        instantiated_ctors = []
+        for ctor in self.original.ctors:
+            instantiated_args = instantiate_args_list(
+                ctor.args.args_list,
+                self.original.template.typenames,
+                self.instantiations,
+                self.cpp_typename(),
+            )
+            instantiated_ctors.append(parser.Constructor(
+                name=self.name,
+                args=parser.ArgumentList(instantiated_args),
+                parent=self,
+            ))
+        return instantiated_ctors
+
+    def instantiate_static_methods(self):
+        instantiated_static_methods = []
+        for static_method in self.original.static_methods:
+            instantiated_args = instantiate_args_list(
+                static_method.args.args_list,
+                self.original.template.typenames,
+                self.instantiations,
+                self.cpp_typename()
+            )
+            instantiated_static_methods.append(
+                parser.StaticMethod(
+                    name=static_method.name,
+                    return_type=instantiate_return_type(
+                        static_method.return_type,
+                        self.original.template.typenames,
+                        self.instantiations,
+                        self.cpp_typename(),
+                        instantiated_class=self
+                    ),
+                    args=parser.ArgumentList(instantiated_args),
+                    parent=self,
+                )
+            )
+        return instantiated_static_methods
+
+    def instantiate_class_templates_in_methods(self):
+        """
+        This function only instantiates class templates in the methods.
+        Template methods are instantiated in InstantiatedMethod in the second
+        round.
+        """
+        class_instantiated_methods = []
+        for method in self.original.methods:
+            instantiated_args = instantiate_args_list(
+                method.args.args_list,
+                self.original.template.typenames,
+                self.instantiations,
+                self.cpp_typename(),
+            )
+            class_instantiated_methods.append(parser.Method(
+                template=method.template,
+                name=method.name,
+                return_type=instantiate_return_type(
+                    method.return_type,
+                    self.original.template.typenames,
+                    self.instantiations,
+                    self.cpp_typename(),
+                ),
+                args=parser.ArgumentList(instantiated_args),
+                is_const=method.is_const,
+                parent=self,
+            ))
+        return class_instantiated_methods
+
+    def instantiate_properties(self):
+        instantiated_properties = instantiate_args_list(
+            self.original.properties,
+            self.original.template.typenames,
+            self.instantiations,
+            self.cpp_typename(),
+        )
+        return instantiated_properties
+
+    def cpp_class(self):
+        return self.cpp_typename().to_cpp()
+
+    def cpp_typename(self):
+        """
+        Return a parser.Typename including namespaces and cpp name of this
+        class.
+        """
+        if self.original.template:
+            name = "{}<{}>".format(
+                self.original.name,
+                ", ".join([inst.to_cpp() for inst in self.instantiations]))
+        else:
+            name = self.original.name
+        namespaces_name = self.namespaces()
+        namespaces_name.append(name)
+        return parser.Typename(namespaces_name)
+
+
+def instantiate_namespace_inplace(namespace):
+    """
+    @param[in/out] namespace The namespace which content will be replaced with
+        the instantiated content.
+    """
+    instantiated_content = []
+    typedef_content = []
+
+    for element in namespace.content:
+        if isinstance(element, parser.Class):
+            original_class = element
+            if not original_class.template:
+                instantiated_content.append(
+                    InstantiatedClass(original_class, []))
+            else:
+                if (len(original_class.template.typenames) > 1
+                        and original_class.template.instantiations[0]):
+                    raise ValueError(
+                        "Can't instantiate multi-parameter templates here. "
+                        "Please use typedef template instantiation."
+                    )
+                for inst in original_class.template.instantiations[0]:
+                    instantiated_content.append(
+                        InstantiatedClass(original_class, [inst]))
+        elif isinstance(element, parser.TypedefTemplateInstantiation):
+            typedef_inst = element
+            original_class = namespace.top_level().find_class(
+                typedef_inst.typename)
+            typedef_content.append(
+                InstantiatedClass(
+                    original_class,
+                    typedef_inst.typename.instantiations,
+                    typedef_inst.new_name
+                )
+            )
+        elif isinstance(element, parser.Namespace):
+            instantiate_namespace_inplace(element)
+            instantiated_content.append(element)
+        else:
+            instantiated_content.append(element)
+
+    instantiated_content.extend(typedef_content)
+    namespace.content = instantiated_content
diff --git a/wrap/tests/.gitignore b/wrap/tests/.gitignore
new file mode 100644
index 000000000..3169b1591
--- /dev/null
+++ b/wrap/tests/.gitignore
@@ -0,0 +1,3 @@
+actual-python/
+actual-matlab/
+actual-xml-generation/
\ No newline at end of file
diff --git a/wrap/tests/expected-cython/geometry.pxd b/wrap/tests/expected-cython/geometry.pxd
deleted file mode 100644
index 3527840a3..000000000
--- a/wrap/tests/expected-cython/geometry.pxd
+++ /dev/null
@@ -1,153 +0,0 @@
-
-from gtsam_eigency.core cimport *
-from libcpp.string cimport string
-from libcpp.vector cimport vector
-from libcpp.pair cimport pair
-from libcpp.set cimport set
-from libcpp.map cimport map
-from libcpp cimport bool
-
-cdef extern from "boost/shared_ptr.hpp" namespace "boost":
-    cppclass shared_ptr[T]:
-        shared_ptr()
-        shared_ptr(T*)
-        T* get()
-        long use_count() const
-        T& operator*()
-
-    cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)
-
-cdef extern from "gtsam/base/make_shared.h" namespace "gtsam":
-    cdef shared_ptr[T] make_shared[T](const T& r)
-
-cdef extern from "gtsam/geometry/Point2.h" namespace "gtsam":
-    cdef cppclass CPoint2 "gtsam::Point2":
-        CPoint2() except +
-        CPoint2(double x, double y) except +
-
-        void argChar(char a) except +
-        void argUChar(unsigned char a) except +
-        int dim() except +
-        void eigenArguments(const VectorXd& v, const MatrixXd& m) except +
-        char returnChar() except +
-        CVectorNotEigen vectorConfusion() except +
-        double x() except +
-        double y() except +
-
-cdef class Point2:
-    cdef shared_ptr[CPoint2] CPoint2_
-    @staticmethod
-    cdef Point2 cyCreateFromShared(const shared_ptr[CPoint2]& other)
-
-
-cdef extern from "gtsam/geometry/Point3.h" namespace "gtsam":
-    cdef cppclass CPoint3 "gtsam::Point3":
-        CPoint3(double x, double y, double z) except +
-
-        @staticmethod
-        CPoint3 StaticFunctionRet "StaticFunctionRet"(double z) except +
-        @staticmethod
-        double staticFunction "staticFunction"() except +
-
-        double norm() except +
-
-cdef class Point3:
-    cdef shared_ptr[CPoint3] CPoint3_
-    @staticmethod
-    cdef Point3 cyCreateFromShared(const shared_ptr[CPoint3]& other)
-
-
-
-cdef extern from "folder/path/to/Test.h":
-    cdef cppclass CTest "Test":
-        CTest() except +
-        CTest(double a, const MatrixXd& b) except +
-
-        void arg_EigenConstRef(const MatrixXd& value) except +
-        pair[CTest,shared_ptr[CTest]] create_MixedPtrs() except +
-        pair[shared_ptr[CTest],shared_ptr[CTest]] create_ptrs() except +
-        void print_ "print"() except +
-        shared_ptr[CPoint2] return_Point2Ptr(bool value) except +
-        CTest return_Test(shared_ptr[CTest]& value) except +
-        shared_ptr[CTest] return_TestPtr(shared_ptr[CTest]& value) except +
-        bool return_bool(bool value) except +
-        double return_double(double value) except +
-        bool return_field(const CTest& t) except +
-        int return_int(int value) except +
-        MatrixXd return_matrix1(const MatrixXd& value) except +
-        MatrixXd return_matrix2(const MatrixXd& value) except +
-        pair[VectorXd,MatrixXd] return_pair(const VectorXd& v, const MatrixXd& A) except +
-        pair[shared_ptr[CTest],shared_ptr[CTest]] return_ptrs(shared_ptr[CTest]& p1, shared_ptr[CTest]& p2) except +
-        size_t return_size_t(size_t value) except +
-        string return_string(string value) except +
-        VectorXd return_vector1(const VectorXd& value) except +
-        VectorXd return_vector2(const VectorXd& value) except +
-
-cdef class Test:
-    cdef shared_ptr[CTest] CTest_
-    @staticmethod
-    cdef Test cyCreateFromShared(const shared_ptr[CTest]& other)
-
-
-cdef extern from "folder/path/to/Test.h":
-    cdef cppclass CMyBase "MyBase":
-        pass
-
-cdef class MyBase:
-    cdef shared_ptr[CMyBase] CMyBase_
-    @staticmethod
-    cdef MyBase cyCreateFromShared(const shared_ptr[CMyBase]& other)
-
-
-cdef extern from "folder/path/to/Test.h":
-    cdef cppclass CMyTemplate "MyTemplate"[T](CMyBase):
-        CMyTemplate() except +
-
-        void accept_T(const T& value) except +
-        void accept_Tptr(shared_ptr[T]& value) except +
-        pair[T,shared_ptr[T]] create_MixedPtrs() except +
-        pair[shared_ptr[T],shared_ptr[T]] create_ptrs() except +
-        T return_T(shared_ptr[T]& value) except +
-        shared_ptr[T] return_Tptr(shared_ptr[T]& value) except +
-        pair[shared_ptr[T],shared_ptr[T]] return_ptrs(shared_ptr[T]& p1, shared_ptr[T]& p2) except +
-        ARG templatedMethod[ARG](const ARG& t) except +
-
-ctypedef CMyTemplate[CPoint2] CMyTemplatePoint2
-
-cdef class MyTemplatePoint2(MyBase):
-    cdef shared_ptr[CMyTemplatePoint2] CMyTemplatePoint2_
-    @staticmethod
-    cdef MyTemplatePoint2 cyCreateFromShared(const shared_ptr[CMyTemplatePoint2]& other)
-
-ctypedef CMyTemplate[MatrixXd] CMyTemplateMatrix
-
-cdef class MyTemplateMatrix(MyBase):
-    cdef shared_ptr[CMyTemplateMatrix] CMyTemplateMatrix_
-    @staticmethod
-    cdef MyTemplateMatrix cyCreateFromShared(const shared_ptr[CMyTemplateMatrix]& other)
-
-
-cdef extern from "folder/path/to/Test.h":
-    cdef cppclass CMyFactor "MyFactor"[POSE,POINT]:
-        CMyFactor(size_t key1, size_t key2, double measured, const shared_ptr[CnoiseModel_Base]& noiseModel) except +
-
-
-ctypedef CMyFactor[CPose2, MatrixXd] CMyFactorPosePoint2
-
-cdef class MyFactorPosePoint2:
-    cdef shared_ptr[CMyFactorPosePoint2] CMyFactorPosePoint2_
-    @staticmethod
-    cdef MyFactorPosePoint2 cyCreateFromShared(const shared_ptr[CMyFactorPosePoint2]& other)
-
-
-cdef extern from "folder/path/to/Test.h":
-    cdef cppclass CMyVector "MyVector"[N]:
-        CMyVector() except +
-
-
-
-cdef extern from "folder/path/to/Test.h" namespace "":
-        VectorXd pxd_aGlobalFunction "aGlobalFunction"() except +
-cdef extern from "folder/path/to/Test.h" namespace "":
-        VectorXd pxd_overloadedGlobalFunction "overloadedGlobalFunction"(int a) except +
-        VectorXd pxd_overloadedGlobalFunction "overloadedGlobalFunction"(int a, double b) except +
diff --git a/wrap/tests/expected-cython/geometry.pyx b/wrap/tests/expected-cython/geometry.pyx
deleted file mode 100644
index 606fde3e3..000000000
--- a/wrap/tests/expected-cython/geometry.pyx
+++ /dev/null
@@ -1,483 +0,0 @@
-# cython: c_string_type=str, c_string_encoding=ascii
-
-cimport numpy as np
-import numpy as npp
-cimport geometry
-from .geometry cimport shared_ptr
-from .geometry cimport dynamic_pointer_cast
-from .geometry cimport make_shared
-# C helper function that copies all arguments into a positional list.
-cdef list process_args(list keywords, tuple args, dict kwargs):
-   cdef str keyword
-   cdef int n = len(args), m = len(keywords)
-   cdef list params = list(args)
-   assert len(args)+len(kwargs) == m, 'Expected {} arguments'.format(m)
-   try:
-       return params + [kwargs[keyword] for keyword in keywords[n:]]
-   except:
-       raise ValueError('Epected arguments ' + str(keywords))
-from gtsam_eigency.core cimport *
-from libcpp cimport bool
-
-from libcpp.pair cimport pair
-from libcpp.string cimport string
-from cython.operator cimport dereference as deref
-
-
-cdef class Point2:
-    def __init__(self, *args, **kwargs):
-        cdef list __params
-        self.CPoint2_ = shared_ptr[CPoint2]()
-        if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
-            return
-        try:
-            __params = process_args([], args, kwargs)
-            self.CPoint2_ = shared_ptr[CPoint2](new CPoint2())
-        except (AssertionError, ValueError):
-            pass
-        try:
-            __params = process_args(['x', 'y'], args, kwargs)
-            x = <double>(__params[0])
-            y = <double>(__params[1])
-            self.CPoint2_ = shared_ptr[CPoint2](new CPoint2(x, y))
-        except (AssertionError, ValueError):
-            pass
-        if (self.CPoint2_.use_count()==0):
-            raise TypeError('Point2 construction failed!')
-
-    @staticmethod
-    cdef Point2 cyCreateFromShared(const shared_ptr[CPoint2]& other):
-        if other.get() == NULL:
-            raise RuntimeError('Cannot create object from a nullptr!')
-        cdef Point2 return_value = Point2(cyCreateFromShared=True)
-        return_value.CPoint2_ = other
-        return return_value
-
-    def argChar(self, char a):
-        self.CPoint2_.get().argChar(a)
-    def argUChar(self, unsigned char a):
-        self.CPoint2_.get().argUChar(a)
-    def dim(self):
-        cdef int ret = self.CPoint2_.get().dim()
-        return ret
-    def eigenArguments(self, np.ndarray v, np.ndarray m):
-        v = v.astype(float, order='F', copy=False)
-        m = m.astype(float, order='F', copy=False)
-        self.CPoint2_.get().eigenArguments(<VectorXd>(Map[VectorXd](v)), <MatrixXd>(Map[MatrixXd](m)))
-    def returnChar(self):
-        cdef char ret = self.CPoint2_.get().returnChar()
-        return ret
-    def vectorConfusion(self):
-        cdef shared_ptr[CVectorNotEigen] ret = make_shared[CVectorNotEigen](self.CPoint2_.get().vectorConfusion())
-        return VectorNotEigen.cyCreateFromShared(ret)
-    def x(self):
-        cdef double ret = self.CPoint2_.get().x()
-        return ret
-    def y(self):
-        cdef double ret = self.CPoint2_.get().y()
-        return ret
-
-
-cdef class Point3:
-    def __init__(self, *args, **kwargs):
-        cdef list __params
-        self.CPoint3_ = shared_ptr[CPoint3]()
-        if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
-            return
-        try:
-            __params = process_args(['x', 'y', 'z'], args, kwargs)
-            x = <double>(__params[0])
-            y = <double>(__params[1])
-            z = <double>(__params[2])
-            self.CPoint3_ = shared_ptr[CPoint3](new CPoint3(x, y, z))
-        except (AssertionError, ValueError):
-            pass
-        if (self.CPoint3_.use_count()==0):
-            raise TypeError('Point3 construction failed!')
-
-    @staticmethod
-    cdef Point3 cyCreateFromShared(const shared_ptr[CPoint3]& other):
-        if other.get() == NULL:
-            raise RuntimeError('Cannot create object from a nullptr!')
-        cdef Point3 return_value = Point3(cyCreateFromShared=True)
-        return_value.CPoint3_ = other
-        return return_value
-
-    @staticmethod
-    def StaticFunctionRet(double z):
-        return Point3.cyCreateFromShared(make_shared[CPoint3](CPoint3.StaticFunctionRet(z)))
-
-    @staticmethod
-    def staticFunction():
-        return CPoint3.staticFunction()
-
-
-    def norm(self):
-        cdef double ret = self.CPoint3_.get().norm()
-        return ret
-
-
-cdef class Test:
-    def __init__(self, *args, **kwargs):
-        cdef list __params
-        self.CTest_ = shared_ptr[CTest]()
-        if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
-            return
-        try:
-            __params = process_args([], args, kwargs)
-            self.CTest_ = shared_ptr[CTest](new CTest())
-        except (AssertionError, ValueError):
-            pass
-        try:
-            __params = process_args(['a', 'b'], args, kwargs)
-            a = <double>(__params[0])
-            b = <np.ndarray>(__params[1])
-            assert isinstance(b, np.ndarray) and b.ndim == 2
-            b = b.astype(float, order='F', copy=False)
-            self.CTest_ = shared_ptr[CTest](new CTest(a, <MatrixXd>(Map[MatrixXd](b))))
-        except (AssertionError, ValueError):
-            pass
-        if (self.CTest_.use_count()==0):
-            raise TypeError('Test construction failed!')
-
-    @staticmethod
-    cdef Test cyCreateFromShared(const shared_ptr[CTest]& other):
-        if other.get() == NULL:
-            raise RuntimeError('Cannot create object from a nullptr!')
-        cdef Test return_value = Test(cyCreateFromShared=True)
-        return_value.CTest_ = other
-        return return_value
-
-    def arg_EigenConstRef(self, np.ndarray value):
-        value = value.astype(float, order='F', copy=False)
-        self.CTest_.get().arg_EigenConstRef(<MatrixXd>(Map[MatrixXd](value)))
-    def create_MixedPtrs(self):
-        cdef pair [CTest,shared_ptr[CTest]] ret = self.CTest_.get().create_MixedPtrs()
-        return (Test.cyCreateFromShared(make_shared[CTest](ret.first)),Test.cyCreateFromShared(ret.second))
-    def create_ptrs(self):
-        cdef pair [shared_ptr[CTest],shared_ptr[CTest]] ret = self.CTest_.get().create_ptrs()
-        return (Test.cyCreateFromShared(ret.first),Test.cyCreateFromShared(ret.second))
-    def __repr__(self):
-        strBuf = RedirectCout()
-        self.print_('')
-        return strBuf.str()
-    def print_(self):
-        self.CTest_.get().print_()
-    def return_Point2Ptr(self, bool value):
-        cdef shared_ptr[CPoint2] ret = self.CTest_.get().return_Point2Ptr(value)
-        return Point2.cyCreateFromShared(ret)
-    def return_Test(self, Test value):
-        cdef shared_ptr[CTest] ret = make_shared[CTest](self.CTest_.get().return_Test(value.CTest_))
-        return Test.cyCreateFromShared(ret)
-    def return_TestPtr(self, Test value):
-        cdef shared_ptr[CTest] ret = self.CTest_.get().return_TestPtr(value.CTest_)
-        return Test.cyCreateFromShared(ret)
-    def return_bool(self, bool value):
-        cdef bool ret = self.CTest_.get().return_bool(value)
-        return ret
-    def return_double(self, double value):
-        cdef double ret = self.CTest_.get().return_double(value)
-        return ret
-    def return_field(self, Test t):
-        cdef bool ret = self.CTest_.get().return_field(deref(t.CTest_))
-        return ret
-    def return_int(self, int value):
-        cdef int ret = self.CTest_.get().return_int(value)
-        return ret
-    def return_matrix1(self, np.ndarray value):
-        value = value.astype(float, order='F', copy=False)
-        cdef MatrixXd ret = self.CTest_.get().return_matrix1(<MatrixXd>(Map[MatrixXd](value)))
-        return ndarray_copy(ret)
-    def return_matrix2(self, np.ndarray value):
-        value = value.astype(float, order='F', copy=False)
-        cdef MatrixXd ret = self.CTest_.get().return_matrix2(<MatrixXd>(Map[MatrixXd](value)))
-        return ndarray_copy(ret)
-    def return_pair(self, np.ndarray v, np.ndarray A):
-        v = v.astype(float, order='F', copy=False)
-        A = A.astype(float, order='F', copy=False)
-        cdef pair [VectorXd,MatrixXd] ret = self.CTest_.get().return_pair(<VectorXd>(Map[VectorXd](v)), <MatrixXd>(Map[MatrixXd](A)))
-        return (ndarray_copy(ret.first).squeeze(),ndarray_copy(ret.second))
-    def return_ptrs(self, Test p1, Test p2):
-        cdef pair [shared_ptr[CTest],shared_ptr[CTest]] ret = self.CTest_.get().return_ptrs(p1.CTest_, p2.CTest_)
-        return (Test.cyCreateFromShared(ret.first),Test.cyCreateFromShared(ret.second))
-    def return_size_t(self, size_t value):
-        cdef size_t ret = self.CTest_.get().return_size_t(value)
-        return ret
-    def return_string(self, string value):
-        cdef string ret = self.CTest_.get().return_string(value)
-        return ret
-    def return_vector1(self, np.ndarray value):
-        value = value.astype(float, order='F', copy=False)
-        cdef VectorXd ret = self.CTest_.get().return_vector1(<VectorXd>(Map[VectorXd](value)))
-        return ndarray_copy(ret).squeeze()
-    def return_vector2(self, np.ndarray value):
-        value = value.astype(float, order='F', copy=False)
-        cdef VectorXd ret = self.CTest_.get().return_vector2(<VectorXd>(Map[VectorXd](value)))
-        return ndarray_copy(ret).squeeze()
-
-
-cdef class MyBase:
-    def __init__(self, *args, **kwargs):
-        cdef list __params
-        self.CMyBase_ = shared_ptr[CMyBase]()
-        if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
-            return
-        if (self.CMyBase_.use_count()==0):
-            raise TypeError('MyBase construction failed!')
-
-    @staticmethod
-    cdef MyBase cyCreateFromShared(const shared_ptr[CMyBase]& other):
-        if other.get() == NULL:
-            raise RuntimeError('Cannot create object from a nullptr!')
-        cdef MyBase return_value = MyBase(cyCreateFromShared=True)
-        return_value.CMyBase_ = other
-        return return_value
-
-
-
-cdef class MyTemplatePoint2(MyBase):
-    def __init__(self, *args, **kwargs):
-        cdef list __params
-        self.CMyTemplatePoint2_ = shared_ptr[CMyTemplatePoint2]()
-        if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
-            return
-        try:
-            __params = process_args([], args, kwargs)
-            self.CMyTemplatePoint2_ = shared_ptr[CMyTemplatePoint2](new CMyTemplatePoint2())
-        except (AssertionError, ValueError):
-            pass
-        if (self.CMyTemplatePoint2_.use_count()==0):
-            raise TypeError('MyTemplatePoint2 construction failed!')
-        self.CMyBase_ = <shared_ptr[CMyBase]>(self.CMyTemplatePoint2_)
-
-    @staticmethod
-    cdef MyTemplatePoint2 cyCreateFromShared(const shared_ptr[CMyTemplatePoint2]& other):
-        if other.get() == NULL:
-            raise RuntimeError('Cannot create object from a nullptr!')
-        cdef MyTemplatePoint2 return_value = MyTemplatePoint2(cyCreateFromShared=True)
-        return_value.CMyTemplatePoint2_ = other
-        return_value.CMyBase_ = <shared_ptr[CMyBase]>(other)
-        return return_value
-
-    def accept_T(self, Point2 value):
-        self.CMyTemplatePoint2_.get().accept_T(deref(value.CPoint2_))
-    def accept_Tptr(self, Point2 value):
-        self.CMyTemplatePoint2_.get().accept_Tptr(value.CPoint2_)
-    def create_MixedPtrs(self):
-        cdef pair [CPoint2,shared_ptr[CPoint2]] ret = self.CMyTemplatePoint2_.get().create_MixedPtrs()
-        return (Point2.cyCreateFromShared(make_shared[CPoint2](ret.first)),Point2.cyCreateFromShared(ret.second))
-    def create_ptrs(self):
-        cdef pair [shared_ptr[CPoint2],shared_ptr[CPoint2]] ret = self.CMyTemplatePoint2_.get().create_ptrs()
-        return (Point2.cyCreateFromShared(ret.first),Point2.cyCreateFromShared(ret.second))
-    def return_T(self, Point2 value):
-        cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.CMyTemplatePoint2_.get().return_T(value.CPoint2_))
-        return Point2.cyCreateFromShared(ret)
-    def return_Tptr(self, Point2 value):
-        cdef shared_ptr[CPoint2] ret = self.CMyTemplatePoint2_.get().return_Tptr(value.CPoint2_)
-        return Point2.cyCreateFromShared(ret)
-    def return_ptrs(self, Point2 p1, Point2 p2):
-        cdef pair [shared_ptr[CPoint2],shared_ptr[CPoint2]] ret = self.CMyTemplatePoint2_.get().return_ptrs(p1.CPoint2_, p2.CPoint2_)
-        return (Point2.cyCreateFromShared(ret.first),Point2.cyCreateFromShared(ret.second))
-    def templatedMethodMatrix(self, np.ndarray t):
-        t = t.astype(float, order='F', copy=False)
-        cdef MatrixXd ret = self.CMyTemplatePoint2_.get().templatedMethod[MatrixXd](<MatrixXd>(Map[MatrixXd](t)))
-        return ndarray_copy(ret)
-    def templatedMethodPoint2(self, Point2 t):
-        cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.CMyTemplatePoint2_.get().templatedMethod[CPoint2](deref(t.CPoint2_)))
-        return Point2.cyCreateFromShared(ret)
-    def templatedMethodPoint3(self, Point3 t):
-        cdef shared_ptr[CPoint3] ret = make_shared[CPoint3](self.CMyTemplatePoint2_.get().templatedMethod[CPoint3](deref(t.CPoint3_)))
-        return Point3.cyCreateFromShared(ret)
-    def templatedMethodVector(self, np.ndarray t):
-        t = t.astype(float, order='F', copy=False)
-        cdef VectorXd ret = self.CMyTemplatePoint2_.get().templatedMethod[VectorXd](<VectorXd>(Map[VectorXd](t)))
-        return ndarray_copy(ret).squeeze()
-def dynamic_cast_MyTemplatePoint2_MyBase(MyBase parent):
-    try:
-        return MyTemplatePoint2.cyCreateFromShared(<shared_ptr[CMyTemplatePoint2]>dynamic_pointer_cast[CMyTemplatePoint2,CMyBase](parent.CMyBase_))
-    except:
-        raise TypeError('dynamic cast failed!')
-
-
-cdef class MyTemplateMatrix(MyBase):
-    def __init__(self, *args, **kwargs):
-        cdef list __params
-        self.CMyTemplateMatrix_ = shared_ptr[CMyTemplateMatrix]()
-        if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
-            return
-        try:
-            __params = process_args([], args, kwargs)
-            self.CMyTemplateMatrix_ = shared_ptr[CMyTemplateMatrix](new CMyTemplateMatrix())
-        except (AssertionError, ValueError):
-            pass
-        if (self.CMyTemplateMatrix_.use_count()==0):
-            raise TypeError('MyTemplateMatrix construction failed!')
-        self.CMyBase_ = <shared_ptr[CMyBase]>(self.CMyTemplateMatrix_)
-
-    @staticmethod
-    cdef MyTemplateMatrix cyCreateFromShared(const shared_ptr[CMyTemplateMatrix]& other):
-        if other.get() == NULL:
-            raise RuntimeError('Cannot create object from a nullptr!')
-        cdef MyTemplateMatrix return_value = MyTemplateMatrix(cyCreateFromShared=True)
-        return_value.CMyTemplateMatrix_ = other
-        return_value.CMyBase_ = <shared_ptr[CMyBase]>(other)
-        return return_value
-
-    def accept_T(self, np.ndarray value):
-        value = value.astype(float, order='F', copy=False)
-        self.CMyTemplateMatrix_.get().accept_T(<MatrixXd>(Map[MatrixXd](value)))
-    def accept_Tptr(self, np.ndarray value):
-        value = value.astype(float, order='F', copy=False)
-        self.CMyTemplateMatrix_.get().accept_Tptr(<MatrixXd>(Map[MatrixXd](value)))
-    def create_MixedPtrs(self):
-        cdef pair [MatrixXd,shared_ptr[MatrixXd]] ret = self.CMyTemplateMatrix_.get().create_MixedPtrs()
-        return (ndarray_copy(ret.first),ndarray_copy(ret.second))
-    def create_ptrs(self):
-        cdef pair [shared_ptr[MatrixXd],shared_ptr[MatrixXd]] ret = self.CMyTemplateMatrix_.get().create_ptrs()
-        return (ndarray_copy(ret.first),ndarray_copy(ret.second))
-    def return_T(self, np.ndarray value):
-        value = value.astype(float, order='F', copy=False)
-        cdef MatrixXd ret = self.CMyTemplateMatrix_.get().return_T(<MatrixXd>(Map[MatrixXd](value)))
-        return ndarray_copy(ret)
-    def return_Tptr(self, np.ndarray value):
-        value = value.astype(float, order='F', copy=False)
-        cdef shared_ptr[MatrixXd] ret = self.CMyTemplateMatrix_.get().return_Tptr(<MatrixXd>(Map[MatrixXd](value)))
-        return ndarray_copy(ret)
-    def return_ptrs(self, np.ndarray p1, np.ndarray p2):
-        p1 = p1.astype(float, order='F', copy=False)
-        p2 = p2.astype(float, order='F', copy=False)
-        cdef pair [shared_ptr[MatrixXd],shared_ptr[MatrixXd]] ret = self.CMyTemplateMatrix_.get().return_ptrs(<MatrixXd>(Map[MatrixXd](p1)), <MatrixXd>(Map[MatrixXd](p2)))
-        return (ndarray_copy(ret.first),ndarray_copy(ret.second))
-    def templatedMethodMatrix(self, np.ndarray t):
-        t = t.astype(float, order='F', copy=False)
-        cdef MatrixXd ret = self.CMyTemplateMatrix_.get().templatedMethod[MatrixXd](<MatrixXd>(Map[MatrixXd](t)))
-        return ndarray_copy(ret)
-    def templatedMethodPoint2(self, Point2 t):
-        cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.CMyTemplateMatrix_.get().templatedMethod[CPoint2](deref(t.CPoint2_)))
-        return Point2.cyCreateFromShared(ret)
-    def templatedMethodPoint3(self, Point3 t):
-        cdef shared_ptr[CPoint3] ret = make_shared[CPoint3](self.CMyTemplateMatrix_.get().templatedMethod[CPoint3](deref(t.CPoint3_)))
-        return Point3.cyCreateFromShared(ret)
-    def templatedMethodVector(self, np.ndarray t):
-        t = t.astype(float, order='F', copy=False)
-        cdef VectorXd ret = self.CMyTemplateMatrix_.get().templatedMethod[VectorXd](<VectorXd>(Map[VectorXd](t)))
-        return ndarray_copy(ret).squeeze()
-def dynamic_cast_MyTemplateMatrix_MyBase(MyBase parent):
-    try:
-        return MyTemplateMatrix.cyCreateFromShared(<shared_ptr[CMyTemplateMatrix]>dynamic_pointer_cast[CMyTemplateMatrix,CMyBase](parent.CMyBase_))
-    except:
-        raise TypeError('dynamic cast failed!')
-
-
-cdef class MyVector3:
-    def __init__(self, *args, **kwargs):
-        cdef list __params
-        self.CMyVector3_ = shared_ptr[CMyVector3]()
-        if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
-            return
-        try:
-            __params = process_args([], args, kwargs)
-            self.CMyVector3_ = shared_ptr[CMyVector3](new CMyVector3())
-        except (AssertionError, ValueError):
-            pass
-        if (self.CMyVector3_.use_count()==0):
-            raise TypeError('MyVector3 construction failed!')
-
-    @staticmethod
-    cdef MyVector3 cyCreateFromShared(const shared_ptr[CMyVector3]& other):
-        if other.get() == NULL:
-            raise RuntimeError('Cannot create object from a nullptr!')
-        cdef MyVector3 return_value = MyVector3(cyCreateFromShared=True)
-        return_value.CMyVector3_ = other
-        return return_value
-
-
-
-cdef class MyVector12:
-    def __init__(self, *args, **kwargs):
-        cdef list __params
-        self.CMyVector12_ = shared_ptr[CMyVector12]()
-        if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
-            return
-        try:
-            __params = process_args([], args, kwargs)
-            self.CMyVector12_ = shared_ptr[CMyVector12](new CMyVector12())
-        except (AssertionError, ValueError):
-            pass
-        if (self.CMyVector12_.use_count()==0):
-            raise TypeError('MyVector12 construction failed!')
-
-    @staticmethod
-    cdef MyVector12 cyCreateFromShared(const shared_ptr[CMyVector12]& other):
-        if other.get() == NULL:
-            raise RuntimeError('Cannot create object from a nullptr!')
-        cdef MyVector12 return_value = MyVector12(cyCreateFromShared=True)
-        return_value.CMyVector12_ = other
-        return return_value
-
-
-
-cdef class MyFactorPosePoint2:
-    def __init__(self, *args, **kwargs):
-        cdef list __params
-        self.CMyFactorPosePoint2_ = shared_ptr[CMyFactorPosePoint2]()
-        if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
-            return
-        try:
-            __params = process_args(['key1', 'key2', 'measured', 'noiseModel'], args, kwargs)
-            key1 = <size_t>(__params[0])
-            key2 = <size_t>(__params[1])
-            measured = <double>(__params[2])
-            noiseModel = <noiseModel_Base>(__params[3])
-            assert isinstance(noiseModel, noiseModel_Base)
-            self.CMyFactorPosePoint2_ = shared_ptr[CMyFactorPosePoint2](new CMyFactorPosePoint2(key1, key2, measured, noiseModel.CnoiseModel_Base_))
-        except (AssertionError, ValueError):
-            pass
-        if (self.CMyFactorPosePoint2_.use_count()==0):
-            raise TypeError('MyFactorPosePoint2 construction failed!')
-
-    @staticmethod
-    cdef MyFactorPosePoint2 cyCreateFromShared(const shared_ptr[CMyFactorPosePoint2]& other):
-        if other.get() == NULL:
-            raise RuntimeError('Cannot create object from a nullptr!')
-        cdef MyFactorPosePoint2 return_value = MyFactorPosePoint2(cyCreateFromShared=True)
-        return_value.CMyFactorPosePoint2_ = other
-        return return_value
-
-
-
-
-def aGlobalFunction():
-    cdef VectorXd ret = pxd_aGlobalFunction()
-    return ndarray_copy(ret).squeeze()
-def overloadedGlobalFunction(*args, **kwargs):
-    success, results = overloadedGlobalFunction_0(args, kwargs)
-    if success:
-            return results
-    success, results = overloadedGlobalFunction_1(args, kwargs)
-    if success:
-            return results
-    raise TypeError('Could not find the correct overload')
-def overloadedGlobalFunction_0(args, kwargs):
-    cdef list __params
-    cdef VectorXd return_value
-    try:
-        __params = process_args(['a'], args, kwargs)
-        a = <int>(__params[0])
-    except:
-        return False, None
-
-    return_value = pxd_overloadedGlobalFunction(a)
-    return True, ndarray_copy(return_value).squeeze()
-def overloadedGlobalFunction_1(args, kwargs):
-    cdef list __params
-    cdef VectorXd return_value
-    try:
-        __params = process_args(['a', 'b'], args, kwargs)
-        a = <int>(__params[0])
-        b = <double>(__params[1])
-    except:
-        return False, None
-
-    return_value = pxd_overloadedGlobalFunction(a, b)
-    return True, ndarray_copy(return_value).squeeze()
diff --git a/wrap/tests/expected/+gtsam/Point2.m b/wrap/tests/expected-matlab/+gtsam/Point2.m
similarity index 53%
rename from wrap/tests/expected/+gtsam/Point2.m
rename to wrap/tests/expected-matlab/+gtsam/Point2.m
index 05ca8b9a0..ca021439a 100644
--- a/wrap/tests/expected/+gtsam/Point2.m
+++ b/wrap/tests/expected-matlab/+gtsam/Point2.m
@@ -1,5 +1,5 @@
 %class Point2, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%at https://gtsam.org/doxygen/
 %
 %-------Constructors-------
 %Point2()
@@ -44,54 +44,82 @@ classdef Point2 < handle
     %DISP Calls print on the object
     function varargout = argChar(this, varargin)
       % ARGCHAR usage: argChar(char a) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      geometry_wrapper(4, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'char')
+        geometry_wrapper(4, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function gtsam.Point2.argChar');
     end
 
     function varargout = argUChar(this, varargin)
       % ARGUCHAR usage: argUChar(unsigned char a) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      geometry_wrapper(5, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'unsigned char')
+        geometry_wrapper(5, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function gtsam.Point2.argUChar');
     end
 
     function varargout = dim(this, varargin)
       % DIM usage: dim() : returns int
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(6, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        varargout{1} = geometry_wrapper(6, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function gtsam.Point2.dim');
     end
 
     function varargout = eigenArguments(this, varargin)
       % EIGENARGUMENTS usage: eigenArguments(Vector v, Matrix m) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},2)==1 && isa(varargin{2},'double')
         geometry_wrapper(7, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function gtsam.Point2.eigenArguments');
+        return
       end
+      error('Arguments do not match any overload of function gtsam.Point2.eigenArguments');
     end
 
     function varargout = returnChar(this, varargin)
       % RETURNCHAR usage: returnChar() : returns char
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(8, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        varargout{1} = geometry_wrapper(8, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function gtsam.Point2.returnChar');
     end
 
     function varargout = vectorConfusion(this, varargin)
       % VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(9, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        varargout{1} = geometry_wrapper(9, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function gtsam.Point2.vectorConfusion');
     end
 
     function varargout = x(this, varargin)
       % X usage: x() : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(10, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        varargout{1} = geometry_wrapper(10, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function gtsam.Point2.x');
     end
 
     function varargout = y(this, varargin)
       % Y usage: y() : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(11, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        varargout{1} = geometry_wrapper(11, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function gtsam.Point2.y');
     end
 
   end
diff --git a/wrap/tests/expected/+gtsam/Point3.m b/wrap/tests/expected-matlab/+gtsam/Point3.m
similarity index 71%
rename from wrap/tests/expected/+gtsam/Point3.m
rename to wrap/tests/expected-matlab/+gtsam/Point3.m
index 7b8a973c0..526d9d3d1 100644
--- a/wrap/tests/expected/+gtsam/Point3.m
+++ b/wrap/tests/expected-matlab/+gtsam/Point3.m
@@ -1,5 +1,5 @@
 %class Point3, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%at https://gtsam.org/doxygen/
 %
 %-------Constructors-------
 %Point3(double x, double y, double z)
@@ -8,7 +8,7 @@
 %norm() : returns double
 %
 %-------Static Methods-------
-%StaticFunctionRet(double z) : returns gtsam::Point3
+%StaticFunctionRet(double z) : returns Point3
 %staticFunction() : returns double
 %
 %-------Serialization Interface-------
@@ -42,13 +42,17 @@ classdef Point3 < handle
     %DISP Calls print on the object
     function varargout = norm(this, varargin)
       % NORM usage: norm() : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(15, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        varargout{1} = geometry_wrapper(15, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function gtsam.Point3.norm');
     end
 
     function varargout = string_serialize(this, varargin)
       % STRING_SERIALIZE usage: string_serialize() : returns string
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 0
         varargout{1} = geometry_wrapper(16, this, varargin{:});
       else
@@ -64,20 +68,30 @@ classdef Point3 < handle
 
   methods(Static = true)
     function varargout = StaticFunctionRet(varargin)
-      % STATICFUNCTIONRET usage: StaticFunctionRet(double z) : returns gtsam::Point3
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(17, varargin{:});
+      % STATICFUNCTIONRET usage: StaticFunctionRet(double z) : returns Point3
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double')
+        varargout{1} = geometry_wrapper(17, varargin{:});
+        return
+      end
+
+      error('Arguments do not match any overload of function Point3.StaticFunctionRet');
     end
 
     function varargout = StaticFunction(varargin)
       % STATICFUNCTION usage: staticFunction() : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(18, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        varargout{1} = geometry_wrapper(18, varargin{:});
+        return
+      end
+
+      error('Arguments do not match any overload of function Point3.staticFunction');
     end
 
     function varargout = string_deserialize(varargin)
       % STRING_DESERIALIZE usage: string_deserialize() : returns gtsam.Point3
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1
         varargout{1} = geometry_wrapper(19, varargin{:});
       else
diff --git a/wrap/tests/expected/MyBase.m b/wrap/tests/expected-matlab/MyBase.m
similarity index 78%
rename from wrap/tests/expected/MyBase.m
rename to wrap/tests/expected-matlab/MyBase.m
index ff8179220..4d4d35ee5 100644
--- a/wrap/tests/expected/MyBase.m
+++ b/wrap/tests/expected-matlab/MyBase.m
@@ -1,5 +1,5 @@
 %class MyBase, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%at https://gtsam.org/doxygen/
 %
 classdef MyBase < handle
   properties
@@ -11,9 +11,9 @@ classdef MyBase < handle
         if nargin == 2
           my_ptr = varargin{2};
         else
-          my_ptr = geometry_wrapper(44, varargin{2});
+          my_ptr = geometry_wrapper(45, varargin{2});
         end
-        geometry_wrapper(43, my_ptr);
+        geometry_wrapper(44, my_ptr);
       else
         error('Arguments do not match any overload of MyBase constructor');
       end
@@ -21,7 +21,7 @@ classdef MyBase < handle
     end
 
     function delete(obj)
-      geometry_wrapper(45, obj.ptr_MyBase);
+      geometry_wrapper(46, obj.ptr_MyBase);
     end
 
     function display(obj), obj.print(''); end
diff --git a/wrap/tests/expected2/MyFactorPosePoint2.m b/wrap/tests/expected-matlab/MyFactorPosePoint2.m
similarity index 81%
rename from wrap/tests/expected2/MyFactorPosePoint2.m
rename to wrap/tests/expected-matlab/MyFactorPosePoint2.m
index a61e54eb2..04381f1dc 100644
--- a/wrap/tests/expected2/MyFactorPosePoint2.m
+++ b/wrap/tests/expected-matlab/MyFactorPosePoint2.m
@@ -1,5 +1,5 @@
 %class MyFactorPosePoint2, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%at https://gtsam.org/doxygen/
 %
 %-------Constructors-------
 %MyFactorPosePoint2(size_t key1, size_t key2, double measured, Base noiseModel)
@@ -12,9 +12,9 @@ classdef MyFactorPosePoint2 < handle
     function obj = MyFactorPosePoint2(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        geometry_wrapper(80, my_ptr);
+        geometry_wrapper(89, my_ptr);
       elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base')
-        my_ptr = geometry_wrapper(81, varargin{1}, varargin{2}, varargin{3}, varargin{4});
+        my_ptr = geometry_wrapper(90, varargin{1}, varargin{2}, varargin{3}, varargin{4});
       else
         error('Arguments do not match any overload of MyFactorPosePoint2 constructor');
       end
@@ -22,7 +22,7 @@ classdef MyFactorPosePoint2 < handle
     end
 
     function delete(obj)
-      geometry_wrapper(82, obj.ptr_MyFactorPosePoint2);
+      geometry_wrapper(91, obj.ptr_MyFactorPosePoint2);
     end
 
     function display(obj), obj.print(''); end
diff --git a/wrap/tests/expected2/MyTemplateMatrix.m b/wrap/tests/expected-matlab/MyTemplateMatrix.m
similarity index 50%
rename from wrap/tests/expected2/MyTemplateMatrix.m
rename to wrap/tests/expected-matlab/MyTemplateMatrix.m
index 977660a15..54245921d 100644
--- a/wrap/tests/expected2/MyTemplateMatrix.m
+++ b/wrap/tests/expected-matlab/MyTemplateMatrix.m
@@ -1,5 +1,5 @@
 %class MyTemplateMatrix, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%at https://gtsam.org/doxygen/
 %
 %-------Constructors-------
 %MyTemplateMatrix()
@@ -13,10 +13,17 @@
 %return_Tptr(Matrix value) : returns Matrix
 %return_ptrs(Matrix p1, Matrix p2) : returns pair< Matrix, Matrix >
 %templatedMethodMatrix(Matrix t) : returns Matrix
-%templatedMethodPoint2(Point2 t) : returns gtsam::Point2
-%templatedMethodPoint3(Point3 t) : returns gtsam::Point3
+%templatedMethodPoint2(Point2 t) : returns Point2
+%templatedMethodPoint3(Point3 t) : returns Point3
 %templatedMethodVector(Vector t) : returns Vector
 %
+%-------Static Methods-------
+%Level(Matrix K) : returns MyTemplate<Matrix>
+%
+%-------Serialization Interface-------
+%string_serialize() : returns string
+%string_deserialize(string serialized) : returns MyTemplateMatrix
+%
 classdef MyTemplateMatrix < MyBase
   properties
     ptr_MyTemplateMatrix = 0
@@ -27,11 +34,11 @@ classdef MyTemplateMatrix < MyBase
         if nargin == 2
           my_ptr = varargin{2};
         else
-          my_ptr = geometry_wrapper(60, varargin{2});
+          my_ptr = geometry_wrapper(64, varargin{2});
         end
-        base_ptr = geometry_wrapper(59, my_ptr);
+        base_ptr = geometry_wrapper(63, my_ptr);
       elseif nargin == 0
-        [ my_ptr, base_ptr ] = geometry_wrapper(61);
+        [ my_ptr, base_ptr ] = geometry_wrapper(65);
       else
         error('Arguments do not match any overload of MyTemplateMatrix constructor');
       end
@@ -40,7 +47,7 @@ classdef MyTemplateMatrix < MyBase
     end
 
     function delete(obj)
-      geometry_wrapper(62, obj.ptr_MyTemplateMatrix);
+      geometry_wrapper(66, obj.ptr_MyTemplateMatrix);
     end
 
     function display(obj), obj.print(''); end
@@ -49,108 +56,127 @@ classdef MyTemplateMatrix < MyBase
     %DISP Calls print on the object
     function varargout = accept_T(this, varargin)
       % ACCEPT_T usage: accept_T(Matrix value) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double')
-        geometry_wrapper(63, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.accept_T');
+        geometry_wrapper(67, this, varargin{:});
+        return
       end
+      error('Arguments do not match any overload of function MyTemplateMatrix.accept_T');
     end
 
     function varargout = accept_Tptr(this, varargin)
       % ACCEPT_TPTR usage: accept_Tptr(Matrix value) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double')
-        geometry_wrapper(64, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.accept_Tptr');
+        geometry_wrapper(68, this, varargin{:});
+        return
       end
+      error('Arguments do not match any overload of function MyTemplateMatrix.accept_Tptr');
     end
 
     function varargout = create_MixedPtrs(this, varargin)
       % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Matrix, Matrix >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(65, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        [ varargout{1} varargout{2} ] = geometry_wrapper(69, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function MyTemplateMatrix.create_MixedPtrs');
     end
 
     function varargout = create_ptrs(this, varargin)
       % CREATE_PTRS usage: create_ptrs() : returns pair< Matrix, Matrix >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(66, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        [ varargout{1} varargout{2} ] = geometry_wrapper(70, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function MyTemplateMatrix.create_ptrs');
     end
 
     function varargout = return_T(this, varargin)
       % RETURN_T usage: return_T(Matrix value) : returns Matrix
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(67, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.return_T');
+        varargout{1} = geometry_wrapper(71, this, varargin{:});
+        return
       end
+      error('Arguments do not match any overload of function MyTemplateMatrix.return_T');
     end
 
     function varargout = return_Tptr(this, varargin)
       % RETURN_TPTR usage: return_Tptr(Matrix value) : returns Matrix
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(68, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.return_Tptr');
+        varargout{1} = geometry_wrapper(72, this, varargin{:});
+        return
       end
+      error('Arguments do not match any overload of function MyTemplateMatrix.return_Tptr');
     end
 
     function varargout = return_ptrs(this, varargin)
       % RETURN_PTRS usage: return_ptrs(Matrix p1, Matrix p2) : returns pair< Matrix, Matrix >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
-        [ varargout{1} varargout{2} ] = geometry_wrapper(69, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.return_ptrs');
+        [ varargout{1} varargout{2} ] = geometry_wrapper(73, this, varargin{:});
+        return
       end
+      error('Arguments do not match any overload of function MyTemplateMatrix.return_ptrs');
     end
 
     function varargout = templatedMethodMatrix(this, varargin)
       % TEMPLATEDMETHODMATRIX usage: templatedMethodMatrix(Matrix t) : returns Matrix
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(70, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
+        varargout{1} = geometry_wrapper(74, this, varargin{:});
+        return
       end
+      error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethodMatrix');
     end
 
     function varargout = templatedMethodPoint2(this, varargin)
-      % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns gtsam::Point2
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
-        varargout{1} = geometry_wrapper(71, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
+      % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns Point2
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1
+        varargout{1} = geometry_wrapper(75, this, varargin{:});
+        return
       end
+      error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethodPoint2');
     end
 
     function varargout = templatedMethodPoint3(this, varargin)
-      % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns gtsam::Point3
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
-        varargout{1} = geometry_wrapper(72, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
+      % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns Point3
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==3 && size(varargin{1},2)==1
+        varargout{1} = geometry_wrapper(76, this, varargin{:});
+        return
       end
+      error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethodPoint3');
     end
 
     function varargout = templatedMethodVector(this, varargin)
       % TEMPLATEDMETHODVECTOR usage: templatedMethodVector(Vector t) : returns Vector
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
-        varargout{1} = geometry_wrapper(73, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
+        varargout{1} = geometry_wrapper(77, this, varargin{:});
+        return
       end
+      error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethodVector');
     end
 
   end
 
   methods(Static = true)
+    function varargout = Level(varargin)
+      % LEVEL usage: Level(Matrix K) : returns MyTemplateMatrix
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double')
+        varargout{1} = geometry_wrapper(78, varargin{:});
+        return
+      end
+
+      error('Arguments do not match any overload of function MyTemplateMatrix.Level');
+    end
+
   end
 end
diff --git a/wrap/tests/expected-matlab/MyTemplatePoint2.m b/wrap/tests/expected-matlab/MyTemplatePoint2.m
new file mode 100644
index 000000000..89fb3c452
--- /dev/null
+++ b/wrap/tests/expected-matlab/MyTemplatePoint2.m
@@ -0,0 +1,182 @@
+%class MyTemplatePoint2, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+%-------Constructors-------
+%MyTemplatePoint2()
+%
+%-------Methods-------
+%accept_T(Point2 value) : returns void
+%accept_Tptr(Point2 value) : returns void
+%create_MixedPtrs() : returns pair< Point2, Point2 >
+%create_ptrs() : returns pair< Point2, Point2 >
+%return_T(Point2 value) : returns Point2
+%return_Tptr(Point2 value) : returns Point2
+%return_ptrs(Point2 p1, Point2 p2) : returns pair< Point2, Point2 >
+%templatedMethodMatrix(Matrix t) : returns Matrix
+%templatedMethodPoint2(Point2 t) : returns Point2
+%templatedMethodPoint3(Point3 t) : returns Point3
+%templatedMethodVector(Vector t) : returns Vector
+%
+%-------Static Methods-------
+%Level(Point2 K) : returns MyTemplate<Point2>
+%
+%-------Serialization Interface-------
+%string_serialize() : returns string
+%string_deserialize(string serialized) : returns MyTemplatePoint2
+%
+classdef MyTemplatePoint2 < MyBase
+  properties
+    ptr_MyTemplatePoint2 = 0
+  end
+  methods
+    function obj = MyTemplatePoint2(varargin)
+      if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        if nargin == 2
+          my_ptr = varargin{2};
+        else
+          my_ptr = geometry_wrapper(48, varargin{2});
+        end
+        base_ptr = geometry_wrapper(47, my_ptr);
+      elseif nargin == 0
+        [ my_ptr, base_ptr ] = geometry_wrapper(49);
+      else
+        error('Arguments do not match any overload of MyTemplatePoint2 constructor');
+      end
+      obj = obj@MyBase(uint64(5139824614673773682), base_ptr);
+      obj.ptr_MyTemplatePoint2 = my_ptr;
+    end
+
+    function delete(obj)
+      geometry_wrapper(50, obj.ptr_MyTemplatePoint2);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+    function varargout = accept_T(this, varargin)
+      % ACCEPT_T usage: accept_T(Point2 value) : returns void
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1
+        geometry_wrapper(51, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function MyTemplatePoint2.accept_T');
+    end
+
+    function varargout = accept_Tptr(this, varargin)
+      % ACCEPT_TPTR usage: accept_Tptr(Point2 value) : returns void
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1
+        geometry_wrapper(52, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function MyTemplatePoint2.accept_Tptr');
+    end
+
+    function varargout = create_MixedPtrs(this, varargin)
+      % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Point2, Point2 >
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        [ varargout{1} varargout{2} ] = geometry_wrapper(53, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function MyTemplatePoint2.create_MixedPtrs');
+    end
+
+    function varargout = create_ptrs(this, varargin)
+      % CREATE_PTRS usage: create_ptrs() : returns pair< Point2, Point2 >
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        [ varargout{1} varargout{2} ] = geometry_wrapper(54, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function MyTemplatePoint2.create_ptrs');
+    end
+
+    function varargout = return_T(this, varargin)
+      % RETURN_T usage: return_T(Point2 value) : returns Point2
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1
+        varargout{1} = geometry_wrapper(55, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function MyTemplatePoint2.return_T');
+    end
+
+    function varargout = return_Tptr(this, varargin)
+      % RETURN_TPTR usage: return_Tptr(Point2 value) : returns Point2
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1
+        varargout{1} = geometry_wrapper(56, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function MyTemplatePoint2.return_Tptr');
+    end
+
+    function varargout = return_ptrs(this, varargin)
+      % RETURN_PTRS usage: return_ptrs(Point2 p1, Point2 p2) : returns pair< Point2, Point2 >
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1 && isa(varargin{2},'double') && size(varargin{2},1)==2 && size(varargin{2},2)==1
+        [ varargout{1} varargout{2} ] = geometry_wrapper(57, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function MyTemplatePoint2.return_ptrs');
+    end
+
+    function varargout = templatedMethodMatrix(this, varargin)
+      % TEMPLATEDMETHODMATRIX usage: templatedMethodMatrix(Matrix t) : returns Matrix
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double')
+        varargout{1} = geometry_wrapper(58, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethodMatrix');
+    end
+
+    function varargout = templatedMethodPoint2(this, varargin)
+      % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns Point2
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1
+        varargout{1} = geometry_wrapper(59, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethodPoint2');
+    end
+
+    function varargout = templatedMethodPoint3(this, varargin)
+      % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns Point3
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==3 && size(varargin{1},2)==1
+        varargout{1} = geometry_wrapper(60, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethodPoint3');
+    end
+
+    function varargout = templatedMethodVector(this, varargin)
+      % TEMPLATEDMETHODVECTOR usage: templatedMethodVector(Vector t) : returns Vector
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
+        varargout{1} = geometry_wrapper(61, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethodVector');
+    end
+
+  end
+
+  methods(Static = true)
+    function varargout = Level(varargin)
+      % LEVEL usage: Level(Point2 K) : returns MyTemplatePoint2
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1
+        varargout{1} = geometry_wrapper(62, varargin{:});
+        return
+      end
+
+      error('Arguments do not match any overload of function MyTemplatePoint2.Level');
+    end
+
+  end
+end
diff --git a/wrap/tests/expected2/MyVector12.m b/wrap/tests/expected-matlab/MyVector12.m
similarity index 79%
rename from wrap/tests/expected2/MyVector12.m
rename to wrap/tests/expected-matlab/MyVector12.m
index 4bd86e8a7..8e13cc993 100644
--- a/wrap/tests/expected2/MyVector12.m
+++ b/wrap/tests/expected-matlab/MyVector12.m
@@ -1,5 +1,5 @@
 %class MyVector12, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%at https://gtsam.org/doxygen/
 %
 %-------Constructors-------
 %MyVector12()
@@ -12,9 +12,9 @@ classdef MyVector12 < handle
     function obj = MyVector12(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        geometry_wrapper(77, my_ptr);
+        geometry_wrapper(86, my_ptr);
       elseif nargin == 0
-        my_ptr = geometry_wrapper(78);
+        my_ptr = geometry_wrapper(87);
       else
         error('Arguments do not match any overload of MyVector12 constructor');
       end
@@ -22,7 +22,7 @@ classdef MyVector12 < handle
     end
 
     function delete(obj)
-      geometry_wrapper(79, obj.ptr_MyVector12);
+      geometry_wrapper(88, obj.ptr_MyVector12);
     end
 
     function display(obj), obj.print(''); end
diff --git a/wrap/tests/expected2/MyVector3.m b/wrap/tests/expected-matlab/MyVector3.m
similarity index 79%
rename from wrap/tests/expected2/MyVector3.m
rename to wrap/tests/expected-matlab/MyVector3.m
index 82f3ed4bd..1f8140f4f 100644
--- a/wrap/tests/expected2/MyVector3.m
+++ b/wrap/tests/expected-matlab/MyVector3.m
@@ -1,5 +1,5 @@
 %class MyVector3, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%at https://gtsam.org/doxygen/
 %
 %-------Constructors-------
 %MyVector3()
@@ -12,9 +12,9 @@ classdef MyVector3 < handle
     function obj = MyVector3(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        geometry_wrapper(74, my_ptr);
+        geometry_wrapper(83, my_ptr);
       elseif nargin == 0
-        my_ptr = geometry_wrapper(75);
+        my_ptr = geometry_wrapper(84);
       else
         error('Arguments do not match any overload of MyVector3 constructor');
       end
@@ -22,7 +22,7 @@ classdef MyVector3 < handle
     end
 
     function delete(obj)
-      geometry_wrapper(76, obj.ptr_MyVector3);
+      geometry_wrapper(85, obj.ptr_MyVector3);
     end
 
     function display(obj), obj.print(''); end
diff --git a/wrap/tests/expected-matlab/PrimitiveRef.m b/wrap/tests/expected-matlab/PrimitiveRef.m
new file mode 100644
index 000000000..63c295078
--- /dev/null
+++ b/wrap/tests/expected-matlab/PrimitiveRef.m
@@ -0,0 +1,54 @@
+%class PrimitiveRef, see Doxygen page for details
+%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%
+%-------Constructors-------
+%PrimitiveRef()
+%
+%-------Static Methods-------
+%Brutal(double t) : returns This
+%
+%-------Serialization Interface-------
+%string_serialize() : returns string
+%string_deserialize(string serialized) : returns PrimitiveRef
+%
+classdef PrimitiveRef < handle
+  properties
+    ptr_PrimitiveRef = 0
+  end
+  methods
+    function obj = PrimitiveRef(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        geometry_wrapper(78, my_ptr);
+      elseif nargin == 0
+        my_ptr = geometry_wrapper(79);
+      else
+        error('Arguments do not match any overload of PrimitiveRef constructor');
+      end
+      obj.ptr_PrimitiveRef = my_ptr;
+    end
+
+    function delete(obj)
+      geometry_wrapper(80, obj.ptr_PrimitiveRef);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+    function varargout = Brutal(varargin)
+      % BRUTAL usage: Brutal(double t) : returns This
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'double')
+        varargout{1} = geometry_wrapper(81, varargin{:});
+        return
+      end
+
+      error('Arguments do not match any overload of function PrimitiveRef.Brutal');
+    end
+
+  end
+end
diff --git a/wrap/tests/expected-matlab/PrimitiveRefdouble.m b/wrap/tests/expected-matlab/PrimitiveRefdouble.m
new file mode 100644
index 000000000..095c132e2
--- /dev/null
+++ b/wrap/tests/expected-matlab/PrimitiveRefdouble.m
@@ -0,0 +1,54 @@
+%class PrimitiveRefdouble, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+%-------Constructors-------
+%PrimitiveRefdouble()
+%
+%-------Static Methods-------
+%Brutal(double t) : returns PrimitiveRef<double>
+%
+%-------Serialization Interface-------
+%string_serialize() : returns string
+%string_deserialize(string serialized) : returns PrimitiveRefdouble
+%
+classdef PrimitiveRefdouble < handle
+  properties
+    ptr_PrimitiveRefdouble = 0
+  end
+  methods
+    function obj = PrimitiveRefdouble(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        geometry_wrapper(79, my_ptr);
+      elseif nargin == 0
+        my_ptr = geometry_wrapper(80);
+      else
+        error('Arguments do not match any overload of PrimitiveRefdouble constructor');
+      end
+      obj.ptr_PrimitiveRefdouble = my_ptr;
+    end
+
+    function delete(obj)
+      geometry_wrapper(81, obj.ptr_PrimitiveRefdouble);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+    function varargout = Brutal(varargin)
+      % BRUTAL usage: Brutal(double t) : returns PrimitiveRefdouble
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double')
+        varargout{1} = geometry_wrapper(82, varargin{:});
+        return
+      end
+
+      error('Arguments do not match any overload of function PrimitiveRefdouble.Brutal');
+    end
+
+  end
+end
diff --git a/wrap/tests/expected/Test.m b/wrap/tests/expected-matlab/Test.m
similarity index 57%
rename from wrap/tests/expected/Test.m
rename to wrap/tests/expected-matlab/Test.m
index 44b8211b9..16b5289c4 100644
--- a/wrap/tests/expected/Test.m
+++ b/wrap/tests/expected-matlab/Test.m
@@ -1,5 +1,5 @@
 %class Test, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%at https://gtsam.org/doxygen/
 %
 %-------Constructors-------
 %Test()
@@ -10,7 +10,7 @@
 %create_MixedPtrs() : returns pair< Test, Test >
 %create_ptrs() : returns pair< Test, Test >
 %print() : returns void
-%return_Point2Ptr(bool value) : returns gtsam::Point2
+%return_Point2Ptr(bool value) : returns Point2
 %return_Test(Test value) : returns Test
 %return_TestPtr(Test value) : returns Test
 %return_bool(bool value) : returns bool
@@ -20,6 +20,7 @@
 %return_matrix1(Matrix value) : returns Matrix
 %return_matrix2(Matrix value) : returns Matrix
 %return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix >
+%return_pair(Vector v) : returns pair< Vector, Matrix >
 %return_ptrs(Test p1, Test p2) : returns pair< Test, Test >
 %return_size_t(size_t value) : returns size_t
 %return_string(string value) : returns string
@@ -55,160 +56,198 @@ classdef Test < handle
     %DISP Calls print on the object
     function varargout = arg_EigenConstRef(this, varargin)
       % ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double')
         geometry_wrapper(24, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.arg_EigenConstRef');
+        return
       end
+      error('Arguments do not match any overload of function Test.arg_EigenConstRef');
     end
 
     function varargout = create_MixedPtrs(this, varargin)
       % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, Test >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function Test.create_MixedPtrs');
     end
 
     function varargout = create_ptrs(this, varargin)
       % CREATE_PTRS usage: create_ptrs() : returns pair< Test, Test >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(26, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        [ varargout{1} varargout{2} ] = geometry_wrapper(26, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function Test.create_ptrs');
     end
 
     function varargout = print(this, varargin)
       % PRINT usage: print() : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      geometry_wrapper(27, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        geometry_wrapper(27, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function Test.print');
     end
 
     function varargout = return_Point2Ptr(this, varargin)
-      % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns gtsam::Point2
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(28, this, varargin{:});
+      % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'logical')
+        varargout{1} = geometry_wrapper(28, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function Test.return_Point2Ptr');
     end
 
     function varargout = return_Test(this, varargin)
       % RETURN_TEST usage: return_Test(Test value) : returns Test
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'Test')
         varargout{1} = geometry_wrapper(29, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_Test');
+        return
       end
+      error('Arguments do not match any overload of function Test.return_Test');
     end
 
     function varargout = return_TestPtr(this, varargin)
       % RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'Test')
         varargout{1} = geometry_wrapper(30, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_TestPtr');
+        return
       end
+      error('Arguments do not match any overload of function Test.return_TestPtr');
     end
 
     function varargout = return_bool(this, varargin)
       % RETURN_BOOL usage: return_bool(bool value) : returns bool
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(31, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'logical')
+        varargout{1} = geometry_wrapper(31, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function Test.return_bool');
     end
 
     function varargout = return_double(this, varargin)
       % RETURN_DOUBLE usage: return_double(double value) : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(32, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double')
+        varargout{1} = geometry_wrapper(32, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function Test.return_double');
     end
 
     function varargout = return_field(this, varargin)
       % RETURN_FIELD usage: return_field(Test t) : returns bool
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'Test')
         varargout{1} = geometry_wrapper(33, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_field');
+        return
       end
+      error('Arguments do not match any overload of function Test.return_field');
     end
 
     function varargout = return_int(this, varargin)
       % RETURN_INT usage: return_int(int value) : returns int
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(34, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'numeric')
+        varargout{1} = geometry_wrapper(34, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function Test.return_int');
     end
 
     function varargout = return_matrix1(this, varargin)
       % RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double')
         varargout{1} = geometry_wrapper(35, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_matrix1');
+        return
       end
+      error('Arguments do not match any overload of function Test.return_matrix1');
     end
 
     function varargout = return_matrix2(this, varargin)
       % RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double')
         varargout{1} = geometry_wrapper(36, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_matrix2');
+        return
       end
+      error('Arguments do not match any overload of function Test.return_matrix2');
     end
 
     function varargout = return_pair(this, varargin)
       % RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},2)==1 && isa(varargin{2},'double')
         [ varargout{1} varargout{2} ] = geometry_wrapper(37, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_pair');
+        return
       end
+      % RETURN_PAIR usage: return_pair(Vector v) : returns pair< Vector, Matrix >
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
+        [ varargout{1} varargout{2} ] = geometry_wrapper(38, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function Test.return_pair');
     end
 
     function varargout = return_ptrs(this, varargin)
       % RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< Test, Test >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test')
-        [ varargout{1} varargout{2} ] = geometry_wrapper(38, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_ptrs');
+        [ varargout{1} varargout{2} ] = geometry_wrapper(39, this, varargin{:});
+        return
       end
+      error('Arguments do not match any overload of function Test.return_ptrs');
     end
 
     function varargout = return_size_t(this, varargin)
       % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(39, this, varargin{:});
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'numeric')
+        varargout{1} = geometry_wrapper(40, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function Test.return_size_t');
     end
 
     function varargout = return_string(this, varargin)
       % RETURN_STRING usage: return_string(string value) : returns string
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'char')
-        varargout{1} = geometry_wrapper(40, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_string');
+        varargout{1} = geometry_wrapper(41, this, varargin{:});
+        return
       end
+      error('Arguments do not match any overload of function Test.return_string');
     end
 
     function varargout = return_vector1(this, varargin)
       % RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
-        varargout{1} = geometry_wrapper(41, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_vector1');
+        varargout{1} = geometry_wrapper(42, this, varargin{:});
+        return
       end
+      error('Arguments do not match any overload of function Test.return_vector1');
     end
 
     function varargout = return_vector2(this, varargin)
       % RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
-        varargout{1} = geometry_wrapper(42, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_vector2');
+        varargout{1} = geometry_wrapper(43, this, varargin{:});
+        return
       end
+      error('Arguments do not match any overload of function Test.return_vector2');
     end
 
   end
diff --git a/wrap/tests/expected/aGlobalFunction.m b/wrap/tests/expected-matlab/aGlobalFunction.m
similarity index 75%
rename from wrap/tests/expected/aGlobalFunction.m
rename to wrap/tests/expected-matlab/aGlobalFunction.m
index e507ad3b0..0f0b225fa 100644
--- a/wrap/tests/expected/aGlobalFunction.m
+++ b/wrap/tests/expected-matlab/aGlobalFunction.m
@@ -1,6 +1,6 @@
 function varargout = aGlobalFunction(varargin)
       if length(varargin) == 0
-        varargout{1} = geometry_wrapper(85, varargin{:});
+        varargout{1} = geometry_wrapper(95, varargin{:});
       else
         error('Arguments do not match any overload of function aGlobalFunction');
       end
diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected-matlab/geometry_wrapper.cpp
similarity index 68%
rename from wrap/tests/expected/geometry_wrapper.cpp
rename to wrap/tests/expected-matlab/geometry_wrapper.cpp
index 6946a969b..70f673d25 100644
--- a/wrap/tests/expected/geometry_wrapper.cpp
+++ b/wrap/tests/expected-matlab/geometry_wrapper.cpp
@@ -10,10 +10,11 @@
 #include <gtsam/geometry/Point3.h>
 
 typedef MyTemplate<gtsam::Point2> MyTemplatePoint2;
-typedef MyTemplate<Matrix> MyTemplateMatrix;
+typedef MyTemplate<gtsam::Matrix> MyTemplateMatrix;
+typedef PrimitiveRef<double> PrimitiveRefdouble;
 typedef MyVector<3> MyVector3;
 typedef MyVector<12> MyVector12;
-typedef MyFactor<gtsam::Pose2, Matrix> MyFactorPosePoint2;
+typedef MyFactor<gtsam::Pose2, gtsam::Matrix> MyFactorPosePoint2;
 
 BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2");
 BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3");
@@ -30,6 +31,8 @@ typedef std::set<boost::shared_ptr<MyTemplatePoint2>*> Collector_MyTemplatePoint
 static Collector_MyTemplatePoint2 collector_MyTemplatePoint2;
 typedef std::set<boost::shared_ptr<MyTemplateMatrix>*> Collector_MyTemplateMatrix;
 static Collector_MyTemplateMatrix collector_MyTemplateMatrix;
+typedef std::set<boost::shared_ptr<PrimitiveRefdouble>*> Collector_PrimitiveRefdouble;
+static Collector_PrimitiveRefdouble collector_PrimitiveRefdouble;
 typedef std::set<boost::shared_ptr<MyVector3>*> Collector_MyVector3;
 static Collector_MyVector3 collector_MyVector3;
 typedef std::set<boost::shared_ptr<MyVector12>*> Collector_MyVector12;
@@ -79,6 +82,12 @@ void _deleteAllObjects()
     collector_MyTemplateMatrix.erase(iter++);
     anyDeleted = true;
   } }
+  { for(Collector_PrimitiveRefdouble::iterator iter = collector_PrimitiveRefdouble.begin();
+      iter != collector_PrimitiveRefdouble.end(); ) {
+    delete *iter;
+    collector_PrimitiveRefdouble.erase(iter++);
+    anyDeleted = true;
+  } }
   { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin();
       iter != collector_MyVector3.end(); ) {
     delete *iter;
@@ -298,7 +307,7 @@ void gtsamPoint3_StaticFunctionRet_17(int nargout, mxArray *out[], int nargin, c
 {
   checkArguments("gtsamPoint3.StaticFunctionRet",nargout,nargin,1);
   double z = unwrap< double >(in[0]);
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point3>(gtsam::Point3::StaticFunctionRet(z)),"gtsam.Point3", false);
+  out[0] = wrap< Point3 >(gtsam::Point3::StaticFunctionRet(z));
 }
 
 void gtsamPoint3_staticFunction_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
@@ -402,7 +411,10 @@ void Test_return_Point2Ptr_28(int nargout, mxArray *out[], int nargin, const mxA
   checkArguments("return_Point2Ptr",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   bool value = unwrap< bool >(in[1]);
-  out[0] = wrap_shared_ptr(obj->return_Point2Ptr(value),"gtsam.Point2", false);
+  {
+  boost::shared_ptr<Point2> shared(obj->return_Point2Ptr(value));
+  out[0] = wrap_shared_ptr(shared,"Point2");
+  }
 }
 
 void Test_return_Test_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
@@ -480,7 +492,17 @@ void Test_return_pair_37(int nargout, mxArray *out[], int nargin, const mxArray
   out[1] = wrap< Matrix >(pairResult.second);
 }
 
-void Test_return_ptrs_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_pair_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("return_pair",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  Vector v = unwrap< Vector >(in[1]);
+  auto pairResult = obj->return_pair(v);
+  out[0] = wrap< Vector >(pairResult.first);
+  out[1] = wrap< Matrix >(pairResult.second);
+}
+
+void Test_return_ptrs_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_ptrs",nargout,nargin-1,2);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -491,7 +513,7 @@ void Test_return_ptrs_38(int nargout, mxArray *out[], int nargin, const mxArray
   out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }
 
-void Test_return_size_t_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_size_t_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_size_t",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -499,7 +521,7 @@ void Test_return_size_t_39(int nargout, mxArray *out[], int nargin, const mxArra
   out[0] = wrap< size_t >(obj->return_size_t(value));
 }
 
-void Test_return_string_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_string_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_string",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -507,7 +529,7 @@ void Test_return_string_40(int nargout, mxArray *out[], int nargin, const mxArra
   out[0] = wrap< string >(obj->return_string(value));
 }
 
-void Test_return_vector1_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_vector1_42(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_vector1",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -515,7 +537,7 @@ void Test_return_vector1_41(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap< Vector >(obj->return_vector1(value));
 }
 
-void Test_return_vector2_42(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_vector2_43(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_vector2",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -523,7 +545,7 @@ void Test_return_vector2_42(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap< Vector >(obj->return_vector2(value));
 }
 
-void MyBase_collectorInsertAndMakeBase_43(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyBase_collectorInsertAndMakeBase_44(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyBase> Shared;
@@ -532,7 +554,7 @@ void MyBase_collectorInsertAndMakeBase_43(int nargout, mxArray *out[], int nargi
   collector_MyBase.insert(self);
 }
 
-void MyBase_upcastFromVoid_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
+void MyBase_upcastFromVoid_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyBase> Shared;
   boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
@@ -541,7 +563,7 @@ void MyBase_upcastFromVoid_44(int nargout, mxArray *out[], int nargin, const mxA
   *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
 }
 
-void MyBase_deconstructor_45(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyBase_deconstructor_46(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyBase> Shared;
   checkArguments("delete_MyBase",nargout,nargin,1);
@@ -554,10 +576,10 @@ void MyBase_deconstructor_45(int nargout, mxArray *out[], int nargin, const mxAr
   }
 }
 
-void MyTemplatePoint2_collectorInsertAndMakeBase_46(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_collectorInsertAndMakeBase_47(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
+  typedef boost::shared_ptr<MyTemplate<gtsam::Point2>> Shared;
 
   Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
   collector_MyTemplatePoint2.insert(self);
@@ -567,21 +589,21 @@ void MyTemplatePoint2_collectorInsertAndMakeBase_46(int nargout, mxArray *out[],
   *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
 }
 
-void MyTemplatePoint2_upcastFromVoid_47(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
+void MyTemplatePoint2_upcastFromVoid_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
   mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
+  typedef boost::shared_ptr<MyTemplate<gtsam::Point2>> Shared;
   boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
   out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  Shared *self = new Shared(boost::static_pointer_cast<MyTemplatePoint2>(*asVoid));
+  Shared *self = new Shared(boost::static_pointer_cast<MyTemplate<gtsam::Point2>>(*asVoid));
   *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
 }
 
-void MyTemplatePoint2_constructor_48(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_constructor_49(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
+  typedef boost::shared_ptr<MyTemplate<gtsam::Point2>> Shared;
 
-  Shared *self = new Shared(new MyTemplatePoint2());
+  Shared *self = new Shared(new MyTemplate<gtsam::Point2>());
   collector_MyTemplatePoint2.insert(self);
   out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
@@ -591,9 +613,9 @@ void MyTemplatePoint2_constructor_48(int nargout, mxArray *out[], int nargin, co
   *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);
 }
 
-void MyTemplatePoint2_deconstructor_49(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_deconstructor_50(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
+  typedef boost::shared_ptr<MyTemplate<gtsam::Point2>> Shared;
   checkArguments("delete_MyTemplatePoint2",nargout,nargin,1);
   Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
   Collector_MyTemplatePoint2::iterator item;
@@ -604,103 +626,128 @@ void MyTemplatePoint2_deconstructor_49(int nargout, mxArray *out[], int nargin,
   }
 }
 
-void MyTemplatePoint2_accept_T_50(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_accept_T_51(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("accept_T",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  gtsam::Point2& value = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
+  Point2 value = unwrap< Point2 >(in[1]);
   obj->accept_T(value);
 }
 
-void MyTemplatePoint2_accept_Tptr_51(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_accept_Tptr_52(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("accept_Tptr",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  boost::shared_ptr<gtsam::Point2> value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
+  Point2 value = unwrap< Point2 >(in[1]);
   obj->accept_Tptr(value);
 }
 
-void MyTemplatePoint2_create_MixedPtrs_52(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_create_MixedPtrs_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("create_MixedPtrs",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
   auto pairResult = obj->create_MixedPtrs();
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(pairResult.first),"gtsam.Point2", false);
-  out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false);
+  out[0] = wrap< Point2 >(pairResult.first);
+  {
+  boost::shared_ptr<Point2> shared(pairResult.second);
+  out[1] = wrap_shared_ptr(shared,"Point2");
+  }
 }
 
-void MyTemplatePoint2_create_ptrs_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_create_ptrs_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("create_ptrs",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
   auto pairResult = obj->create_ptrs();
-  out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false);
-  out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false);
+  {
+  boost::shared_ptr<Point2> shared(pairResult.first);
+  out[0] = wrap_shared_ptr(shared,"Point2");
+  }
+  {
+  boost::shared_ptr<Point2> shared(pairResult.second);
+  out[1] = wrap_shared_ptr(shared,"Point2");
+  }
 }
 
-void MyTemplatePoint2_return_T_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_return_T_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_T",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  boost::shared_ptr<gtsam::Point2> value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(obj->return_T(value)),"gtsam.Point2", false);
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
+  Point2 value = unwrap< Point2 >(in[1]);
+  out[0] = wrap< Point2 >(obj->return_T(value));
 }
 
-void MyTemplatePoint2_return_Tptr_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_return_Tptr_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_Tptr",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  boost::shared_ptr<gtsam::Point2> value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(obj->return_Tptr(value),"gtsam.Point2", false);
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
+  Point2 value = unwrap< Point2 >(in[1]);
+  {
+  boost::shared_ptr<Point2> shared(obj->return_Tptr(value));
+  out[0] = wrap_shared_ptr(shared,"Point2");
+  }
 }
 
-void MyTemplatePoint2_return_ptrs_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_return_ptrs_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_ptrs",nargout,nargin-1,2);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  boost::shared_ptr<gtsam::Point2> p1 = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  boost::shared_ptr<gtsam::Point2> p2 = unwrap_shared_ptr< gtsam::Point2 >(in[2], "ptr_gtsamPoint2");
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
+  Point2 p1 = unwrap< Point2 >(in[1]);
+  Point2 p2 = unwrap< Point2 >(in[2]);
   auto pairResult = obj->return_ptrs(p1,p2);
-  out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false);
-  out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false);
-}
-
-void MyTemplatePoint2_templatedMethod_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  Matrix t = unwrap< Matrix >(in[1]);
-  out[0] = wrap< Matrix >(obj->templatedMethod<Matrix>(t));
+  {
+  boost::shared_ptr<Point2> shared(pairResult.first);
+  out[0] = wrap_shared_ptr(shared,"Point2");
+  }
+  {
+  boost::shared_ptr<Point2> shared(pairResult.second);
+  out[1] = wrap_shared_ptr(shared,"Point2");
+  }
 }
 
 void MyTemplatePoint2_templatedMethod_58(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(obj->templatedMethod<gtsam::Point2>(t)),"gtsam.Point2", false);
+  checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
+  Matrix t = unwrap< Matrix >(in[1]);
+  out[0] = wrap< Matrix >(obj->templatedMethod<gtsam::Matrix>(t));
 }
 
 void MyTemplatePoint2_templatedMethod_59(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point3>(obj->templatedMethod<gtsam::Point3>(t)),"gtsam.Point3", false);
+  checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
+  Point2 t = unwrap< Point2 >(in[1]);
+  out[0] = wrap< Point2 >(obj->templatedMethod<gtsam::Point2>(t));
 }
 
 void MyTemplatePoint2_templatedMethod_60(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("templatedMethodVector",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  Vector t = unwrap< Vector >(in[1]);
-  out[0] = wrap< Vector >(obj->templatedMethod<Vector>(t));
+  checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
+  Point3 t = unwrap< Point3 >(in[1]);
+  out[0] = wrap< Point3 >(obj->templatedMethod<gtsam::Point3>(t));
 }
 
-void MyTemplateMatrix_collectorInsertAndMakeBase_61(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_templatedMethod_61(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("templatedMethodVector",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
+  Vector t = unwrap< Vector >(in[1]);
+  out[0] = wrap< Vector >(obj->templatedMethod<gtsam::Vector>(t));
+}
+
+void MyTemplatePoint2_Level_62(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("MyTemplatePoint2.Level",nargout,nargin,1);
+  Point2 K = unwrap< Point2 >(in[0]);
+  out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Point2>>(MyTemplate<gtsam::Point2>::Level(K)),"MyTemplatePoint2", false);
+}
+
+void MyTemplateMatrix_collectorInsertAndMakeBase_63(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
+  typedef boost::shared_ptr<MyTemplate<gtsam::Matrix>> Shared;
 
   Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
   collector_MyTemplateMatrix.insert(self);
@@ -710,21 +757,21 @@ void MyTemplateMatrix_collectorInsertAndMakeBase_61(int nargout, mxArray *out[],
   *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
 }
 
-void MyTemplateMatrix_upcastFromVoid_62(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
+void MyTemplateMatrix_upcastFromVoid_64(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
   mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
+  typedef boost::shared_ptr<MyTemplate<gtsam::Matrix>> Shared;
   boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
   out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  Shared *self = new Shared(boost::static_pointer_cast<MyTemplateMatrix>(*asVoid));
+  Shared *self = new Shared(boost::static_pointer_cast<MyTemplate<gtsam::Matrix>>(*asVoid));
   *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
 }
 
-void MyTemplateMatrix_constructor_63(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_constructor_65(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
+  typedef boost::shared_ptr<MyTemplate<gtsam::Matrix>> Shared;
 
-  Shared *self = new Shared(new MyTemplateMatrix());
+  Shared *self = new Shared(new MyTemplate<gtsam::Matrix>());
   collector_MyTemplateMatrix.insert(self);
   out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
@@ -734,9 +781,9 @@ void MyTemplateMatrix_constructor_63(int nargout, mxArray *out[], int nargin, co
   *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);
 }
 
-void MyTemplateMatrix_deconstructor_64(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_deconstructor_66(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
+  typedef boost::shared_ptr<MyTemplate<gtsam::Matrix>> Shared;
   checkArguments("delete_MyTemplateMatrix",nargout,nargin,1);
   Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
   Collector_MyTemplateMatrix::iterator item;
@@ -747,26 +794,26 @@ void MyTemplateMatrix_deconstructor_64(int nargout, mxArray *out[], int nargin,
   }
 }
 
-void MyTemplateMatrix_accept_T_65(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_accept_T_67(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("accept_T",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
   Matrix value = unwrap< Matrix >(in[1]);
   obj->accept_T(value);
 }
 
-void MyTemplateMatrix_accept_Tptr_66(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_accept_Tptr_68(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("accept_Tptr",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
   Matrix value = unwrap< Matrix >(in[1]);
   obj->accept_Tptr(value);
 }
 
-void MyTemplateMatrix_create_MixedPtrs_67(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_create_MixedPtrs_69(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("create_MixedPtrs",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
   auto pairResult = obj->create_MixedPtrs();
   out[0] = wrap< Matrix >(pairResult.first);
   {
@@ -775,10 +822,10 @@ void MyTemplateMatrix_create_MixedPtrs_67(int nargout, mxArray *out[], int nargi
   }
 }
 
-void MyTemplateMatrix_create_ptrs_68(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_create_ptrs_70(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("create_ptrs",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
   auto pairResult = obj->create_ptrs();
   {
   boost::shared_ptr<Matrix> shared(pairResult.first);
@@ -790,18 +837,18 @@ void MyTemplateMatrix_create_ptrs_68(int nargout, mxArray *out[], int nargin, co
   }
 }
 
-void MyTemplateMatrix_return_T_69(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_return_T_71(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_T",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
   Matrix value = unwrap< Matrix >(in[1]);
   out[0] = wrap< Matrix >(obj->return_T(value));
 }
 
-void MyTemplateMatrix_return_Tptr_70(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_return_Tptr_72(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_Tptr",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
   Matrix value = unwrap< Matrix >(in[1]);
   {
   boost::shared_ptr<Matrix> shared(obj->return_Tptr(value));
@@ -809,10 +856,10 @@ void MyTemplateMatrix_return_Tptr_70(int nargout, mxArray *out[], int nargin, co
   }
 }
 
-void MyTemplateMatrix_return_ptrs_71(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_return_ptrs_73(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_ptrs",nargout,nargin-1,2);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
   Matrix p1 = unwrap< Matrix >(in[1]);
   Matrix p2 = unwrap< Matrix >(in[2]);
   auto pairResult = obj->return_ptrs(p1,p2);
@@ -826,61 +873,108 @@ void MyTemplateMatrix_return_ptrs_71(int nargout, mxArray *out[], int nargin, co
   }
 }
 
-void MyTemplateMatrix_templatedMethod_72(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  Matrix t = unwrap< Matrix >(in[1]);
-  out[0] = wrap< Matrix >(obj->templatedMethod<Matrix>(t));
-}
-
-void MyTemplateMatrix_templatedMethod_73(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(obj->templatedMethod<gtsam::Point2>(t)),"gtsam.Point2", false);
-}
-
 void MyTemplateMatrix_templatedMethod_74(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point3>(obj->templatedMethod<gtsam::Point3>(t)),"gtsam.Point3", false);
+  checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
+  Matrix t = unwrap< Matrix >(in[1]);
+  out[0] = wrap< Matrix >(obj->templatedMethod<gtsam::Matrix>(t));
 }
 
 void MyTemplateMatrix_templatedMethod_75(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("templatedMethodVector",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  Vector t = unwrap< Vector >(in[1]);
-  out[0] = wrap< Vector >(obj->templatedMethod<Vector>(t));
+  checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
+  Point2 t = unwrap< Point2 >(in[1]);
+  out[0] = wrap< Point2 >(obj->templatedMethod<gtsam::Point2>(t));
 }
 
-void MyVector3_collectorInsertAndMakeBase_76(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_templatedMethod_76(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
+  Point3 t = unwrap< Point3 >(in[1]);
+  out[0] = wrap< Point3 >(obj->templatedMethod<gtsam::Point3>(t));
+}
+
+void MyTemplateMatrix_templatedMethod_77(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("templatedMethodVector",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
+  Vector t = unwrap< Vector >(in[1]);
+  out[0] = wrap< Vector >(obj->templatedMethod<gtsam::Vector>(t));
+}
+
+void MyTemplateMatrix_Level_78(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("MyTemplateMatrix.Level",nargout,nargin,1);
+  Matrix K = unwrap< Matrix >(in[0]);
+  out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Matrix>>(MyTemplate<gtsam::Matrix>::Level(K)),"MyTemplateMatrix", false);
+}
+
+void PrimitiveRefdouble_collectorInsertAndMakeBase_79(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyVector3> Shared;
+  typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
+
+  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
+  collector_PrimitiveRefdouble.insert(self);
+}
+
+void PrimitiveRefdouble_constructor_80(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
+
+  Shared *self = new Shared(new PrimitiveRef<double>());
+  collector_PrimitiveRefdouble.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+}
+
+void PrimitiveRefdouble_deconstructor_81(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
+  checkArguments("delete_PrimitiveRefdouble",nargout,nargin,1);
+  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
+  Collector_PrimitiveRefdouble::iterator item;
+  item = collector_PrimitiveRefdouble.find(self);
+  if(item != collector_PrimitiveRefdouble.end()) {
+    delete self;
+    collector_PrimitiveRefdouble.erase(item);
+  }
+}
+
+void PrimitiveRefdouble_Brutal_82(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("PrimitiveRefdouble.Brutal",nargout,nargin,1);
+  double t = unwrap< double >(in[0]);
+  out[0] = wrap_shared_ptr(boost::make_shared<PrimitiveRef<double>>(PrimitiveRef<double>::Brutal(t)),"PrimitiveRefdouble", false);
+}
+
+void MyVector3_collectorInsertAndMakeBase_83(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<MyVector<3>> Shared;
 
   Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
   collector_MyVector3.insert(self);
 }
 
-void MyVector3_constructor_77(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector3_constructor_84(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyVector3> Shared;
+  typedef boost::shared_ptr<MyVector<3>> Shared;
 
-  Shared *self = new Shared(new MyVector3());
+  Shared *self = new Shared(new MyVector<3>());
   collector_MyVector3.insert(self);
   out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyVector3_deconstructor_78(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector3_deconstructor_85(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyVector3> Shared;
+  typedef boost::shared_ptr<MyVector<3>> Shared;
   checkArguments("delete_MyVector3",nargout,nargin,1);
   Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
   Collector_MyVector3::iterator item;
@@ -891,29 +985,29 @@ void MyVector3_deconstructor_78(int nargout, mxArray *out[], int nargin, const m
   }
 }
 
-void MyVector12_collectorInsertAndMakeBase_79(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_collectorInsertAndMakeBase_86(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyVector12> Shared;
+  typedef boost::shared_ptr<MyVector<12>> Shared;
 
   Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
   collector_MyVector12.insert(self);
 }
 
-void MyVector12_constructor_80(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_constructor_87(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyVector12> Shared;
+  typedef boost::shared_ptr<MyVector<12>> Shared;
 
-  Shared *self = new Shared(new MyVector12());
+  Shared *self = new Shared(new MyVector<12>());
   collector_MyVector12.insert(self);
   out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyVector12_deconstructor_81(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_deconstructor_88(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyVector12> Shared;
+  typedef boost::shared_ptr<MyVector<12>> Shared;
   checkArguments("delete_MyVector12",nargout,nargin,1);
   Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
   Collector_MyVector12::iterator item;
@@ -924,33 +1018,33 @@ void MyVector12_deconstructor_81(int nargout, mxArray *out[], int nargin, const
   }
 }
 
-void MyFactorPosePoint2_collectorInsertAndMakeBase_82(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_collectorInsertAndMakeBase_89(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyFactorPosePoint2> Shared;
+  typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
 
   Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
   collector_MyFactorPosePoint2.insert(self);
 }
 
-void MyFactorPosePoint2_constructor_83(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_constructor_90(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyFactorPosePoint2> Shared;
+  typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
 
   size_t key1 = unwrap< size_t >(in[0]);
   size_t key2 = unwrap< size_t >(in[1]);
   double measured = unwrap< double >(in[2]);
   boost::shared_ptr<gtsam::noiseModel::Base> noiseModel = unwrap_shared_ptr< gtsam::noiseModel::Base >(in[3], "ptr_gtsamnoiseModelBase");
-  Shared *self = new Shared(new MyFactorPosePoint2(key1,key2,measured,noiseModel));
+  Shared *self = new Shared(new MyFactor<gtsam::Pose2, gtsam::Matrix>(key1,key2,measured,noiseModel));
   collector_MyFactorPosePoint2.insert(self);
   out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyFactorPosePoint2_deconstructor_84(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_deconstructor_91(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyFactorPosePoint2> Shared;
+  typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
   checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1);
   Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
   Collector_MyFactorPosePoint2::iterator item;
@@ -961,18 +1055,51 @@ void MyFactorPosePoint2_deconstructor_84(int nargout, mxArray *out[], int nargin
   }
 }
 
-void aGlobalFunction_85(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void load2D_92(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("load2D",nargout,nargin,5);
+  string filename = unwrap< string >(in[0]);
+  boost::shared_ptr<Test> model = unwrap_shared_ptr< Test >(in[1], "ptr_Test");
+  int maxID = unwrap< int >(in[2]);
+  bool addNoise = unwrap< bool >(in[3]);
+  bool smart = unwrap< bool >(in[4]);
+  auto pairResult = load2D(filename,model,maxID,addNoise,smart);
+  out[0] = wrap_shared_ptr(pairResult.first,"gtsam.NonlinearFactorGraph", false);
+  out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Values", false);
+}
+void load2D_93(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("load2D",nargout,nargin,5);
+  string filename = unwrap< string >(in[0]);
+  boost::shared_ptr<gtsam::noiseModel::Diagonal> model = unwrap_shared_ptr< gtsam::noiseModel::Diagonal >(in[1], "ptr_gtsamnoiseModelDiagonal");
+  int maxID = unwrap< int >(in[2]);
+  bool addNoise = unwrap< bool >(in[3]);
+  bool smart = unwrap< bool >(in[4]);
+  auto pairResult = load2D(filename,model,maxID,addNoise,smart);
+  out[0] = wrap_shared_ptr(pairResult.first,"gtsam.NonlinearFactorGraph", false);
+  out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Values", false);
+}
+void load2D_94(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("load2D",nargout,nargin,2);
+  string filename = unwrap< string >(in[0]);
+  boost::shared_ptr<gtsam::noiseModel::Diagonal> model = unwrap_shared_ptr< gtsam::noiseModel::Diagonal >(in[1], "ptr_gtsamnoiseModelDiagonal");
+  auto pairResult = load2D(filename,model);
+  out[0] = wrap_shared_ptr(pairResult.first,"gtsam.NonlinearFactorGraph", false);
+  out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Values", false);
+}
+void aGlobalFunction_95(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("aGlobalFunction",nargout,nargin,0);
   out[0] = wrap< Vector >(aGlobalFunction());
 }
-void overloadedGlobalFunction_86(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void overloadedGlobalFunction_96(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("overloadedGlobalFunction",nargout,nargin,1);
   int a = unwrap< int >(in[0]);
   out[0] = wrap< Vector >(overloadedGlobalFunction(a));
 }
-void overloadedGlobalFunction_87(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void overloadedGlobalFunction_97(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("overloadedGlobalFunction",nargout,nargin,2);
   int a = unwrap< int >(in[0]);
@@ -1106,64 +1233,64 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       Test_return_pair_37(nargout, out, nargin-1, in+1);
       break;
     case 38:
-      Test_return_ptrs_38(nargout, out, nargin-1, in+1);
+      Test_return_pair_38(nargout, out, nargin-1, in+1);
       break;
     case 39:
-      Test_return_size_t_39(nargout, out, nargin-1, in+1);
+      Test_return_ptrs_39(nargout, out, nargin-1, in+1);
       break;
     case 40:
-      Test_return_string_40(nargout, out, nargin-1, in+1);
+      Test_return_size_t_40(nargout, out, nargin-1, in+1);
       break;
     case 41:
-      Test_return_vector1_41(nargout, out, nargin-1, in+1);
+      Test_return_string_41(nargout, out, nargin-1, in+1);
       break;
     case 42:
-      Test_return_vector2_42(nargout, out, nargin-1, in+1);
+      Test_return_vector1_42(nargout, out, nargin-1, in+1);
       break;
     case 43:
-      MyBase_collectorInsertAndMakeBase_43(nargout, out, nargin-1, in+1);
+      Test_return_vector2_43(nargout, out, nargin-1, in+1);
       break;
     case 44:
-      MyBase_upcastFromVoid_44(nargout, out, nargin-1, in+1);
+      MyBase_collectorInsertAndMakeBase_44(nargout, out, nargin-1, in+1);
       break;
     case 45:
-      MyBase_deconstructor_45(nargout, out, nargin-1, in+1);
+      MyBase_upcastFromVoid_45(nargout, out, nargin-1, in+1);
       break;
     case 46:
-      MyTemplatePoint2_collectorInsertAndMakeBase_46(nargout, out, nargin-1, in+1);
+      MyBase_deconstructor_46(nargout, out, nargin-1, in+1);
       break;
     case 47:
-      MyTemplatePoint2_upcastFromVoid_47(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_collectorInsertAndMakeBase_47(nargout, out, nargin-1, in+1);
       break;
     case 48:
-      MyTemplatePoint2_constructor_48(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_upcastFromVoid_48(nargout, out, nargin-1, in+1);
       break;
     case 49:
-      MyTemplatePoint2_deconstructor_49(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_constructor_49(nargout, out, nargin-1, in+1);
       break;
     case 50:
-      MyTemplatePoint2_accept_T_50(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_deconstructor_50(nargout, out, nargin-1, in+1);
       break;
     case 51:
-      MyTemplatePoint2_accept_Tptr_51(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_accept_T_51(nargout, out, nargin-1, in+1);
       break;
     case 52:
-      MyTemplatePoint2_create_MixedPtrs_52(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_accept_Tptr_52(nargout, out, nargin-1, in+1);
       break;
     case 53:
-      MyTemplatePoint2_create_ptrs_53(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_create_MixedPtrs_53(nargout, out, nargin-1, in+1);
       break;
     case 54:
-      MyTemplatePoint2_return_T_54(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_create_ptrs_54(nargout, out, nargin-1, in+1);
       break;
     case 55:
-      MyTemplatePoint2_return_Tptr_55(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_return_T_55(nargout, out, nargin-1, in+1);
       break;
     case 56:
-      MyTemplatePoint2_return_ptrs_56(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_return_Tptr_56(nargout, out, nargin-1, in+1);
       break;
     case 57:
-      MyTemplatePoint2_templatedMethod_57(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_return_ptrs_57(nargout, out, nargin-1, in+1);
       break;
     case 58:
       MyTemplatePoint2_templatedMethod_58(nargout, out, nargin-1, in+1);
@@ -1175,43 +1302,43 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       MyTemplatePoint2_templatedMethod_60(nargout, out, nargin-1, in+1);
       break;
     case 61:
-      MyTemplateMatrix_collectorInsertAndMakeBase_61(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_templatedMethod_61(nargout, out, nargin-1, in+1);
       break;
     case 62:
-      MyTemplateMatrix_upcastFromVoid_62(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_Level_62(nargout, out, nargin-1, in+1);
       break;
     case 63:
-      MyTemplateMatrix_constructor_63(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_collectorInsertAndMakeBase_63(nargout, out, nargin-1, in+1);
       break;
     case 64:
-      MyTemplateMatrix_deconstructor_64(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_upcastFromVoid_64(nargout, out, nargin-1, in+1);
       break;
     case 65:
-      MyTemplateMatrix_accept_T_65(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_constructor_65(nargout, out, nargin-1, in+1);
       break;
     case 66:
-      MyTemplateMatrix_accept_Tptr_66(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_deconstructor_66(nargout, out, nargin-1, in+1);
       break;
     case 67:
-      MyTemplateMatrix_create_MixedPtrs_67(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_accept_T_67(nargout, out, nargin-1, in+1);
       break;
     case 68:
-      MyTemplateMatrix_create_ptrs_68(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_accept_Tptr_68(nargout, out, nargin-1, in+1);
       break;
     case 69:
-      MyTemplateMatrix_return_T_69(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_create_MixedPtrs_69(nargout, out, nargin-1, in+1);
       break;
     case 70:
-      MyTemplateMatrix_return_Tptr_70(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_create_ptrs_70(nargout, out, nargin-1, in+1);
       break;
     case 71:
-      MyTemplateMatrix_return_ptrs_71(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_return_T_71(nargout, out, nargin-1, in+1);
       break;
     case 72:
-      MyTemplateMatrix_templatedMethod_72(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_return_Tptr_72(nargout, out, nargin-1, in+1);
       break;
     case 73:
-      MyTemplateMatrix_templatedMethod_73(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_return_ptrs_73(nargout, out, nargin-1, in+1);
       break;
     case 74:
       MyTemplateMatrix_templatedMethod_74(nargout, out, nargin-1, in+1);
@@ -1220,40 +1347,70 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       MyTemplateMatrix_templatedMethod_75(nargout, out, nargin-1, in+1);
       break;
     case 76:
-      MyVector3_collectorInsertAndMakeBase_76(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_templatedMethod_76(nargout, out, nargin-1, in+1);
       break;
     case 77:
-      MyVector3_constructor_77(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_templatedMethod_77(nargout, out, nargin-1, in+1);
       break;
     case 78:
-      MyVector3_deconstructor_78(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_Level_78(nargout, out, nargin-1, in+1);
       break;
     case 79:
-      MyVector12_collectorInsertAndMakeBase_79(nargout, out, nargin-1, in+1);
+      PrimitiveRefdouble_collectorInsertAndMakeBase_79(nargout, out, nargin-1, in+1);
       break;
     case 80:
-      MyVector12_constructor_80(nargout, out, nargin-1, in+1);
+      PrimitiveRefdouble_constructor_80(nargout, out, nargin-1, in+1);
       break;
     case 81:
-      MyVector12_deconstructor_81(nargout, out, nargin-1, in+1);
+      PrimitiveRefdouble_deconstructor_81(nargout, out, nargin-1, in+1);
       break;
     case 82:
-      MyFactorPosePoint2_collectorInsertAndMakeBase_82(nargout, out, nargin-1, in+1);
+      PrimitiveRefdouble_Brutal_82(nargout, out, nargin-1, in+1);
       break;
     case 83:
-      MyFactorPosePoint2_constructor_83(nargout, out, nargin-1, in+1);
+      MyVector3_collectorInsertAndMakeBase_83(nargout, out, nargin-1, in+1);
       break;
     case 84:
-      MyFactorPosePoint2_deconstructor_84(nargout, out, nargin-1, in+1);
+      MyVector3_constructor_84(nargout, out, nargin-1, in+1);
       break;
     case 85:
-      aGlobalFunction_85(nargout, out, nargin-1, in+1);
+      MyVector3_deconstructor_85(nargout, out, nargin-1, in+1);
       break;
     case 86:
-      overloadedGlobalFunction_86(nargout, out, nargin-1, in+1);
+      MyVector12_collectorInsertAndMakeBase_86(nargout, out, nargin-1, in+1);
       break;
     case 87:
-      overloadedGlobalFunction_87(nargout, out, nargin-1, in+1);
+      MyVector12_constructor_87(nargout, out, nargin-1, in+1);
+      break;
+    case 88:
+      MyVector12_deconstructor_88(nargout, out, nargin-1, in+1);
+      break;
+    case 89:
+      MyFactorPosePoint2_collectorInsertAndMakeBase_89(nargout, out, nargin-1, in+1);
+      break;
+    case 90:
+      MyFactorPosePoint2_constructor_90(nargout, out, nargin-1, in+1);
+      break;
+    case 91:
+      MyFactorPosePoint2_deconstructor_91(nargout, out, nargin-1, in+1);
+      break;
+    case 92:
+      load2D_92(nargout, out, nargin-1, in+1);
+      break;
+    case 93:
+      load2D_93(nargout, out, nargin-1, in+1);
+      break;
+    case 94:
+      load2D_94(nargout, out, nargin-1, in+1);
+      break;
+    case 95:
+      aGlobalFunction_95(nargout, out, nargin-1, in+1);
+      break;
+    case 96:
+      overloadedGlobalFunction_96(nargout, out, nargin-1, in+1);
+      break;
+    case 97:
+      overloadedGlobalFunction_97(nargout, out, nargin-1, in+1);
       break;
     }
   } catch(const std::exception& e) {
diff --git a/wrap/tests/expected-matlab/load2D.m b/wrap/tests/expected-matlab/load2D.m
new file mode 100644
index 000000000..fec762dc5
--- /dev/null
+++ b/wrap/tests/expected-matlab/load2D.m
@@ -0,0 +1,10 @@
+function varargout = load2D(varargin)
+      if length(varargin) == 5 && isa(varargin{1},'char') && isa(varargin{2},'Test') && isa(varargin{3},'numeric') && isa(varargin{4},'logical') && isa(varargin{5},'logical')
+        [ varargout{1} varargout{2} ] = geometry_wrapper(92, varargin{:});
+      elseif length(varargin) == 5 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.noiseModel.Diagonal') && isa(varargin{3},'numeric') && isa(varargin{4},'logical') && isa(varargin{5},'logical')
+        [ varargout{1} varargout{2} ] = geometry_wrapper(93, varargin{:});
+      elseif length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.noiseModel.Diagonal')
+        [ varargout{1} varargout{2} ] = geometry_wrapper(94, varargin{:});
+      else
+        error('Arguments do not match any overload of function load2D');
+      end
diff --git a/wrap/tests/expected/overloadedGlobalFunction.m b/wrap/tests/expected-matlab/overloadedGlobalFunction.m
similarity index 73%
rename from wrap/tests/expected/overloadedGlobalFunction.m
rename to wrap/tests/expected-matlab/overloadedGlobalFunction.m
index 458779e2c..168c83c67 100644
--- a/wrap/tests/expected/overloadedGlobalFunction.m
+++ b/wrap/tests/expected-matlab/overloadedGlobalFunction.m
@@ -1,8 +1,8 @@
 function varargout = overloadedGlobalFunction(varargin)
       if length(varargin) == 1 && isa(varargin{1},'numeric')
-        varargout{1} = geometry_wrapper(86, varargin{:});
+        varargout{1} = geometry_wrapper(96, varargin{:});
       elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double')
-        varargout{1} = geometry_wrapper(87, varargin{:});
+        varargout{1} = geometry_wrapper(97, varargin{:});
       else
         error('Arguments do not match any overload of function overloadedGlobalFunction');
       end
diff --git a/wrap/tests/expected-python/geometry_pybind.cpp b/wrap/tests/expected-python/geometry_pybind.cpp
new file mode 100644
index 000000000..6e18f83d7
--- /dev/null
+++ b/wrap/tests/expected-python/geometry_pybind.cpp
@@ -0,0 +1,153 @@
+
+
+#include <pybind11/eigen.h>
+#include <pybind11/stl_bind.h>
+#include <pybind11/pybind11.h>
+#include "gtsam/nonlinear/utilities.h"  // for RedirectCout.
+
+#include "gtsam/geometry/Point2.h"
+#include "gtsam/geometry/Point3.h"
+#include "folder/path/to/Test.h"
+
+#include "wrap/serialization.h"
+#include <boost/serialization/export.hpp>
+
+BOOST_CLASS_EXPORT(gtsam::Point2)
+BOOST_CLASS_EXPORT(gtsam::Point3)
+
+
+
+
+using namespace std;
+
+namespace py = pybind11;
+
+PYBIND11_MODULE(geometry_py, m_) {
+    m_.doc() = "pybind11 wrapper of geometry_py";
+
+    pybind11::module m_gtsam = m_.def_submodule("gtsam", "gtsam submodule");
+
+    py::class_<gtsam::Point2, std::shared_ptr<gtsam::Point2>>(m_gtsam, "Point2")
+        .def(py::init<>())
+        .def(py::init< double,  double>(), py::arg("x"), py::arg("y"))
+        .def("x",[](gtsam::Point2* self){return self->x();})
+        .def("y",[](gtsam::Point2* self){return self->y();})
+        .def("dim",[](gtsam::Point2* self){return self->dim();})
+        .def("returnChar",[](gtsam::Point2* self){return self->returnChar();})
+        .def("argChar",[](gtsam::Point2* self, char a){ self->argChar(a);}, py::arg("a"))
+        .def("argUChar",[](gtsam::Point2* self, unsigned char a){ self->argUChar(a);}, py::arg("a"))
+        .def("eigenArguments",[](gtsam::Point2* self,const gtsam::Vector& v,const gtsam::Matrix& m){ self->eigenArguments(v, m);}, py::arg("v"), py::arg("m"))
+        .def("vectorConfusion",[](gtsam::Point2* self){return self->vectorConfusion();})
+.def("serialize",
+    [](gtsam::Point2* self){
+        return gtsam::serialize(self);
+    }
+)
+.def("deserialize",
+    [](gtsam::Point2* self, string serialized){
+        return gtsam::deserialize(serialized, self);
+    })
+;
+
+    py::class_<gtsam::Point3, std::shared_ptr<gtsam::Point3>>(m_gtsam, "Point3")
+        .def(py::init< double,  double,  double>(), py::arg("x"), py::arg("y"), py::arg("z"))
+        .def("norm",[](gtsam::Point3* self){return self->norm();})
+.def("serialize",
+    [](gtsam::Point3* self){
+        return gtsam::serialize(self);
+    }
+)
+.def("deserialize",
+    [](gtsam::Point3* self, string serialized){
+        return gtsam::deserialize(serialized, self);
+    })
+
+        .def_static("staticFunction",[](){return gtsam::Point3::staticFunction();})
+        .def_static("StaticFunctionRet",[]( double z){return gtsam::Point3::StaticFunctionRet(z);}, py::arg("z"));
+
+    py::class_<Test, std::shared_ptr<Test>>(m_, "Test")
+        .def(py::init<>())
+        .def(py::init< double, const gtsam::Matrix&>(), py::arg("a"), py::arg("b"))
+        .def("return_pair",[](Test* self,const gtsam::Vector& v,const gtsam::Matrix& A){return self->return_pair(v, A);}, py::arg("v"), py::arg("A"))
+        .def("return_pair",[](Test* self,const gtsam::Vector& v){return self->return_pair(v);}, py::arg("v"))
+        .def("return_bool",[](Test* self, bool value){return self->return_bool(value);}, py::arg("value"))
+        .def("return_size_t",[](Test* self, size_t value){return self->return_size_t(value);}, py::arg("value"))
+        .def("return_int",[](Test* self, int value){return self->return_int(value);}, py::arg("value"))
+        .def("return_double",[](Test* self, double value){return self->return_double(value);}, py::arg("value"))
+        .def("return_string",[](Test* self, string value){return self->return_string(value);}, py::arg("value"))
+        .def("return_vector1",[](Test* self,const gtsam::Vector& value){return self->return_vector1(value);}, py::arg("value"))
+        .def("return_matrix1",[](Test* self,const gtsam::Matrix& value){return self->return_matrix1(value);}, py::arg("value"))
+        .def("return_vector2",[](Test* self,const gtsam::Vector& value){return self->return_vector2(value);}, py::arg("value"))
+        .def("return_matrix2",[](Test* self,const gtsam::Matrix& value){return self->return_matrix2(value);}, py::arg("value"))
+        .def("arg_EigenConstRef",[](Test* self,const gtsam::Matrix& value){ self->arg_EigenConstRef(value);}, py::arg("value"))
+        .def("return_field",[](Test* self,const Test& t){return self->return_field(t);}, py::arg("t"))
+        .def("return_TestPtr",[](Test* self,const std::shared_ptr<Test>& value){return self->return_TestPtr(value);}, py::arg("value"))
+        .def("return_Test",[](Test* self,const std::shared_ptr<Test>& value){return self->return_Test(value);}, py::arg("value"))
+        .def("return_Point2Ptr",[](Test* self, bool value){return self->return_Point2Ptr(value);}, py::arg("value"))
+        .def("create_ptrs",[](Test* self){return self->create_ptrs();})
+        .def("create_MixedPtrs",[](Test* self){return self->create_MixedPtrs();})
+        .def("return_ptrs",[](Test* self,const std::shared_ptr<Test>& p1,const std::shared_ptr<Test>& p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"))
+        .def("print_",[](Test* self){ self->print();})
+        .def("__repr__",
+                    [](const Test &a) {
+                        gtsam::RedirectCout redirect;
+                        a.print();
+                        return redirect.str();
+                    });
+
+    py::class_<MyBase, std::shared_ptr<MyBase>>(m_, "MyBase");
+
+    py::class_<MyTemplate<gtsam::Point2>, MyBase, std::shared_ptr<MyTemplate<gtsam::Point2>>>(m_, "MyTemplatePoint2")
+        .def(py::init<>())
+        .def("templatedMethodPoint2",[](MyTemplate<gtsam::Point2>* self,const gtsam::Point2& t){return self->templatedMethod<gtsam::Point2>(t);}, py::arg("t"))
+        .def("templatedMethodPoint3",[](MyTemplate<gtsam::Point2>* self,const gtsam::Point3& t){return self->templatedMethod<gtsam::Point3>(t);}, py::arg("t"))
+        .def("templatedMethodVector",[](MyTemplate<gtsam::Point2>* self,const gtsam::Vector& t){return self->templatedMethod<gtsam::Vector>(t);}, py::arg("t"))
+        .def("templatedMethodMatrix",[](MyTemplate<gtsam::Point2>* self,const gtsam::Matrix& t){return self->templatedMethod<gtsam::Matrix>(t);}, py::arg("t"))
+        .def("accept_T",[](MyTemplate<gtsam::Point2>* self,const gtsam::Point2& value){ self->accept_T(value);}, py::arg("value"))
+        .def("accept_Tptr",[](MyTemplate<gtsam::Point2>* self,const std::shared_ptr<gtsam::Point2>& value){ self->accept_Tptr(value);}, py::arg("value"))
+        .def("return_Tptr",[](MyTemplate<gtsam::Point2>* self,const std::shared_ptr<gtsam::Point2>& value){return self->return_Tptr(value);}, py::arg("value"))
+        .def("return_T",[](MyTemplate<gtsam::Point2>* self,const std::shared_ptr<gtsam::Point2>& value){return self->return_T(value);}, py::arg("value"))
+        .def("create_ptrs",[](MyTemplate<gtsam::Point2>* self){return self->create_ptrs();})
+        .def("create_MixedPtrs",[](MyTemplate<gtsam::Point2>* self){return self->create_MixedPtrs();})
+        .def("return_ptrs",[](MyTemplate<gtsam::Point2>* self,const std::shared_ptr<gtsam::Point2>& p1,const std::shared_ptr<gtsam::Point2>& p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"))
+        .def_static("Level",[](const gtsam::Point2& K){return MyTemplate<gtsam::Point2>::Level(K);}, py::arg("K"));
+
+    py::class_<MyTemplate<gtsam::Matrix>, MyBase, std::shared_ptr<MyTemplate<gtsam::Matrix>>>(m_, "MyTemplateMatrix")
+        .def(py::init<>())
+        .def("templatedMethodPoint2",[](MyTemplate<gtsam::Matrix>* self,const gtsam::Point2& t){return self->templatedMethod<gtsam::Point2>(t);}, py::arg("t"))
+        .def("templatedMethodPoint3",[](MyTemplate<gtsam::Matrix>* self,const gtsam::Point3& t){return self->templatedMethod<gtsam::Point3>(t);}, py::arg("t"))
+        .def("templatedMethodVector",[](MyTemplate<gtsam::Matrix>* self,const gtsam::Vector& t){return self->templatedMethod<gtsam::Vector>(t);}, py::arg("t"))
+        .def("templatedMethodMatrix",[](MyTemplate<gtsam::Matrix>* self,const gtsam::Matrix& t){return self->templatedMethod<gtsam::Matrix>(t);}, py::arg("t"))
+        .def("accept_T",[](MyTemplate<gtsam::Matrix>* self,const gtsam::Matrix& value){ self->accept_T(value);}, py::arg("value"))
+        .def("accept_Tptr",[](MyTemplate<gtsam::Matrix>* self,const std::shared_ptr<gtsam::Matrix>& value){ self->accept_Tptr(value);}, py::arg("value"))
+        .def("return_Tptr",[](MyTemplate<gtsam::Matrix>* self,const std::shared_ptr<gtsam::Matrix>& value){return self->return_Tptr(value);}, py::arg("value"))
+        .def("return_T",[](MyTemplate<gtsam::Matrix>* self,const std::shared_ptr<gtsam::Matrix>& value){return self->return_T(value);}, py::arg("value"))
+        .def("create_ptrs",[](MyTemplate<gtsam::Matrix>* self){return self->create_ptrs();})
+        .def("create_MixedPtrs",[](MyTemplate<gtsam::Matrix>* self){return self->create_MixedPtrs();})
+        .def("return_ptrs",[](MyTemplate<gtsam::Matrix>* self,const std::shared_ptr<gtsam::Matrix>& p1,const std::shared_ptr<gtsam::Matrix>& p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"))
+        .def_static("Level",[](const gtsam::Matrix& K){return MyTemplate<gtsam::Matrix>::Level(K);}, py::arg("K"));
+
+    py::class_<PrimitiveRef<double>, std::shared_ptr<PrimitiveRef<double>>>(m_, "PrimitiveRefdouble")
+        .def(py::init<>())
+        .def_static("Brutal",[](const double& t){return PrimitiveRef<double>::Brutal(t);}, py::arg("t"));
+
+    py::class_<MyVector<3>, std::shared_ptr<MyVector<3>>>(m_, "MyVector3")
+        .def(py::init<>());
+
+    py::class_<MyVector<12>, std::shared_ptr<MyVector<12>>>(m_, "MyVector12")
+        .def(py::init<>());
+
+    py::class_<MyFactor<gtsam::Pose2, gtsam::Matrix>, std::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>>>(m_, "MyFactorPosePoint2")
+        .def(py::init< size_t,  size_t,  double, const std::shared_ptr<gtsam::noiseModel::Base>&>(), py::arg("key1"), py::arg("key2"), py::arg("measured"), py::arg("noiseModel"));
+
+    m_.def("load2D",[]( string filename,const std::shared_ptr<Test>& model, int maxID, bool addNoise, bool smart){return ::load2D(filename, model, maxID, addNoise, smart);}, py::arg("filename"), py::arg("model"), py::arg("maxID"), py::arg("addNoise"), py::arg("smart"));
+    m_.def("load2D",[]( string filename,const std::shared_ptr<gtsam::noiseModel::Diagonal>& model, int maxID, bool addNoise, bool smart){return ::load2D(filename, model, maxID, addNoise, smart);}, py::arg("filename"), py::arg("model"), py::arg("maxID"), py::arg("addNoise"), py::arg("smart"));
+    m_.def("load2D",[]( string filename,const std::shared_ptr<gtsam::noiseModel::Diagonal>& model){return ::load2D(filename, model);}, py::arg("filename"), py::arg("model"));
+    m_.def("aGlobalFunction",[](){return ::aGlobalFunction();});
+    m_.def("overloadedGlobalFunction",[]( int a){return ::overloadedGlobalFunction(a);}, py::arg("a"));
+    m_.def("overloadedGlobalFunction",[]( int a, double b){return ::overloadedGlobalFunction(a, b);}, py::arg("a"), py::arg("b"));
+
+#include "python/specializations.h"
+
+}
+
diff --git a/wrap/tests/expected-python/geometry_python.cpp b/wrap/tests/expected-python/geometry_python.cpp
deleted file mode 100644
index 582fdfc13..000000000
--- a/wrap/tests/expected-python/geometry_python.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-#include <boost/python.hpp>
-
-using namespace boost::python;
-BOOST_PYTHON_MODULE(geometry)
-{
-class_<Point2>("Point2")
-  .def("Point2", &Point2::Point2);
-  .def("argChar", &Point2::argChar);
-  .def("argUChar", &Point2::argUChar);
-  .def("dim", &Point2::dim);
-  .def("eigenArguments", &Point2::eigenArguments);
-  .def("returnChar", &Point2::returnChar);
-  .def("vectorConfusion", &Point2::vectorConfusion);
-  .def("x", &Point2::x);
-  .def("y", &Point2::y);
-;
-
-class_<Point3>("Point3")
-  .def("Point3", &Point3::Point3);
-  .def("StaticFunctionRet", &Point3::StaticFunctionRet);
-  .def("staticFunction", &Point3::staticFunction);
-  .def("norm", &Point3::norm);
-;
-
-class_<Test>("Test")
-  .def("Test", &Test::Test);
-  .def("arg_EigenConstRef", &Test::arg_EigenConstRef);
-  .def("create_MixedPtrs", &Test::create_MixedPtrs);
-  .def("create_ptrs", &Test::create_ptrs);
-  .def("print", &Test::print);
-  .def("return_Point2Ptr", &Test::return_Point2Ptr);
-  .def("return_Test", &Test::return_Test);
-  .def("return_TestPtr", &Test::return_TestPtr);
-  .def("return_bool", &Test::return_bool);
-  .def("return_double", &Test::return_double);
-  .def("return_field", &Test::return_field);
-  .def("return_int", &Test::return_int);
-  .def("return_matrix1", &Test::return_matrix1);
-  .def("return_matrix2", &Test::return_matrix2);
-  .def("return_pair", &Test::return_pair);
-  .def("return_ptrs", &Test::return_ptrs);
-  .def("return_size_t", &Test::return_size_t);
-  .def("return_string", &Test::return_string);
-  .def("return_vector1", &Test::return_vector1);
-  .def("return_vector2", &Test::return_vector2);
-;
-
-class_<MyBase>("MyBase")
-  .def("MyBase", &MyBase::MyBase);
-;
-
-class_<MyTemplatePoint2>("MyTemplatePoint2")
-  .def("MyTemplatePoint2", &MyTemplatePoint2::MyTemplatePoint2);
-  .def("accept_T", &MyTemplatePoint2::accept_T);
-  .def("accept_Tptr", &MyTemplatePoint2::accept_Tptr);
-  .def("create_MixedPtrs", &MyTemplatePoint2::create_MixedPtrs);
-  .def("create_ptrs", &MyTemplatePoint2::create_ptrs);
-  .def("return_T", &MyTemplatePoint2::return_T);
-  .def("return_Tptr", &MyTemplatePoint2::return_Tptr);
-  .def("return_ptrs", &MyTemplatePoint2::return_ptrs);
-  .def("templatedMethod", &MyTemplatePoint2::templatedMethod);
-  .def("templatedMethod", &MyTemplatePoint2::templatedMethod);
-  .def("templatedMethod", &MyTemplatePoint2::templatedMethod);
-  .def("templatedMethod", &MyTemplatePoint2::templatedMethod);
-;
-
-class_<MyTemplateMatrix>("MyTemplateMatrix")
-  .def("MyTemplateMatrix", &MyTemplateMatrix::MyTemplateMatrix);
-  .def("accept_T", &MyTemplateMatrix::accept_T);
-  .def("accept_Tptr", &MyTemplateMatrix::accept_Tptr);
-  .def("create_MixedPtrs", &MyTemplateMatrix::create_MixedPtrs);
-  .def("create_ptrs", &MyTemplateMatrix::create_ptrs);
-  .def("return_T", &MyTemplateMatrix::return_T);
-  .def("return_Tptr", &MyTemplateMatrix::return_Tptr);
-  .def("return_ptrs", &MyTemplateMatrix::return_ptrs);
-  .def("templatedMethod", &MyTemplateMatrix::templatedMethod);
-  .def("templatedMethod", &MyTemplateMatrix::templatedMethod);
-  .def("templatedMethod", &MyTemplateMatrix::templatedMethod);
-  .def("templatedMethod", &MyTemplateMatrix::templatedMethod);
-;
-
-class_<MyVector3>("MyVector3")
-  .def("MyVector3", &MyVector3::MyVector3);
-;
-
-class_<MyVector12>("MyVector12")
-  .def("MyVector12", &MyVector12::MyVector12);
-;
-
-class_<MyFactorPosePoint2>("MyFactorPosePoint2")
-  .def("MyFactorPosePoint2", &MyFactorPosePoint2::MyFactorPosePoint2);
-;
-
-def("aGlobalFunction", aGlobalFunction);
-def("overloadedGlobalFunction", overloadedGlobalFunction);
-}
diff --git a/wrap/tests/expected-xml-generation/xml/JacobianFactorQ_8h.xml b/wrap/tests/expected-xml-generation/xml/JacobianFactorQ_8h.xml
new file mode 100644
index 000000000..abce75972
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/JacobianFactorQ_8h.xml
@@ -0,0 +1,103 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="JacobianFactorQ_8h" kind="file" language="C++">
+    <compoundname>JacobianFactorQ.h</compoundname>
+    <includes local="no">gtsam/linear/RegularJacobianFactor.h</includes>
+    <incdepgraph>
+      <node id="10">
+        <label>gtsam/linear/RegularJacobianFactor.h</label>
+      </node>
+      <node id="9">
+        <label>tests/doc-test-files/JacobianFactorQ.h</label>
+        <link refid="JacobianFactorQ.h"/>
+        <childnode refid="10" relation="include">
+        </childnode>
+      </node>
+    </incdepgraph>
+    <innerclass refid="classgtsam_1_1JacobianFactorQ" prot="public">gtsam::JacobianFactorQ</innerclass>
+    <innerclass refid="structgtsam_1_1traits_3_01JacobianFactorQ_3_01D_00_01ZDim_01_4_01_4" prot="public">gtsam::traits&lt; JacobianFactorQ&lt; D, ZDim &gt; &gt;</innerclass>
+    <innernamespace refid="namespacegtsam">gtsam</innernamespace>
+    <briefdescription>
+    </briefdescription>
+    <detaileddescription>
+    </detaileddescription>
+    <programlisting>
+<codeline lineno="1"><highlight class="comment">/*<sp/>----------------------------------------------------------------------------</highlight></codeline>
+<codeline lineno="2"><highlight class="comment"></highlight></codeline>
+<codeline lineno="3"><highlight class="comment"><sp/>*<sp/>GTSAM<sp/>Copyright<sp/>2010,<sp/>Georgia<sp/>Tech<sp/>Research<sp/>Corporation,</highlight></codeline>
+<codeline lineno="4"><highlight class="comment"><sp/>*<sp/>Atlanta,<sp/>Georgia<sp/>30332-0415</highlight></codeline>
+<codeline lineno="5"><highlight class="comment"><sp/>*<sp/>All<sp/>Rights<sp/>Reserved</highlight></codeline>
+<codeline lineno="6"><highlight class="comment"><sp/>*<sp/>Authors:<sp/>Frank<sp/>Dellaert,<sp/>et<sp/>al.<sp/>(see<sp/>THANKS<sp/>for<sp/>the<sp/>full<sp/>author<sp/>list)</highlight></codeline>
+<codeline lineno="7"><highlight class="comment"></highlight></codeline>
+<codeline lineno="8"><highlight class="comment"><sp/>*<sp/>See<sp/>LICENSE<sp/>for<sp/>the<sp/>license<sp/>information</highlight></codeline>
+<codeline lineno="9"><highlight class="comment"></highlight></codeline>
+<codeline lineno="10"><highlight class="comment"><sp/>*<sp/>--------------------------------------------------------------------------<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="11"><highlight class="normal"></highlight></codeline>
+<codeline lineno="12"><highlight class="normal"></highlight><highlight class="comment">/*</highlight></codeline>
+<codeline lineno="13"><highlight class="comment"><sp/>*<sp/>@file<sp/><sp/>JacobianFactorQ.h</highlight></codeline>
+<codeline lineno="14"><highlight class="comment"><sp/>*<sp/>@date<sp/><sp/>Oct<sp/>27,<sp/>2013</highlight></codeline>
+<codeline lineno="15"><highlight class="comment"><sp/>*<sp/>@uthor<sp/>Frank<sp/>Dellaert</highlight></codeline>
+<codeline lineno="16"><highlight class="comment"><sp/>*/</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="17"><highlight class="normal"></highlight></codeline>
+<codeline lineno="18"><highlight class="normal"></highlight><highlight class="preprocessor">#pragma<sp/>once</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="19"><highlight class="normal"></highlight></codeline>
+<codeline lineno="20"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&lt;gtsam/linear/RegularJacobianFactor.h&gt;</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="21"><highlight class="normal"></highlight></codeline>
+<codeline lineno="22"><highlight class="normal"></highlight><highlight class="keyword">namespace<sp/></highlight><highlight class="normal"><ref refid="namespacegtsam" kindref="compound">gtsam</ref><sp/>{</highlight></codeline>
+<codeline lineno="26"><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keywordtype">size_t</highlight><highlight class="normal"><sp/>D,<sp/></highlight><highlight class="keywordtype">size_t</highlight><highlight class="normal"><sp/>ZDim&gt;</highlight></codeline>
+<codeline lineno="27" refid="classgtsam_1_1JacobianFactorQ" refkind="compound"><highlight class="normal"></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal"><ref refid="classgtsam_1_1JacobianFactorQ" kindref="compound">JacobianFactorQ</ref>:<sp/></highlight><highlight class="keyword">public</highlight><highlight class="normal"><sp/>RegularJacobianFactor&lt;D&gt;<sp/>{</highlight></codeline>
+<codeline lineno="28"><highlight class="normal"></highlight></codeline>
+<codeline lineno="29"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>RegularJacobianFactor&lt;D&gt;<sp/>Base;</highlight></codeline>
+<codeline lineno="30"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>Eigen::Matrix&lt;double,<sp/>ZDim,<sp/>D&gt;<sp/>MatrixZD;</highlight></codeline>
+<codeline lineno="31"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>std::pair&lt;Key,<sp/>Matrix&gt;<sp/>KeyMatrix;</highlight></codeline>
+<codeline lineno="32"><highlight class="normal"></highlight></codeline>
+<codeline lineno="33"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="34"><highlight class="normal"></highlight></codeline>
+<codeline lineno="36" refid="classgtsam_1_1JacobianFactorQ_1a4dd3fbba46a6dff75bcf902931a3b769" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1JacobianFactorQ_1a4dd3fbba46a6dff75bcf902931a3b769" kindref="member">JacobianFactorQ</ref>()<sp/>{</highlight></codeline>
+<codeline lineno="37"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="38"><highlight class="normal"></highlight></codeline>
+<codeline lineno="40" refid="classgtsam_1_1JacobianFactorQ_1abfb38e58b5b2d1293cf374b7eca2b2d4" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1JacobianFactorQ_1abfb38e58b5b2d1293cf374b7eca2b2d4" kindref="member">JacobianFactorQ</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>KeyVector&amp;<sp/>keys,<sp/></highlight><highlight class="comment">//</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="41"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>SharedDiagonal&amp;<sp/>model<sp/>=<sp/>SharedDiagonal())<sp/>:</highlight></codeline>
+<codeline lineno="42"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>Base()<sp/>{</highlight></codeline>
+<codeline lineno="43"><highlight class="normal"><sp/><sp/><sp/><sp/>Matrix<sp/>zeroMatrix<sp/>=<sp/>Matrix::Zero(0,<sp/>D);</highlight></codeline>
+<codeline lineno="44"><highlight class="normal"><sp/><sp/><sp/><sp/>Vector<sp/>zeroVector<sp/>=<sp/>Vector::Zero(0);</highlight></codeline>
+<codeline lineno="45"><highlight class="normal"><sp/><sp/><sp/><sp/>std::vector&lt;KeyMatrix&gt;<sp/>QF;</highlight></codeline>
+<codeline lineno="46"><highlight class="normal"><sp/><sp/><sp/><sp/>QF.reserve(keys.size());</highlight></codeline>
+<codeline lineno="47"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">for</highlight><highlight class="normal">(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Key&amp;<sp/>key:<sp/>keys)</highlight></codeline>
+<codeline lineno="48"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>QF.push_back(KeyMatrix(key,<sp/>zeroMatrix));</highlight></codeline>
+<codeline lineno="49"><highlight class="normal"><sp/><sp/><sp/><sp/>JacobianFactor::fillTerms(QF,<sp/>zeroVector,<sp/>model);</highlight></codeline>
+<codeline lineno="50"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="51"><highlight class="normal"></highlight></codeline>
+<codeline lineno="53" refid="classgtsam_1_1JacobianFactorQ_1a549fef1272025c45472f2e4294bd319e" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1JacobianFactorQ_1a549fef1272025c45472f2e4294bd319e" kindref="member">JacobianFactorQ</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>KeyVector&amp;<sp/>keys,</highlight></codeline>
+<codeline lineno="54"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>std::vector&lt;MatrixZD,<sp/>Eigen::aligned_allocator&lt;MatrixZD&gt;<sp/>&gt;&amp;<sp/>FBlocks,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Matrix&amp;<sp/>E,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Matrix3&amp;<sp/>P,</highlight></codeline>
+<codeline lineno="55"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Vector&amp;<sp/>b,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>SharedDiagonal&amp;<sp/>model<sp/>=<sp/>SharedDiagonal())<sp/>:</highlight></codeline>
+<codeline lineno="56"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>Base()<sp/>{</highlight></codeline>
+<codeline lineno="57"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordtype">size_t</highlight><highlight class="normal"><sp/>j<sp/>=<sp/>0,<sp/>m2<sp/>=<sp/>E.rows(),<sp/>m<sp/>=<sp/>m2<sp/>/<sp/>ZDim;</highlight></codeline>
+<codeline lineno="58"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>Calculate<sp/>projector<sp/>Q</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="59"><highlight class="normal"><sp/><sp/><sp/><sp/>Matrix<sp/>Q<sp/>=<sp/>Matrix::Identity(m2,m2)<sp/>-<sp/>E<sp/>*<sp/>P<sp/>*<sp/>E.transpose();</highlight></codeline>
+<codeline lineno="60"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>Calculate<sp/>pre-computed<sp/>Jacobian<sp/>matrices</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="61"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>TODO:<sp/>can<sp/>we<sp/>do<sp/>better<sp/>?</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="62"><highlight class="normal"><sp/><sp/><sp/><sp/>std::vector&lt;KeyMatrix&gt;<sp/>QF;</highlight></codeline>
+<codeline lineno="63"><highlight class="normal"><sp/><sp/><sp/><sp/>QF.reserve(m);</highlight></codeline>
+<codeline lineno="64"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>Below,<sp/>we<sp/>compute<sp/>each<sp/>mZDim*D<sp/>block<sp/>A_j<sp/>=<sp/>Q_j<sp/>*<sp/>F_j<sp/>=<sp/>(mZDim*ZDim)<sp/>*<sp/>(Zdim*D)</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="65"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">for</highlight><highlight class="normal"><sp/>(</highlight><highlight class="keywordtype">size_t</highlight><highlight class="normal"><sp/>k<sp/>=<sp/>0;<sp/>k<sp/>&lt;<sp/>FBlocks.size();<sp/>++k)<sp/>{</highlight></codeline>
+<codeline lineno="66"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>Key<sp/>key<sp/>=<sp/>keys[k];</highlight></codeline>
+<codeline lineno="67"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>QF.push_back(</highlight></codeline>
+<codeline lineno="68"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>KeyMatrix(key,<sp/>-<sp/>Q.block(0,<sp/>ZDim<sp/>*<sp/>j++,<sp/>m2,<sp/>ZDim)<sp/>*<sp/>FBlocks[k]));</highlight></codeline>
+<codeline lineno="69"><highlight class="normal"><sp/><sp/><sp/><sp/>}</highlight></codeline>
+<codeline lineno="70"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>Which<sp/>is<sp/>then<sp/>passed<sp/>to<sp/>the<sp/>normal<sp/>JacobianFactor<sp/>constructor</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="71"><highlight class="normal"><sp/><sp/><sp/><sp/>JacobianFactor::fillTerms(QF,<sp/>-<sp/>Q<sp/>*<sp/>b,<sp/>model);</highlight></codeline>
+<codeline lineno="72"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="73"><highlight class="normal">};</highlight></codeline>
+<codeline lineno="74"><highlight class="normal"></highlight><highlight class="comment">//<sp/>end<sp/>class<sp/>JacobianFactorQ</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="75"><highlight class="normal"></highlight></codeline>
+<codeline lineno="76"><highlight class="normal"></highlight><highlight class="comment">//<sp/>traits</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="77" refid="structgtsam_1_1traits_3_01JacobianFactorQ_3_01D_00_01ZDim_01_4_01_4" refkind="compound"><highlight class="normal"></highlight><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keywordtype">size_t</highlight><highlight class="normal"><sp/>D,<sp/></highlight><highlight class="keywordtype">size_t</highlight><highlight class="normal"><sp/>ZDim&gt;<sp/></highlight><highlight class="keyword">struct<sp/></highlight><highlight class="normal">traits&lt;<ref refid="classgtsam_1_1JacobianFactorQ" kindref="compound">JacobianFactorQ</ref>&lt;D,<sp/>ZDim&gt;<sp/>&gt;<sp/>:<sp/></highlight><highlight class="keyword">public</highlight><highlight class="normal"><sp/>Testable&lt;</highlight></codeline>
+<codeline lineno="78"><highlight class="normal"><sp/><sp/><sp/><sp/>JacobianFactorQ&lt;D,<sp/>ZDim&gt;<sp/>&gt;<sp/>{</highlight></codeline>
+<codeline lineno="79"><highlight class="normal">};</highlight></codeline>
+<codeline lineno="80"><highlight class="normal"></highlight></codeline>
+<codeline lineno="81"><highlight class="normal">}</highlight></codeline>
+    </programlisting>
+    <location file="tests/doc-test-files/JacobianFactorQ.h"/>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/NonlinearFactor_8h.xml b/wrap/tests/expected-xml-generation/xml/NonlinearFactor_8h.xml
new file mode 100644
index 000000000..7a6d73729
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/NonlinearFactor_8h.xml
@@ -0,0 +1,596 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="NonlinearFactor_8h" kind="file" language="C++">
+    <compoundname>NonlinearFactor.h</compoundname>
+    <includes local="no">gtsam/nonlinear/Values.h</includes>
+    <includes local="no">gtsam/linear/NoiseModel.h</includes>
+    <includes local="no">gtsam/linear/JacobianFactor.h</includes>
+    <includes local="no">gtsam/inference/Factor.h</includes>
+    <includes local="no">gtsam/base/OptionalJacobian.h</includes>
+    <includes local="no">boost/serialization/base_object.hpp</includes>
+    <includes local="no">boost/assign/list_of.hpp</includes>
+    <incdepgraph>
+      <node id="15">
+        <label>gtsam/linear/JacobianFactor.h</label>
+      </node>
+      <node id="17">
+        <label>gtsam/base/OptionalJacobian.h</label>
+      </node>
+      <node id="12">
+        <label>tests/doc-test-files/NonlinearFactor.h</label>
+        <link refid="NonlinearFactor.h"/>
+        <childnode refid="13" relation="include">
+        </childnode>
+        <childnode refid="14" relation="include">
+        </childnode>
+        <childnode refid="15" relation="include">
+        </childnode>
+        <childnode refid="16" relation="include">
+        </childnode>
+        <childnode refid="17" relation="include">
+        </childnode>
+        <childnode refid="18" relation="include">
+        </childnode>
+        <childnode refid="19" relation="include">
+        </childnode>
+      </node>
+      <node id="16">
+        <label>gtsam/inference/Factor.h</label>
+      </node>
+      <node id="14">
+        <label>gtsam/linear/NoiseModel.h</label>
+      </node>
+      <node id="18">
+        <label>boost/serialization/base_object.hpp</label>
+      </node>
+      <node id="19">
+        <label>boost/assign/list_of.hpp</label>
+      </node>
+      <node id="13">
+        <label>gtsam/nonlinear/Values.h</label>
+      </node>
+    </incdepgraph>
+    <innerclass refid="classgtsam_1_1NonlinearFactor" prot="public">gtsam::NonlinearFactor</innerclass>
+    <innerclass refid="structgtsam_1_1traits_3_01NonlinearFactor_01_4" prot="public">gtsam::traits&lt; NonlinearFactor &gt;</innerclass>
+    <innerclass refid="classgtsam_1_1NoiseModelFactor" prot="public">gtsam::NoiseModelFactor</innerclass>
+    <innerclass refid="classgtsam_1_1NoiseModelFactor1" prot="public">gtsam::NoiseModelFactor1</innerclass>
+    <innerclass refid="classgtsam_1_1NoiseModelFactor2" prot="public">gtsam::NoiseModelFactor2</innerclass>
+    <innerclass refid="classgtsam_1_1NoiseModelFactor3" prot="public">gtsam::NoiseModelFactor3</innerclass>
+    <innerclass refid="classgtsam_1_1NoiseModelFactor4" prot="public">gtsam::NoiseModelFactor4</innerclass>
+    <innerclass refid="classgtsam_1_1NoiseModelFactor5" prot="public">gtsam::NoiseModelFactor5</innerclass>
+    <innerclass refid="classgtsam_1_1NoiseModelFactor6" prot="public">gtsam::NoiseModelFactor6</innerclass>
+    <innernamespace refid="namespacegtsam">gtsam</innernamespace>
+      <sectiondef kind="define">
+      <memberdef kind="define" id="NonlinearFactor_8h_1acd444d887da7c7f7107e8a5caa703b22" prot="public" static="no">
+        <name>ADD_CLONE_NONLINEAR_FACTOR</name>
+        <param><defname>Derived</defname></param>
+        <initializer>virtual gtsam::NonlinearFactor::shared_ptr clone() const { \
+  return std::static_pointer_cast&lt;<ref refid="classgtsam_1_1NonlinearFactor" kindref="compound">gtsam::NonlinearFactor</ref>&gt;( \
+      gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); }</initializer>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Macro to add a standard clone function to a derived factor <xrefsect id="deprecated_1_deprecated000001"><xreftitle>Deprecated</xreftitle><xrefdescription><para>: will go away shortly - just add the clone function directly </para></xrefdescription></xrefsect></para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="36" column="9" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="36" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+    <briefdescription>
+<para>Non-linear factor base classes. </para>    </briefdescription>
+    <detaileddescription>
+<para><simplesect kind="author"><para>Frank Dellaert </para></simplesect>
+<simplesect kind="author"><para>Richard Roberts </para></simplesect>
+</para>    </detaileddescription>
+    <programlisting>
+<codeline lineno="1"><highlight class="comment">/*<sp/>----------------------------------------------------------------------------</highlight></codeline>
+<codeline lineno="2"><highlight class="comment"></highlight></codeline>
+<codeline lineno="3"><highlight class="comment"><sp/>*<sp/>GTSAM<sp/>Copyright<sp/>2010,<sp/>Georgia<sp/>Tech<sp/>Research<sp/>Corporation,</highlight></codeline>
+<codeline lineno="4"><highlight class="comment"><sp/>*<sp/>Atlanta,<sp/>Georgia<sp/>30332-0415</highlight></codeline>
+<codeline lineno="5"><highlight class="comment"><sp/>*<sp/>All<sp/>Rights<sp/>Reserved</highlight></codeline>
+<codeline lineno="6"><highlight class="comment"><sp/>*<sp/>Authors:<sp/>Frank<sp/>Dellaert,<sp/>et<sp/>al.<sp/>(see<sp/>THANKS<sp/>for<sp/>the<sp/>full<sp/>author<sp/>list)</highlight></codeline>
+<codeline lineno="7"><highlight class="comment"></highlight></codeline>
+<codeline lineno="8"><highlight class="comment"><sp/>*<sp/>See<sp/>LICENSE<sp/>for<sp/>the<sp/>license<sp/>information</highlight></codeline>
+<codeline lineno="9"><highlight class="comment"></highlight></codeline>
+<codeline lineno="10"><highlight class="comment"><sp/>*<sp/>--------------------------------------------------------------------------<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="11"><highlight class="normal"></highlight></codeline>
+<codeline lineno="19"><highlight class="comment">//<sp/>\callgraph</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="20"><highlight class="normal"></highlight></codeline>
+<codeline lineno="21"><highlight class="normal"></highlight><highlight class="preprocessor">#pragma<sp/>once</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="22"><highlight class="normal"></highlight></codeline>
+<codeline lineno="23"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&lt;gtsam/nonlinear/Values.h&gt;</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="24"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&lt;gtsam/linear/NoiseModel.h&gt;</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="25"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&lt;gtsam/linear/JacobianFactor.h&gt;</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="26"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&lt;gtsam/inference/Factor.h&gt;</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="27"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&lt;gtsam/base/OptionalJacobian.h&gt;</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="28"><highlight class="normal"></highlight></codeline>
+<codeline lineno="29"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&lt;boost/serialization/base_object.hpp&gt;</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="30"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>&lt;boost/assign/list_of.hpp&gt;</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="31"><highlight class="normal"></highlight></codeline>
+<codeline lineno="36" refid="NonlinearFactor_8h_1acd444d887da7c7f7107e8a5caa703b22" refkind="member"><highlight class="preprocessor">#define<sp/>ADD_CLONE_NONLINEAR_FACTOR(Derived)<sp/>\</highlight></codeline>
+<codeline lineno="37"><highlight class="preprocessor"><sp/><sp/>virtual<sp/>gtsam::NonlinearFactor::shared_ptr<sp/>clone()<sp/>const<sp/>{<sp/>\</highlight></codeline>
+<codeline lineno="38"><highlight class="preprocessor"><sp/><sp/>return<sp/>std::static_pointer_cast&lt;gtsam::NonlinearFactor&gt;(<sp/>\</highlight></codeline>
+<codeline lineno="39"><highlight class="preprocessor"><sp/><sp/><sp/><sp/><sp/><sp/>gtsam::NonlinearFactor::shared_ptr(new<sp/>Derived(*this)));<sp/>}</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="40"><highlight class="normal"></highlight></codeline>
+<codeline lineno="41"><highlight class="normal"></highlight><highlight class="keyword">namespace<sp/></highlight><highlight class="normal"><ref refid="namespacegtsam" kindref="compound">gtsam</ref><sp/>{</highlight></codeline>
+<codeline lineno="42"><highlight class="normal"></highlight></codeline>
+<codeline lineno="43"><highlight class="normal"></highlight><highlight class="keyword">using</highlight><highlight class="normal"><sp/>boost::assign::cref_list_of;</highlight></codeline>
+<codeline lineno="44"><highlight class="normal"></highlight></codeline>
+<codeline lineno="45"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>*************************************************************************<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="46"><highlight class="normal"></highlight></codeline>
+<codeline lineno="52" refid="classgtsam_1_1NonlinearFactor" refkind="compound"><highlight class="keyword">class<sp/></highlight><highlight class="normal">GTSAM_EXPORT<sp/><ref refid="classgtsam_1_1NonlinearFactor" kindref="compound">NonlinearFactor</ref>:<sp/></highlight><highlight class="keyword">public</highlight><highlight class="normal"><sp/>Factor<sp/>{</highlight></codeline>
+<codeline lineno="53"><highlight class="normal"></highlight></codeline>
+<codeline lineno="54"><highlight class="normal"></highlight><highlight class="keyword">protected</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="55"><highlight class="normal"></highlight></codeline>
+<codeline lineno="56"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>Some<sp/>handy<sp/>typedefs</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="57"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>Factor<sp/>Base;</highlight></codeline>
+<codeline lineno="58"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NonlinearFactor" kindref="compound">NonlinearFactor</ref><sp/><ref refid="classgtsam_1_1NonlinearFactor" kindref="compound">This</ref>;</highlight></codeline>
+<codeline lineno="59"><highlight class="normal"></highlight></codeline>
+<codeline lineno="60"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="61"><highlight class="normal"></highlight></codeline>
+<codeline lineno="62"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>std::shared_ptr&lt;This&gt;<sp/>shared_ptr;</highlight></codeline>
+<codeline lineno="63"><highlight class="normal"></highlight></codeline>
+<codeline lineno="66"></codeline>
+<codeline lineno="68" refid="classgtsam_1_1NonlinearFactor_1a7c719ae67f222f66569ccb3b526cb9cb" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NonlinearFactor_1a7c719ae67f222f66569ccb3b526cb9cb" kindref="member">NonlinearFactor</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="69"><highlight class="normal"></highlight></codeline>
+<codeline lineno="73"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">typename</highlight><highlight class="normal"><sp/>CONTAINER&gt;</highlight></codeline>
+<codeline lineno="74" refid="classgtsam_1_1NonlinearFactor_1a8a7610dba7e9a6f878268c2769ea0e04" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NonlinearFactor_1a8a7610dba7e9a6f878268c2769ea0e04" kindref="member">NonlinearFactor</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>CONTAINER&amp;<sp/>keys)<sp/>:</highlight></codeline>
+<codeline lineno="75"><highlight class="normal"><sp/><sp/><sp/><sp/>Base(keys)<sp/>{}</highlight></codeline>
+<codeline lineno="76"><highlight class="normal"></highlight></codeline>
+<codeline lineno="80"></codeline>
+<codeline lineno="82"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>print(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>std::string&amp;<sp/>s<sp/>=<sp/></highlight><highlight class="stringliteral">&quot;&quot;</highlight><highlight class="normal">,</highlight></codeline>
+<codeline lineno="83"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>KeyFormatter&amp;<sp/>keyFormatter<sp/>=<sp/>DefaultKeyFormatter)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal">;</highlight></codeline>
+<codeline lineno="84"><highlight class="normal"></highlight></codeline>
+<codeline lineno="86"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">bool</highlight><highlight class="normal"><sp/>equals(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NonlinearFactor" kindref="compound">NonlinearFactor</ref>&amp;<sp/>f,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>tol<sp/>=<sp/>1e-9)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal">;</highlight></codeline>
+<codeline lineno="90"></codeline>
+<codeline lineno="92" refid="classgtsam_1_1NonlinearFactor_1ac457810c504de58732d51d61f715c2de" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NonlinearFactor_1ac457810c504de58732d51d61f715c2de" kindref="member">~NonlinearFactor</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="93"><highlight class="normal"></highlight></codeline>
+<codeline lineno="94"><highlight class="normal"></highlight></codeline>
+<codeline lineno="100"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>error(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Values&amp;<sp/>c)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>=<sp/>0;</highlight></codeline>
+<codeline lineno="101"><highlight class="normal"></highlight></codeline>
+<codeline lineno="103"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">size_t</highlight><highlight class="normal"><sp/>dim()<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>=<sp/>0;</highlight></codeline>
+<codeline lineno="104"><highlight class="normal"></highlight></codeline>
+<codeline lineno="115" refid="classgtsam_1_1NonlinearFactor_1a1968f471dc3ea33aaf0226ff7be5ae8c" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">bool</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NonlinearFactor_1a1968f471dc3ea33aaf0226ff7be5ae8c" kindref="member">active</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Values&amp;<sp/></highlight><highlight class="comment">/*c*/</highlight><highlight class="normal">)</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/></highlight><highlight class="keyword">true</highlight><highlight class="normal">;<sp/>}</highlight></codeline>
+<codeline lineno="116"><highlight class="normal"></highlight></codeline>
+<codeline lineno="118"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>std::shared_ptr&lt;GaussianFactor&gt;</highlight></codeline>
+<codeline lineno="119"><highlight class="normal"><sp/><sp/>linearize(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Values&amp;<sp/>c)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>=<sp/>0;</highlight></codeline>
+<codeline lineno="120"><highlight class="normal"></highlight></codeline>
+<codeline lineno="127" refid="classgtsam_1_1NonlinearFactor_1aa98a30a78ade8489214ce2e5b8302777" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>shared_ptr<sp/><ref refid="classgtsam_1_1NonlinearFactor_1aa98a30a78ade8489214ce2e5b8302777" kindref="member">clone</ref>()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{</highlight></codeline>
+<codeline lineno="128"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>TODO:<sp/>choose<sp/>better<sp/>exception<sp/>to<sp/>throw<sp/>here</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="129"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">throw</highlight><highlight class="normal"><sp/>std::runtime_error(</highlight><highlight class="stringliteral">&quot;NonlinearFactor::clone():<sp/>Attempting<sp/>to<sp/>clone<sp/>factor<sp/>with<sp/>no<sp/>clone()<sp/>implemented!&quot;</highlight><highlight class="normal">);</highlight></codeline>
+<codeline lineno="130"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>shared_ptr();</highlight></codeline>
+<codeline lineno="131"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="132"><highlight class="normal"></highlight></codeline>
+<codeline lineno="138"><highlight class="normal"><sp/><sp/>shared_ptr<sp/>rekey(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>std::map&lt;Key,Key&gt;&amp;<sp/>rekey_mapping)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal">;</highlight></codeline>
+<codeline lineno="139"><highlight class="normal"></highlight></codeline>
+<codeline lineno="144"><highlight class="normal"><sp/><sp/>shared_ptr<sp/>rekey(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>KeyVector&amp;<sp/>new_keys)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal">;</highlight></codeline>
+<codeline lineno="145"><highlight class="normal"></highlight></codeline>
+<codeline lineno="146"><highlight class="normal">};<sp/></highlight><highlight class="comment">//<sp/>\class<sp/>NonlinearFactor</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="147"><highlight class="normal"></highlight></codeline>
+<codeline lineno="149" refid="structgtsam_1_1traits_3_01NonlinearFactor_01_4" refkind="compound"><highlight class="keyword">template</highlight><highlight class="normal">&lt;&gt;<sp/></highlight><highlight class="keyword">struct<sp/></highlight><highlight class="normal">traits&lt;<ref refid="classgtsam_1_1NonlinearFactor" kindref="compound">NonlinearFactor</ref>&gt;<sp/>:<sp/></highlight><highlight class="keyword">public</highlight><highlight class="normal"><sp/>Testable&lt;NonlinearFactor&gt;<sp/>{</highlight></codeline>
+<codeline lineno="150"><highlight class="normal">};</highlight></codeline>
+<codeline lineno="151"><highlight class="normal"></highlight></codeline>
+<codeline lineno="152"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>*************************************************************************<sp/>*/</highlight></codeline>
+<codeline lineno="163" refid="classgtsam_1_1NoiseModelFactor" refkind="compound"><highlight class="keyword">class<sp/></highlight><highlight class="normal">GTSAM_EXPORT<sp/><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref>:<sp/></highlight><highlight class="keyword">public</highlight><highlight class="normal"><sp/>NonlinearFactor<sp/>{</highlight></codeline>
+<codeline lineno="164"><highlight class="normal"></highlight></codeline>
+<codeline lineno="165"><highlight class="normal"></highlight><highlight class="keyword">protected</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="166"><highlight class="normal"></highlight></codeline>
+<codeline lineno="167"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>handy<sp/>typedefs</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="168"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>NonlinearFactor<sp/>Base;</highlight></codeline>
+<codeline lineno="169"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref><sp/><ref refid="classgtsam_1_1NonlinearFactor" kindref="compound">This</ref>;</highlight></codeline>
+<codeline lineno="170"><highlight class="normal"></highlight></codeline>
+<codeline lineno="171"><highlight class="normal"><sp/><sp/>SharedNoiseModel<sp/>noiseModel_;<sp/></highlight></codeline>
+<codeline lineno="173"><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="174"><highlight class="normal"></highlight></codeline>
+<codeline lineno="175" refid="classgtsam_1_1NoiseModelFactor_1a57d98d52fb6f91ebc54d74746aeb90e4" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>std::shared_ptr&lt;This&gt;<sp/><ref refid="classgtsam_1_1NoiseModelFactor_1a57d98d52fb6f91ebc54d74746aeb90e4" kindref="member">shared_ptr</ref>;</highlight></codeline>
+<codeline lineno="176"><highlight class="normal"></highlight></codeline>
+<codeline lineno="178" refid="classgtsam_1_1NoiseModelFactor_1ad6e8ef3348e042d598617d1d55f20300" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor_1ad6e8ef3348e042d598617d1d55f20300" kindref="member">NoiseModelFactor</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="179"><highlight class="normal"></highlight></codeline>
+<codeline lineno="181" refid="classgtsam_1_1NoiseModelFactor_1ae1ed5e0fa6c471b57172f7ebaf552187" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor_1ae1ed5e0fa6c471b57172f7ebaf552187" kindref="member">~NoiseModelFactor</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="182"><highlight class="normal"></highlight></codeline>
+<codeline lineno="186"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">typename</highlight><highlight class="normal"><sp/>CONTAINER&gt;</highlight></codeline>
+<codeline lineno="187" refid="classgtsam_1_1NoiseModelFactor_1a02cea97db30d27b51feaf1f0444d743c" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor_1a02cea97db30d27b51feaf1f0444d743c" kindref="member">NoiseModelFactor</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>SharedNoiseModel&amp;<sp/>noiseModel,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>CONTAINER&amp;<sp/>keys)<sp/>:</highlight></codeline>
+<codeline lineno="188"><highlight class="normal"><sp/><sp/><sp/><sp/>Base(keys),<sp/>noiseModel_(noiseModel)<sp/>{}</highlight></codeline>
+<codeline lineno="189"><highlight class="normal"></highlight></codeline>
+<codeline lineno="190"><highlight class="normal"></highlight><highlight class="keyword">protected</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="191"><highlight class="normal"></highlight></codeline>
+<codeline lineno="195" refid="classgtsam_1_1NoiseModelFactor_1a4605224417029e9dc58c5267efa9a46d" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor_1a4605224417029e9dc58c5267efa9a46d" kindref="member">NoiseModelFactor</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>SharedNoiseModel&amp;<sp/>noiseModel)<sp/>:<sp/>noiseModel_(noiseModel)<sp/>{}</highlight></codeline>
+<codeline lineno="196"><highlight class="normal"></highlight></codeline>
+<codeline lineno="197"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="198"><highlight class="normal"></highlight></codeline>
+<codeline lineno="200"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>print(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>std::string&amp;<sp/>s<sp/>=<sp/></highlight><highlight class="stringliteral">&quot;&quot;</highlight><highlight class="normal">,</highlight></codeline>
+<codeline lineno="201"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>KeyFormatter&amp;<sp/>keyFormatter<sp/>=<sp/>DefaultKeyFormatter)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal">;</highlight></codeline>
+<codeline lineno="202"><highlight class="normal"></highlight></codeline>
+<codeline lineno="204"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">bool</highlight><highlight class="normal"><sp/>equals(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>NonlinearFactor&amp;<sp/>f,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>tol<sp/>=<sp/>1e-9)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal">;</highlight></codeline>
+<codeline lineno="205"><highlight class="normal"></highlight></codeline>
+<codeline lineno="207" refid="classgtsam_1_1NoiseModelFactor_1af4f62b52fff1a0527ecdb279de023728" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">size_t</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor_1af4f62b52fff1a0527ecdb279de023728" kindref="member">dim</ref>()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{</highlight></codeline>
+<codeline lineno="208"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>noiseModel_-&gt;dim();</highlight></codeline>
+<codeline lineno="209"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="210"><highlight class="normal"></highlight></codeline>
+<codeline lineno="212" refid="classgtsam_1_1NoiseModelFactor_1a123e5729d70b4acd6d7e1f70b1a7ced4" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>SharedNoiseModel&amp;<sp/><ref refid="classgtsam_1_1NoiseModelFactor_1a123e5729d70b4acd6d7e1f70b1a7ced4" kindref="member">noiseModel</ref>()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{</highlight></codeline>
+<codeline lineno="213"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>noiseModel_;</highlight></codeline>
+<codeline lineno="214"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="215"><highlight class="normal"></highlight></codeline>
+<codeline lineno="217" refid="classgtsam_1_1NoiseModelFactor_1a16a8a23069ce6ae664f516edc8f36254" refkind="member"><highlight class="normal"><sp/><sp/>SharedNoiseModel<sp/><ref refid="classgtsam_1_1NoiseModelFactor_1a16a8a23069ce6ae664f516edc8f36254" kindref="member">get_noiseModel</ref>()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{</highlight></codeline>
+<codeline lineno="218"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>noiseModel_;</highlight></codeline>
+<codeline lineno="219"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="220"><highlight class="normal"></highlight></codeline>
+<codeline lineno="227"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>Vector<sp/>unwhitenedError(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Values&amp;<sp/>x,</highlight></codeline>
+<codeline lineno="228"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;std::vector&lt;Matrix&gt;&amp;&gt;<sp/>H<sp/>=<sp/>boost::none)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>=<sp/>0;</highlight></codeline>
+<codeline lineno="229"><highlight class="normal"></highlight></codeline>
+<codeline lineno="234"><highlight class="normal"><sp/><sp/>Vector<sp/>whitenedError(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Values&amp;<sp/>c)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal">;</highlight></codeline>
+<codeline lineno="235"><highlight class="normal"></highlight></codeline>
+<codeline lineno="242"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>error(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Values&amp;<sp/>c)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal">;</highlight></codeline>
+<codeline lineno="243"><highlight class="normal"></highlight></codeline>
+<codeline lineno="249"><highlight class="normal"><sp/><sp/>std::shared_ptr&lt;GaussianFactor&gt;<sp/>linearize(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Values&amp;<sp/>x)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal">;</highlight></codeline>
+<codeline lineno="250"><highlight class="normal"></highlight></codeline>
+<codeline lineno="251"><highlight class="normal"></highlight><highlight class="keyword">private</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="252"><highlight class="normal"></highlight></codeline>
+<codeline lineno="254" refid="classgtsam_1_1NoiseModelFactor_1ac98d07dd8f7b70e16ccb9a01abf56b9c" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">friend</highlight><highlight class="normal"><sp/></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal">boost::serialization::access;</highlight></codeline>
+<codeline lineno="255"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>ARCHIVE&gt;</highlight></codeline>
+<codeline lineno="256"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>serialize(ARCHIVE<sp/>&amp;<sp/>ar,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">unsigned</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/></highlight><highlight class="comment">/*version*/</highlight><highlight class="normal">)<sp/>{</highlight></codeline>
+<codeline lineno="257"><highlight class="normal"><sp/><sp/><sp/><sp/>ar<sp/>&amp;<sp/>boost::serialization::make_nvp(</highlight><highlight class="stringliteral">&quot;NonlinearFactor&quot;</highlight><highlight class="normal">,</highlight></codeline>
+<codeline lineno="258"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>boost::serialization::base_object&lt;Base&gt;(*</highlight><highlight class="keyword">this</highlight><highlight class="normal">));</highlight></codeline>
+<codeline lineno="259"><highlight class="normal"><sp/><sp/><sp/><sp/>ar<sp/>&amp;<sp/>BOOST_SERIALIZATION_NVP(noiseModel_);</highlight></codeline>
+<codeline lineno="260"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="261"><highlight class="normal"></highlight></codeline>
+<codeline lineno="262"><highlight class="normal">};<sp/></highlight><highlight class="comment">//<sp/>\class<sp/>NoiseModelFactor</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="263"><highlight class="normal"></highlight></codeline>
+<codeline lineno="264"><highlight class="normal"></highlight></codeline>
+<codeline lineno="265"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>*************************************************************************<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="266"><highlight class="normal"></highlight></codeline>
+<codeline lineno="275"><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE&gt;</highlight></codeline>
+<codeline lineno="276" refid="classgtsam_1_1NoiseModelFactor1" refkind="compound"><highlight class="normal"></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal"><ref refid="classgtsam_1_1NoiseModelFactor1" kindref="compound">NoiseModelFactor1</ref>:<sp/></highlight><highlight class="keyword">public</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref><sp/>{</highlight></codeline>
+<codeline lineno="277"><highlight class="normal"></highlight></codeline>
+<codeline lineno="278"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="279"><highlight class="normal"></highlight></codeline>
+<codeline lineno="280"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>typedefs<sp/>for<sp/>value<sp/>types<sp/>pulled<sp/>from<sp/>keys</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="281"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE<sp/>X;</highlight></codeline>
+<codeline lineno="282"><highlight class="normal"></highlight></codeline>
+<codeline lineno="283"><highlight class="normal"></highlight><highlight class="keyword">protected</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="284"><highlight class="normal"></highlight></codeline>
+<codeline lineno="285"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref><sp/>Base;</highlight></codeline>
+<codeline lineno="286"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor1" kindref="compound">NoiseModelFactor1&lt;VALUE&gt;</ref><sp/>This;</highlight></codeline>
+<codeline lineno="287"><highlight class="normal"></highlight></codeline>
+<codeline lineno="288"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="289"><highlight class="normal"></highlight></codeline>
+<codeline lineno="291" refid="classgtsam_1_1NoiseModelFactor1_1a85c649f81b216f5b5d7e321febc7762c" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor1_1a85c649f81b216f5b5d7e321febc7762c" kindref="member">NoiseModelFactor1</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="292"><highlight class="normal"></highlight></codeline>
+<codeline lineno="293"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>~<ref refid="classgtsam_1_1NoiseModelFactor1" kindref="compound">NoiseModelFactor1</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="294"><highlight class="normal"></highlight></codeline>
+<codeline lineno="295"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[0];<sp/>}</highlight></codeline>
+<codeline lineno="296"><highlight class="normal"></highlight></codeline>
+<codeline lineno="302" refid="classgtsam_1_1NoiseModelFactor1_1a70a0b32dd5feba3631815614d8386c86" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor1_1a70a0b32dd5feba3631815614d8386c86" kindref="member">NoiseModelFactor1</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>SharedNoiseModel&amp;<sp/>noiseModel,<sp/>Key<sp/>key1)<sp/>:</highlight></codeline>
+<codeline lineno="303"><highlight class="normal"><sp/><sp/><sp/><sp/>Base(noiseModel,<sp/>cref_list_of&lt;1&gt;(key1))<sp/>{}</highlight></codeline>
+<codeline lineno="304"><highlight class="normal"></highlight></codeline>
+<codeline lineno="308" refid="classgtsam_1_1NoiseModelFactor1_1a886bb13688a62b2680c10cdaf7999918" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>Vector<sp/><ref refid="classgtsam_1_1NoiseModelFactor1_1a886bb13688a62b2680c10cdaf7999918" kindref="member">unwhitenedError</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Values&amp;<sp/>x,<sp/>boost::optional&lt;std::vector&lt;Matrix&gt;&amp;&gt;<sp/>H<sp/>=<sp/>boost::none)</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{</highlight></codeline>
+<codeline lineno="309"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal">(this-&gt;active(x))<sp/>{</highlight></codeline>
+<codeline lineno="310"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X&amp;<sp/>x1<sp/>=<sp/>x.at&lt;X&gt;(keys_[0]);</highlight></codeline>
+<codeline lineno="311"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal">(H)<sp/>{</highlight></codeline>
+<codeline lineno="312"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>evaluateError(x1,<sp/>(*H)[0]);</highlight></codeline>
+<codeline lineno="313"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>}<sp/></highlight><highlight class="keywordflow">else</highlight><highlight class="normal"><sp/>{</highlight></codeline>
+<codeline lineno="314"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>evaluateError(x1);</highlight></codeline>
+<codeline lineno="315"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>}</highlight></codeline>
+<codeline lineno="316"><highlight class="normal"><sp/><sp/><sp/><sp/>}<sp/></highlight><highlight class="keywordflow">else</highlight><highlight class="normal"><sp/>{</highlight></codeline>
+<codeline lineno="317"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>Vector::Zero(this-&gt;dim());</highlight></codeline>
+<codeline lineno="318"><highlight class="normal"><sp/><sp/><sp/><sp/>}</highlight></codeline>
+<codeline lineno="319"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="320"><highlight class="normal"></highlight></codeline>
+<codeline lineno="326"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>Vector<sp/>evaluateError(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X&amp;<sp/>x,<sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H<sp/>=</highlight></codeline>
+<codeline lineno="327"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::none)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>=<sp/>0;</highlight></codeline>
+<codeline lineno="328"><highlight class="normal"></highlight></codeline>
+<codeline lineno="329"><highlight class="normal"></highlight><highlight class="keyword">private</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="330"><highlight class="normal"></highlight></codeline>
+<codeline lineno="332" refid="classgtsam_1_1NoiseModelFactor1_1ac98d07dd8f7b70e16ccb9a01abf56b9c" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">friend</highlight><highlight class="normal"><sp/></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal">boost::serialization::access;</highlight></codeline>
+<codeline lineno="333"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>ARCHIVE&gt;</highlight></codeline>
+<codeline lineno="334"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>serialize(ARCHIVE<sp/>&amp;<sp/>ar,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">unsigned</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/></highlight><highlight class="comment">/*version*/</highlight><highlight class="normal">)<sp/>{</highlight></codeline>
+<codeline lineno="335"><highlight class="normal"><sp/><sp/><sp/><sp/>ar<sp/>&amp;<sp/>boost::serialization::make_nvp(</highlight><highlight class="stringliteral">&quot;NoiseModelFactor&quot;</highlight><highlight class="normal">,</highlight></codeline>
+<codeline lineno="336"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>boost::serialization::base_object&lt;Base&gt;(*</highlight><highlight class="keyword">this</highlight><highlight class="normal">));</highlight></codeline>
+<codeline lineno="337"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="338"><highlight class="normal">};</highlight><highlight class="comment">//<sp/>\class<sp/>NoiseModelFactor1</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="339"><highlight class="normal"></highlight></codeline>
+<codeline lineno="340"><highlight class="normal"></highlight></codeline>
+<codeline lineno="341"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>*************************************************************************<sp/>*/</highlight></codeline>
+<codeline lineno="344"><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE1,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE2&gt;</highlight></codeline>
+<codeline lineno="345" refid="classgtsam_1_1NoiseModelFactor2" refkind="compound"><highlight class="normal"></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal"><ref refid="classgtsam_1_1NoiseModelFactor2" kindref="compound">NoiseModelFactor2</ref>:<sp/></highlight><highlight class="keyword">public</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref><sp/>{</highlight></codeline>
+<codeline lineno="346"><highlight class="normal"></highlight></codeline>
+<codeline lineno="347"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="348"><highlight class="normal"></highlight></codeline>
+<codeline lineno="349"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>typedefs<sp/>for<sp/>value<sp/>types<sp/>pulled<sp/>from<sp/>keys</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="350"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE1<sp/>X1;</highlight></codeline>
+<codeline lineno="351"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE2<sp/>X2;</highlight></codeline>
+<codeline lineno="352"><highlight class="normal"></highlight></codeline>
+<codeline lineno="353"><highlight class="normal"></highlight><highlight class="keyword">protected</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="354"><highlight class="normal"></highlight></codeline>
+<codeline lineno="355"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref><sp/>Base;</highlight></codeline>
+<codeline lineno="356"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor2" kindref="compound">NoiseModelFactor2&lt;VALUE1, VALUE2&gt;</ref><sp/>This;</highlight></codeline>
+<codeline lineno="357"><highlight class="normal"></highlight></codeline>
+<codeline lineno="358"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="359"><highlight class="normal"></highlight></codeline>
+<codeline lineno="363" refid="classgtsam_1_1NoiseModelFactor2_1a04f5873776c784cafd55b4ac05b4b757" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor2_1a04f5873776c784cafd55b4ac05b4b757" kindref="member">NoiseModelFactor2</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="364"><highlight class="normal"></highlight></codeline>
+<codeline lineno="371" refid="classgtsam_1_1NoiseModelFactor2_1a35453b054509f5679a29a06b59340cf1" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor2_1a35453b054509f5679a29a06b59340cf1" kindref="member">NoiseModelFactor2</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>SharedNoiseModel&amp;<sp/>noiseModel,<sp/>Key<sp/>j1,<sp/>Key<sp/>j2)<sp/>:</highlight></codeline>
+<codeline lineno="372"><highlight class="normal"><sp/><sp/><sp/><sp/>Base(noiseModel,<sp/>cref_list_of&lt;2&gt;(j1)(j2))<sp/>{}</highlight></codeline>
+<codeline lineno="373"><highlight class="normal"></highlight></codeline>
+<codeline lineno="374"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>~<ref refid="classgtsam_1_1NoiseModelFactor2" kindref="compound">NoiseModelFactor2</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="375"><highlight class="normal"></highlight></codeline>
+<codeline lineno="377" refid="classgtsam_1_1NoiseModelFactor2_1ae3b88b40b4ee6e9af9eefcde77bf9768" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/><ref refid="classgtsam_1_1NoiseModelFactor2_1ae3b88b40b4ee6e9af9eefcde77bf9768" kindref="member">key1</ref>()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[0];<sp/><sp/>}</highlight></codeline>
+<codeline lineno="378"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key2()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[1];<sp/><sp/>}</highlight></codeline>
+<codeline lineno="379"><highlight class="normal"></highlight></codeline>
+<codeline lineno="382" refid="classgtsam_1_1NoiseModelFactor2_1a9640bbbb433383f0b40b5c2d9439415e" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>Vector<sp/><ref refid="classgtsam_1_1NoiseModelFactor2_1a9640bbbb433383f0b40b5c2d9439415e" kindref="member">unwhitenedError</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Values&amp;<sp/>x,<sp/>boost::optional&lt;std::vector&lt;Matrix&gt;&amp;&gt;<sp/>H<sp/>=<sp/>boost::none)</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{</highlight></codeline>
+<codeline lineno="383"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal">(this-&gt;active(x))<sp/>{</highlight></codeline>
+<codeline lineno="384"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X1&amp;<sp/>x1<sp/>=<sp/>x.at&lt;X1&gt;(keys_[0]);</highlight></codeline>
+<codeline lineno="385"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X2&amp;<sp/>x2<sp/>=<sp/>x.at&lt;X2&gt;(keys_[1]);</highlight></codeline>
+<codeline lineno="386"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal">(H)<sp/>{</highlight></codeline>
+<codeline lineno="387"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>evaluateError(x1,<sp/>x2,<sp/>(*H)[0],<sp/>(*H)[1]);</highlight></codeline>
+<codeline lineno="388"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>}<sp/></highlight><highlight class="keywordflow">else</highlight><highlight class="normal"><sp/>{</highlight></codeline>
+<codeline lineno="389"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>evaluateError(x1,<sp/>x2);</highlight></codeline>
+<codeline lineno="390"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>}</highlight></codeline>
+<codeline lineno="391"><highlight class="normal"><sp/><sp/><sp/><sp/>}<sp/></highlight><highlight class="keywordflow">else</highlight><highlight class="normal"><sp/>{</highlight></codeline>
+<codeline lineno="392"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>Vector::Zero(this-&gt;dim());</highlight></codeline>
+<codeline lineno="393"><highlight class="normal"><sp/><sp/><sp/><sp/>}</highlight></codeline>
+<codeline lineno="394"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="395"><highlight class="normal"></highlight></codeline>
+<codeline lineno="401"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>Vector</highlight></codeline>
+<codeline lineno="402"><highlight class="normal"><sp/><sp/>evaluateError(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X1&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X2&amp;,<sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H1<sp/>=</highlight></codeline>
+<codeline lineno="403"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::none,<sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H2<sp/>=<sp/>boost::none)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>=<sp/>0;</highlight></codeline>
+<codeline lineno="404"><highlight class="normal"></highlight></codeline>
+<codeline lineno="405"><highlight class="normal"></highlight><highlight class="keyword">private</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="406"><highlight class="normal"></highlight></codeline>
+<codeline lineno="408" refid="classgtsam_1_1NoiseModelFactor2_1ac98d07dd8f7b70e16ccb9a01abf56b9c" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">friend</highlight><highlight class="normal"><sp/></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal">boost::serialization::access;</highlight></codeline>
+<codeline lineno="409"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>ARCHIVE&gt;</highlight></codeline>
+<codeline lineno="410"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>serialize(ARCHIVE<sp/>&amp;<sp/>ar,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">unsigned</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/></highlight><highlight class="comment">/*version*/</highlight><highlight class="normal">)<sp/>{</highlight></codeline>
+<codeline lineno="411"><highlight class="normal"><sp/><sp/><sp/><sp/>ar<sp/>&amp;<sp/>boost::serialization::make_nvp(</highlight><highlight class="stringliteral">&quot;NoiseModelFactor&quot;</highlight><highlight class="normal">,</highlight></codeline>
+<codeline lineno="412"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>boost::serialization::base_object&lt;Base&gt;(*</highlight><highlight class="keyword">this</highlight><highlight class="normal">));</highlight></codeline>
+<codeline lineno="413"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="414"><highlight class="normal">};<sp/></highlight><highlight class="comment">//<sp/>\class<sp/>NoiseModelFactor2</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="415"><highlight class="normal"></highlight></codeline>
+<codeline lineno="416"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>*************************************************************************<sp/>*/</highlight></codeline>
+<codeline lineno="419"><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE1,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE2,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE3&gt;</highlight></codeline>
+<codeline lineno="420" refid="classgtsam_1_1NoiseModelFactor3" refkind="compound"><highlight class="normal"></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal"><ref refid="classgtsam_1_1NoiseModelFactor3" kindref="compound">NoiseModelFactor3</ref>:<sp/></highlight><highlight class="keyword">public</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref><sp/>{</highlight></codeline>
+<codeline lineno="421"><highlight class="normal"></highlight></codeline>
+<codeline lineno="422"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="423"><highlight class="normal"></highlight></codeline>
+<codeline lineno="424"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>typedefs<sp/>for<sp/>value<sp/>types<sp/>pulled<sp/>from<sp/>keys</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="425"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE1<sp/>X1;</highlight></codeline>
+<codeline lineno="426"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE2<sp/>X2;</highlight></codeline>
+<codeline lineno="427"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE3<sp/>X3;</highlight></codeline>
+<codeline lineno="428"><highlight class="normal"></highlight></codeline>
+<codeline lineno="429"><highlight class="normal"></highlight><highlight class="keyword">protected</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="430"><highlight class="normal"></highlight></codeline>
+<codeline lineno="431"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref><sp/>Base;</highlight></codeline>
+<codeline lineno="432"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor3" kindref="compound">NoiseModelFactor3&lt;VALUE1, VALUE2, VALUE3&gt;</ref><sp/>This;</highlight></codeline>
+<codeline lineno="433"><highlight class="normal"></highlight></codeline>
+<codeline lineno="434"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="435"><highlight class="normal"></highlight></codeline>
+<codeline lineno="439" refid="classgtsam_1_1NoiseModelFactor3_1a0bf4383cea2cdb0d263e570e98499ac7" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor3_1a0bf4383cea2cdb0d263e570e98499ac7" kindref="member">NoiseModelFactor3</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="440"><highlight class="normal"></highlight></codeline>
+<codeline lineno="448" refid="classgtsam_1_1NoiseModelFactor3_1ac2a8e8247e7d55768915fac27a4c078c" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor3_1ac2a8e8247e7d55768915fac27a4c078c" kindref="member">NoiseModelFactor3</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>SharedNoiseModel&amp;<sp/>noiseModel,<sp/>Key<sp/>j1,<sp/>Key<sp/>j2,<sp/>Key<sp/>j3)<sp/>:</highlight></codeline>
+<codeline lineno="449"><highlight class="normal"><sp/><sp/><sp/><sp/>Base(noiseModel,<sp/>cref_list_of&lt;3&gt;(j1)(j2)(j3))<sp/>{}</highlight></codeline>
+<codeline lineno="450"><highlight class="normal"></highlight></codeline>
+<codeline lineno="451"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>~<ref refid="classgtsam_1_1NoiseModelFactor3" kindref="compound">NoiseModelFactor3</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="452"><highlight class="normal"></highlight></codeline>
+<codeline lineno="454" refid="classgtsam_1_1NoiseModelFactor3_1a535f2bec61538509696513b2ab6bbf0b" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/><ref refid="classgtsam_1_1NoiseModelFactor3_1a535f2bec61538509696513b2ab6bbf0b" kindref="member">key1</ref>()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[0];<sp/>}</highlight></codeline>
+<codeline lineno="455"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key2()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[1];<sp/>}</highlight></codeline>
+<codeline lineno="456"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key3()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[2];<sp/>}</highlight></codeline>
+<codeline lineno="457"><highlight class="normal"></highlight></codeline>
+<codeline lineno="460" refid="classgtsam_1_1NoiseModelFactor3_1aab8e3316ef77cecc1bd1909a2c0eed33" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>Vector<sp/><ref refid="classgtsam_1_1NoiseModelFactor3_1aab8e3316ef77cecc1bd1909a2c0eed33" kindref="member">unwhitenedError</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Values&amp;<sp/>x,<sp/>boost::optional&lt;std::vector&lt;Matrix&gt;&amp;&gt;<sp/>H<sp/>=<sp/>boost::none)</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{</highlight></codeline>
+<codeline lineno="461"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal">(this-&gt;active(x))<sp/>{</highlight></codeline>
+<codeline lineno="462"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal">(H)</highlight></codeline>
+<codeline lineno="463"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>evaluateError(x.at&lt;X1&gt;(keys_[0]),<sp/>x.at&lt;X2&gt;(keys_[1]),<sp/>x.at&lt;X3&gt;(keys_[2]),<sp/>(*H)[0],<sp/>(*H)[1],<sp/>(*H)[2]);</highlight></codeline>
+<codeline lineno="464"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">else</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="465"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>evaluateError(x.at&lt;X1&gt;(keys_[0]),<sp/>x.at&lt;X2&gt;(keys_[1]),<sp/>x.at&lt;X3&gt;(keys_[2]));</highlight></codeline>
+<codeline lineno="466"><highlight class="normal"><sp/><sp/><sp/><sp/>}<sp/></highlight><highlight class="keywordflow">else</highlight><highlight class="normal"><sp/>{</highlight></codeline>
+<codeline lineno="467"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>Vector::Zero(this-&gt;dim());</highlight></codeline>
+<codeline lineno="468"><highlight class="normal"><sp/><sp/><sp/><sp/>}</highlight></codeline>
+<codeline lineno="469"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="470"><highlight class="normal"></highlight></codeline>
+<codeline lineno="476"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>Vector</highlight></codeline>
+<codeline lineno="477"><highlight class="normal"><sp/><sp/>evaluateError(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X1&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X2&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X3&amp;,</highlight></codeline>
+<codeline lineno="478"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H1<sp/>=<sp/>boost::none,</highlight></codeline>
+<codeline lineno="479"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H2<sp/>=<sp/>boost::none,</highlight></codeline>
+<codeline lineno="480"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H3<sp/>=<sp/>boost::none)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>=<sp/>0;</highlight></codeline>
+<codeline lineno="481"><highlight class="normal"></highlight></codeline>
+<codeline lineno="482"><highlight class="normal"></highlight><highlight class="keyword">private</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="483"><highlight class="normal"></highlight></codeline>
+<codeline lineno="485" refid="classgtsam_1_1NoiseModelFactor3_1ac98d07dd8f7b70e16ccb9a01abf56b9c" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">friend</highlight><highlight class="normal"><sp/></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal">boost::serialization::access;</highlight></codeline>
+<codeline lineno="486"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>ARCHIVE&gt;</highlight></codeline>
+<codeline lineno="487"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>serialize(ARCHIVE<sp/>&amp;<sp/>ar,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">unsigned</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/></highlight><highlight class="comment">/*version*/</highlight><highlight class="normal">)<sp/>{</highlight></codeline>
+<codeline lineno="488"><highlight class="normal"><sp/><sp/><sp/><sp/>ar<sp/>&amp;<sp/>boost::serialization::make_nvp(</highlight><highlight class="stringliteral">&quot;NoiseModelFactor&quot;</highlight><highlight class="normal">,</highlight></codeline>
+<codeline lineno="489"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>boost::serialization::base_object&lt;Base&gt;(*</highlight><highlight class="keyword">this</highlight><highlight class="normal">));</highlight></codeline>
+<codeline lineno="490"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="491"><highlight class="normal">};<sp/></highlight><highlight class="comment">//<sp/>\class<sp/>NoiseModelFactor3</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="492"><highlight class="normal"></highlight></codeline>
+<codeline lineno="493"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>*************************************************************************<sp/>*/</highlight></codeline>
+<codeline lineno="496"><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE1,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE2,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE3,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE4&gt;</highlight></codeline>
+<codeline lineno="497" refid="classgtsam_1_1NoiseModelFactor4" refkind="compound"><highlight class="normal"></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal"><ref refid="classgtsam_1_1NoiseModelFactor4" kindref="compound">NoiseModelFactor4</ref>:<sp/></highlight><highlight class="keyword">public</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref><sp/>{</highlight></codeline>
+<codeline lineno="498"><highlight class="normal"></highlight></codeline>
+<codeline lineno="499"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="500"><highlight class="normal"></highlight></codeline>
+<codeline lineno="501"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>typedefs<sp/>for<sp/>value<sp/>types<sp/>pulled<sp/>from<sp/>keys</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="502"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE1<sp/>X1;</highlight></codeline>
+<codeline lineno="503"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE2<sp/>X2;</highlight></codeline>
+<codeline lineno="504"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE3<sp/>X3;</highlight></codeline>
+<codeline lineno="505"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE4<sp/>X4;</highlight></codeline>
+<codeline lineno="506"><highlight class="normal"></highlight></codeline>
+<codeline lineno="507"><highlight class="normal"></highlight><highlight class="keyword">protected</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="508"><highlight class="normal"></highlight></codeline>
+<codeline lineno="509"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref><sp/>Base;</highlight></codeline>
+<codeline lineno="510"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor4" kindref="compound">NoiseModelFactor4&lt;VALUE1, VALUE2, VALUE3, VALUE4&gt;</ref><sp/>This;</highlight></codeline>
+<codeline lineno="511"><highlight class="normal"></highlight></codeline>
+<codeline lineno="512"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="513"><highlight class="normal"></highlight></codeline>
+<codeline lineno="517" refid="classgtsam_1_1NoiseModelFactor4_1a65adc8d1272df63390168b26f4f7bc70" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor4_1a65adc8d1272df63390168b26f4f7bc70" kindref="member">NoiseModelFactor4</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="518"><highlight class="normal"></highlight></codeline>
+<codeline lineno="527" refid="classgtsam_1_1NoiseModelFactor4_1a8ce3daa05897e12db9e38a9c96043ce3" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor4_1a8ce3daa05897e12db9e38a9c96043ce3" kindref="member">NoiseModelFactor4</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>SharedNoiseModel&amp;<sp/>noiseModel,<sp/>Key<sp/>j1,<sp/>Key<sp/>j2,<sp/>Key<sp/>j3,<sp/>Key<sp/>j4)<sp/>:</highlight></codeline>
+<codeline lineno="528"><highlight class="normal"><sp/><sp/><sp/><sp/>Base(noiseModel,<sp/>cref_list_of&lt;4&gt;(j1)(j2)(j3)(j4))<sp/>{}</highlight></codeline>
+<codeline lineno="529"><highlight class="normal"></highlight></codeline>
+<codeline lineno="530"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>~<ref refid="classgtsam_1_1NoiseModelFactor4" kindref="compound">NoiseModelFactor4</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="531"><highlight class="normal"></highlight></codeline>
+<codeline lineno="533" refid="classgtsam_1_1NoiseModelFactor4_1a4339880be87fa2a533aa07324481f7b6" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/><ref refid="classgtsam_1_1NoiseModelFactor4_1a4339880be87fa2a533aa07324481f7b6" kindref="member">key1</ref>()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[0];<sp/>}</highlight></codeline>
+<codeline lineno="534"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key2()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[1];<sp/>}</highlight></codeline>
+<codeline lineno="535"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key3()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[2];<sp/>}</highlight></codeline>
+<codeline lineno="536"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key4()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[3];<sp/>}</highlight></codeline>
+<codeline lineno="537"><highlight class="normal"></highlight></codeline>
+<codeline lineno="540" refid="classgtsam_1_1NoiseModelFactor4_1a6a79ae22fc40447c8dc183a951243440" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>Vector<sp/><ref refid="classgtsam_1_1NoiseModelFactor4_1a6a79ae22fc40447c8dc183a951243440" kindref="member">unwhitenedError</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Values&amp;<sp/>x,<sp/>boost::optional&lt;std::vector&lt;Matrix&gt;&amp;&gt;<sp/>H<sp/>=<sp/>boost::none)</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{</highlight></codeline>
+<codeline lineno="541"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal">(this-&gt;active(x))<sp/>{</highlight></codeline>
+<codeline lineno="542"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal">(H)</highlight></codeline>
+<codeline lineno="543"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>evaluateError(x.at&lt;X1&gt;(keys_[0]),<sp/>x.at&lt;X2&gt;(keys_[1]),<sp/>x.at&lt;X3&gt;(keys_[2]),<sp/>x.at&lt;X4&gt;(keys_[3]),<sp/>(*H)[0],<sp/>(*H)[1],<sp/>(*H)[2],<sp/>(*H)[3]);</highlight></codeline>
+<codeline lineno="544"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">else</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="545"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>evaluateError(x.at&lt;X1&gt;(keys_[0]),<sp/>x.at&lt;X2&gt;(keys_[1]),<sp/>x.at&lt;X3&gt;(keys_[2]),<sp/>x.at&lt;X4&gt;(keys_[3]));</highlight></codeline>
+<codeline lineno="546"><highlight class="normal"><sp/><sp/><sp/><sp/>}<sp/></highlight><highlight class="keywordflow">else</highlight><highlight class="normal"><sp/>{</highlight></codeline>
+<codeline lineno="547"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>Vector::Zero(this-&gt;dim());</highlight></codeline>
+<codeline lineno="548"><highlight class="normal"><sp/><sp/><sp/><sp/>}</highlight></codeline>
+<codeline lineno="549"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="550"><highlight class="normal"></highlight></codeline>
+<codeline lineno="556"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>Vector</highlight></codeline>
+<codeline lineno="557"><highlight class="normal"><sp/><sp/>evaluateError(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X1&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X2&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X3&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X4&amp;,</highlight></codeline>
+<codeline lineno="558"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H1<sp/>=<sp/>boost::none,</highlight></codeline>
+<codeline lineno="559"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H2<sp/>=<sp/>boost::none,</highlight></codeline>
+<codeline lineno="560"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H3<sp/>=<sp/>boost::none,</highlight></codeline>
+<codeline lineno="561"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H4<sp/>=<sp/>boost::none)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>=<sp/>0;</highlight></codeline>
+<codeline lineno="562"><highlight class="normal"></highlight></codeline>
+<codeline lineno="563"><highlight class="normal"></highlight><highlight class="keyword">private</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="564"><highlight class="normal"></highlight></codeline>
+<codeline lineno="566" refid="classgtsam_1_1NoiseModelFactor4_1ac98d07dd8f7b70e16ccb9a01abf56b9c" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">friend</highlight><highlight class="normal"><sp/></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal">boost::serialization::access;</highlight></codeline>
+<codeline lineno="567"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>ARCHIVE&gt;</highlight></codeline>
+<codeline lineno="568"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>serialize(ARCHIVE<sp/>&amp;<sp/>ar,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">unsigned</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/></highlight><highlight class="comment">/*version*/</highlight><highlight class="normal">)<sp/>{</highlight></codeline>
+<codeline lineno="569"><highlight class="normal"><sp/><sp/><sp/><sp/>ar<sp/>&amp;<sp/>boost::serialization::make_nvp(</highlight><highlight class="stringliteral">&quot;NoiseModelFactor&quot;</highlight><highlight class="normal">,</highlight></codeline>
+<codeline lineno="570"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>boost::serialization::base_object&lt;Base&gt;(*</highlight><highlight class="keyword">this</highlight><highlight class="normal">));</highlight></codeline>
+<codeline lineno="571"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="572"><highlight class="normal">};<sp/></highlight><highlight class="comment">//<sp/>\class<sp/>NoiseModelFactor4</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="573"><highlight class="normal"></highlight></codeline>
+<codeline lineno="574"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>*************************************************************************<sp/>*/</highlight></codeline>
+<codeline lineno="577"><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE1,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE2,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE3,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE4,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE5&gt;</highlight></codeline>
+<codeline lineno="578" refid="classgtsam_1_1NoiseModelFactor5" refkind="compound"><highlight class="normal"></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal"><ref refid="classgtsam_1_1NoiseModelFactor5" kindref="compound">NoiseModelFactor5</ref>:<sp/></highlight><highlight class="keyword">public</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref><sp/>{</highlight></codeline>
+<codeline lineno="579"><highlight class="normal"></highlight></codeline>
+<codeline lineno="580"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="581"><highlight class="normal"></highlight></codeline>
+<codeline lineno="582"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>typedefs<sp/>for<sp/>value<sp/>types<sp/>pulled<sp/>from<sp/>keys</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="583"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE1<sp/>X1;</highlight></codeline>
+<codeline lineno="584"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE2<sp/>X2;</highlight></codeline>
+<codeline lineno="585"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE3<sp/>X3;</highlight></codeline>
+<codeline lineno="586"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE4<sp/>X4;</highlight></codeline>
+<codeline lineno="587"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE5<sp/>X5;</highlight></codeline>
+<codeline lineno="588"><highlight class="normal"></highlight></codeline>
+<codeline lineno="589"><highlight class="normal"></highlight><highlight class="keyword">protected</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="590"><highlight class="normal"></highlight></codeline>
+<codeline lineno="591"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref><sp/>Base;</highlight></codeline>
+<codeline lineno="592"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor5" kindref="compound">NoiseModelFactor5&lt;VALUE1, VALUE2, VALUE3, VALUE4, VALUE5&gt;</ref><sp/>This;</highlight></codeline>
+<codeline lineno="593"><highlight class="normal"></highlight></codeline>
+<codeline lineno="594"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="595"><highlight class="normal"></highlight></codeline>
+<codeline lineno="599" refid="classgtsam_1_1NoiseModelFactor5_1a424c4633f6c1907d0dd3381cffbed82a" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor5_1a424c4633f6c1907d0dd3381cffbed82a" kindref="member">NoiseModelFactor5</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="600"><highlight class="normal"></highlight></codeline>
+<codeline lineno="610" refid="classgtsam_1_1NoiseModelFactor5_1a01d2e8a5253a67ceac02320a719c5280" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor5_1a01d2e8a5253a67ceac02320a719c5280" kindref="member">NoiseModelFactor5</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>SharedNoiseModel&amp;<sp/>noiseModel,<sp/>Key<sp/>j1,<sp/>Key<sp/>j2,<sp/>Key<sp/>j3,<sp/>Key<sp/>j4,<sp/>Key<sp/>j5)<sp/>:</highlight></codeline>
+<codeline lineno="611"><highlight class="normal"><sp/><sp/><sp/><sp/>Base(noiseModel,<sp/>cref_list_of&lt;5&gt;(j1)(j2)(j3)(j4)(j5))<sp/>{}</highlight></codeline>
+<codeline lineno="612"><highlight class="normal"></highlight></codeline>
+<codeline lineno="613"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>~<ref refid="classgtsam_1_1NoiseModelFactor5" kindref="compound">NoiseModelFactor5</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="614"><highlight class="normal"></highlight></codeline>
+<codeline lineno="616" refid="classgtsam_1_1NoiseModelFactor5_1a23886196ba086677b1e2168e17a34280" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/><ref refid="classgtsam_1_1NoiseModelFactor5_1a23886196ba086677b1e2168e17a34280" kindref="member">key1</ref>()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[0];<sp/>}</highlight></codeline>
+<codeline lineno="617"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key2()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[1];<sp/>}</highlight></codeline>
+<codeline lineno="618"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key3()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[2];<sp/>}</highlight></codeline>
+<codeline lineno="619"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key4()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[3];<sp/>}</highlight></codeline>
+<codeline lineno="620"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key5()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[4];<sp/>}</highlight></codeline>
+<codeline lineno="621"><highlight class="normal"></highlight></codeline>
+<codeline lineno="624" refid="classgtsam_1_1NoiseModelFactor5_1a701b7a764f18310b4eea14ee023c922b" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>Vector<sp/><ref refid="classgtsam_1_1NoiseModelFactor5_1a701b7a764f18310b4eea14ee023c922b" kindref="member">unwhitenedError</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Values&amp;<sp/>x,<sp/>boost::optional&lt;std::vector&lt;Matrix&gt;&amp;&gt;<sp/>H<sp/>=<sp/>boost::none)</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{</highlight></codeline>
+<codeline lineno="625"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal">(this-&gt;active(x))<sp/>{</highlight></codeline>
+<codeline lineno="626"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal">(H)</highlight></codeline>
+<codeline lineno="627"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>evaluateError(x.at&lt;X1&gt;(keys_[0]),<sp/>x.at&lt;X2&gt;(keys_[1]),<sp/>x.at&lt;X3&gt;(keys_[2]),<sp/>x.at&lt;X4&gt;(keys_[3]),<sp/>x.at&lt;X5&gt;(keys_[4]),<sp/>(*H)[0],<sp/>(*H)[1],<sp/>(*H)[2],<sp/>(*H)[3],<sp/>(*H)[4]);</highlight></codeline>
+<codeline lineno="628"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">else</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="629"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>evaluateError(x.at&lt;X1&gt;(keys_[0]),<sp/>x.at&lt;X2&gt;(keys_[1]),<sp/>x.at&lt;X3&gt;(keys_[2]),<sp/>x.at&lt;X4&gt;(keys_[3]),<sp/>x.at&lt;X5&gt;(keys_[4]));</highlight></codeline>
+<codeline lineno="630"><highlight class="normal"><sp/><sp/><sp/><sp/>}<sp/></highlight><highlight class="keywordflow">else</highlight><highlight class="normal"><sp/>{</highlight></codeline>
+<codeline lineno="631"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>Vector::Zero(this-&gt;dim());</highlight></codeline>
+<codeline lineno="632"><highlight class="normal"><sp/><sp/><sp/><sp/>}</highlight></codeline>
+<codeline lineno="633"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="634"><highlight class="normal"></highlight></codeline>
+<codeline lineno="640"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>Vector</highlight></codeline>
+<codeline lineno="641"><highlight class="normal"><sp/><sp/>evaluateError(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X1&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X2&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X3&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X4&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X5&amp;,</highlight></codeline>
+<codeline lineno="642"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H1<sp/>=<sp/>boost::none,</highlight></codeline>
+<codeline lineno="643"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H2<sp/>=<sp/>boost::none,</highlight></codeline>
+<codeline lineno="644"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H3<sp/>=<sp/>boost::none,</highlight></codeline>
+<codeline lineno="645"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H4<sp/>=<sp/>boost::none,</highlight></codeline>
+<codeline lineno="646"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H5<sp/>=<sp/>boost::none)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>=<sp/>0;</highlight></codeline>
+<codeline lineno="647"><highlight class="normal"></highlight></codeline>
+<codeline lineno="648"><highlight class="normal"></highlight><highlight class="keyword">private</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="649"><highlight class="normal"></highlight></codeline>
+<codeline lineno="651" refid="classgtsam_1_1NoiseModelFactor5_1ac98d07dd8f7b70e16ccb9a01abf56b9c" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">friend</highlight><highlight class="normal"><sp/></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal">boost::serialization::access;</highlight></codeline>
+<codeline lineno="652"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>ARCHIVE&gt;</highlight></codeline>
+<codeline lineno="653"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>serialize(ARCHIVE<sp/>&amp;<sp/>ar,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">unsigned</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/></highlight><highlight class="comment">/*version*/</highlight><highlight class="normal">)<sp/>{</highlight></codeline>
+<codeline lineno="654"><highlight class="normal"><sp/><sp/><sp/><sp/>ar<sp/>&amp;<sp/>boost::serialization::make_nvp(</highlight><highlight class="stringliteral">&quot;NoiseModelFactor&quot;</highlight><highlight class="normal">,</highlight></codeline>
+<codeline lineno="655"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>boost::serialization::base_object&lt;Base&gt;(*</highlight><highlight class="keyword">this</highlight><highlight class="normal">));</highlight></codeline>
+<codeline lineno="656"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="657"><highlight class="normal">};<sp/></highlight><highlight class="comment">//<sp/>\class<sp/>NoiseModelFactor5</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="658"><highlight class="normal"></highlight></codeline>
+<codeline lineno="659"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>*************************************************************************<sp/>*/</highlight></codeline>
+<codeline lineno="662"><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE1,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE2,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE3,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE4,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE5,<sp/></highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>VALUE6&gt;</highlight></codeline>
+<codeline lineno="663" refid="classgtsam_1_1NoiseModelFactor6" refkind="compound"><highlight class="normal"></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal"><ref refid="classgtsam_1_1NoiseModelFactor6" kindref="compound">NoiseModelFactor6</ref>:<sp/></highlight><highlight class="keyword">public</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref><sp/>{</highlight></codeline>
+<codeline lineno="664"><highlight class="normal"></highlight></codeline>
+<codeline lineno="665"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="666"><highlight class="normal"></highlight></codeline>
+<codeline lineno="667"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>typedefs<sp/>for<sp/>value<sp/>types<sp/>pulled<sp/>from<sp/>keys</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="668"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE1<sp/>X1;</highlight></codeline>
+<codeline lineno="669"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE2<sp/>X2;</highlight></codeline>
+<codeline lineno="670"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE3<sp/>X3;</highlight></codeline>
+<codeline lineno="671"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE4<sp/>X4;</highlight></codeline>
+<codeline lineno="672"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE5<sp/>X5;</highlight></codeline>
+<codeline lineno="673"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>VALUE6<sp/>X6;</highlight></codeline>
+<codeline lineno="674"><highlight class="normal"></highlight></codeline>
+<codeline lineno="675"><highlight class="normal"></highlight><highlight class="keyword">protected</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="676"><highlight class="normal"></highlight></codeline>
+<codeline lineno="677"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref><sp/>Base;</highlight></codeline>
+<codeline lineno="678"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/><ref refid="classgtsam_1_1NoiseModelFactor6" kindref="compound">NoiseModelFactor6&lt;VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6&gt;</ref><sp/>This;</highlight></codeline>
+<codeline lineno="679"><highlight class="normal"></highlight></codeline>
+<codeline lineno="680"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="681"><highlight class="normal"></highlight></codeline>
+<codeline lineno="685" refid="classgtsam_1_1NoiseModelFactor6_1a15bcca054e397e486fb9dda0d0436cdb" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor6_1a15bcca054e397e486fb9dda0d0436cdb" kindref="member">NoiseModelFactor6</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="686"><highlight class="normal"></highlight></codeline>
+<codeline lineno="697" refid="classgtsam_1_1NoiseModelFactor6_1ad3d89a85f0c70d7a424f2060d514cab9" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1NoiseModelFactor6_1ad3d89a85f0c70d7a424f2060d514cab9" kindref="member">NoiseModelFactor6</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>SharedNoiseModel&amp;<sp/>noiseModel,<sp/>Key<sp/>j1,<sp/>Key<sp/>j2,<sp/>Key<sp/>j3,<sp/>Key<sp/>j4,<sp/>Key<sp/>j5,<sp/>Key<sp/>j6)<sp/>:</highlight></codeline>
+<codeline lineno="698"><highlight class="normal"><sp/><sp/><sp/><sp/>Base(noiseModel,<sp/>cref_list_of&lt;6&gt;(j1)(j2)(j3)(j4)(j5)(j6))<sp/>{}</highlight></codeline>
+<codeline lineno="699"><highlight class="normal"></highlight></codeline>
+<codeline lineno="700"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>~<ref refid="classgtsam_1_1NoiseModelFactor6" kindref="compound">NoiseModelFactor6</ref>()<sp/>{}</highlight></codeline>
+<codeline lineno="701"><highlight class="normal"></highlight></codeline>
+<codeline lineno="703" refid="classgtsam_1_1NoiseModelFactor6_1a587ce14373742350ff30a78c856ecaa0" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/><ref refid="classgtsam_1_1NoiseModelFactor6_1a587ce14373742350ff30a78c856ecaa0" kindref="member">key1</ref>()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[0];<sp/>}</highlight></codeline>
+<codeline lineno="704"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key2()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[1];<sp/>}</highlight></codeline>
+<codeline lineno="705"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key3()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[2];<sp/>}</highlight></codeline>
+<codeline lineno="706"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key4()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[3];<sp/>}</highlight></codeline>
+<codeline lineno="707"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key5()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[4];<sp/>}</highlight></codeline>
+<codeline lineno="708"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">inline</highlight><highlight class="normal"><sp/>Key<sp/>key6()</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{<sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>keys_[5];<sp/>}</highlight></codeline>
+<codeline lineno="709"><highlight class="normal"></highlight></codeline>
+<codeline lineno="712" refid="classgtsam_1_1NoiseModelFactor6_1a945d269f9a722d585e3332be23030450" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>Vector<sp/><ref refid="classgtsam_1_1NoiseModelFactor6_1a945d269f9a722d585e3332be23030450" kindref="member">unwhitenedError</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Values&amp;<sp/>x,<sp/>boost::optional&lt;std::vector&lt;Matrix&gt;&amp;&gt;<sp/>H<sp/>=<sp/>boost::none)</highlight><highlight class="keyword"><sp/>const<sp/></highlight><highlight class="normal">{</highlight></codeline>
+<codeline lineno="713"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal">(this-&gt;active(x))<sp/>{</highlight></codeline>
+<codeline lineno="714"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">if</highlight><highlight class="normal">(H)</highlight></codeline>
+<codeline lineno="715"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>evaluateError(x.at&lt;X1&gt;(keys_[0]),<sp/>x.at&lt;X2&gt;(keys_[1]),<sp/>x.at&lt;X3&gt;(keys_[2]),<sp/>x.at&lt;X4&gt;(keys_[3]),<sp/>x.at&lt;X5&gt;(keys_[4]),<sp/>x.at&lt;X6&gt;(keys_[5]),<sp/>(*H)[0],<sp/>(*H)[1],<sp/>(*H)[2],<sp/>(*H)[3],<sp/>(*H)[4],<sp/>(*H)[5]);</highlight></codeline>
+<codeline lineno="716"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">else</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="717"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>evaluateError(x.at&lt;X1&gt;(keys_[0]),<sp/>x.at&lt;X2&gt;(keys_[1]),<sp/>x.at&lt;X3&gt;(keys_[2]),<sp/>x.at&lt;X4&gt;(keys_[3]),<sp/>x.at&lt;X5&gt;(keys_[4]),<sp/>x.at&lt;X6&gt;(keys_[5]));</highlight></codeline>
+<codeline lineno="718"><highlight class="normal"><sp/><sp/><sp/><sp/>}<sp/></highlight><highlight class="keywordflow">else</highlight><highlight class="normal"><sp/>{</highlight></codeline>
+<codeline lineno="719"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">return</highlight><highlight class="normal"><sp/>Vector::Zero(this-&gt;dim());</highlight></codeline>
+<codeline lineno="720"><highlight class="normal"><sp/><sp/><sp/><sp/>}</highlight></codeline>
+<codeline lineno="721"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="722"><highlight class="normal"></highlight></codeline>
+<codeline lineno="728"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">virtual</highlight><highlight class="normal"><sp/>Vector</highlight></codeline>
+<codeline lineno="729"><highlight class="normal"><sp/><sp/>evaluateError(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X1&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X2&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X3&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X4&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X5&amp;,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>X6&amp;,</highlight></codeline>
+<codeline lineno="730"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H1<sp/>=<sp/>boost::none,</highlight></codeline>
+<codeline lineno="731"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H2<sp/>=<sp/>boost::none,</highlight></codeline>
+<codeline lineno="732"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H3<sp/>=<sp/>boost::none,</highlight></codeline>
+<codeline lineno="733"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H4<sp/>=<sp/>boost::none,</highlight></codeline>
+<codeline lineno="734"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H5<sp/>=<sp/>boost::none,</highlight></codeline>
+<codeline lineno="735"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>boost::optional&lt;Matrix&amp;&gt;<sp/>H6<sp/>=<sp/>boost::none)<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>=<sp/>0;</highlight></codeline>
+<codeline lineno="736"><highlight class="normal"></highlight></codeline>
+<codeline lineno="737"><highlight class="normal"></highlight><highlight class="keyword">private</highlight><highlight class="normal">:</highlight></codeline>
+<codeline lineno="738"><highlight class="normal"></highlight></codeline>
+<codeline lineno="740" refid="classgtsam_1_1NoiseModelFactor6_1ac98d07dd8f7b70e16ccb9a01abf56b9c" refkind="member"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">friend</highlight><highlight class="normal"><sp/></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal">boost::serialization::access;</highlight></codeline>
+<codeline lineno="741"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">template</highlight><highlight class="normal">&lt;</highlight><highlight class="keyword">class</highlight><highlight class="normal"><sp/>ARCHIVE&gt;</highlight></codeline>
+<codeline lineno="742"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>serialize(ARCHIVE<sp/>&amp;<sp/>ar,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">unsigned</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/></highlight><highlight class="comment">/*version*/</highlight><highlight class="normal">)<sp/>{</highlight></codeline>
+<codeline lineno="743"><highlight class="normal"><sp/><sp/><sp/><sp/>ar<sp/>&amp;<sp/>boost::serialization::make_nvp(</highlight><highlight class="stringliteral">&quot;NoiseModelFactor&quot;</highlight><highlight class="normal">,</highlight></codeline>
+<codeline lineno="744"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>boost::serialization::base_object&lt;Base&gt;(*</highlight><highlight class="keyword">this</highlight><highlight class="normal">));</highlight></codeline>
+<codeline lineno="745"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
+<codeline lineno="746"><highlight class="normal">};<sp/></highlight><highlight class="comment">//<sp/>\class<sp/>NoiseModelFactor6</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="747"><highlight class="normal"></highlight></codeline>
+<codeline lineno="748"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>*************************************************************************<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
+<codeline lineno="749"><highlight class="normal"></highlight></codeline>
+<codeline lineno="750"><highlight class="normal">}<sp/></highlight><highlight class="comment">//<sp/>\namespace<sp/>gtsam</highlight><highlight class="normal"></highlight></codeline>
+    </programlisting>
+    <location file="tests/doc-test-files/NonlinearFactor.h"/>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/classgtsam_1_1JacobianFactorQ.xml b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1JacobianFactorQ.xml
new file mode 100644
index 000000000..0ecf9b141
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1JacobianFactorQ.xml
@@ -0,0 +1,171 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="classgtsam_1_1JacobianFactorQ" kind="class" language="C++" prot="public">
+    <compoundname>gtsam::JacobianFactorQ</compoundname>
+    <basecompoundref prot="public" virt="non-virtual">RegularJacobianFactor&lt; D &gt;</basecompoundref>
+    <includes refid="JacobianFactorQ_8h" local="no">JacobianFactorQ.h</includes>
+    <templateparamlist>
+      <param>
+        <type>size_t</type>
+        <declname>D</declname>
+        <defname>D</defname>
+      </param>
+      <param>
+        <type>size_t</type>
+        <declname>ZDim</declname>
+        <defname>ZDim</defname>
+      </param>
+    </templateparamlist>
+      <sectiondef kind="private-type">
+      <memberdef kind="typedef" id="classgtsam_1_1JacobianFactorQ_1abb244ea915349e3beb40b669fe8317c4" prot="private" static="no">
+        <type>RegularJacobianFactor&lt; D &gt;</type>
+        <definition>typedef RegularJacobianFactor&lt;D&gt; gtsam::JacobianFactorQ&lt; D, ZDim &gt;::Base</definition>
+        <argsstring></argsstring>
+        <name>Base</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/JacobianFactorQ.h" line="29" column="1" bodyfile="tests/doc-test-files/JacobianFactorQ.h" bodystart="29" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1JacobianFactorQ_1aa64b12e917e0c89d0d0e9782ea9e06a4" prot="private" static="no">
+        <type>Eigen::Matrix&lt; double, ZDim, D &gt;</type>
+        <definition>typedef Eigen::Matrix&lt;double, ZDim, D&gt; gtsam::JacobianFactorQ&lt; D, ZDim &gt;::MatrixZD</definition>
+        <argsstring></argsstring>
+        <name>MatrixZD</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/JacobianFactorQ.h" line="30" column="1" bodyfile="tests/doc-test-files/JacobianFactorQ.h" bodystart="30" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1JacobianFactorQ_1a2dbb8aa1aaf1d46d64181c5aeeb1a06c" prot="private" static="no">
+        <type>std::pair&lt; Key, Matrix &gt;</type>
+        <definition>typedef std::pair&lt;Key, Matrix&gt; gtsam::JacobianFactorQ&lt; D, ZDim &gt;::KeyMatrix</definition>
+        <argsstring></argsstring>
+        <name>KeyMatrix</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/JacobianFactorQ.h" line="31" column="1" bodyfile="tests/doc-test-files/JacobianFactorQ.h" bodystart="31" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="public-func">
+      <memberdef kind="function" id="classgtsam_1_1JacobianFactorQ_1a4dd3fbba46a6dff75bcf902931a3b769" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::JacobianFactorQ&lt; D, ZDim &gt;::JacobianFactorQ</definition>
+        <argsstring>()</argsstring>
+        <name>JacobianFactorQ</name>
+        <briefdescription>
+<para>Default constructor. </para>        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/JacobianFactorQ.h" line="36" column="1" bodyfile="tests/doc-test-files/JacobianFactorQ.h" bodystart="36" bodyend="37"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1JacobianFactorQ_1abfb38e58b5b2d1293cf374b7eca2b2d4" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::JacobianFactorQ&lt; D, ZDim &gt;::JacobianFactorQ</definition>
+        <argsstring>(const KeyVector &amp;keys, const SharedDiagonal &amp;model=SharedDiagonal())</argsstring>
+        <name>JacobianFactorQ</name>
+        <param>
+          <type>const KeyVector &amp;</type>
+          <declname>keys</declname>
+        </param>
+        <param>
+          <type>const SharedDiagonal &amp;</type>
+          <declname>model</declname>
+          <defval>SharedDiagonal()</defval>
+        </param>
+        <briefdescription>
+<para>Empty constructor with keys. </para>        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/JacobianFactorQ.h" line="40" column="1" bodyfile="tests/doc-test-files/JacobianFactorQ.h" bodystart="40" bodyend="50"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1JacobianFactorQ_1a549fef1272025c45472f2e4294bd319e" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::JacobianFactorQ&lt; D, ZDim &gt;::JacobianFactorQ</definition>
+        <argsstring>(const KeyVector &amp;keys, const std::vector&lt; MatrixZD, Eigen::aligned_allocator&lt; MatrixZD &gt; &gt; &amp;FBlocks, const Matrix &amp;E, const Matrix3 &amp;P, const Vector &amp;b, const SharedDiagonal &amp;model=SharedDiagonal())</argsstring>
+        <name>JacobianFactorQ</name>
+        <param>
+          <type>const KeyVector &amp;</type>
+          <declname>keys</declname>
+        </param>
+        <param>
+          <type>const std::vector&lt; MatrixZD, Eigen::aligned_allocator&lt; MatrixZD &gt; &gt; &amp;</type>
+          <declname>FBlocks</declname>
+        </param>
+        <param>
+          <type>const Matrix &amp;</type>
+          <declname>E</declname>
+        </param>
+        <param>
+          <type>const Matrix3 &amp;</type>
+          <declname>P</declname>
+        </param>
+        <param>
+          <type>const Vector &amp;</type>
+          <declname>b</declname>
+        </param>
+        <param>
+          <type>const SharedDiagonal &amp;</type>
+          <declname>model</declname>
+          <defval>SharedDiagonal()</defval>
+        </param>
+        <briefdescription>
+<para>Constructor. </para>        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/JacobianFactorQ.h" line="53" column="1" bodyfile="tests/doc-test-files/JacobianFactorQ.h" bodystart="53" bodyend="72"/>
+      </memberdef>
+      </sectiondef>
+    <briefdescription>
+    </briefdescription>
+    <detaileddescription>
+<para>JacobianFactor for Schur complement that uses Q noise model </para>    </detaileddescription>
+    <inheritancegraph>
+      <node id="83">
+        <label>gtsam::JacobianFactorQ&lt; D, ZDim &gt;</label>
+        <link refid="classgtsam_1_1JacobianFactorQ"/>
+        <childnode refid="84" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="84">
+        <label>RegularJacobianFactor&lt; D &gt;</label>
+      </node>
+    </inheritancegraph>
+    <collaborationgraph>
+      <node id="85">
+        <label>gtsam::JacobianFactorQ&lt; D, ZDim &gt;</label>
+        <link refid="classgtsam_1_1JacobianFactorQ"/>
+        <childnode refid="86" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="86">
+        <label>RegularJacobianFactor&lt; D &gt;</label>
+      </node>
+    </collaborationgraph>
+    <location file="tests/doc-test-files/JacobianFactorQ.h" line="27" column="1" bodyfile="tests/doc-test-files/JacobianFactorQ.h" bodystart="27" bodyend="73"/>
+    <listofallmembers>
+      <member refid="classgtsam_1_1JacobianFactorQ_1abb244ea915349e3beb40b669fe8317c4" prot="private" virt="non-virtual"><scope>gtsam::JacobianFactorQ</scope><name>Base</name></member>
+      <member refid="classgtsam_1_1JacobianFactorQ_1a4dd3fbba46a6dff75bcf902931a3b769" prot="public" virt="non-virtual"><scope>gtsam::JacobianFactorQ</scope><name>JacobianFactorQ</name></member>
+      <member refid="classgtsam_1_1JacobianFactorQ_1abfb38e58b5b2d1293cf374b7eca2b2d4" prot="public" virt="non-virtual"><scope>gtsam::JacobianFactorQ</scope><name>JacobianFactorQ</name></member>
+      <member refid="classgtsam_1_1JacobianFactorQ_1a549fef1272025c45472f2e4294bd319e" prot="public" virt="non-virtual"><scope>gtsam::JacobianFactorQ</scope><name>JacobianFactorQ</name></member>
+      <member refid="classgtsam_1_1JacobianFactorQ_1a2dbb8aa1aaf1d46d64181c5aeeb1a06c" prot="private" virt="non-virtual"><scope>gtsam::JacobianFactorQ</scope><name>KeyMatrix</name></member>
+      <member refid="classgtsam_1_1JacobianFactorQ_1aa64b12e917e0c89d0d0e9782ea9e06a4" prot="private" virt="non-virtual"><scope>gtsam::JacobianFactorQ</scope><name>MatrixZD</name></member>
+    </listofallmembers>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor.xml b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor.xml
new file mode 100644
index 000000000..e402b3556
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor.xml
@@ -0,0 +1,459 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="classgtsam_1_1NoiseModelFactor" kind="class" language="C++" prot="public" abstract="yes">
+    <compoundname>gtsam::NoiseModelFactor</compoundname>
+    <basecompoundref refid="classgtsam_1_1NonlinearFactor" prot="public" virt="non-virtual">gtsam::NonlinearFactor</basecompoundref>
+    <derivedcompoundref refid="classgtsam_1_1NoiseModelFactor1" prot="public" virt="non-virtual">gtsam::NoiseModelFactor1&lt; VALUE &gt;</derivedcompoundref>
+    <derivedcompoundref refid="classgtsam_1_1NoiseModelFactor2" prot="public" virt="non-virtual">gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;</derivedcompoundref>
+    <derivedcompoundref refid="classgtsam_1_1NoiseModelFactor3" prot="public" virt="non-virtual">gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;</derivedcompoundref>
+    <derivedcompoundref refid="classgtsam_1_1NoiseModelFactor4" prot="public" virt="non-virtual">gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;</derivedcompoundref>
+    <derivedcompoundref refid="classgtsam_1_1NoiseModelFactor5" prot="public" virt="non-virtual">gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;</derivedcompoundref>
+    <derivedcompoundref refid="classgtsam_1_1NoiseModelFactor6" prot="public" virt="non-virtual">gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;</derivedcompoundref>
+    <includes refid="NonlinearFactor_8h" local="no">NonlinearFactor.h</includes>
+      <sectiondef kind="protected-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor_1a9ac7f77c3ae2d5bfd14d25f4bdc3deaa" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NonlinearFactor" kindref="compound">NonlinearFactor</ref></type>
+        <definition>typedef NonlinearFactor gtsam::NoiseModelFactor::Base</definition>
+        <argsstring></argsstring>
+        <name>Base</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="168" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="168" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor_1a44e242fb0945cf6c60d30c36c3f229af" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref></type>
+        <definition>typedef NoiseModelFactor gtsam::NoiseModelFactor::This</definition>
+        <argsstring></argsstring>
+        <name>This</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="169" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="169" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="public-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor_1a57d98d52fb6f91ebc54d74746aeb90e4" prot="public" static="no">
+        <type>std::shared_ptr&lt; <ref refid="classgtsam_1_1NonlinearFactor" kindref="compound">This</ref> &gt;</type>
+        <definition>typedef std::shared_ptr&lt;This&gt; gtsam::NoiseModelFactor::shared_ptr</definition>
+        <argsstring></argsstring>
+        <name>shared_ptr</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Noise model </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="175" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="175" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="protected-attrib">
+      <memberdef kind="variable" id="classgtsam_1_1NoiseModelFactor_1a2263eabbefe0eede8c9d10486f61bb4d" prot="protected" static="no" mutable="no">
+        <type>SharedNoiseModel</type>
+        <definition>SharedNoiseModel gtsam::NoiseModelFactor::noiseModel_</definition>
+        <argsstring></argsstring>
+        <name>noiseModel_</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="171" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="171" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="friend">
+      <memberdef kind="friend" id="classgtsam_1_1NoiseModelFactor_1ac98d07dd8f7b70e16ccb9a01abf56b9c" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual">
+        <type>friend class</type>
+        <definition>friend class boost::serialization::access</definition>
+        <argsstring></argsstring>
+        <name>boost::serialization::access</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Serialization function </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="254" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="254" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="public-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor_1ad6e8ef3348e042d598617d1d55f20300" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NoiseModelFactor::NoiseModelFactor</definition>
+        <argsstring>()</argsstring>
+        <name>NoiseModelFactor</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Default constructor for I/O only </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="178" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="178" bodyend="178"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor_1ae1ed5e0fa6c471b57172f7ebaf552187" prot="public" static="no" const="no" explicit="no" inline="yes" virt="virtual">
+        <type></type>
+        <definition>virtual gtsam::NoiseModelFactor::~NoiseModelFactor</definition>
+        <argsstring>()</argsstring>
+        <name>~NoiseModelFactor</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Destructor </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="181" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="181" bodyend="181"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor_1a02cea97db30d27b51feaf1f0444d743c" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <templateparamlist>
+          <param>
+            <type>typename CONTAINER</type>
+          </param>
+        </templateparamlist>
+        <type></type>
+        <definition>gtsam::NoiseModelFactor::NoiseModelFactor</definition>
+        <argsstring>(const SharedNoiseModel &amp;noiseModel, const CONTAINER &amp;keys)</argsstring>
+        <name>NoiseModelFactor</name>
+        <param>
+          <type>const SharedNoiseModel &amp;</type>
+          <declname>noiseModel</declname>
+        </param>
+        <param>
+          <type>const CONTAINER &amp;</type>
+          <declname>keys</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Constructor </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="187" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="187" bodyend="188"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor_1a682e0bbaf0c9638b7d0d96148082d1d7" prot="public" static="no" const="yes" explicit="no" inline="no" virt="virtual">
+        <type>void</type>
+        <definition>virtual void gtsam::NoiseModelFactor::print</definition>
+        <argsstring>(const std::string &amp;s=&quot;&quot;, const KeyFormatter &amp;keyFormatter=DefaultKeyFormatter) const </argsstring>
+        <name>print</name>
+        <reimplements refid="classgtsam_1_1NonlinearFactor_1a758285fe17213cbf9c23b9f42fb960e6">print</reimplements>
+        <param>
+          <type>const std::string &amp;</type>
+          <declname>s</declname>
+          <defval>&quot;&quot;</defval>
+        </param>
+        <param>
+          <type>const KeyFormatter &amp;</type>
+          <declname>keyFormatter</declname>
+          <defval>DefaultKeyFormatter</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Print </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="200" column="1"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor_1a616d0335db512e9c333cab83b1923399" prot="public" static="no" const="yes" explicit="no" inline="no" virt="virtual">
+        <type>bool</type>
+        <definition>virtual bool gtsam::NoiseModelFactor::equals</definition>
+        <argsstring>(const NonlinearFactor &amp;f, double tol=1e-9) const </argsstring>
+        <name>equals</name>
+        <reimplements refid="classgtsam_1_1NonlinearFactor_1a38cdabb3302973815e357bd00573734a">equals</reimplements>
+        <param>
+          <type>const <ref refid="classgtsam_1_1NonlinearFactor" kindref="compound">NonlinearFactor</ref> &amp;</type>
+          <declname>f</declname>
+        </param>
+        <param>
+          <type>double</type>
+          <declname>tol</declname>
+          <defval>1e-9</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Check if two factors are equal </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="204" column="1"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor_1af4f62b52fff1a0527ecdb279de023728" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="virtual">
+        <type>size_t</type>
+        <definition>virtual size_t gtsam::NoiseModelFactor::dim</definition>
+        <argsstring>() const </argsstring>
+        <name>dim</name>
+        <reimplements refid="classgtsam_1_1NonlinearFactor_1a5f0fcf1c93ded207d4a31e8ede07d797">dim</reimplements>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>get the dimension of the factor (number of rows on linearization) </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="207" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="207" bodyend="209"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor_1a123e5729d70b4acd6d7e1f70b1a7ced4" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>const SharedNoiseModel &amp;</type>
+        <definition>const SharedNoiseModel&amp; gtsam::NoiseModelFactor::noiseModel</definition>
+        <argsstring>() const </argsstring>
+        <name>noiseModel</name>
+        <briefdescription>
+<para>access to the noise model </para>        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="212" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="212" bodyend="214"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor_1a16a8a23069ce6ae664f516edc8f36254" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>SharedNoiseModel</type>
+        <definition>SharedNoiseModel gtsam::NoiseModelFactor::get_noiseModel</definition>
+        <argsstring>() const </argsstring>
+        <name>get_noiseModel</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para><xrefsect id="deprecated_1_deprecated000002"><xreftitle>Deprecated</xreftitle><xrefdescription><para>access to the noise model </para></xrefdescription></xrefsect></para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="217" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="217" bodyend="219"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor_1afd61874915a8076bf83f9d8311fc0e26" prot="public" static="no" const="yes" explicit="no" inline="no" virt="pure-virtual">
+        <type>Vector</type>
+        <definition>virtual Vector gtsam::NoiseModelFactor::unwhitenedError</definition>
+        <argsstring>(const Values &amp;x, boost::optional&lt; std::vector&lt; Matrix &gt; &amp; &gt; H=boost::none) const =0</argsstring>
+        <name>unwhitenedError</name>
+        <reimplementedby refid="classgtsam_1_1NoiseModelFactor1_1a886bb13688a62b2680c10cdaf7999918">unwhitenedError</reimplementedby>
+        <reimplementedby refid="classgtsam_1_1NoiseModelFactor2_1a9640bbbb433383f0b40b5c2d9439415e">unwhitenedError</reimplementedby>
+        <reimplementedby refid="classgtsam_1_1NoiseModelFactor3_1aab8e3316ef77cecc1bd1909a2c0eed33">unwhitenedError</reimplementedby>
+        <reimplementedby refid="classgtsam_1_1NoiseModelFactor4_1a6a79ae22fc40447c8dc183a951243440">unwhitenedError</reimplementedby>
+        <reimplementedby refid="classgtsam_1_1NoiseModelFactor5_1a701b7a764f18310b4eea14ee023c922b">unwhitenedError</reimplementedby>
+        <reimplementedby refid="classgtsam_1_1NoiseModelFactor6_1a945d269f9a722d585e3332be23030450">unwhitenedError</reimplementedby>
+        <param>
+          <type>const Values &amp;</type>
+          <declname>x</declname>
+        </param>
+        <param>
+          <type>boost::optional&lt; std::vector&lt; Matrix &gt; &amp; &gt;</type>
+          <declname>H</declname>
+          <defval>boost::none</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Error function <emphasis>without</emphasis> the NoiseModel, <formula id="3">$ z-h(x) $</formula>. Override this method to finish implementing an N-way factor. If the optional arguments is specified, it should compute both the function evaluation and its derivative(s) in H. </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="227" column="1"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor_1a1b3ea51380315bfd63f60d8d9195b0c1" prot="public" static="no" const="yes" explicit="no" inline="no" virt="non-virtual">
+        <type>Vector</type>
+        <definition>Vector gtsam::NoiseModelFactor::whitenedError</definition>
+        <argsstring>(const Values &amp;c) const </argsstring>
+        <name>whitenedError</name>
+        <param>
+          <type>const Values &amp;</type>
+          <declname>c</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Vector of errors, whitened This is the raw error, i.e., i.e. <formula id="4">$ (h(x)-z)/\sigma $</formula> in case of a Gaussian </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="234" column="1"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor_1af6147aff3a33f734435601f6dcafeaf9" prot="public" static="no" const="yes" explicit="no" inline="no" virt="virtual">
+        <type>double</type>
+        <definition>virtual double gtsam::NoiseModelFactor::error</definition>
+        <argsstring>(const Values &amp;c) const </argsstring>
+        <name>error</name>
+        <reimplements refid="classgtsam_1_1NonlinearFactor_1a8e67e58ff0e4f9d58f07e5eb227adc52">error</reimplements>
+        <param>
+          <type>const Values &amp;</type>
+          <declname>c</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Calculate the error of the factor. This is the log-likelihood, e.g. <formula id="5">$ 0.5(h(x)-z)^2/\sigma^2 $</formula> in case of Gaussian. In this class, we take the raw prediction error <formula id="6">$ h(x)-z $</formula>, ask the noise model to transform it to <formula id="7">$ (h(x)-z)^2/\sigma^2 $</formula>, and then multiply by 0.5. </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="242" column="1"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor_1a0c83582a0bd04e112e4c0a17cd4ed324" prot="public" static="no" const="yes" explicit="no" inline="no" virt="virtual">
+        <type>std::shared_ptr&lt; GaussianFactor &gt;</type>
+        <definition>std::shared_ptr&lt;GaussianFactor&gt; gtsam::NoiseModelFactor::linearize</definition>
+        <argsstring>(const Values &amp;x) const </argsstring>
+        <name>linearize</name>
+        <reimplements refid="classgtsam_1_1NonlinearFactor_1af2cc20b4ec180610dbf224f2165fa50a">linearize</reimplements>
+        <param>
+          <type>const Values &amp;</type>
+          <declname>x</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Linearize a non-linearFactorN to get a GaussianFactor, <formula id="8">$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z $</formula> Hence <formula id="9">$ b = z - h(x) = - \mathtt{error\_vector}(x) $</formula> </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="249" column="1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="protected-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor_1a4605224417029e9dc58c5267efa9a46d" prot="protected" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NoiseModelFactor::NoiseModelFactor</definition>
+        <argsstring>(const SharedNoiseModel &amp;noiseModel)</argsstring>
+        <name>NoiseModelFactor</name>
+        <param>
+          <type>const SharedNoiseModel &amp;</type>
+          <declname>noiseModel</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Constructor - only for subclasses, as this does not set keys. </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="195" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="195" bodyend="195"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="private-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor_1abb60fdb39cb6939b08744605713e153e" prot="private" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <templateparamlist>
+          <param>
+            <type>class ARCHIVE</type>
+          </param>
+        </templateparamlist>
+        <type>void</type>
+        <definition>void gtsam::NoiseModelFactor::serialize</definition>
+        <argsstring>(ARCHIVE &amp;ar, const unsigned int)</argsstring>
+        <name>serialize</name>
+        <param>
+          <type>ARCHIVE &amp;</type>
+          <declname>ar</declname>
+        </param>
+        <param>
+          <type>const unsigned</type>
+          <declname>int</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="256" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="256" bodyend="260"/>
+      </memberdef>
+      </sectiondef>
+    <briefdescription>
+    </briefdescription>
+    <detaileddescription>
+<para>A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density <formula id="0">$ P(z|x) \propto exp -0.5*|z-h(x)|^2_C $</formula> Templated on the parameter type X and the values structure Values There is no return type specified for h(x). Instead, we require the derived class implements <formula id="1">$ \mathtt{error\_vector}(x) = h(x)-z \approx A \delta x - b $</formula> This allows a graph to have factors with measurements of mixed type.</para><para>The noise model is typically Gaussian, but robust and constrained error models are also supported. </para>    </detaileddescription>
+    <inheritancegraph>
+      <node id="89">
+        <label>Factor</label>
+      </node>
+      <node id="93">
+        <label>gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor4"/>
+        <childnode refid="87" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="95">
+        <label>gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor6"/>
+        <childnode refid="87" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="94">
+        <label>gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor5"/>
+        <childnode refid="87" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="88">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="89" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="87">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="88" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="90">
+        <label>gtsam::NoiseModelFactor1&lt; VALUE &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor1"/>
+        <childnode refid="87" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="92">
+        <label>gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor3"/>
+        <childnode refid="87" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="91">
+        <label>gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor2"/>
+        <childnode refid="87" relation="public-inheritance">
+        </childnode>
+      </node>
+    </inheritancegraph>
+    <collaborationgraph>
+      <node id="98">
+        <label>Factor</label>
+      </node>
+      <node id="97">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="98" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="96">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="97" relation="public-inheritance">
+        </childnode>
+      </node>
+    </collaborationgraph>
+    <location file="tests/doc-test-files/NonlinearFactor.h" line="163" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="163" bodyend="262"/>
+    <listofallmembers>
+      <member refid="classgtsam_1_1NonlinearFactor_1a1968f471dc3ea33aaf0226ff7be5ae8c" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor</scope><name>active</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a9ac7f77c3ae2d5bfd14d25f4bdc3deaa" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>Base</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ac98d07dd8f7b70e16ccb9a01abf56b9c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>boost::serialization::access</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1aa98a30a78ade8489214ce2e5b8302777" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor</scope><name>clone</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1af4f62b52fff1a0527ecdb279de023728" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor</scope><name>dim</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a616d0335db512e9c333cab83b1923399" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor</scope><name>equals</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1af6147aff3a33f734435601f6dcafeaf9" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor</scope><name>error</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a16a8a23069ce6ae664f516edc8f36254" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>get_noiseModel</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a0c83582a0bd04e112e4c0a17cd4ed324" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor</scope><name>linearize</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a123e5729d70b4acd6d7e1f70b1a7ced4" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>noiseModel</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a2263eabbefe0eede8c9d10486f61bb4d" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>noiseModel_</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ad6e8ef3348e042d598617d1d55f20300" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a02cea97db30d27b51feaf1f0444d743c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a4605224417029e9dc58c5267efa9a46d" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a7c719ae67f222f66569ccb3b526cb9cb" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a8a7610dba7e9a6f878268c2769ea0e04" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a682e0bbaf0c9638b7d0d96148082d1d7" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor</scope><name>print</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a95f4d57e8646b6df634496aaabb683b5" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a5f760600e8608ef15c825a0f8d1d1855" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1abb60fdb39cb6939b08744605713e153e" prot="private" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>serialize</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a57d98d52fb6f91ebc54d74746aeb90e4" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>shared_ptr</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a44e242fb0945cf6c60d30c36c3f229af" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>This</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1afd61874915a8076bf83f9d8311fc0e26" prot="public" virt="pure-virtual"><scope>gtsam::NoiseModelFactor</scope><name>unwhitenedError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a1b3ea51380315bfd63f60d8d9195b0c1" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor</scope><name>whitenedError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ae1ed5e0fa6c471b57172f7ebaf552187" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor</scope><name>~NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1ac457810c504de58732d51d61f715c2de" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor</scope><name>~NonlinearFactor</name></member>
+    </listofallmembers>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor1.xml b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor1.xml
new file mode 100644
index 000000000..0d657125a
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor1.xml
@@ -0,0 +1,306 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="classgtsam_1_1NoiseModelFactor1" kind="class" language="C++" prot="public" abstract="yes">
+    <compoundname>gtsam::NoiseModelFactor1</compoundname>
+    <basecompoundref refid="classgtsam_1_1NoiseModelFactor" prot="public" virt="non-virtual">gtsam::NoiseModelFactor</basecompoundref>
+    <includes refid="NonlinearFactor_8h" local="no">NonlinearFactor.h</includes>
+    <templateparamlist>
+      <param>
+        <type>class VALUE</type>
+      </param>
+    </templateparamlist>
+      <sectiondef kind="public-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor1_1a37d19c88478e96cf1dfc065229734ece" prot="public" static="no">
+        <type>VALUE</type>
+        <definition>typedef VALUE gtsam::NoiseModelFactor1&lt; VALUE &gt;::X</definition>
+        <argsstring></argsstring>
+        <name>X</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="281" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="281" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="protected-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor1_1a8ed3a4fbfc8843df8a2f21a805e93ef0" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref></type>
+        <definition>typedef NoiseModelFactor gtsam::NoiseModelFactor1&lt; VALUE &gt;::Base</definition>
+        <argsstring></argsstring>
+        <name>Base</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="285" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="285" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor1_1a22cb5d89cc9ec25e2d94004917d890fc" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NoiseModelFactor1" kindref="compound">NoiseModelFactor1</ref>&lt; VALUE &gt;</type>
+        <definition>typedef NoiseModelFactor1&lt;VALUE&gt; gtsam::NoiseModelFactor1&lt; VALUE &gt;::This</definition>
+        <argsstring></argsstring>
+        <name>This</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="286" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="286" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="friend">
+      <memberdef kind="friend" id="classgtsam_1_1NoiseModelFactor1_1ac98d07dd8f7b70e16ccb9a01abf56b9c" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual">
+        <type>friend class</type>
+        <definition>friend class boost::serialization::access</definition>
+        <argsstring></argsstring>
+        <name>boost::serialization::access</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Serialization function </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="332" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="332" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="public-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor1_1a85c649f81b216f5b5d7e321febc7762c" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NoiseModelFactor1&lt; VALUE &gt;::NoiseModelFactor1</definition>
+        <argsstring>()</argsstring>
+        <name>NoiseModelFactor1</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Default constructor for I/O only </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="291" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="291" bodyend="291"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor1_1aecf0afe14b2af551d62a6e62ee8a8d27" prot="public" static="no" const="no" explicit="no" inline="yes" virt="virtual">
+        <type></type>
+        <definition>virtual gtsam::NoiseModelFactor1&lt; VALUE &gt;::~NoiseModelFactor1</definition>
+        <argsstring>()</argsstring>
+        <name>~NoiseModelFactor1</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="293" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="293" bodyend="293"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor1_1af7fe05db115c50d7f3bc2aaa1d6c7950" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor1&lt; VALUE &gt;::key</definition>
+        <argsstring>() const </argsstring>
+        <name>key</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="295" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="295" bodyend="295"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor1_1a70a0b32dd5feba3631815614d8386c86" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NoiseModelFactor1&lt; VALUE &gt;::NoiseModelFactor1</definition>
+        <argsstring>(const SharedNoiseModel &amp;noiseModel, Key key1)</argsstring>
+        <name>NoiseModelFactor1</name>
+        <param>
+          <type>const SharedNoiseModel &amp;</type>
+          <declname>noiseModel</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>key1</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Constructor <parameterlist kind="param"><parameteritem>
+<parameternamelist>
+<parametername>noiseModel</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>shared pointer to noise model </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>key1</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>by which to look up X value in Values </para></parameterdescription>
+</parameteritem>
+</parameterlist>
+</para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="302" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="302" bodyend="303"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor1_1a886bb13688a62b2680c10cdaf7999918" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="virtual">
+        <type>Vector</type>
+        <definition>virtual Vector gtsam::NoiseModelFactor1&lt; VALUE &gt;::unwhitenedError</definition>
+        <argsstring>(const Values &amp;x, boost::optional&lt; std::vector&lt; Matrix &gt; &amp; &gt; H=boost::none) const </argsstring>
+        <name>unwhitenedError</name>
+        <reimplements refid="classgtsam_1_1NoiseModelFactor_1afd61874915a8076bf83f9d8311fc0e26">unwhitenedError</reimplements>
+        <param>
+          <type>const Values &amp;</type>
+          <declname>x</declname>
+        </param>
+        <param>
+          <type>boost::optional&lt; std::vector&lt; Matrix &gt; &amp; &gt;</type>
+          <declname>H</declname>
+          <defval>boost::none</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Calls the 1-key specific version of evaluateError, which is pure virtual so must be implemented in the derived class. </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="308" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="308" bodyend="319"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor1_1a42ed690bfeafe79ab396126886b05820" prot="public" static="no" const="yes" explicit="no" inline="no" virt="pure-virtual">
+        <type>Vector</type>
+        <definition>virtual Vector gtsam::NoiseModelFactor1&lt; VALUE &gt;::evaluateError</definition>
+        <argsstring>(const X &amp;x, boost::optional&lt; Matrix &amp; &gt; H=boost::none) const =0</argsstring>
+        <name>evaluateError</name>
+        <param>
+          <type>const X &amp;</type>
+          <declname>x</declname>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H</declname>
+          <defval>boost::none</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Override this method to finish implementing a unary factor. If the optional Matrix reference argument is specified, it should compute both the function evaluation and its derivative in X. </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="326" column="1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="private-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor1_1a7f153cb8f238a3be2260979af5b590ab" prot="private" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <templateparamlist>
+          <param>
+            <type>class ARCHIVE</type>
+          </param>
+        </templateparamlist>
+        <type>void</type>
+        <definition>void gtsam::NoiseModelFactor1&lt; VALUE &gt;::serialize</definition>
+        <argsstring>(ARCHIVE &amp;ar, const unsigned int)</argsstring>
+        <name>serialize</name>
+        <param>
+          <type>ARCHIVE &amp;</type>
+          <declname>ar</declname>
+        </param>
+        <param>
+          <type>const unsigned</type>
+          <declname>int</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="334" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="334" bodyend="337"/>
+      </memberdef>
+      </sectiondef>
+    <briefdescription>
+    </briefdescription>
+    <detaileddescription>
+<para>A convenient base class for creating your own <ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref> with 1 variable. To derive from this class, implement <ref refid="classgtsam_1_1NoiseModelFactor1_1a42ed690bfeafe79ab396126886b05820" kindref="member">evaluateError()</ref>.</para><para>Templated on a values structure type. The values structures are typically more general than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds (Lie groups). </para>    </detaileddescription>
+    <inheritancegraph>
+      <node id="102">
+        <label>Factor</label>
+      </node>
+      <node id="101">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="102" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="100">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="101" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="99">
+        <label>gtsam::NoiseModelFactor1&lt; VALUE &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor1"/>
+        <childnode refid="100" relation="public-inheritance">
+        </childnode>
+      </node>
+    </inheritancegraph>
+    <collaborationgraph>
+      <node id="106">
+        <label>Factor</label>
+      </node>
+      <node id="105">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="106" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="104">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="105" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="103">
+        <label>gtsam::NoiseModelFactor1&lt; VALUE &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor1"/>
+        <childnode refid="104" relation="public-inheritance">
+        </childnode>
+      </node>
+    </collaborationgraph>
+    <location file="tests/doc-test-files/NonlinearFactor.h" line="276" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="276" bodyend="338"/>
+    <listofallmembers>
+      <member refid="classgtsam_1_1NonlinearFactor_1a1968f471dc3ea33aaf0226ff7be5ae8c" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor1</scope><name>active</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor1_1a8ed3a4fbfc8843df8a2f21a805e93ef0" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>Base</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor1_1ac98d07dd8f7b70e16ccb9a01abf56b9c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>boost::serialization::access</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1aa98a30a78ade8489214ce2e5b8302777" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor1</scope><name>clone</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1af4f62b52fff1a0527ecdb279de023728" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor1</scope><name>dim</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a616d0335db512e9c333cab83b1923399" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor1</scope><name>equals</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1af6147aff3a33f734435601f6dcafeaf9" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor1</scope><name>error</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor1_1a42ed690bfeafe79ab396126886b05820" prot="public" virt="pure-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>evaluateError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a16a8a23069ce6ae664f516edc8f36254" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>get_noiseModel</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor1_1af7fe05db115c50d7f3bc2aaa1d6c7950" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>key</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a0c83582a0bd04e112e4c0a17cd4ed324" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor1</scope><name>linearize</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a123e5729d70b4acd6d7e1f70b1a7ced4" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>noiseModel</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a2263eabbefe0eede8c9d10486f61bb4d" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>noiseModel_</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ad6e8ef3348e042d598617d1d55f20300" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a02cea97db30d27b51feaf1f0444d743c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a4605224417029e9dc58c5267efa9a46d" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor1_1a85c649f81b216f5b5d7e321febc7762c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>NoiseModelFactor1</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor1_1a70a0b32dd5feba3631815614d8386c86" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>NoiseModelFactor1</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a7c719ae67f222f66569ccb3b526cb9cb" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a8a7610dba7e9a6f878268c2769ea0e04" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a682e0bbaf0c9638b7d0d96148082d1d7" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor1</scope><name>print</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a95f4d57e8646b6df634496aaabb683b5" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a5f760600e8608ef15c825a0f8d1d1855" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor1_1a7f153cb8f238a3be2260979af5b590ab" prot="private" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>serialize</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a57d98d52fb6f91ebc54d74746aeb90e4" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>shared_ptr</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor1_1a22cb5d89cc9ec25e2d94004917d890fc" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>This</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor1_1a886bb13688a62b2680c10cdaf7999918" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor1</scope><name>unwhitenedError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a1b3ea51380315bfd63f60d8d9195b0c1" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>whitenedError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor1_1a37d19c88478e96cf1dfc065229734ece" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor1</scope><name>X</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ae1ed5e0fa6c471b57172f7ebaf552187" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor1</scope><name>~NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor1_1aecf0afe14b2af551d62a6e62ee8a8d27" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor1</scope><name>~NoiseModelFactor1</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1ac457810c504de58732d51d61f715c2de" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor1</scope><name>~NonlinearFactor</name></member>
+    </listofallmembers>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor2.xml b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor2.xml
new file mode 100644
index 000000000..9240ef2d4
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor2.xml
@@ -0,0 +1,355 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="classgtsam_1_1NoiseModelFactor2" kind="class" language="C++" prot="public" abstract="yes">
+    <compoundname>gtsam::NoiseModelFactor2</compoundname>
+    <basecompoundref refid="classgtsam_1_1NoiseModelFactor" prot="public" virt="non-virtual">gtsam::NoiseModelFactor</basecompoundref>
+    <includes refid="NonlinearFactor_8h" local="no">NonlinearFactor.h</includes>
+    <templateparamlist>
+      <param>
+        <type>class VALUE1</type>
+      </param>
+      <param>
+        <type>class VALUE2</type>
+      </param>
+    </templateparamlist>
+      <sectiondef kind="public-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor2_1a5fb0fe8b056bad2171f84922c5184ef1" prot="public" static="no">
+        <type>VALUE1</type>
+        <definition>typedef VALUE1 gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;::X1</definition>
+        <argsstring></argsstring>
+        <name>X1</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="350" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="350" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor2_1a3b3777f359b34892f33784d73cb62234" prot="public" static="no">
+        <type>VALUE2</type>
+        <definition>typedef VALUE2 gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;::X2</definition>
+        <argsstring></argsstring>
+        <name>X2</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="351" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="351" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="protected-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor2_1a8c2961991a7f2b253874612fc8124c09" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref></type>
+        <definition>typedef NoiseModelFactor gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;::Base</definition>
+        <argsstring></argsstring>
+        <name>Base</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="355" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="355" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor2_1a2743c4c486e80127e8eca591ee3ffbed" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NoiseModelFactor2" kindref="compound">NoiseModelFactor2</ref>&lt; VALUE1, VALUE2 &gt;</type>
+        <definition>typedef NoiseModelFactor2&lt;VALUE1, VALUE2&gt; gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;::This</definition>
+        <argsstring></argsstring>
+        <name>This</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="356" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="356" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="friend">
+      <memberdef kind="friend" id="classgtsam_1_1NoiseModelFactor2_1ac98d07dd8f7b70e16ccb9a01abf56b9c" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual">
+        <type>friend class</type>
+        <definition>friend class boost::serialization::access</definition>
+        <argsstring></argsstring>
+        <name>boost::serialization::access</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Serialization function </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="408" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="408" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="public-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor2_1a04f5873776c784cafd55b4ac05b4b757" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;::NoiseModelFactor2</definition>
+        <argsstring>()</argsstring>
+        <name>NoiseModelFactor2</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Default Constructor for I/O </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="363" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="363" bodyend="363"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor2_1a35453b054509f5679a29a06b59340cf1" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;::NoiseModelFactor2</definition>
+        <argsstring>(const SharedNoiseModel &amp;noiseModel, Key j1, Key j2)</argsstring>
+        <name>NoiseModelFactor2</name>
+        <param>
+          <type>const SharedNoiseModel &amp;</type>
+          <declname>noiseModel</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j1</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j2</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Constructor <parameterlist kind="param"><parameteritem>
+<parameternamelist>
+<parametername>noiseModel</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>shared pointer to noise model </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j1</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the first variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j2</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the second variable </para></parameterdescription>
+</parameteritem>
+</parameterlist>
+</para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="371" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="371" bodyend="372"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor2_1ab629a5dc71461efe61d68faaf5a659f3" prot="public" static="no" const="no" explicit="no" inline="yes" virt="virtual">
+        <type></type>
+        <definition>virtual gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;::~NoiseModelFactor2</definition>
+        <argsstring>()</argsstring>
+        <name>~NoiseModelFactor2</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="374" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="374" bodyend="374"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor2_1ae3b88b40b4ee6e9af9eefcde77bf9768" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;::key1</definition>
+        <argsstring>() const </argsstring>
+        <name>key1</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>methods to retrieve both keys </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="377" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="377" bodyend="377"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor2_1a4d27f5d0beaa35e1e4a5f0ef8974c5a6" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;::key2</definition>
+        <argsstring>() const </argsstring>
+        <name>key2</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="378" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="378" bodyend="378"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor2_1a9640bbbb433383f0b40b5c2d9439415e" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="virtual">
+        <type>Vector</type>
+        <definition>virtual Vector gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;::unwhitenedError</definition>
+        <argsstring>(const Values &amp;x, boost::optional&lt; std::vector&lt; Matrix &gt; &amp; &gt; H=boost::none) const </argsstring>
+        <name>unwhitenedError</name>
+        <reimplements refid="classgtsam_1_1NoiseModelFactor_1afd61874915a8076bf83f9d8311fc0e26">unwhitenedError</reimplements>
+        <param>
+          <type>const Values &amp;</type>
+          <declname>x</declname>
+        </param>
+        <param>
+          <type>boost::optional&lt; std::vector&lt; Matrix &gt; &amp; &gt;</type>
+          <declname>H</declname>
+          <defval>boost::none</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Calls the 2-key specific version of evaluateError, which is pure virtual so must be implemented in the derived class. </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="382" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="382" bodyend="394"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor2_1a9aa6dcd7db6734479e2212213b021ec1" prot="public" static="no" const="yes" explicit="no" inline="no" virt="pure-virtual">
+        <type>Vector</type>
+        <definition>virtual Vector gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;::evaluateError</definition>
+        <argsstring>(const X1 &amp;, const X2 &amp;, boost::optional&lt; Matrix &amp; &gt; H1=boost::none, boost::optional&lt; Matrix &amp; &gt; H2=boost::none) const =0</argsstring>
+        <name>evaluateError</name>
+        <param>
+          <type>const X1 &amp;</type>
+        </param>
+        <param>
+          <type>const X2 &amp;</type>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H1</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H2</declname>
+          <defval>boost::none</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Override this method to finish implementing a binary factor. If any of the optional Matrix reference arguments are specified, it should compute both the function evaluation and its derivative(s) in X1 (and/or X2). </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="402" column="1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="private-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor2_1ae600c15f59be175aca5b059e488d7166" prot="private" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <templateparamlist>
+          <param>
+            <type>class ARCHIVE</type>
+          </param>
+        </templateparamlist>
+        <type>void</type>
+        <definition>void gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;::serialize</definition>
+        <argsstring>(ARCHIVE &amp;ar, const unsigned int)</argsstring>
+        <name>serialize</name>
+        <param>
+          <type>ARCHIVE &amp;</type>
+          <declname>ar</declname>
+        </param>
+        <param>
+          <type>const unsigned</type>
+          <declname>int</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="410" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="410" bodyend="413"/>
+      </memberdef>
+      </sectiondef>
+    <briefdescription>
+    </briefdescription>
+    <detaileddescription>
+<para>A convenient base class for creating your own <ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref> with 2 variables. To derive from this class, implement <ref refid="classgtsam_1_1NoiseModelFactor2_1a9aa6dcd7db6734479e2212213b021ec1" kindref="member">evaluateError()</ref>. </para>    </detaileddescription>
+    <inheritancegraph>
+      <node id="110">
+        <label>Factor</label>
+      </node>
+      <node id="109">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="110" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="108">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="109" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="107">
+        <label>gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor2"/>
+        <childnode refid="108" relation="public-inheritance">
+        </childnode>
+      </node>
+    </inheritancegraph>
+    <collaborationgraph>
+      <node id="114">
+        <label>Factor</label>
+      </node>
+      <node id="113">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="114" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="112">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="113" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="111">
+        <label>gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor2"/>
+        <childnode refid="112" relation="public-inheritance">
+        </childnode>
+      </node>
+    </collaborationgraph>
+    <location file="tests/doc-test-files/NonlinearFactor.h" line="345" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="345" bodyend="414"/>
+    <listofallmembers>
+      <member refid="classgtsam_1_1NonlinearFactor_1a1968f471dc3ea33aaf0226ff7be5ae8c" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor2</scope><name>active</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor2_1a8c2961991a7f2b253874612fc8124c09" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>Base</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor2_1ac98d07dd8f7b70e16ccb9a01abf56b9c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>boost::serialization::access</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1aa98a30a78ade8489214ce2e5b8302777" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor2</scope><name>clone</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1af4f62b52fff1a0527ecdb279de023728" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor2</scope><name>dim</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a616d0335db512e9c333cab83b1923399" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor2</scope><name>equals</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1af6147aff3a33f734435601f6dcafeaf9" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor2</scope><name>error</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor2_1a9aa6dcd7db6734479e2212213b021ec1" prot="public" virt="pure-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>evaluateError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a16a8a23069ce6ae664f516edc8f36254" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>get_noiseModel</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor2_1ae3b88b40b4ee6e9af9eefcde77bf9768" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>key1</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor2_1a4d27f5d0beaa35e1e4a5f0ef8974c5a6" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>key2</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a0c83582a0bd04e112e4c0a17cd4ed324" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor2</scope><name>linearize</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a123e5729d70b4acd6d7e1f70b1a7ced4" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>noiseModel</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a2263eabbefe0eede8c9d10486f61bb4d" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>noiseModel_</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ad6e8ef3348e042d598617d1d55f20300" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a02cea97db30d27b51feaf1f0444d743c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a4605224417029e9dc58c5267efa9a46d" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor2_1a04f5873776c784cafd55b4ac05b4b757" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>NoiseModelFactor2</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor2_1a35453b054509f5679a29a06b59340cf1" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>NoiseModelFactor2</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a7c719ae67f222f66569ccb3b526cb9cb" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a8a7610dba7e9a6f878268c2769ea0e04" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a682e0bbaf0c9638b7d0d96148082d1d7" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor2</scope><name>print</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a95f4d57e8646b6df634496aaabb683b5" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a5f760600e8608ef15c825a0f8d1d1855" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor2_1ae600c15f59be175aca5b059e488d7166" prot="private" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>serialize</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a57d98d52fb6f91ebc54d74746aeb90e4" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>shared_ptr</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor2_1a2743c4c486e80127e8eca591ee3ffbed" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>This</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor2_1a9640bbbb433383f0b40b5c2d9439415e" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor2</scope><name>unwhitenedError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a1b3ea51380315bfd63f60d8d9195b0c1" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>whitenedError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor2_1a5fb0fe8b056bad2171f84922c5184ef1" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>X1</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor2_1a3b3777f359b34892f33784d73cb62234" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor2</scope><name>X2</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ae1ed5e0fa6c471b57172f7ebaf552187" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor2</scope><name>~NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor2_1ab629a5dc71461efe61d68faaf5a659f3" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor2</scope><name>~NoiseModelFactor2</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1ac457810c504de58732d51d61f715c2de" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor2</scope><name>~NonlinearFactor</name></member>
+    </listofallmembers>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor3.xml b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor3.xml
new file mode 100644
index 000000000..78d4292d6
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor3.xml
@@ -0,0 +1,405 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="classgtsam_1_1NoiseModelFactor3" kind="class" language="C++" prot="public" abstract="yes">
+    <compoundname>gtsam::NoiseModelFactor3</compoundname>
+    <basecompoundref refid="classgtsam_1_1NoiseModelFactor" prot="public" virt="non-virtual">gtsam::NoiseModelFactor</basecompoundref>
+    <includes refid="NonlinearFactor_8h" local="no">NonlinearFactor.h</includes>
+    <templateparamlist>
+      <param>
+        <type>class VALUE1</type>
+      </param>
+      <param>
+        <type>class VALUE2</type>
+      </param>
+      <param>
+        <type>class VALUE3</type>
+      </param>
+    </templateparamlist>
+      <sectiondef kind="public-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor3_1a85e11c8ae6f70e0bdf17f9b6405e6501" prot="public" static="no">
+        <type>VALUE1</type>
+        <definition>typedef VALUE1 gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;::X1</definition>
+        <argsstring></argsstring>
+        <name>X1</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="425" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="425" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor3_1aaa8424af6ceb0ce3458efe56d52d5187" prot="public" static="no">
+        <type>VALUE2</type>
+        <definition>typedef VALUE2 gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;::X2</definition>
+        <argsstring></argsstring>
+        <name>X2</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="426" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="426" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor3_1a9924ba92623c9a10a286ddd1a8b33f2e" prot="public" static="no">
+        <type>VALUE3</type>
+        <definition>typedef VALUE3 gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;::X3</definition>
+        <argsstring></argsstring>
+        <name>X3</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="427" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="427" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="protected-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor3_1a45c5f50294af1756fac4d4f59b7e31e5" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref></type>
+        <definition>typedef NoiseModelFactor gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;::Base</definition>
+        <argsstring></argsstring>
+        <name>Base</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="431" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="431" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor3_1a91a0dfdc66913a71ba5daea0ac681745" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NoiseModelFactor3" kindref="compound">NoiseModelFactor3</ref>&lt; VALUE1, VALUE2, VALUE3 &gt;</type>
+        <definition>typedef NoiseModelFactor3&lt;VALUE1, VALUE2, VALUE3&gt; gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;::This</definition>
+        <argsstring></argsstring>
+        <name>This</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="432" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="432" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="friend">
+      <memberdef kind="friend" id="classgtsam_1_1NoiseModelFactor3_1ac98d07dd8f7b70e16ccb9a01abf56b9c" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual">
+        <type>friend class</type>
+        <definition>friend class boost::serialization::access</definition>
+        <argsstring></argsstring>
+        <name>boost::serialization::access</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Serialization function </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="485" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="485" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="public-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor3_1a0bf4383cea2cdb0d263e570e98499ac7" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;::NoiseModelFactor3</definition>
+        <argsstring>()</argsstring>
+        <name>NoiseModelFactor3</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Default Constructor for I/O </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="439" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="439" bodyend="439"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor3_1ac2a8e8247e7d55768915fac27a4c078c" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;::NoiseModelFactor3</definition>
+        <argsstring>(const SharedNoiseModel &amp;noiseModel, Key j1, Key j2, Key j3)</argsstring>
+        <name>NoiseModelFactor3</name>
+        <param>
+          <type>const SharedNoiseModel &amp;</type>
+          <declname>noiseModel</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j1</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j2</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j3</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Constructor <parameterlist kind="param"><parameteritem>
+<parameternamelist>
+<parametername>noiseModel</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>shared pointer to noise model </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j1</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the first variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j2</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the second variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j3</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the third variable </para></parameterdescription>
+</parameteritem>
+</parameterlist>
+</para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="448" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="448" bodyend="449"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor3_1a4b4dcb0572c11a2f920b67d0ca7ac8b7" prot="public" static="no" const="no" explicit="no" inline="yes" virt="virtual">
+        <type></type>
+        <definition>virtual gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;::~NoiseModelFactor3</definition>
+        <argsstring>()</argsstring>
+        <name>~NoiseModelFactor3</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="451" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="451" bodyend="451"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor3_1a535f2bec61538509696513b2ab6bbf0b" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;::key1</definition>
+        <argsstring>() const </argsstring>
+        <name>key1</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>methods to retrieve keys </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="454" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="454" bodyend="454"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor3_1af049e3934c4588d78810051272bdd995" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;::key2</definition>
+        <argsstring>() const </argsstring>
+        <name>key2</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="455" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="455" bodyend="455"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor3_1ae8722af2eb7ac282ba82e49fca1264e1" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;::key3</definition>
+        <argsstring>() const </argsstring>
+        <name>key3</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="456" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="456" bodyend="456"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor3_1aab8e3316ef77cecc1bd1909a2c0eed33" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="virtual">
+        <type>Vector</type>
+        <definition>virtual Vector gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;::unwhitenedError</definition>
+        <argsstring>(const Values &amp;x, boost::optional&lt; std::vector&lt; Matrix &gt; &amp; &gt; H=boost::none) const </argsstring>
+        <name>unwhitenedError</name>
+        <reimplements refid="classgtsam_1_1NoiseModelFactor_1afd61874915a8076bf83f9d8311fc0e26">unwhitenedError</reimplements>
+        <param>
+          <type>const Values &amp;</type>
+          <declname>x</declname>
+        </param>
+        <param>
+          <type>boost::optional&lt; std::vector&lt; Matrix &gt; &amp; &gt;</type>
+          <declname>H</declname>
+          <defval>boost::none</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Calls the 3-key specific version of evaluateError, which is pure virtual so must be implemented in the derived class. </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="460" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="460" bodyend="469"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor3_1a78613881d348f96be1a158b9bb812950" prot="public" static="no" const="yes" explicit="no" inline="no" virt="pure-virtual">
+        <type>Vector</type>
+        <definition>virtual Vector gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;::evaluateError</definition>
+        <argsstring>(const X1 &amp;, const X2 &amp;, const X3 &amp;, boost::optional&lt; Matrix &amp; &gt; H1=boost::none, boost::optional&lt; Matrix &amp; &gt; H2=boost::none, boost::optional&lt; Matrix &amp; &gt; H3=boost::none) const =0</argsstring>
+        <name>evaluateError</name>
+        <param>
+          <type>const X1 &amp;</type>
+        </param>
+        <param>
+          <type>const X2 &amp;</type>
+        </param>
+        <param>
+          <type>const X3 &amp;</type>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H1</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H2</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H3</declname>
+          <defval>boost::none</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Override this method to finish implementing a trinary factor. If any of the optional Matrix reference arguments are specified, it should compute both the function evaluation and its derivative(s) in X1 (and/or X2, X3). </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="477" column="1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="private-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor3_1a61eb4f72c087d840f4c66db44772437b" prot="private" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <templateparamlist>
+          <param>
+            <type>class ARCHIVE</type>
+          </param>
+        </templateparamlist>
+        <type>void</type>
+        <definition>void gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;::serialize</definition>
+        <argsstring>(ARCHIVE &amp;ar, const unsigned int)</argsstring>
+        <name>serialize</name>
+        <param>
+          <type>ARCHIVE &amp;</type>
+          <declname>ar</declname>
+        </param>
+        <param>
+          <type>const unsigned</type>
+          <declname>int</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="487" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="487" bodyend="490"/>
+      </memberdef>
+      </sectiondef>
+    <briefdescription>
+    </briefdescription>
+    <detaileddescription>
+<para>A convenient base class for creating your own <ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref> with 3 variables. To derive from this class, implement <ref refid="classgtsam_1_1NoiseModelFactor3_1a78613881d348f96be1a158b9bb812950" kindref="member">evaluateError()</ref>. </para>    </detaileddescription>
+    <inheritancegraph>
+      <node id="118">
+        <label>Factor</label>
+      </node>
+      <node id="117">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="118" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="116">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="117" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="115">
+        <label>gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor3"/>
+        <childnode refid="116" relation="public-inheritance">
+        </childnode>
+      </node>
+    </inheritancegraph>
+    <collaborationgraph>
+      <node id="122">
+        <label>Factor</label>
+      </node>
+      <node id="121">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="122" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="120">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="121" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="119">
+        <label>gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor3"/>
+        <childnode refid="120" relation="public-inheritance">
+        </childnode>
+      </node>
+    </collaborationgraph>
+    <location file="tests/doc-test-files/NonlinearFactor.h" line="420" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="420" bodyend="491"/>
+    <listofallmembers>
+      <member refid="classgtsam_1_1NonlinearFactor_1a1968f471dc3ea33aaf0226ff7be5ae8c" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor3</scope><name>active</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1a45c5f50294af1756fac4d4f59b7e31e5" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>Base</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1ac98d07dd8f7b70e16ccb9a01abf56b9c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>boost::serialization::access</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1aa98a30a78ade8489214ce2e5b8302777" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor3</scope><name>clone</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1af4f62b52fff1a0527ecdb279de023728" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor3</scope><name>dim</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a616d0335db512e9c333cab83b1923399" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor3</scope><name>equals</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1af6147aff3a33f734435601f6dcafeaf9" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor3</scope><name>error</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1a78613881d348f96be1a158b9bb812950" prot="public" virt="pure-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>evaluateError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a16a8a23069ce6ae664f516edc8f36254" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>get_noiseModel</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1a535f2bec61538509696513b2ab6bbf0b" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>key1</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1af049e3934c4588d78810051272bdd995" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>key2</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1ae8722af2eb7ac282ba82e49fca1264e1" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>key3</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a0c83582a0bd04e112e4c0a17cd4ed324" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor3</scope><name>linearize</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a123e5729d70b4acd6d7e1f70b1a7ced4" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>noiseModel</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a2263eabbefe0eede8c9d10486f61bb4d" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>noiseModel_</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ad6e8ef3348e042d598617d1d55f20300" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a02cea97db30d27b51feaf1f0444d743c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a4605224417029e9dc58c5267efa9a46d" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1a0bf4383cea2cdb0d263e570e98499ac7" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>NoiseModelFactor3</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1ac2a8e8247e7d55768915fac27a4c078c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>NoiseModelFactor3</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a7c719ae67f222f66569ccb3b526cb9cb" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a8a7610dba7e9a6f878268c2769ea0e04" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a682e0bbaf0c9638b7d0d96148082d1d7" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor3</scope><name>print</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a95f4d57e8646b6df634496aaabb683b5" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a5f760600e8608ef15c825a0f8d1d1855" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1a61eb4f72c087d840f4c66db44772437b" prot="private" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>serialize</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a57d98d52fb6f91ebc54d74746aeb90e4" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>shared_ptr</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1a91a0dfdc66913a71ba5daea0ac681745" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>This</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1aab8e3316ef77cecc1bd1909a2c0eed33" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor3</scope><name>unwhitenedError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a1b3ea51380315bfd63f60d8d9195b0c1" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>whitenedError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1a85e11c8ae6f70e0bdf17f9b6405e6501" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>X1</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1aaa8424af6ceb0ce3458efe56d52d5187" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>X2</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1a9924ba92623c9a10a286ddd1a8b33f2e" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor3</scope><name>X3</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ae1ed5e0fa6c471b57172f7ebaf552187" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor3</scope><name>~NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor3_1a4b4dcb0572c11a2f920b67d0ca7ac8b7" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor3</scope><name>~NoiseModelFactor3</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1ac457810c504de58732d51d61f715c2de" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor3</scope><name>~NonlinearFactor</name></member>
+    </listofallmembers>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor4.xml b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor4.xml
new file mode 100644
index 000000000..a78100793
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor4.xml
@@ -0,0 +1,455 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="classgtsam_1_1NoiseModelFactor4" kind="class" language="C++" prot="public" abstract="yes">
+    <compoundname>gtsam::NoiseModelFactor4</compoundname>
+    <basecompoundref refid="classgtsam_1_1NoiseModelFactor" prot="public" virt="non-virtual">gtsam::NoiseModelFactor</basecompoundref>
+    <includes refid="NonlinearFactor_8h" local="no">NonlinearFactor.h</includes>
+    <templateparamlist>
+      <param>
+        <type>class VALUE1</type>
+      </param>
+      <param>
+        <type>class VALUE2</type>
+      </param>
+      <param>
+        <type>class VALUE3</type>
+      </param>
+      <param>
+        <type>class VALUE4</type>
+      </param>
+    </templateparamlist>
+      <sectiondef kind="public-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor4_1ae43138cc390ef4b2956e334f160df42e" prot="public" static="no">
+        <type>VALUE1</type>
+        <definition>typedef VALUE1 gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::X1</definition>
+        <argsstring></argsstring>
+        <name>X1</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="502" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="502" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor4_1aafd5bc6e588c7e1084897814831713c5" prot="public" static="no">
+        <type>VALUE2</type>
+        <definition>typedef VALUE2 gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::X2</definition>
+        <argsstring></argsstring>
+        <name>X2</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="503" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="503" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor4_1aa07132c1d7f42154fafd026c702ca240" prot="public" static="no">
+        <type>VALUE3</type>
+        <definition>typedef VALUE3 gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::X3</definition>
+        <argsstring></argsstring>
+        <name>X3</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="504" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="504" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor4_1a6ca1a804d790e17f108c8e14df2c5077" prot="public" static="no">
+        <type>VALUE4</type>
+        <definition>typedef VALUE4 gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::X4</definition>
+        <argsstring></argsstring>
+        <name>X4</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="505" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="505" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="protected-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor4_1a846f05b113e5553fe7b42a9a49906f98" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref></type>
+        <definition>typedef NoiseModelFactor gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::Base</definition>
+        <argsstring></argsstring>
+        <name>Base</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="509" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="509" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor4_1a372757bbf75dee8eaf33b479b31f06a1" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NoiseModelFactor4" kindref="compound">NoiseModelFactor4</ref>&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;</type>
+        <definition>typedef NoiseModelFactor4&lt;VALUE1, VALUE2, VALUE3, VALUE4&gt; gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::This</definition>
+        <argsstring></argsstring>
+        <name>This</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="510" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="510" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="friend">
+      <memberdef kind="friend" id="classgtsam_1_1NoiseModelFactor4_1ac98d07dd8f7b70e16ccb9a01abf56b9c" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual">
+        <type>friend class</type>
+        <definition>friend class boost::serialization::access</definition>
+        <argsstring></argsstring>
+        <name>boost::serialization::access</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Serialization function </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="566" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="566" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="public-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor4_1a65adc8d1272df63390168b26f4f7bc70" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::NoiseModelFactor4</definition>
+        <argsstring>()</argsstring>
+        <name>NoiseModelFactor4</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Default Constructor for I/O </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="517" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="517" bodyend="517"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor4_1a8ce3daa05897e12db9e38a9c96043ce3" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::NoiseModelFactor4</definition>
+        <argsstring>(const SharedNoiseModel &amp;noiseModel, Key j1, Key j2, Key j3, Key j4)</argsstring>
+        <name>NoiseModelFactor4</name>
+        <param>
+          <type>const SharedNoiseModel &amp;</type>
+          <declname>noiseModel</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j1</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j2</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j3</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j4</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Constructor <parameterlist kind="param"><parameteritem>
+<parameternamelist>
+<parametername>noiseModel</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>shared pointer to noise model </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j1</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the first variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j2</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the second variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j3</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the third variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j4</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the fourth variable </para></parameterdescription>
+</parameteritem>
+</parameterlist>
+</para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="527" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="527" bodyend="528"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor4_1a3e53af328261232767713e26fa0d35d5" prot="public" static="no" const="no" explicit="no" inline="yes" virt="virtual">
+        <type></type>
+        <definition>virtual gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::~NoiseModelFactor4</definition>
+        <argsstring>()</argsstring>
+        <name>~NoiseModelFactor4</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="530" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="530" bodyend="530"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor4_1a4339880be87fa2a533aa07324481f7b6" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::key1</definition>
+        <argsstring>() const </argsstring>
+        <name>key1</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>methods to retrieve keys </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="533" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="533" bodyend="533"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor4_1acb1d3940d9b5ee1aa7c9c3c3c1a21ed9" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::key2</definition>
+        <argsstring>() const </argsstring>
+        <name>key2</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="534" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="534" bodyend="534"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor4_1ac1111c7becb0904f141dc07eff1a4e7c" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::key3</definition>
+        <argsstring>() const </argsstring>
+        <name>key3</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="535" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="535" bodyend="535"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor4_1a6d9ee65e09dfe61a663c60d97f01b341" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::key4</definition>
+        <argsstring>() const </argsstring>
+        <name>key4</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="536" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="536" bodyend="536"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor4_1a6a79ae22fc40447c8dc183a951243440" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="virtual">
+        <type>Vector</type>
+        <definition>virtual Vector gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::unwhitenedError</definition>
+        <argsstring>(const Values &amp;x, boost::optional&lt; std::vector&lt; Matrix &gt; &amp; &gt; H=boost::none) const </argsstring>
+        <name>unwhitenedError</name>
+        <reimplements refid="classgtsam_1_1NoiseModelFactor_1afd61874915a8076bf83f9d8311fc0e26">unwhitenedError</reimplements>
+        <param>
+          <type>const Values &amp;</type>
+          <declname>x</declname>
+        </param>
+        <param>
+          <type>boost::optional&lt; std::vector&lt; Matrix &gt; &amp; &gt;</type>
+          <declname>H</declname>
+          <defval>boost::none</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Calls the 4-key specific version of evaluateError, which is pure virtual so must be implemented in the derived class. </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="540" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="540" bodyend="549"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor4_1a5e4197ee6a113e77d2abc1dbbebb0857" prot="public" static="no" const="yes" explicit="no" inline="no" virt="pure-virtual">
+        <type>Vector</type>
+        <definition>virtual Vector gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::evaluateError</definition>
+        <argsstring>(const X1 &amp;, const X2 &amp;, const X3 &amp;, const X4 &amp;, boost::optional&lt; Matrix &amp; &gt; H1=boost::none, boost::optional&lt; Matrix &amp; &gt; H2=boost::none, boost::optional&lt; Matrix &amp; &gt; H3=boost::none, boost::optional&lt; Matrix &amp; &gt; H4=boost::none) const =0</argsstring>
+        <name>evaluateError</name>
+        <param>
+          <type>const X1 &amp;</type>
+        </param>
+        <param>
+          <type>const X2 &amp;</type>
+        </param>
+        <param>
+          <type>const X3 &amp;</type>
+        </param>
+        <param>
+          <type>const X4 &amp;</type>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H1</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H2</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H3</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H4</declname>
+          <defval>boost::none</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Override this method to finish implementing a 4-way factor. If any of the optional Matrix reference arguments are specified, it should compute both the function evaluation and its derivative(s) in X1 (and/or X2, X3). </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="557" column="1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="private-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor4_1a51fda42bdf2b1b442b5a8ed204386816" prot="private" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <templateparamlist>
+          <param>
+            <type>class ARCHIVE</type>
+          </param>
+        </templateparamlist>
+        <type>void</type>
+        <definition>void gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;::serialize</definition>
+        <argsstring>(ARCHIVE &amp;ar, const unsigned int)</argsstring>
+        <name>serialize</name>
+        <param>
+          <type>ARCHIVE &amp;</type>
+          <declname>ar</declname>
+        </param>
+        <param>
+          <type>const unsigned</type>
+          <declname>int</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="568" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="568" bodyend="571"/>
+      </memberdef>
+      </sectiondef>
+    <briefdescription>
+    </briefdescription>
+    <detaileddescription>
+<para>A convenient base class for creating your own <ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref> with 4 variables. To derive from this class, implement <ref refid="classgtsam_1_1NoiseModelFactor4_1a5e4197ee6a113e77d2abc1dbbebb0857" kindref="member">evaluateError()</ref>. </para>    </detaileddescription>
+    <inheritancegraph>
+      <node id="126">
+        <label>Factor</label>
+      </node>
+      <node id="123">
+        <label>gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor4"/>
+        <childnode refid="124" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="125">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="126" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="124">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="125" relation="public-inheritance">
+        </childnode>
+      </node>
+    </inheritancegraph>
+    <collaborationgraph>
+      <node id="130">
+        <label>Factor</label>
+      </node>
+      <node id="127">
+        <label>gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor4"/>
+        <childnode refid="128" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="129">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="130" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="128">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="129" relation="public-inheritance">
+        </childnode>
+      </node>
+    </collaborationgraph>
+    <location file="tests/doc-test-files/NonlinearFactor.h" line="497" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="497" bodyend="572"/>
+    <listofallmembers>
+      <member refid="classgtsam_1_1NonlinearFactor_1a1968f471dc3ea33aaf0226ff7be5ae8c" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor4</scope><name>active</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1a846f05b113e5553fe7b42a9a49906f98" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>Base</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1ac98d07dd8f7b70e16ccb9a01abf56b9c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>boost::serialization::access</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1aa98a30a78ade8489214ce2e5b8302777" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor4</scope><name>clone</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1af4f62b52fff1a0527ecdb279de023728" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor4</scope><name>dim</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a616d0335db512e9c333cab83b1923399" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor4</scope><name>equals</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1af6147aff3a33f734435601f6dcafeaf9" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor4</scope><name>error</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1a5e4197ee6a113e77d2abc1dbbebb0857" prot="public" virt="pure-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>evaluateError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a16a8a23069ce6ae664f516edc8f36254" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>get_noiseModel</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1a4339880be87fa2a533aa07324481f7b6" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>key1</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1acb1d3940d9b5ee1aa7c9c3c3c1a21ed9" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>key2</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1ac1111c7becb0904f141dc07eff1a4e7c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>key3</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1a6d9ee65e09dfe61a663c60d97f01b341" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>key4</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a0c83582a0bd04e112e4c0a17cd4ed324" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor4</scope><name>linearize</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a123e5729d70b4acd6d7e1f70b1a7ced4" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>noiseModel</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a2263eabbefe0eede8c9d10486f61bb4d" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>noiseModel_</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ad6e8ef3348e042d598617d1d55f20300" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a02cea97db30d27b51feaf1f0444d743c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a4605224417029e9dc58c5267efa9a46d" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1a65adc8d1272df63390168b26f4f7bc70" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>NoiseModelFactor4</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1a8ce3daa05897e12db9e38a9c96043ce3" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>NoiseModelFactor4</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a7c719ae67f222f66569ccb3b526cb9cb" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a8a7610dba7e9a6f878268c2769ea0e04" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a682e0bbaf0c9638b7d0d96148082d1d7" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor4</scope><name>print</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a95f4d57e8646b6df634496aaabb683b5" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a5f760600e8608ef15c825a0f8d1d1855" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1a51fda42bdf2b1b442b5a8ed204386816" prot="private" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>serialize</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a57d98d52fb6f91ebc54d74746aeb90e4" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>shared_ptr</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1a372757bbf75dee8eaf33b479b31f06a1" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>This</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1a6a79ae22fc40447c8dc183a951243440" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor4</scope><name>unwhitenedError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a1b3ea51380315bfd63f60d8d9195b0c1" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>whitenedError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1ae43138cc390ef4b2956e334f160df42e" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>X1</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1aafd5bc6e588c7e1084897814831713c5" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>X2</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1aa07132c1d7f42154fafd026c702ca240" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>X3</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1a6ca1a804d790e17f108c8e14df2c5077" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor4</scope><name>X4</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ae1ed5e0fa6c471b57172f7ebaf552187" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor4</scope><name>~NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor4_1a3e53af328261232767713e26fa0d35d5" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor4</scope><name>~NoiseModelFactor4</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1ac457810c504de58732d51d61f715c2de" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor4</scope><name>~NonlinearFactor</name></member>
+    </listofallmembers>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor5.xml b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor5.xml
new file mode 100644
index 000000000..e40b636fb
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor5.xml
@@ -0,0 +1,505 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="classgtsam_1_1NoiseModelFactor5" kind="class" language="C++" prot="public" abstract="yes">
+    <compoundname>gtsam::NoiseModelFactor5</compoundname>
+    <basecompoundref refid="classgtsam_1_1NoiseModelFactor" prot="public" virt="non-virtual">gtsam::NoiseModelFactor</basecompoundref>
+    <includes refid="NonlinearFactor_8h" local="no">NonlinearFactor.h</includes>
+    <templateparamlist>
+      <param>
+        <type>class VALUE1</type>
+      </param>
+      <param>
+        <type>class VALUE2</type>
+      </param>
+      <param>
+        <type>class VALUE3</type>
+      </param>
+      <param>
+        <type>class VALUE4</type>
+      </param>
+      <param>
+        <type>class VALUE5</type>
+      </param>
+    </templateparamlist>
+      <sectiondef kind="public-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor5_1a514869f1357a9e05118bfab0c6fa97e2" prot="public" static="no">
+        <type>VALUE1</type>
+        <definition>typedef VALUE1 gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::X1</definition>
+        <argsstring></argsstring>
+        <name>X1</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="583" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="583" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor5_1ad9181205117e8efb56ebe4f297645d3e" prot="public" static="no">
+        <type>VALUE2</type>
+        <definition>typedef VALUE2 gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::X2</definition>
+        <argsstring></argsstring>
+        <name>X2</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="584" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="584" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor5_1aeb3efb9dbda7a2fef53c55a4513ab4ec" prot="public" static="no">
+        <type>VALUE3</type>
+        <definition>typedef VALUE3 gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::X3</definition>
+        <argsstring></argsstring>
+        <name>X3</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="585" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="585" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor5_1a4e272b4742d0477f444995ec73e097c7" prot="public" static="no">
+        <type>VALUE4</type>
+        <definition>typedef VALUE4 gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::X4</definition>
+        <argsstring></argsstring>
+        <name>X4</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="586" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="586" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor5_1afaf024c990892895e5f5ed0b34758f27" prot="public" static="no">
+        <type>VALUE5</type>
+        <definition>typedef VALUE5 gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::X5</definition>
+        <argsstring></argsstring>
+        <name>X5</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="587" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="587" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="protected-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor5_1a594617cffc3d1a9985cd64e34936a7f5" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref></type>
+        <definition>typedef NoiseModelFactor gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::Base</definition>
+        <argsstring></argsstring>
+        <name>Base</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="591" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="591" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor5_1a19020d6d909366475872911b6dfc16df" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NoiseModelFactor5" kindref="compound">NoiseModelFactor5</ref>&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;</type>
+        <definition>typedef NoiseModelFactor5&lt;VALUE1, VALUE2, VALUE3, VALUE4, VALUE5&gt; gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::This</definition>
+        <argsstring></argsstring>
+        <name>This</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="592" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="592" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="friend">
+      <memberdef kind="friend" id="classgtsam_1_1NoiseModelFactor5_1ac98d07dd8f7b70e16ccb9a01abf56b9c" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual">
+        <type>friend class</type>
+        <definition>friend class boost::serialization::access</definition>
+        <argsstring></argsstring>
+        <name>boost::serialization::access</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Serialization function </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="651" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="651" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="public-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor5_1a424c4633f6c1907d0dd3381cffbed82a" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::NoiseModelFactor5</definition>
+        <argsstring>()</argsstring>
+        <name>NoiseModelFactor5</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Default Constructor for I/O </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="599" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="599" bodyend="599"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor5_1a01d2e8a5253a67ceac02320a719c5280" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::NoiseModelFactor5</definition>
+        <argsstring>(const SharedNoiseModel &amp;noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5)</argsstring>
+        <name>NoiseModelFactor5</name>
+        <param>
+          <type>const SharedNoiseModel &amp;</type>
+          <declname>noiseModel</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j1</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j2</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j3</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j4</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j5</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Constructor <parameterlist kind="param"><parameteritem>
+<parameternamelist>
+<parametername>noiseModel</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>shared pointer to noise model </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j1</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the first variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j2</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the second variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j3</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the third variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j4</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the fourth variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j5</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the fifth variable </para></parameterdescription>
+</parameteritem>
+</parameterlist>
+</para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="610" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="610" bodyend="611"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor5_1ae16654ff529ddcfedd1b394f7716412d" prot="public" static="no" const="no" explicit="no" inline="yes" virt="virtual">
+        <type></type>
+        <definition>virtual gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::~NoiseModelFactor5</definition>
+        <argsstring>()</argsstring>
+        <name>~NoiseModelFactor5</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="613" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="613" bodyend="613"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor5_1a23886196ba086677b1e2168e17a34280" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::key1</definition>
+        <argsstring>() const </argsstring>
+        <name>key1</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>methods to retrieve keys </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="616" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="616" bodyend="616"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor5_1a464749bd22a65ee29f99452f194eaf54" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::key2</definition>
+        <argsstring>() const </argsstring>
+        <name>key2</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="617" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="617" bodyend="617"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor5_1a028c4fbddfe09a66ac3780e23bd25fcd" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::key3</definition>
+        <argsstring>() const </argsstring>
+        <name>key3</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="618" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="618" bodyend="618"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor5_1aac1933a0fa685709867f12cb636c98d3" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::key4</definition>
+        <argsstring>() const </argsstring>
+        <name>key4</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="619" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="619" bodyend="619"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor5_1a529d24a558fe3e6bd1dc4839c9778f0c" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::key5</definition>
+        <argsstring>() const </argsstring>
+        <name>key5</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="620" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="620" bodyend="620"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor5_1a701b7a764f18310b4eea14ee023c922b" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="virtual">
+        <type>Vector</type>
+        <definition>virtual Vector gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::unwhitenedError</definition>
+        <argsstring>(const Values &amp;x, boost::optional&lt; std::vector&lt; Matrix &gt; &amp; &gt; H=boost::none) const </argsstring>
+        <name>unwhitenedError</name>
+        <reimplements refid="classgtsam_1_1NoiseModelFactor_1afd61874915a8076bf83f9d8311fc0e26">unwhitenedError</reimplements>
+        <param>
+          <type>const Values &amp;</type>
+          <declname>x</declname>
+        </param>
+        <param>
+          <type>boost::optional&lt; std::vector&lt; Matrix &gt; &amp; &gt;</type>
+          <declname>H</declname>
+          <defval>boost::none</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Calls the 5-key specific version of evaluateError, which is pure virtual so must be implemented in the derived class. </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="624" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="624" bodyend="633"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor5_1af0adaca3d551e42404aaf98c3053b009" prot="public" static="no" const="yes" explicit="no" inline="no" virt="pure-virtual">
+        <type>Vector</type>
+        <definition>virtual Vector gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::evaluateError</definition>
+        <argsstring>(const X1 &amp;, const X2 &amp;, const X3 &amp;, const X4 &amp;, const X5 &amp;, boost::optional&lt; Matrix &amp; &gt; H1=boost::none, boost::optional&lt; Matrix &amp; &gt; H2=boost::none, boost::optional&lt; Matrix &amp; &gt; H3=boost::none, boost::optional&lt; Matrix &amp; &gt; H4=boost::none, boost::optional&lt; Matrix &amp; &gt; H5=boost::none) const =0</argsstring>
+        <name>evaluateError</name>
+        <param>
+          <type>const X1 &amp;</type>
+        </param>
+        <param>
+          <type>const X2 &amp;</type>
+        </param>
+        <param>
+          <type>const X3 &amp;</type>
+        </param>
+        <param>
+          <type>const X4 &amp;</type>
+        </param>
+        <param>
+          <type>const X5 &amp;</type>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H1</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H2</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H3</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H4</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H5</declname>
+          <defval>boost::none</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Override this method to finish implementing a 5-way factor. If any of the optional Matrix reference arguments are specified, it should compute both the function evaluation and its derivative(s) in X1 (and/or X2, X3). </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="641" column="1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="private-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor5_1a02db3a5d618d3738c5ec277626a2b598" prot="private" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <templateparamlist>
+          <param>
+            <type>class ARCHIVE</type>
+          </param>
+        </templateparamlist>
+        <type>void</type>
+        <definition>void gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;::serialize</definition>
+        <argsstring>(ARCHIVE &amp;ar, const unsigned int)</argsstring>
+        <name>serialize</name>
+        <param>
+          <type>ARCHIVE &amp;</type>
+          <declname>ar</declname>
+        </param>
+        <param>
+          <type>const unsigned</type>
+          <declname>int</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="653" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="653" bodyend="656"/>
+      </memberdef>
+      </sectiondef>
+    <briefdescription>
+    </briefdescription>
+    <detaileddescription>
+<para>A convenient base class for creating your own <ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref> with 5 variables. To derive from this class, implement <ref refid="classgtsam_1_1NoiseModelFactor5_1af0adaca3d551e42404aaf98c3053b009" kindref="member">evaluateError()</ref>. </para>    </detaileddescription>
+    <inheritancegraph>
+      <node id="134">
+        <label>Factor</label>
+      </node>
+      <node id="131">
+        <label>gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor5"/>
+        <childnode refid="132" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="133">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="134" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="132">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="133" relation="public-inheritance">
+        </childnode>
+      </node>
+    </inheritancegraph>
+    <collaborationgraph>
+      <node id="138">
+        <label>Factor</label>
+      </node>
+      <node id="135">
+        <label>gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor5"/>
+        <childnode refid="136" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="137">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="138" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="136">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="137" relation="public-inheritance">
+        </childnode>
+      </node>
+    </collaborationgraph>
+    <location file="tests/doc-test-files/NonlinearFactor.h" line="578" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="578" bodyend="657"/>
+    <listofallmembers>
+      <member refid="classgtsam_1_1NonlinearFactor_1a1968f471dc3ea33aaf0226ff7be5ae8c" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor5</scope><name>active</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1a594617cffc3d1a9985cd64e34936a7f5" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>Base</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1ac98d07dd8f7b70e16ccb9a01abf56b9c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>boost::serialization::access</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1aa98a30a78ade8489214ce2e5b8302777" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor5</scope><name>clone</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1af4f62b52fff1a0527ecdb279de023728" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor5</scope><name>dim</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a616d0335db512e9c333cab83b1923399" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor5</scope><name>equals</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1af6147aff3a33f734435601f6dcafeaf9" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor5</scope><name>error</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1af0adaca3d551e42404aaf98c3053b009" prot="public" virt="pure-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>evaluateError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a16a8a23069ce6ae664f516edc8f36254" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>get_noiseModel</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1a23886196ba086677b1e2168e17a34280" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>key1</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1a464749bd22a65ee29f99452f194eaf54" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>key2</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1a028c4fbddfe09a66ac3780e23bd25fcd" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>key3</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1aac1933a0fa685709867f12cb636c98d3" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>key4</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1a529d24a558fe3e6bd1dc4839c9778f0c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>key5</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a0c83582a0bd04e112e4c0a17cd4ed324" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor5</scope><name>linearize</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a123e5729d70b4acd6d7e1f70b1a7ced4" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>noiseModel</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a2263eabbefe0eede8c9d10486f61bb4d" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>noiseModel_</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ad6e8ef3348e042d598617d1d55f20300" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a02cea97db30d27b51feaf1f0444d743c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a4605224417029e9dc58c5267efa9a46d" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1a424c4633f6c1907d0dd3381cffbed82a" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>NoiseModelFactor5</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1a01d2e8a5253a67ceac02320a719c5280" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>NoiseModelFactor5</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a7c719ae67f222f66569ccb3b526cb9cb" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a8a7610dba7e9a6f878268c2769ea0e04" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a682e0bbaf0c9638b7d0d96148082d1d7" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor5</scope><name>print</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a95f4d57e8646b6df634496aaabb683b5" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a5f760600e8608ef15c825a0f8d1d1855" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1a02db3a5d618d3738c5ec277626a2b598" prot="private" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>serialize</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a57d98d52fb6f91ebc54d74746aeb90e4" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>shared_ptr</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1a19020d6d909366475872911b6dfc16df" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>This</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1a701b7a764f18310b4eea14ee023c922b" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor5</scope><name>unwhitenedError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a1b3ea51380315bfd63f60d8d9195b0c1" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>whitenedError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1a514869f1357a9e05118bfab0c6fa97e2" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>X1</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1ad9181205117e8efb56ebe4f297645d3e" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>X2</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1aeb3efb9dbda7a2fef53c55a4513ab4ec" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>X3</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1a4e272b4742d0477f444995ec73e097c7" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>X4</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1afaf024c990892895e5f5ed0b34758f27" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor5</scope><name>X5</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ae1ed5e0fa6c471b57172f7ebaf552187" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor5</scope><name>~NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor5_1ae16654ff529ddcfedd1b394f7716412d" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor5</scope><name>~NoiseModelFactor5</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1ac457810c504de58732d51d61f715c2de" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor5</scope><name>~NonlinearFactor</name></member>
+    </listofallmembers>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor6.xml b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor6.xml
new file mode 100644
index 000000000..f5751fe32
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NoiseModelFactor6.xml
@@ -0,0 +1,555 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="classgtsam_1_1NoiseModelFactor6" kind="class" language="C++" prot="public" abstract="yes">
+    <compoundname>gtsam::NoiseModelFactor6</compoundname>
+    <basecompoundref refid="classgtsam_1_1NoiseModelFactor" prot="public" virt="non-virtual">gtsam::NoiseModelFactor</basecompoundref>
+    <includes refid="NonlinearFactor_8h" local="no">NonlinearFactor.h</includes>
+    <templateparamlist>
+      <param>
+        <type>class VALUE1</type>
+      </param>
+      <param>
+        <type>class VALUE2</type>
+      </param>
+      <param>
+        <type>class VALUE3</type>
+      </param>
+      <param>
+        <type>class VALUE4</type>
+      </param>
+      <param>
+        <type>class VALUE5</type>
+      </param>
+      <param>
+        <type>class VALUE6</type>
+      </param>
+    </templateparamlist>
+      <sectiondef kind="public-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor6_1a369d9accd53012d84bbbaf87b176222b" prot="public" static="no">
+        <type>VALUE1</type>
+        <definition>typedef VALUE1 gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::X1</definition>
+        <argsstring></argsstring>
+        <name>X1</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="668" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="668" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor6_1a6107bc96fff5d12783429158e79b9390" prot="public" static="no">
+        <type>VALUE2</type>
+        <definition>typedef VALUE2 gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::X2</definition>
+        <argsstring></argsstring>
+        <name>X2</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="669" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="669" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor6_1a76c477f40c2b911a9f28d456ea9164a2" prot="public" static="no">
+        <type>VALUE3</type>
+        <definition>typedef VALUE3 gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::X3</definition>
+        <argsstring></argsstring>
+        <name>X3</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="670" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="670" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor6_1a2a90aff22e4bd88dae81bf824956a113" prot="public" static="no">
+        <type>VALUE4</type>
+        <definition>typedef VALUE4 gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::X4</definition>
+        <argsstring></argsstring>
+        <name>X4</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="671" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="671" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor6_1ae13e19a4b3a7a331d04d5b460e73216f" prot="public" static="no">
+        <type>VALUE5</type>
+        <definition>typedef VALUE5 gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::X5</definition>
+        <argsstring></argsstring>
+        <name>X5</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="672" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="672" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor6_1af44204ef6947a65b973dee5707495601" prot="public" static="no">
+        <type>VALUE6</type>
+        <definition>typedef VALUE6 gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::X6</definition>
+        <argsstring></argsstring>
+        <name>X6</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="673" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="673" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="protected-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor6_1ad4a36f4c5b39c0ea03806592fc1c3474" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref></type>
+        <definition>typedef NoiseModelFactor gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::Base</definition>
+        <argsstring></argsstring>
+        <name>Base</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="677" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="677" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NoiseModelFactor6_1a6939c52138ed37f395a57db8b50c71ec" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NoiseModelFactor6" kindref="compound">NoiseModelFactor6</ref>&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;</type>
+        <definition>typedef NoiseModelFactor6&lt;VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6&gt; gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::This</definition>
+        <argsstring></argsstring>
+        <name>This</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="678" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="678" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="friend">
+      <memberdef kind="friend" id="classgtsam_1_1NoiseModelFactor6_1ac98d07dd8f7b70e16ccb9a01abf56b9c" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual">
+        <type>friend class</type>
+        <definition>friend class boost::serialization::access</definition>
+        <argsstring></argsstring>
+        <name>boost::serialization::access</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Serialization function </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="740" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="740" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="public-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor6_1a15bcca054e397e486fb9dda0d0436cdb" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::NoiseModelFactor6</definition>
+        <argsstring>()</argsstring>
+        <name>NoiseModelFactor6</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Default Constructor for I/O </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="685" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="685" bodyend="685"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor6_1ad3d89a85f0c70d7a424f2060d514cab9" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::NoiseModelFactor6</definition>
+        <argsstring>(const SharedNoiseModel &amp;noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6)</argsstring>
+        <name>NoiseModelFactor6</name>
+        <param>
+          <type>const SharedNoiseModel &amp;</type>
+          <declname>noiseModel</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j1</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j2</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j3</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j4</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j5</declname>
+        </param>
+        <param>
+          <type>Key</type>
+          <declname>j6</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Constructor <parameterlist kind="param"><parameteritem>
+<parameternamelist>
+<parametername>noiseModel</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>shared pointer to noise model </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j1</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the first variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j2</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the second variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j3</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the third variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j4</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the fourth variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j5</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the fifth variable </para></parameterdescription>
+</parameteritem>
+<parameteritem>
+<parameternamelist>
+<parametername>j6</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>key of the fifth variable </para></parameterdescription>
+</parameteritem>
+</parameterlist>
+</para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="697" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="697" bodyend="698"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor6_1ad62b1867b67dee83e8733392a0aa579b" prot="public" static="no" const="no" explicit="no" inline="yes" virt="virtual">
+        <type></type>
+        <definition>virtual gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::~NoiseModelFactor6</definition>
+        <argsstring>()</argsstring>
+        <name>~NoiseModelFactor6</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="700" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="700" bodyend="700"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor6_1a587ce14373742350ff30a78c856ecaa0" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::key1</definition>
+        <argsstring>() const </argsstring>
+        <name>key1</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>methods to retrieve keys </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="703" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="703" bodyend="703"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor6_1a57f3ba7d334871bcb688727788ac95d3" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::key2</definition>
+        <argsstring>() const </argsstring>
+        <name>key2</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="704" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="704" bodyend="704"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor6_1a37bbec5754c7c6bd7cee9b3e94d9cbb1" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::key3</definition>
+        <argsstring>() const </argsstring>
+        <name>key3</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="705" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="705" bodyend="705"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor6_1a675e3df6fed6189dd9b8977907348960" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::key4</definition>
+        <argsstring>() const </argsstring>
+        <name>key4</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="706" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="706" bodyend="706"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor6_1a926065f63458a5673888e0b53054061d" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::key5</definition>
+        <argsstring>() const </argsstring>
+        <name>key5</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="707" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="707" bodyend="707"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor6_1a16a7857c857694bc66e19f455ad90a57" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="non-virtual">
+        <type>Key</type>
+        <definition>Key gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::key6</definition>
+        <argsstring>() const </argsstring>
+        <name>key6</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="708" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="708" bodyend="708"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor6_1a945d269f9a722d585e3332be23030450" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="virtual">
+        <type>Vector</type>
+        <definition>virtual Vector gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::unwhitenedError</definition>
+        <argsstring>(const Values &amp;x, boost::optional&lt; std::vector&lt; Matrix &gt; &amp; &gt; H=boost::none) const </argsstring>
+        <name>unwhitenedError</name>
+        <reimplements refid="classgtsam_1_1NoiseModelFactor_1afd61874915a8076bf83f9d8311fc0e26">unwhitenedError</reimplements>
+        <param>
+          <type>const Values &amp;</type>
+          <declname>x</declname>
+        </param>
+        <param>
+          <type>boost::optional&lt; std::vector&lt; Matrix &gt; &amp; &gt;</type>
+          <declname>H</declname>
+          <defval>boost::none</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Calls the 6-key specific version of evaluateError, which is pure virtual so must be implemented in the derived class. </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="712" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="712" bodyend="721"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor6_1a896a223b88b621bfe9f45bf8e2cd9960" prot="public" static="no" const="yes" explicit="no" inline="no" virt="pure-virtual">
+        <type>Vector</type>
+        <definition>virtual Vector gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::evaluateError</definition>
+        <argsstring>(const X1 &amp;, const X2 &amp;, const X3 &amp;, const X4 &amp;, const X5 &amp;, const X6 &amp;, boost::optional&lt; Matrix &amp; &gt; H1=boost::none, boost::optional&lt; Matrix &amp; &gt; H2=boost::none, boost::optional&lt; Matrix &amp; &gt; H3=boost::none, boost::optional&lt; Matrix &amp; &gt; H4=boost::none, boost::optional&lt; Matrix &amp; &gt; H5=boost::none, boost::optional&lt; Matrix &amp; &gt; H6=boost::none) const =0</argsstring>
+        <name>evaluateError</name>
+        <param>
+          <type>const X1 &amp;</type>
+        </param>
+        <param>
+          <type>const X2 &amp;</type>
+        </param>
+        <param>
+          <type>const X3 &amp;</type>
+        </param>
+        <param>
+          <type>const X4 &amp;</type>
+        </param>
+        <param>
+          <type>const X5 &amp;</type>
+        </param>
+        <param>
+          <type>const X6 &amp;</type>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H1</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H2</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H3</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H4</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H5</declname>
+          <defval>boost::none</defval>
+        </param>
+        <param>
+          <type>boost::optional&lt; Matrix &amp; &gt;</type>
+          <declname>H6</declname>
+          <defval>boost::none</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Override this method to finish implementing a 6-way factor. If any of the optional Matrix reference arguments are specified, it should compute both the function evaluation and its derivative(s) in X1 (and/or X2, X3). </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="729" column="1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="private-func">
+      <memberdef kind="function" id="classgtsam_1_1NoiseModelFactor6_1ac8f738f856920913a79e5017c1ced402" prot="private" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <templateparamlist>
+          <param>
+            <type>class ARCHIVE</type>
+          </param>
+        </templateparamlist>
+        <type>void</type>
+        <definition>void gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;::serialize</definition>
+        <argsstring>(ARCHIVE &amp;ar, const unsigned int)</argsstring>
+        <name>serialize</name>
+        <param>
+          <type>ARCHIVE &amp;</type>
+          <declname>ar</declname>
+        </param>
+        <param>
+          <type>const unsigned</type>
+          <declname>int</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="742" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="742" bodyend="745"/>
+      </memberdef>
+      </sectiondef>
+    <briefdescription>
+    </briefdescription>
+    <detaileddescription>
+<para>A convenient base class for creating your own <ref refid="classgtsam_1_1NoiseModelFactor" kindref="compound">NoiseModelFactor</ref> with 6 variables. To derive from this class, implement <ref refid="classgtsam_1_1NoiseModelFactor6_1a896a223b88b621bfe9f45bf8e2cd9960" kindref="member">evaluateError()</ref>. </para>    </detaileddescription>
+    <inheritancegraph>
+      <node id="142">
+        <label>Factor</label>
+      </node>
+      <node id="139">
+        <label>gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor6"/>
+        <childnode refid="140" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="141">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="142" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="140">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="141" relation="public-inheritance">
+        </childnode>
+      </node>
+    </inheritancegraph>
+    <collaborationgraph>
+      <node id="146">
+        <label>Factor</label>
+      </node>
+      <node id="143">
+        <label>gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor6"/>
+        <childnode refid="144" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="145">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="146" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="144">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="145" relation="public-inheritance">
+        </childnode>
+      </node>
+    </collaborationgraph>
+    <location file="tests/doc-test-files/NonlinearFactor.h" line="663" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="663" bodyend="746"/>
+    <listofallmembers>
+      <member refid="classgtsam_1_1NonlinearFactor_1a1968f471dc3ea33aaf0226ff7be5ae8c" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor6</scope><name>active</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1ad4a36f4c5b39c0ea03806592fc1c3474" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>Base</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1ac98d07dd8f7b70e16ccb9a01abf56b9c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>boost::serialization::access</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1aa98a30a78ade8489214ce2e5b8302777" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor6</scope><name>clone</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1af4f62b52fff1a0527ecdb279de023728" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor6</scope><name>dim</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a616d0335db512e9c333cab83b1923399" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor6</scope><name>equals</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1af6147aff3a33f734435601f6dcafeaf9" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor6</scope><name>error</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1a896a223b88b621bfe9f45bf8e2cd9960" prot="public" virt="pure-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>evaluateError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a16a8a23069ce6ae664f516edc8f36254" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>get_noiseModel</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1a587ce14373742350ff30a78c856ecaa0" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>key1</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1a57f3ba7d334871bcb688727788ac95d3" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>key2</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1a37bbec5754c7c6bd7cee9b3e94d9cbb1" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>key3</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1a675e3df6fed6189dd9b8977907348960" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>key4</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1a926065f63458a5673888e0b53054061d" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>key5</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1a16a7857c857694bc66e19f455ad90a57" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>key6</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a0c83582a0bd04e112e4c0a17cd4ed324" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor6</scope><name>linearize</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a123e5729d70b4acd6d7e1f70b1a7ced4" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>noiseModel</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a2263eabbefe0eede8c9d10486f61bb4d" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>noiseModel_</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ad6e8ef3348e042d598617d1d55f20300" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a02cea97db30d27b51feaf1f0444d743c" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a4605224417029e9dc58c5267efa9a46d" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1a15bcca054e397e486fb9dda0d0436cdb" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>NoiseModelFactor6</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1ad3d89a85f0c70d7a424f2060d514cab9" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>NoiseModelFactor6</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a7c719ae67f222f66569ccb3b526cb9cb" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a8a7610dba7e9a6f878268c2769ea0e04" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a682e0bbaf0c9638b7d0d96148082d1d7" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor6</scope><name>print</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a95f4d57e8646b6df634496aaabb683b5" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a5f760600e8608ef15c825a0f8d1d1855" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1ac8f738f856920913a79e5017c1ced402" prot="private" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>serialize</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a57d98d52fb6f91ebc54d74746aeb90e4" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>shared_ptr</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1a6939c52138ed37f395a57db8b50c71ec" prot="protected" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>This</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1a945d269f9a722d585e3332be23030450" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor6</scope><name>unwhitenedError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1a1b3ea51380315bfd63f60d8d9195b0c1" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>whitenedError</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1a369d9accd53012d84bbbaf87b176222b" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>X1</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1a6107bc96fff5d12783429158e79b9390" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>X2</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1a76c477f40c2b911a9f28d456ea9164a2" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>X3</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1a2a90aff22e4bd88dae81bf824956a113" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>X4</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1ae13e19a4b3a7a331d04d5b460e73216f" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>X5</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1af44204ef6947a65b973dee5707495601" prot="public" virt="non-virtual"><scope>gtsam::NoiseModelFactor6</scope><name>X6</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor_1ae1ed5e0fa6c471b57172f7ebaf552187" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor6</scope><name>~NoiseModelFactor</name></member>
+      <member refid="classgtsam_1_1NoiseModelFactor6_1ad62b1867b67dee83e8733392a0aa579b" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor6</scope><name>~NoiseModelFactor6</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1ac457810c504de58732d51d61f715c2de" prot="public" virt="virtual"><scope>gtsam::NoiseModelFactor6</scope><name>~NonlinearFactor</name></member>
+    </listofallmembers>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NonlinearFactor.xml b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NonlinearFactor.xml
new file mode 100644
index 000000000..4153fbccf
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/classgtsam_1_1NonlinearFactor.xml
@@ -0,0 +1,364 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="classgtsam_1_1NonlinearFactor" kind="class" language="C++" prot="public" abstract="yes">
+    <compoundname>gtsam::NonlinearFactor</compoundname>
+    <basecompoundref prot="public" virt="non-virtual">Factor</basecompoundref>
+    <derivedcompoundref refid="classgtsam_1_1NoiseModelFactor" prot="public" virt="non-virtual">gtsam::NoiseModelFactor</derivedcompoundref>
+    <includes refid="NonlinearFactor_8h" local="no">NonlinearFactor.h</includes>
+      <sectiondef kind="user-defined">
+      <header>Standard Constructors</header>
+      <memberdef kind="function" id="classgtsam_1_1NonlinearFactor_1a7c719ae67f222f66569ccb3b526cb9cb" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <type></type>
+        <definition>gtsam::NonlinearFactor::NonlinearFactor</definition>
+        <argsstring>()</argsstring>
+        <name>NonlinearFactor</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Default constructor for I/O only </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="68" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="68" bodyend="68"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NonlinearFactor_1a8a7610dba7e9a6f878268c2769ea0e04" prot="public" static="no" const="no" explicit="no" inline="yes" virt="non-virtual">
+        <templateparamlist>
+          <param>
+            <type>typename CONTAINER</type>
+          </param>
+        </templateparamlist>
+        <type></type>
+        <definition>gtsam::NonlinearFactor::NonlinearFactor</definition>
+        <argsstring>(const CONTAINER &amp;keys)</argsstring>
+        <name>NonlinearFactor</name>
+        <param>
+          <type>const CONTAINER &amp;</type>
+          <declname>keys</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Constructor from a collection of the keys involved in this factor </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="74" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="74" bodyend="75"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="user-defined">
+      <header>Testable</header>
+      <memberdef kind="function" id="classgtsam_1_1NonlinearFactor_1a758285fe17213cbf9c23b9f42fb960e6" prot="public" static="no" const="yes" explicit="no" inline="no" virt="virtual">
+        <type>void</type>
+        <definition>virtual void gtsam::NonlinearFactor::print</definition>
+        <argsstring>(const std::string &amp;s=&quot;&quot;, const KeyFormatter &amp;keyFormatter=DefaultKeyFormatter) const </argsstring>
+        <name>print</name>
+        <reimplementedby refid="classgtsam_1_1NoiseModelFactor_1a682e0bbaf0c9638b7d0d96148082d1d7">print</reimplementedby>
+        <param>
+          <type>const std::string &amp;</type>
+          <declname>s</declname>
+          <defval>&quot;&quot;</defval>
+        </param>
+        <param>
+          <type>const KeyFormatter &amp;</type>
+          <declname>keyFormatter</declname>
+          <defval>DefaultKeyFormatter</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>print </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="82" column="1"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NonlinearFactor_1a38cdabb3302973815e357bd00573734a" prot="public" static="no" const="yes" explicit="no" inline="no" virt="virtual">
+        <type>bool</type>
+        <definition>virtual bool gtsam::NonlinearFactor::equals</definition>
+        <argsstring>(const NonlinearFactor &amp;f, double tol=1e-9) const </argsstring>
+        <name>equals</name>
+        <reimplementedby refid="classgtsam_1_1NoiseModelFactor_1a616d0335db512e9c333cab83b1923399">equals</reimplementedby>
+        <param>
+          <type>const <ref refid="classgtsam_1_1NonlinearFactor" kindref="compound">NonlinearFactor</ref> &amp;</type>
+          <declname>f</declname>
+        </param>
+        <param>
+          <type>double</type>
+          <declname>tol</declname>
+          <defval>1e-9</defval>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Check if two factors are equal </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="86" column="1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="user-defined">
+      <header>Standard Interface</header>
+      <memberdef kind="function" id="classgtsam_1_1NonlinearFactor_1ac457810c504de58732d51d61f715c2de" prot="public" static="no" const="no" explicit="no" inline="yes" virt="virtual">
+        <type></type>
+        <definition>virtual gtsam::NonlinearFactor::~NonlinearFactor</definition>
+        <argsstring>()</argsstring>
+        <name>~NonlinearFactor</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Destructor </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="92" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="92" bodyend="92"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NonlinearFactor_1a8e67e58ff0e4f9d58f07e5eb227adc52" prot="public" static="no" const="yes" explicit="no" inline="no" virt="pure-virtual">
+        <type>double</type>
+        <definition>virtual double gtsam::NonlinearFactor::error</definition>
+        <argsstring>(const Values &amp;c) const =0</argsstring>
+        <name>error</name>
+        <reimplementedby refid="classgtsam_1_1NoiseModelFactor_1af6147aff3a33f734435601f6dcafeaf9">error</reimplementedby>
+        <param>
+          <type>const Values &amp;</type>
+          <declname>c</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Calculate the error of the factor This is typically equal to log-likelihood, e.g. <formula id="2">$ 0.5(h(x)-z)^2/sigma^2 $</formula> in case of Gaussian. You can override this for systems with unusual noise models. </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="100" column="1"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NonlinearFactor_1a5f0fcf1c93ded207d4a31e8ede07d797" prot="public" static="no" const="yes" explicit="no" inline="no" virt="pure-virtual">
+        <type>size_t</type>
+        <definition>virtual size_t gtsam::NonlinearFactor::dim</definition>
+        <argsstring>() const =0</argsstring>
+        <name>dim</name>
+        <reimplementedby refid="classgtsam_1_1NoiseModelFactor_1af4f62b52fff1a0527ecdb279de023728">dim</reimplementedby>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>get the dimension of the factor (number of rows on linearization) </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="103" column="1"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NonlinearFactor_1a1968f471dc3ea33aaf0226ff7be5ae8c" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="virtual">
+        <type>bool</type>
+        <definition>virtual bool gtsam::NonlinearFactor::active</definition>
+        <argsstring>(const Values &amp;) const </argsstring>
+        <name>active</name>
+        <param>
+          <type>const Values &amp;</type>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Checks whether a factor should be used based on a set of values. This is primarily used to implment inequality constraints that require a variable active set. For all others, the default implementation returning true solves this problem.</para><para>In an inequality/bounding constraint, this <ref refid="classgtsam_1_1NonlinearFactor_1a1968f471dc3ea33aaf0226ff7be5ae8c" kindref="member">active()</ref> returns true when the constraint is <emphasis>NOT</emphasis> fulfilled. <simplesect kind="return"><para>true if the constraint is active </para></simplesect>
+</para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="115" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="115" bodyend="115"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NonlinearFactor_1af2cc20b4ec180610dbf224f2165fa50a" prot="public" static="no" const="yes" explicit="no" inline="no" virt="pure-virtual">
+        <type>std::shared_ptr&lt; GaussianFactor &gt;</type>
+        <definition>virtual std::shared_ptr&lt;GaussianFactor&gt; gtsam::NonlinearFactor::linearize</definition>
+        <argsstring>(const Values &amp;c) const =0</argsstring>
+        <name>linearize</name>
+        <reimplementedby refid="classgtsam_1_1NoiseModelFactor_1a0c83582a0bd04e112e4c0a17cd4ed324">linearize</reimplementedby>
+        <param>
+          <type>const Values &amp;</type>
+          <declname>c</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>linearize to a GaussianFactor </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="119" column="1"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NonlinearFactor_1aa98a30a78ade8489214ce2e5b8302777" prot="public" static="no" const="yes" explicit="no" inline="yes" virt="virtual">
+        <type>shared_ptr</type>
+        <definition>virtual shared_ptr gtsam::NonlinearFactor::clone</definition>
+        <argsstring>() const </argsstring>
+        <name>clone</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses</para><para>By default, throws exception if subclass does not implement the function. </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="127" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="127" bodyend="131"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NonlinearFactor_1a95f4d57e8646b6df634496aaabb683b5" prot="public" static="no" const="yes" explicit="no" inline="no" virt="non-virtual">
+        <type>shared_ptr</type>
+        <definition>shared_ptr gtsam::NonlinearFactor::rekey</definition>
+        <argsstring>(const std::map&lt; Key, Key &gt; &amp;rekey_mapping) const </argsstring>
+        <name>rekey</name>
+        <param>
+          <type>const std::map&lt; Key, Key &gt; &amp;</type>
+          <declname>rekey_mapping</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Creates a shared_ptr clone of the factor with different keys using a map from old-&gt;new keys </para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="138" column="1"/>
+      </memberdef>
+      <memberdef kind="function" id="classgtsam_1_1NonlinearFactor_1a5f760600e8608ef15c825a0f8d1d1855" prot="public" static="no" const="yes" explicit="no" inline="no" virt="non-virtual">
+        <type>shared_ptr</type>
+        <definition>shared_ptr gtsam::NonlinearFactor::rekey</definition>
+        <argsstring>(const KeyVector &amp;new_keys) const </argsstring>
+        <name>rekey</name>
+        <param>
+          <type>const KeyVector &amp;</type>
+          <declname>new_keys</declname>
+        </param>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+<para>Clones a factor and fully replaces its keys <parameterlist kind="param"><parameteritem>
+<parameternamelist>
+<parametername>new_keys</parametername>
+</parameternamelist>
+<parameterdescription>
+<para>is the full replacement set of keys </para></parameterdescription>
+</parameteritem>
+</parameterlist>
+</para>        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="144" column="1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="protected-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NonlinearFactor_1aa76bae0d7aeb6591a824daa7c5dc2ac6" prot="protected" static="no">
+        <type>Factor</type>
+        <definition>typedef Factor gtsam::NonlinearFactor::Base</definition>
+        <argsstring></argsstring>
+        <name>Base</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="57" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="57" bodyend="-1"/>
+      </memberdef>
+      <memberdef kind="typedef" id="classgtsam_1_1NonlinearFactor_1a15c628be5e7433ee4ece0c5fd12d049e" prot="protected" static="no">
+        <type><ref refid="classgtsam_1_1NonlinearFactor" kindref="compound">NonlinearFactor</ref></type>
+        <definition>typedef NonlinearFactor gtsam::NonlinearFactor::This</definition>
+        <argsstring></argsstring>
+        <name>This</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="58" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="58" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+      <sectiondef kind="public-type">
+      <memberdef kind="typedef" id="classgtsam_1_1NonlinearFactor_1a3bf72f88f6e726577c89f894bf481515" prot="public" static="no">
+        <type>std::shared_ptr&lt; <ref refid="classgtsam_1_1NonlinearFactor" kindref="compound">This</ref> &gt;</type>
+        <definition>typedef std::shared_ptr&lt;This&gt; gtsam::NonlinearFactor::shared_ptr</definition>
+        <argsstring></argsstring>
+        <name>shared_ptr</name>
+        <briefdescription>
+        </briefdescription>
+        <detaileddescription>
+        </detaileddescription>
+        <inbodydescription>
+        </inbodydescription>
+        <location file="tests/doc-test-files/NonlinearFactor.h" line="62" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="62" bodyend="-1"/>
+      </memberdef>
+      </sectiondef>
+    <briefdescription>
+    </briefdescription>
+    <detaileddescription>
+<para>Nonlinear factor base class </para>    </detaileddescription>
+    <inheritancegraph>
+      <node id="148">
+        <label>Factor</label>
+      </node>
+      <node id="153">
+        <label>gtsam::NoiseModelFactor4&lt; VALUE1, VALUE2, VALUE3, VALUE4 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor4"/>
+        <childnode refid="149" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="155">
+        <label>gtsam::NoiseModelFactor6&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor6"/>
+        <childnode refid="149" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="154">
+        <label>gtsam::NoiseModelFactor5&lt; VALUE1, VALUE2, VALUE3, VALUE4, VALUE5 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor5"/>
+        <childnode refid="149" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="147">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="148" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="149">
+        <label>gtsam::NoiseModelFactor</label>
+        <link refid="classgtsam_1_1NoiseModelFactor"/>
+        <childnode refid="147" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="150">
+        <label>gtsam::NoiseModelFactor1&lt; VALUE &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor1"/>
+        <childnode refid="149" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="152">
+        <label>gtsam::NoiseModelFactor3&lt; VALUE1, VALUE2, VALUE3 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor3"/>
+        <childnode refid="149" relation="public-inheritance">
+        </childnode>
+      </node>
+      <node id="151">
+        <label>gtsam::NoiseModelFactor2&lt; VALUE1, VALUE2 &gt;</label>
+        <link refid="classgtsam_1_1NoiseModelFactor2"/>
+        <childnode refid="149" relation="public-inheritance">
+        </childnode>
+      </node>
+    </inheritancegraph>
+    <collaborationgraph>
+      <node id="157">
+        <label>Factor</label>
+      </node>
+      <node id="156">
+        <label>gtsam::NonlinearFactor</label>
+        <link refid="classgtsam_1_1NonlinearFactor"/>
+        <childnode refid="157" relation="public-inheritance">
+        </childnode>
+      </node>
+    </collaborationgraph>
+    <location file="tests/doc-test-files/NonlinearFactor.h" line="52" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="52" bodyend="146"/>
+    <listofallmembers>
+      <member refid="classgtsam_1_1NonlinearFactor_1a1968f471dc3ea33aaf0226ff7be5ae8c" prot="public" virt="virtual"><scope>gtsam::NonlinearFactor</scope><name>active</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1aa76bae0d7aeb6591a824daa7c5dc2ac6" prot="protected" virt="non-virtual"><scope>gtsam::NonlinearFactor</scope><name>Base</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1aa98a30a78ade8489214ce2e5b8302777" prot="public" virt="virtual"><scope>gtsam::NonlinearFactor</scope><name>clone</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a5f0fcf1c93ded207d4a31e8ede07d797" prot="public" virt="pure-virtual"><scope>gtsam::NonlinearFactor</scope><name>dim</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a38cdabb3302973815e357bd00573734a" prot="public" virt="virtual"><scope>gtsam::NonlinearFactor</scope><name>equals</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a8e67e58ff0e4f9d58f07e5eb227adc52" prot="public" virt="pure-virtual"><scope>gtsam::NonlinearFactor</scope><name>error</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1af2cc20b4ec180610dbf224f2165fa50a" prot="public" virt="pure-virtual"><scope>gtsam::NonlinearFactor</scope><name>linearize</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a7c719ae67f222f66569ccb3b526cb9cb" prot="public" virt="non-virtual"><scope>gtsam::NonlinearFactor</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a8a7610dba7e9a6f878268c2769ea0e04" prot="public" virt="non-virtual"><scope>gtsam::NonlinearFactor</scope><name>NonlinearFactor</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a758285fe17213cbf9c23b9f42fb960e6" prot="public" virt="virtual"><scope>gtsam::NonlinearFactor</scope><name>print</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a95f4d57e8646b6df634496aaabb683b5" prot="public" virt="non-virtual"><scope>gtsam::NonlinearFactor</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a5f760600e8608ef15c825a0f8d1d1855" prot="public" virt="non-virtual"><scope>gtsam::NonlinearFactor</scope><name>rekey</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a3bf72f88f6e726577c89f894bf481515" prot="public" virt="non-virtual"><scope>gtsam::NonlinearFactor</scope><name>shared_ptr</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1a15c628be5e7433ee4ece0c5fd12d049e" prot="protected" virt="non-virtual"><scope>gtsam::NonlinearFactor</scope><name>This</name></member>
+      <member refid="classgtsam_1_1NonlinearFactor_1ac457810c504de58732d51d61f715c2de" prot="public" virt="virtual"><scope>gtsam::NonlinearFactor</scope><name>~NonlinearFactor</name></member>
+    </listofallmembers>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/combine.xslt b/wrap/tests/expected-xml-generation/xml/combine.xslt
new file mode 100644
index 000000000..c148ee4f2
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/combine.xslt
@@ -0,0 +1,15 @@
+<!-- XSLT script to combine the generated output into a single file. 
+     If you have xsltproc you could use:
+     xsltproc combine.xslt index.xml >all.xml
+-->
+<xsl:stylesheet xmlns:xsl="http://www.w3.org/1999/XSL/Transform" version="1.0">
+  <xsl:output method="xml" version="1.0" indent="no" standalone="yes" />
+  <xsl:template match="/">
+    <doxygen version="{doxygenindex/@version}">
+      <!-- Load all doxgen generated xml files -->
+      <xsl:for-each select="doxygenindex/compound">
+        <xsl:copy-of select="document( concat( @refid, '.xml' ) )/doxygen/*" />
+      </xsl:for-each>
+    </doxygen>
+  </xsl:template>
+</xsl:stylesheet>
diff --git a/wrap/tests/expected-xml-generation/xml/compound.xsd b/wrap/tests/expected-xml-generation/xml/compound.xsd
new file mode 100644
index 000000000..b4df8e011
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/compound.xsd
@@ -0,0 +1,1115 @@
+<?xml version='1.0' encoding='utf-8' ?>
+<xsd:schema xmlns:xsd="http://www.w3.org/2001/XMLSchema">
+  <xsd:element name="doxygen" type="DoxygenType"/>
+
+  <!-- Complex types -->
+
+  <xsd:complexType name="DoxygenType">
+    <xsd:sequence maxOccurs="unbounded">
+      <xsd:element name="compounddef" type="compounddefType" minOccurs="0" />
+    </xsd:sequence>
+    <xsd:attribute name="version" type="DoxVersionNumber" use="required" />
+  </xsd:complexType>
+
+  <xsd:complexType name="compounddefType">
+    <xsd:sequence>
+      <xsd:element name="compoundname" type="xsd:string"/>
+      <xsd:element name="title" type="xsd:string" minOccurs="0" />
+      <xsd:element name="basecompoundref" type="compoundRefType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="derivedcompoundref" type="compoundRefType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="includes" type="incType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="includedby" type="incType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="incdepgraph" type="graphType" minOccurs="0" />
+      <xsd:element name="invincdepgraph" type="graphType" minOccurs="0" />
+      <xsd:element name="innerdir" type="refType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="innerfile" type="refType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="innerclass" type="refType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="innernamespace" type="refType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="innerpage" type="refType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="innergroup" type="refType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="templateparamlist" type="templateparamlistType" minOccurs="0" />
+      <xsd:element name="sectiondef" type="sectiondefType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="briefdescription" type="descriptionType" minOccurs="0" />
+      <xsd:element name="detaileddescription" type="descriptionType" minOccurs="0" />
+      <xsd:element name="inheritancegraph" type="graphType" minOccurs="0" />
+      <xsd:element name="collaborationgraph" type="graphType" minOccurs="0" />
+      <xsd:element name="programlisting" type="listingType" minOccurs="0" />
+      <xsd:element name="location" type="locationType" minOccurs="0" />
+      <xsd:element name="listofallmembers" type="listofallmembersType" minOccurs="0" />
+    </xsd:sequence>
+    <xsd:attribute name="id" type="xsd:string" />
+    <xsd:attribute name="kind" type="DoxCompoundKind" />
+    <xsd:attribute name="language" type="DoxLanguage" use="optional"/>
+    <xsd:attribute name="prot" type="DoxProtectionKind" />
+    <xsd:attribute name="final" type="DoxBool" use="optional"/>
+    <xsd:attribute name="sealed" type="DoxBool" use="optional"/>
+    <xsd:attribute name="abstract" type="DoxBool" use="optional"/>
+  </xsd:complexType>
+
+  <xsd:complexType name="listofallmembersType">
+    <xsd:sequence>
+      <xsd:element name="member" type="memberRefType" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="memberRefType">
+    <xsd:sequence>
+      <xsd:element name="scope" />
+      <xsd:element name="name" />
+    </xsd:sequence>
+    <xsd:attribute name="refid" type="xsd:string" />
+    <xsd:attribute name="prot" type="DoxProtectionKind" />
+    <xsd:attribute name="virt" type="DoxVirtualKind" />
+    <xsd:attribute name="ambiguityscope" type="xsd:string" />
+  </xsd:complexType>
+
+  <xsd:complexType name="compoundRefType">
+    <xsd:simpleContent>
+      <xsd:extension base="xsd:string">
+        <xsd:attribute name="refid" type="xsd:string" use="optional" />
+        <xsd:attribute name="prot" type="DoxProtectionKind" />
+        <xsd:attribute name="virt" type="DoxVirtualKind" />
+      </xsd:extension>
+    </xsd:simpleContent>
+  </xsd:complexType>
+
+  <xsd:complexType name="reimplementType">
+    <xsd:simpleContent>
+      <xsd:extension base="xsd:string">
+        <xsd:attribute name="refid" type="xsd:string" />
+      </xsd:extension>
+    </xsd:simpleContent>
+  </xsd:complexType>
+
+  <xsd:complexType name="incType">
+    <xsd:simpleContent>
+      <xsd:extension base="xsd:string">
+        <xsd:attribute name="refid" type="xsd:string" />
+        <xsd:attribute name="local" type="DoxBool" />
+      </xsd:extension>
+    </xsd:simpleContent>
+  </xsd:complexType>
+
+  <xsd:complexType name="refType">
+    <xsd:simpleContent>
+      <xsd:extension base="xsd:string">
+        <xsd:attribute name="refid" type="xsd:string" />
+        <xsd:attribute name="prot" type="DoxProtectionKind" use="optional"/>
+      </xsd:extension>
+    </xsd:simpleContent>
+  </xsd:complexType>
+
+  <xsd:complexType name="refTextType">
+    <xsd:simpleContent>
+      <xsd:extension base="xsd:string">
+       <xsd:attribute name="refid" type="xsd:string" />
+       <xsd:attribute name="kindref" type="DoxRefKind" />
+       <xsd:attribute name="external" type="xsd:string" use="optional"/>
+       <xsd:attribute name="tooltip" type="xsd:string" use="optional"/>
+      </xsd:extension>
+    </xsd:simpleContent>
+  </xsd:complexType>
+
+  <xsd:complexType name="sectiondefType">
+    <xsd:sequence>
+      <xsd:element name="header" type="xsd:string" minOccurs="0" />
+      <xsd:element name="description" type="descriptionType" minOccurs="0" />
+      <xsd:element name="memberdef" type="memberdefType" maxOccurs="unbounded" />
+    </xsd:sequence>
+    <xsd:attribute name="kind" type="DoxSectionKind" />
+  </xsd:complexType>
+
+  <xsd:complexType name="memberdefType">
+    <xsd:sequence>
+      <xsd:element name="templateparamlist" type="templateparamlistType" minOccurs="0" />
+      <xsd:element name="type" type="linkedTextType" minOccurs="0" />
+      <xsd:element name="definition" minOccurs="0" />
+      <xsd:element name="argsstring" minOccurs="0" />
+      <xsd:element name="name" />
+      <xsd:element name="read" minOccurs="0" />
+      <xsd:element name="write" minOccurs="0" />
+      <xsd:element name="bitfield" minOccurs="0" />
+      <xsd:element name="reimplements" type="reimplementType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="reimplementedby" type="reimplementType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="param" type="paramType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="enumvalue" type="enumvalueType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="initializer" type="linkedTextType" minOccurs="0" />
+      <xsd:element name="exceptions" type="linkedTextType" minOccurs="0" />
+      <xsd:element name="briefdescription" type="descriptionType" minOccurs="0" />
+      <xsd:element name="detaileddescription" type="descriptionType" minOccurs="0" />
+      <xsd:element name="inbodydescription" type="descriptionType" minOccurs="0" />
+      <xsd:element name="location" type="locationType" />
+      <xsd:element name="references" type="referenceType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="referencedby" type="referenceType" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+    <xsd:attribute name="kind" type="DoxMemberKind" />
+    <xsd:attribute name="id" type="xsd:string" />
+    <xsd:attribute name="prot" type="DoxProtectionKind" />
+    <xsd:attribute name="static" type="DoxBool" />
+    <xsd:attribute name="const" type="DoxBool" use="optional"/>
+    <xsd:attribute name="explicit" type="DoxBool" use="optional"/>
+    <xsd:attribute name="inline" type="DoxBool" use="optional"/>
+    <xsd:attribute name="virt" type="DoxVirtualKind" use="optional"/>
+    <xsd:attribute name="volatile" type="DoxBool" use="optional"/>
+    <xsd:attribute name="mutable" type="DoxBool" use="optional"/>
+    <!-- Qt property -->
+    <xsd:attribute name="readable" type="DoxBool" use="optional"/>
+    <xsd:attribute name="writable" type="DoxBool" use="optional"/>
+    <!-- C++/CLI variable -->
+    <xsd:attribute name="initonly" type="DoxBool" use="optional"/>
+    <!-- C++/CLI and C# property -->
+    <xsd:attribute name="settable" type="DoxBool" use="optional"/>
+    <xsd:attribute name="gettable" type="DoxBool" use="optional"/>
+    <!-- C++/CLI function -->
+    <xsd:attribute name="final" type="DoxBool" use="optional"/>
+    <xsd:attribute name="sealed" type="DoxBool" use="optional"/>
+    <xsd:attribute name="new" type="DoxBool" use="optional"/>
+    <!-- C++/CLI event -->
+    <xsd:attribute name="add" type="DoxBool" use="optional"/>
+    <xsd:attribute name="remove" type="DoxBool" use="optional"/>
+    <xsd:attribute name="raise" type="DoxBool" use="optional"/>
+    <!-- Objective-C 2.0 protocol method -->
+    <xsd:attribute name="optional" type="DoxBool" use="optional"/>
+    <xsd:attribute name="required" type="DoxBool" use="optional"/>
+    <!-- Objective-C 2.0 property accessor -->
+    <xsd:attribute name="accessor" type="DoxAccessor" use="optional"/>
+    <!-- UNO IDL -->
+    <xsd:attribute name="attribute" type="DoxBool" use="optional"/>
+    <xsd:attribute name="property" type="DoxBool" use="optional"/>
+    <xsd:attribute name="readonly" type="DoxBool" use="optional"/>
+    <xsd:attribute name="bound" type="DoxBool" use="optional"/>
+    <xsd:attribute name="removable" type="DoxBool" use="optional"/>
+    <xsd:attribute name="contrained" type="DoxBool" use="optional"/>
+    <xsd:attribute name="transient" type="DoxBool" use="optional"/>
+    <xsd:attribute name="maybevoid" type="DoxBool" use="optional"/>
+    <xsd:attribute name="maybedefault" type="DoxBool" use="optional"/>
+    <xsd:attribute name="maybeambiguous" type="DoxBool" use="optional"/>
+
+  </xsd:complexType>
+
+  <xsd:complexType name="descriptionType" mixed="true">
+    <xsd:sequence>
+      <xsd:element name="title" type="xsd:string" minOccurs="0"/>	    
+      <xsd:element name="para" type="docParaType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="sect1" type="docSect1Type" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="internal" type="docInternalType" minOccurs="0" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="enumvalueType" mixed="true">
+    <xsd:sequence>
+      <xsd:element name="name" />
+      <xsd:element name="initializer" type="linkedTextType" minOccurs="0" />
+      <xsd:element name="briefdescription" type="descriptionType" minOccurs="0" />
+      <xsd:element name="detaileddescription" type="descriptionType" minOccurs="0" />
+    </xsd:sequence>
+    <xsd:attribute name="id" type="xsd:string" />
+    <xsd:attribute name="prot" type="DoxProtectionKind" />
+  </xsd:complexType>
+
+  <xsd:complexType name="templateparamlistType">
+    <xsd:sequence>
+      <xsd:element name="param" type="paramType" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="paramType">
+    <xsd:sequence>
+      <xsd:element name="type" type="linkedTextType" minOccurs="0" />
+      <xsd:element name="declname" minOccurs="0" />
+      <xsd:element name="defname" minOccurs="0" />
+      <xsd:element name="array" minOccurs="0" />
+      <xsd:element name="defval" type="linkedTextType" minOccurs="0" />
+      <xsd:element name="typeconstraint" type="linkedTextType" minOccurs="0" />
+      <xsd:element name="briefdescription" type="descriptionType" minOccurs="0" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="linkedTextType" mixed="true">
+    <xsd:sequence>
+    <xsd:element name="ref" type="refTextType" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="graphType">
+    <xsd:sequence>
+      <xsd:element name="node" type="nodeType" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="nodeType">
+    <xsd:sequence>
+      <xsd:element name="label" />
+      <xsd:element name="link" type="linkType" minOccurs="0" />
+      <xsd:element name="childnode" type="childnodeType" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+    <xsd:attribute name="id" type="xsd:string" />
+  </xsd:complexType>
+
+  <xsd:complexType name="childnodeType">
+    <xsd:sequence>
+      <xsd:element name="edgelabel" minOccurs="0" maxOccurs="unbounded"/>
+    </xsd:sequence>
+    <xsd:attribute name="refid" type="xsd:string" />
+    <xsd:attribute name="relation" type="DoxGraphRelation" />
+  </xsd:complexType>
+
+  <xsd:complexType name="linkType">
+    <xsd:attribute name="refid" type="xsd:string" />
+    <xsd:attribute name="external" type="xsd:string" use="optional"/>
+  </xsd:complexType>
+
+  <xsd:complexType name="listingType">
+    <xsd:sequence>
+      <xsd:element name="codeline" type="codelineType" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="codelineType">
+    <xsd:sequence>
+      <xsd:element name="highlight" type="highlightType" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+    <xsd:attribute name="lineno" type="xsd:integer" />
+    <xsd:attribute name="refid" type="xsd:string" />
+    <xsd:attribute name="refkind" type="DoxRefKind" />
+    <xsd:attribute name="external" type="DoxBool" />
+  </xsd:complexType>
+
+  <xsd:complexType name="highlightType" mixed="true">
+    <xsd:choice minOccurs="0" maxOccurs="unbounded">
+      <xsd:element name="sp" />
+      <xsd:element name="ref" type="refTextType" />
+    </xsd:choice>
+    <xsd:attribute name="class" type="DoxHighlightClass" />
+  </xsd:complexType>
+
+  <xsd:complexType name="referenceType" mixed="true">
+    <xsd:attribute name="refid" type="xsd:string" />
+    <xsd:attribute name="compoundref" type="xsd:string" use="optional" />
+    <xsd:attribute name="startline" type="xsd:integer" />
+    <xsd:attribute name="endline" type="xsd:integer" />
+  </xsd:complexType>
+
+  <xsd:complexType name="locationType">
+    <xsd:attribute name="file" type="xsd:string" />
+    <xsd:attribute name="line" type="xsd:integer" />
+    <xsd:attribute name="column" type="xsd:integer" use="optional"/>
+    <xsd:attribute name="bodyfile" type="xsd:string" />
+    <xsd:attribute name="bodystart" type="xsd:integer" />
+    <xsd:attribute name="bodyend" type="xsd:integer" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docSect1Type" mixed="true">
+    <xsd:sequence>
+      <xsd:element name="title" type="xsd:string" />	    
+      <xsd:element name="para" type="docParaType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="sect2" type="docSect2Type" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="internal" type="docInternalS1Type" minOccurs="0" />
+    </xsd:sequence>
+    <xsd:attribute name="id" type="xsd:string" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docSect2Type" mixed="true">
+    <xsd:sequence>
+      <xsd:element name="title" type="xsd:string" />	    
+      <xsd:element name="para" type="docParaType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="sect3" type="docSect3Type" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="internal" type="docInternalS2Type" minOccurs="0" />
+    </xsd:sequence>
+    <xsd:attribute name="id" type="xsd:string" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docSect3Type" mixed="true">
+    <xsd:sequence>
+      <xsd:element name="title" type="xsd:string" />	    
+      <xsd:element name="para" type="docParaType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="sect4" type="docSect4Type" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="internal" type="docInternalS3Type" minOccurs="0" />
+    </xsd:sequence>
+    <xsd:attribute name="id" type="xsd:string" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docSect4Type" mixed="true">
+    <xsd:sequence>
+      <xsd:element name="title" type="xsd:string" />	    
+      <xsd:element name="para" type="docParaType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="internal" type="docInternalS4Type" minOccurs="0" />
+    </xsd:sequence>
+    <xsd:attribute name="id" type="xsd:string" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docInternalType" mixed="true">
+    <xsd:sequence>
+      <xsd:element name="para"  type="docParaType"  minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="sect1" type="docSect1Type" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docInternalS1Type" mixed="true">
+    <xsd:sequence>
+      <xsd:element name="para"  type="docParaType"  minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="sect2" type="docSect2Type" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docInternalS2Type" mixed="true">
+    <xsd:sequence>
+      <xsd:element name="para"  type="docParaType"  minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="sect3" type="docSect3Type" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docInternalS3Type" mixed="true">
+    <xsd:sequence>
+      <xsd:element name="para"  type="docParaType"  minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="sect3" type="docSect4Type" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docInternalS4Type" mixed="true">
+    <xsd:sequence>
+      <xsd:element name="para"  type="docParaType"  minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+ 
+  <xsd:group name="docTitleCmdGroup">
+    <xsd:choice>
+      <xsd:element name="ulink" type="docURLLink" />
+      <xsd:element name="bold" type="docMarkupType" />
+      <xsd:element name="emphasis" type="docMarkupType" />
+      <xsd:element name="computeroutput" type="docMarkupType" />
+      <xsd:element name="subscript" type="docMarkupType" />
+      <xsd:element name="superscript" type="docMarkupType" />
+      <xsd:element name="center" type="docMarkupType" />
+      <xsd:element name="small" type="docMarkupType" />
+      <xsd:element name="htmlonly" type="xsd:string" />
+      <xsd:element name="manonly" type="xsd:string" />
+      <xsd:element name="xmlonly" type="xsd:string" />
+      <xsd:element name="rtfonly" type="xsd:string" />
+      <xsd:element name="latexonly" type="xsd:string" />
+      <xsd:element name="dot" type="xsd:string" />
+      <xsd:element name="plantuml" type="xsd:string" />
+      <xsd:element name="anchor" type="docAnchorType" />
+      <xsd:element name="formula" type="docFormulaType" />
+      <xsd:element name="ref" type="docRefTextType" />
+      <xsd:element name="nonbreakablespace" type="docEmptyType" />
+      <xsd:element name="iexcl" type="docEmptyType" />
+      <xsd:element name="cent" type="docEmptyType" />
+      <xsd:element name="pound" type="docEmptyType" />
+      <xsd:element name="curren" type="docEmptyType" />
+      <xsd:element name="yen" type="docEmptyType" />
+      <xsd:element name="brvbar" type="docEmptyType" />
+      <xsd:element name="sect" type="docEmptyType" />
+      <xsd:element name="umlaut" type="docEmptyType" />
+      <xsd:element name="copy" type="docEmptyType" />
+      <xsd:element name="ordf" type="docEmptyType" />
+      <xsd:element name="laquo" type="docEmptyType" />
+      <xsd:element name="not" type="docEmptyType" />
+      <xsd:element name="shy" type="docEmptyType" />
+      <xsd:element name="registered" type="docEmptyType" />
+      <xsd:element name="macr" type="docEmptyType" />
+      <xsd:element name="deg" type="docEmptyType" />
+      <xsd:element name="plusmn" type="docEmptyType" />
+      <xsd:element name="sup2" type="docEmptyType" />
+      <xsd:element name="sup3" type="docEmptyType" />
+      <xsd:element name="acute" type="docEmptyType" />
+      <xsd:element name="micro" type="docEmptyType" />
+      <xsd:element name="para" type="docEmptyType" />
+      <xsd:element name="middot" type="docEmptyType" />
+      <xsd:element name="cedil" type="docEmptyType" />
+      <xsd:element name="sup1" type="docEmptyType" />
+      <xsd:element name="ordm" type="docEmptyType" />
+      <xsd:element name="raquo" type="docEmptyType" />
+      <xsd:element name="frac14" type="docEmptyType" />
+      <xsd:element name="frac12" type="docEmptyType" />
+      <xsd:element name="frac34" type="docEmptyType" />
+      <xsd:element name="iquest" type="docEmptyType" />
+      <xsd:element name="Agrave" type="docEmptyType" />
+      <xsd:element name="Aacute" type="docEmptyType" />
+      <xsd:element name="Acirc" type="docEmptyType" />
+      <xsd:element name="Atilde" type="docEmptyType" />
+      <xsd:element name="Aumlaut" type="docEmptyType" />
+      <xsd:element name="Aring" type="docEmptyType" />
+      <xsd:element name="AElig" type="docEmptyType" />
+      <xsd:element name="Ccedil" type="docEmptyType" />
+      <xsd:element name="Egrave" type="docEmptyType" />
+      <xsd:element name="Eacute" type="docEmptyType" />
+      <xsd:element name="Ecirc" type="docEmptyType" />
+      <xsd:element name="Eumlaut" type="docEmptyType" />
+      <xsd:element name="Igrave" type="docEmptyType" />
+      <xsd:element name="Iacute" type="docEmptyType" />
+      <xsd:element name="Icirc" type="docEmptyType" />
+      <xsd:element name="Iumlaut" type="docEmptyType" />
+      <xsd:element name="ETH" type="docEmptyType" />
+      <xsd:element name="Ntilde" type="docEmptyType" />
+      <xsd:element name="Ograve" type="docEmptyType" />
+      <xsd:element name="Oacute" type="docEmptyType" />
+      <xsd:element name="Ocirc" type="docEmptyType" />
+      <xsd:element name="Otilde" type="docEmptyType" />
+      <xsd:element name="Oumlaut" type="docEmptyType" />
+      <xsd:element name="times" type="docEmptyType" />
+      <xsd:element name="Oslash" type="docEmptyType" />
+      <xsd:element name="Ugrave" type="docEmptyType" />
+      <xsd:element name="Uacute" type="docEmptyType" />
+      <xsd:element name="Ucirc" type="docEmptyType" />
+      <xsd:element name="Uumlaut" type="docEmptyType" />
+      <xsd:element name="Yacute" type="docEmptyType" />
+      <xsd:element name="THORN" type="docEmptyType" />
+      <xsd:element name="szlig" type="docEmptyType" />
+      <xsd:element name="agrave" type="docEmptyType" />
+      <xsd:element name="aacute" type="docEmptyType" />
+      <xsd:element name="acirc" type="docEmptyType" />
+      <xsd:element name="atilde" type="docEmptyType" />
+      <xsd:element name="aumlaut" type="docEmptyType" />
+      <xsd:element name="aring" type="docEmptyType" />
+      <xsd:element name="aelig" type="docEmptyType" />
+      <xsd:element name="ccedil" type="docEmptyType" />
+      <xsd:element name="egrave" type="docEmptyType" />
+      <xsd:element name="eacute" type="docEmptyType" />
+      <xsd:element name="ecirc" type="docEmptyType" />
+      <xsd:element name="eumlaut" type="docEmptyType" />
+      <xsd:element name="igrave" type="docEmptyType" />
+      <xsd:element name="iacute" type="docEmptyType" />
+      <xsd:element name="icirc" type="docEmptyType" />
+      <xsd:element name="iumlaut" type="docEmptyType" />
+      <xsd:element name="eth" type="docEmptyType" />
+      <xsd:element name="ntilde" type="docEmptyType" />
+      <xsd:element name="ograve" type="docEmptyType" />
+      <xsd:element name="oacute" type="docEmptyType" />
+      <xsd:element name="ocirc" type="docEmptyType" />
+      <xsd:element name="otilde" type="docEmptyType" />
+      <xsd:element name="oumlaut" type="docEmptyType" />
+      <xsd:element name="divide" type="docEmptyType" />
+      <xsd:element name="oslash" type="docEmptyType" />
+      <xsd:element name="ugrave" type="docEmptyType" />
+      <xsd:element name="uacute" type="docEmptyType" />
+      <xsd:element name="ucirc" type="docEmptyType" />
+      <xsd:element name="uumlaut" type="docEmptyType" />
+      <xsd:element name="yacute" type="docEmptyType" />
+      <xsd:element name="thorn" type="docEmptyType" />
+      <xsd:element name="yumlaut" type="docEmptyType" />
+      <xsd:element name="fnof" type="docEmptyType" />
+      <xsd:element name="Alpha" type="docEmptyType" />
+      <xsd:element name="Beta" type="docEmptyType" />
+      <xsd:element name="Gamma" type="docEmptyType" />
+      <xsd:element name="Delta" type="docEmptyType" />
+      <xsd:element name="Epsilon" type="docEmptyType" />
+      <xsd:element name="Zeta" type="docEmptyType" />
+      <xsd:element name="Eta" type="docEmptyType" />
+      <xsd:element name="Theta" type="docEmptyType" />
+      <xsd:element name="Iota" type="docEmptyType" />
+      <xsd:element name="Kappa" type="docEmptyType" />
+      <xsd:element name="Lambda" type="docEmptyType" />
+      <xsd:element name="Mu" type="docEmptyType" />
+      <xsd:element name="Nu" type="docEmptyType" />
+      <xsd:element name="Xi" type="docEmptyType" />
+      <xsd:element name="Omicron" type="docEmptyType" />
+      <xsd:element name="Pi" type="docEmptyType" />
+      <xsd:element name="Rho" type="docEmptyType" />
+      <xsd:element name="Sigma" type="docEmptyType" />
+      <xsd:element name="Tau" type="docEmptyType" />
+      <xsd:element name="Upsilon" type="docEmptyType" />
+      <xsd:element name="Phi" type="docEmptyType" />
+      <xsd:element name="Chi" type="docEmptyType" />
+      <xsd:element name="Psi" type="docEmptyType" />
+      <xsd:element name="Omega" type="docEmptyType" />
+      <xsd:element name="alpha" type="docEmptyType" />
+      <xsd:element name="beta" type="docEmptyType" />
+      <xsd:element name="gamma" type="docEmptyType" />
+      <xsd:element name="delta" type="docEmptyType" />
+      <xsd:element name="epsilon" type="docEmptyType" />
+      <xsd:element name="zeta" type="docEmptyType" />
+      <xsd:element name="eta" type="docEmptyType" />
+      <xsd:element name="theta" type="docEmptyType" />
+      <xsd:element name="iota" type="docEmptyType" />
+      <xsd:element name="kappa" type="docEmptyType" />
+      <xsd:element name="lambda" type="docEmptyType" />
+      <xsd:element name="mu" type="docEmptyType" />
+      <xsd:element name="nu" type="docEmptyType" />
+      <xsd:element name="xi" type="docEmptyType" />
+      <xsd:element name="omicron" type="docEmptyType" />
+      <xsd:element name="pi" type="docEmptyType" />
+      <xsd:element name="rho" type="docEmptyType" />
+      <xsd:element name="sigmaf" type="docEmptyType" />
+      <xsd:element name="sigma" type="docEmptyType" />
+      <xsd:element name="tau" type="docEmptyType" />
+      <xsd:element name="upsilon" type="docEmptyType" />
+      <xsd:element name="phi" type="docEmptyType" />
+      <xsd:element name="chi" type="docEmptyType" />
+      <xsd:element name="psi" type="docEmptyType" />
+      <xsd:element name="omega" type="docEmptyType" />
+      <xsd:element name="thetasym" type="docEmptyType" />
+      <xsd:element name="upsih" type="docEmptyType" />
+      <xsd:element name="piv" type="docEmptyType" />
+      <xsd:element name="bull" type="docEmptyType" />
+      <xsd:element name="hellip" type="docEmptyType" />
+      <xsd:element name="prime" type="docEmptyType" />
+      <xsd:element name="Prime" type="docEmptyType" />
+      <xsd:element name="oline" type="docEmptyType" />
+      <xsd:element name="frasl" type="docEmptyType" />
+      <xsd:element name="weierp" type="docEmptyType" />
+      <xsd:element name="image" type="docEmptyType" />
+      <xsd:element name="real" type="docEmptyType" />
+      <xsd:element name="trademark" type="docEmptyType" />
+      <xsd:element name="alefsym" type="docEmptyType" />
+      <xsd:element name="larr" type="docEmptyType" />
+      <xsd:element name="uarr" type="docEmptyType" />
+      <xsd:element name="rarr" type="docEmptyType" />
+      <xsd:element name="darr" type="docEmptyType" />
+      <xsd:element name="harr" type="docEmptyType" />
+      <xsd:element name="crarr" type="docEmptyType" />
+      <xsd:element name="lArr" type="docEmptyType" />
+      <xsd:element name="uArr" type="docEmptyType" />
+      <xsd:element name="rArr" type="docEmptyType" />
+      <xsd:element name="dArr" type="docEmptyType" />
+      <xsd:element name="hArr" type="docEmptyType" />
+      <xsd:element name="forall" type="docEmptyType" />
+      <xsd:element name="part" type="docEmptyType" />
+      <xsd:element name="exist" type="docEmptyType" />
+      <xsd:element name="empty" type="docEmptyType" />
+      <xsd:element name="nabla" type="docEmptyType" />
+      <xsd:element name="isin" type="docEmptyType" />
+      <xsd:element name="notin" type="docEmptyType" />
+      <xsd:element name="ni" type="docEmptyType" />
+      <xsd:element name="prod" type="docEmptyType" />
+      <xsd:element name="sum" type="docEmptyType" />
+      <xsd:element name="minus" type="docEmptyType" />
+      <xsd:element name="lowast" type="docEmptyType" />
+      <xsd:element name="radic" type="docEmptyType" />
+      <xsd:element name="prop" type="docEmptyType" />
+      <xsd:element name="infin" type="docEmptyType" />
+      <xsd:element name="ang" type="docEmptyType" />
+      <xsd:element name="and" type="docEmptyType" />
+      <xsd:element name="or" type="docEmptyType" />
+      <xsd:element name="cap" type="docEmptyType" />
+      <xsd:element name="cup" type="docEmptyType" />
+      <xsd:element name="int" type="docEmptyType" />
+      <xsd:element name="there4" type="docEmptyType" />
+      <xsd:element name="sim" type="docEmptyType" />
+      <xsd:element name="cong" type="docEmptyType" />
+      <xsd:element name="asymp" type="docEmptyType" />
+      <xsd:element name="ne" type="docEmptyType" />
+      <xsd:element name="equiv" type="docEmptyType" />
+      <xsd:element name="le" type="docEmptyType" />
+      <xsd:element name="ge" type="docEmptyType" />
+      <xsd:element name="sub" type="docEmptyType" />
+      <xsd:element name="sup" type="docEmptyType" />
+      <xsd:element name="nsub" type="docEmptyType" />
+      <xsd:element name="sube" type="docEmptyType" />
+      <xsd:element name="supe" type="docEmptyType" />
+      <xsd:element name="oplus" type="docEmptyType" />
+      <xsd:element name="otimes" type="docEmptyType" />
+      <xsd:element name="perp" type="docEmptyType" />
+      <xsd:element name="sdot" type="docEmptyType" />
+      <xsd:element name="lceil" type="docEmptyType" />
+      <xsd:element name="rceil" type="docEmptyType" />
+      <xsd:element name="lfloor" type="docEmptyType" />
+      <xsd:element name="rfloor" type="docEmptyType" />
+      <xsd:element name="lang" type="docEmptyType" />
+      <xsd:element name="rang" type="docEmptyType" />
+      <xsd:element name="loz" type="docEmptyType" />
+      <xsd:element name="spades" type="docEmptyType" />
+      <xsd:element name="clubs" type="docEmptyType" />
+      <xsd:element name="hearts" type="docEmptyType" />
+      <xsd:element name="diams" type="docEmptyType" />
+      <xsd:element name="OElig" type="docEmptyType" />
+      <xsd:element name="oelig" type="docEmptyType" />
+      <xsd:element name="Scaron" type="docEmptyType" />
+      <xsd:element name="scaron" type="docEmptyType" />
+      <xsd:element name="Yumlaut" type="docEmptyType" />
+      <xsd:element name="circ" type="docEmptyType" />
+      <xsd:element name="tilde" type="docEmptyType" />
+      <xsd:element name="ensp" type="docEmptyType" />
+      <xsd:element name="emsp" type="docEmptyType" />
+      <xsd:element name="thinsp" type="docEmptyType" />
+      <xsd:element name="zwnj" type="docEmptyType" />
+      <xsd:element name="zwj" type="docEmptyType" />
+      <xsd:element name="lrm" type="docEmptyType" />
+      <xsd:element name="rlm" type="docEmptyType" />
+      <xsd:element name="ndash" type="docEmptyType" />
+      <xsd:element name="mdash" type="docEmptyType" />
+      <xsd:element name="lsquo" type="docEmptyType" />
+      <xsd:element name="rsquo" type="docEmptyType" />
+      <xsd:element name="sbquo" type="docEmptyType" />
+      <xsd:element name="ldquo" type="docEmptyType" />
+      <xsd:element name="rdquo" type="docEmptyType" />
+      <xsd:element name="bdquo" type="docEmptyType" />
+      <xsd:element name="dagger" type="docEmptyType" />
+      <xsd:element name="Dagger" type="docEmptyType" />
+      <xsd:element name="permil" type="docEmptyType" />
+      <xsd:element name="lsaquo" type="docEmptyType" />
+      <xsd:element name="rsaquo" type="docEmptyType" />
+      <xsd:element name="euro" type="docEmptyType" />
+      <xsd:element name="trademark" type="docEmptyType" />
+    </xsd:choice>
+  </xsd:group>
+
+  <xsd:complexType name="docTitleType" mixed="true">
+    <xsd:group ref="docTitleCmdGroup" minOccurs="0" maxOccurs="unbounded" />
+  </xsd:complexType>
+
+  <xsd:group name="docCmdGroup">
+    <xsd:choice>
+      <xsd:group ref="docTitleCmdGroup"/>
+      <xsd:element name="linebreak" type="docEmptyType" />
+      <xsd:element name="hruler" type="docEmptyType" />
+      <xsd:element name="preformatted" type="docMarkupType" />
+      <xsd:element name="programlisting" type="listingType" />
+      <xsd:element name="verbatim" type="xsd:string" />
+      <xsd:element name="indexentry" type="docIndexEntryType" />
+      <xsd:element name="orderedlist" type="docListType" />
+      <xsd:element name="itemizedlist" type="docListType" />
+      <xsd:element name="simplesect" type="docSimpleSectType" />
+      <xsd:element name="title" type="docTitleType" />
+      <xsd:element name="variablelist" type="docVariableListType" />
+      <xsd:element name="table" type="docTableType" />
+      <xsd:element name="heading" type="docHeadingType" />
+      <xsd:element name="image" type="docImageType" />
+      <xsd:element name="dotfile" type="docFileType" />
+      <xsd:element name="mscfile" type="docFileType" />
+      <xsd:element name="diafile" type="docFileType" />
+      <xsd:element name="toclist" type="docTocListType" />
+      <xsd:element name="language" type="docLanguageType" />
+      <xsd:element name="parameterlist" type="docParamListType" />
+      <xsd:element name="xrefsect" type="docXRefSectType" />
+      <xsd:element name="copydoc" type="docCopyType" />
+      <xsd:element name="blockquote" type="docBlockQuoteType" />
+      <xsd:element name="parblock" type="docParBlockType" />
+    </xsd:choice>
+  </xsd:group>
+
+  <xsd:complexType name="docParaType" mixed="true">
+    <xsd:group ref="docCmdGroup" minOccurs="0" maxOccurs="unbounded" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docMarkupType" mixed="true">
+    <xsd:group ref="docCmdGroup" minOccurs="0" maxOccurs="unbounded" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docURLLink" mixed="true">
+    <xsd:group ref="docTitleCmdGroup" minOccurs="0" maxOccurs="unbounded" />
+    <xsd:attribute name="url" type="xsd:string" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docAnchorType" mixed="true">
+    <xsd:attribute name="id" type="xsd:string" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docFormulaType" mixed="true">
+    <xsd:attribute name="id" type="xsd:string" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docIndexEntryType">
+    <xsd:sequence>
+      <xsd:element name="primaryie" type="xsd:string" />
+      <xsd:element name="secondaryie" type="xsd:string" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docListType">
+    <xsd:sequence>
+      <xsd:element name="listitem" type="docListItemType" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docListItemType">
+    <xsd:sequence>
+      <xsd:element name="para" type="docParaType" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docSimpleSectType">
+    <xsd:sequence>
+      <xsd:element name="title" type="docTitleType" minOccurs="0" />
+      <xsd:sequence minOccurs="0" maxOccurs="unbounded">
+        <xsd:element name="para" type="docParaType" minOccurs="1" maxOccurs="unbounded" />
+      </xsd:sequence>
+    </xsd:sequence>
+    <xsd:attribute name="kind" type="DoxSimpleSectKind" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docVarListEntryType">
+    <xsd:sequence>
+      <xsd:element name="term" type="docTitleType" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:group name="docVariableListGroup">
+    <xsd:sequence>
+      <xsd:element name="varlistentry" type="docVarListEntryType" />
+      <xsd:element name="listitem" type="docListItemType" />
+    </xsd:sequence>
+  </xsd:group>
+
+  <xsd:complexType name="docVariableListType">
+    <xsd:sequence>
+      <xsd:group ref="docVariableListGroup" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docRefTextType" mixed="true">
+    <xsd:group ref="docTitleCmdGroup" minOccurs="0" maxOccurs="unbounded" />
+    <xsd:attribute name="refid" type="xsd:string" />
+    <xsd:attribute name="kindref" type="DoxRefKind" />
+    <xsd:attribute name="external" type="xsd:string" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docTableType">
+    <xsd:sequence>
+      <xsd:element name="row" type="docRowType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="caption" type="docCaptionType" minOccurs="0" />
+    </xsd:sequence>
+    <xsd:attribute name="rows" type="xsd:integer" />
+    <xsd:attribute name="cols" type="xsd:integer" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docRowType">
+    <xsd:sequence>
+      <xsd:element name="entry" type="docEntryType" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docEntryType">
+    <xsd:sequence>
+      <xsd:element name="para" type="docParaType" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+    <xsd:attribute name="thead" type="DoxBool" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docCaptionType" mixed="true">
+    <xsd:group ref="docTitleCmdGroup" minOccurs="0" maxOccurs="unbounded" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docHeadingType" mixed="true">
+    <xsd:group ref="docTitleCmdGroup" minOccurs="0" maxOccurs="unbounded" />
+    <xsd:attribute name="level" type="xsd:integer" /> <!-- todo: range 1-6 -->
+  </xsd:complexType>
+
+  <xsd:complexType name="docImageType" mixed="true">
+    <xsd:group ref="docTitleCmdGroup" minOccurs="0" maxOccurs="unbounded" />
+    <xsd:attribute name="type" type="DoxImageKind" /> 
+    <xsd:attribute name="name" type="xsd:string" /> 
+    <xsd:attribute name="width" type="xsd:string" /> 
+    <xsd:attribute name="height" type="xsd:string" /> 
+  </xsd:complexType>
+
+  <xsd:complexType name="docFileType" mixed="true">
+    <xsd:group ref="docTitleCmdGroup" minOccurs="0" maxOccurs="unbounded" />
+    <xsd:attribute name="name" type="xsd:string" /> 
+  </xsd:complexType>
+
+  <xsd:complexType name="docTocItemType" mixed="true">
+    <xsd:group ref="docTitleCmdGroup" minOccurs="0" maxOccurs="unbounded" />
+    <xsd:attribute name="id" type="xsd:string" /> 
+  </xsd:complexType>
+
+  <xsd:complexType name="docTocListType">
+    <xsd:sequence>
+      <xsd:element name="tocitem" type="docTocItemType" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docLanguageType">
+    <xsd:sequence>
+      <xsd:element name="para" type="docParaType" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+    <xsd:attribute name="langid" type="xsd:string" /> 
+  </xsd:complexType>
+
+  <xsd:complexType name="docParamListType">
+    <xsd:sequence>
+      <xsd:element name="parameteritem" type="docParamListItem" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+    <xsd:attribute name="kind" type="DoxParamListKind" /> 
+  </xsd:complexType>
+
+  <xsd:complexType name="docParamListItem">
+    <xsd:sequence>
+      <xsd:element name="parameternamelist" type="docParamNameList" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="parameterdescription" type="descriptionType" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docParamNameList">
+    <xsd:sequence>
+      <xsd:element name="parametertype" type="docParamType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="parametername" type="docParamName" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docParamType" mixed="true">
+    <xsd:sequence>
+      <xsd:element name="ref" type="refTextType" minOccurs="0" maxOccurs="1" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docParamName" mixed="true">
+    <xsd:sequence>
+      <xsd:element name="ref" type="refTextType" minOccurs="0" maxOccurs="1" />
+    </xsd:sequence>
+    <xsd:attribute name="direction" type="DoxParamDir" use="optional" />
+  </xsd:complexType>
+
+  <xsd:complexType name="docXRefSectType">
+    <xsd:sequence>
+      <xsd:element name="xreftitle" type="xsd:string" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="xrefdescription" type="descriptionType" />
+    </xsd:sequence>
+    <xsd:attribute name="id" type="xsd:string" /> 
+  </xsd:complexType>
+
+  <xsd:complexType name="docCopyType">
+    <xsd:sequence>
+      <xsd:element name="para" type="docParaType" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="sect1" type="docSect1Type" minOccurs="0" maxOccurs="unbounded" />
+      <xsd:element name="internal" type="docInternalType" minOccurs="0" />
+    </xsd:sequence>
+    <xsd:attribute name="link" type="xsd:string" /> 
+  </xsd:complexType>
+
+  <xsd:complexType name="docBlockQuoteType">
+    <xsd:sequence>
+      <xsd:element name="para" type="docParaType" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docParBlockType">
+    <xsd:sequence>
+      <xsd:element name="para" type="docParaType" minOccurs="0" maxOccurs="unbounded" />
+    </xsd:sequence>
+  </xsd:complexType>
+
+  <xsd:complexType name="docEmptyType"/>
+
+  <!-- Simple types -->
+
+  <xsd:simpleType name="DoxBool">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="yes" />
+      <xsd:enumeration value="no" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxGraphRelation">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="include" />
+      <xsd:enumeration value="usage" />
+      <xsd:enumeration value="template-instance" />
+      <xsd:enumeration value="public-inheritance" />
+      <xsd:enumeration value="protected-inheritance" />
+      <xsd:enumeration value="private-inheritance" />
+      <xsd:enumeration value="type-constraint" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxRefKind">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="compound" />
+      <xsd:enumeration value="member" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxMemberKind">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="define" />
+      <xsd:enumeration value="property" />
+      <xsd:enumeration value="event" />
+      <xsd:enumeration value="variable" />
+      <xsd:enumeration value="typedef" />
+      <xsd:enumeration value="enum" />
+      <xsd:enumeration value="function" />
+      <xsd:enumeration value="signal" />
+      <xsd:enumeration value="prototype" />
+      <xsd:enumeration value="friend" />
+      <xsd:enumeration value="dcop" />
+      <xsd:enumeration value="slot" />
+      <xsd:enumeration value="interface" />
+      <xsd:enumeration value="service" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxProtectionKind">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="public" />
+      <xsd:enumeration value="protected" />
+      <xsd:enumeration value="private" />
+      <xsd:enumeration value="package" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxLanguage">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="Unknown" />
+      <xsd:enumeration value="IDL" />
+      <xsd:enumeration value="Java" />
+      <xsd:enumeration value="C#" />
+      <xsd:enumeration value="D" />
+      <xsd:enumeration value="PHP" />
+      <xsd:enumeration value="Objective-C" />
+      <xsd:enumeration value="C++" />
+      <xsd:enumeration value="Javascript" />
+      <xsd:enumeration value="Python" />
+      <xsd:enumeration value="Fortran" />
+      <xsd:enumeration value="VHDL" />
+      <xsd:enumeration value="XML" />
+      <xsd:enumeration value="Tcl" />
+      <xsd:enumeration value="Markdown" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxVirtualKind">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="non-virtual" />
+      <xsd:enumeration value="virtual" />
+      <xsd:enumeration value="pure-virtual" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxCompoundKind">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="class" />
+      <xsd:enumeration value="struct" />
+      <xsd:enumeration value="union" />
+      <xsd:enumeration value="interface" />
+      <xsd:enumeration value="protocol" />
+      <xsd:enumeration value="category" />
+      <xsd:enumeration value="exception" />
+      <xsd:enumeration value="service" />
+      <xsd:enumeration value="singleton" />
+      <xsd:enumeration value="module" />
+      <xsd:enumeration value="type" />
+      <xsd:enumeration value="file" />
+      <xsd:enumeration value="namespace" />
+      <xsd:enumeration value="group" />
+      <xsd:enumeration value="page" />
+      <xsd:enumeration value="example" />
+      <xsd:enumeration value="dir" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxSectionKind">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="user-defined" />
+      <xsd:enumeration value="public-type" />
+      <xsd:enumeration value="public-func" />
+      <xsd:enumeration value="public-attrib" />
+      <xsd:enumeration value="public-slot" />
+      <xsd:enumeration value="signal" />
+      <xsd:enumeration value="dcop-func" />
+      <xsd:enumeration value="property" />
+      <xsd:enumeration value="event" />
+      <xsd:enumeration value="public-static-func" />
+      <xsd:enumeration value="public-static-attrib" />
+      <xsd:enumeration value="protected-type" />
+      <xsd:enumeration value="protected-func" />
+      <xsd:enumeration value="protected-attrib" />
+      <xsd:enumeration value="protected-slot" />
+      <xsd:enumeration value="protected-static-func" />
+      <xsd:enumeration value="protected-static-attrib" />
+      <xsd:enumeration value="package-type" />
+      <xsd:enumeration value="package-func" />
+      <xsd:enumeration value="package-attrib" />
+      <xsd:enumeration value="package-static-func" />
+      <xsd:enumeration value="package-static-attrib" />
+      <xsd:enumeration value="private-type" />
+      <xsd:enumeration value="private-func" />
+      <xsd:enumeration value="private-attrib" />
+      <xsd:enumeration value="private-slot" />
+      <xsd:enumeration value="private-static-func" />
+      <xsd:enumeration value="private-static-attrib" />
+      <xsd:enumeration value="friend" />
+      <xsd:enumeration value="related" />
+      <xsd:enumeration value="define" />
+      <xsd:enumeration value="prototype" />
+      <xsd:enumeration value="typedef" />
+      <xsd:enumeration value="enum" />
+      <xsd:enumeration value="func" />
+      <xsd:enumeration value="var" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxHighlightClass">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="comment" />
+      <xsd:enumeration value="normal" />
+      <xsd:enumeration value="preprocessor" />
+      <xsd:enumeration value="keyword" />
+      <xsd:enumeration value="keywordtype" />
+      <xsd:enumeration value="keywordflow" />
+      <xsd:enumeration value="stringliteral" />
+      <xsd:enumeration value="charliteral" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxSimpleSectKind">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="see" />
+      <xsd:enumeration value="return" />
+      <xsd:enumeration value="author" />
+      <xsd:enumeration value="authors" />
+      <xsd:enumeration value="version" />
+      <xsd:enumeration value="since" />
+      <xsd:enumeration value="date" />
+      <xsd:enumeration value="note" />
+      <xsd:enumeration value="warning" />
+      <xsd:enumeration value="pre" />
+      <xsd:enumeration value="post" />
+      <xsd:enumeration value="copyright" />
+      <xsd:enumeration value="invariant" />
+      <xsd:enumeration value="remark" />
+      <xsd:enumeration value="attention" />
+      <xsd:enumeration value="par" />
+      <xsd:enumeration value="rcs" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxVersionNumber">
+    <xsd:restriction base="xsd:string">
+      <xsd:pattern value="\d+\.\d+.*" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxImageKind">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="html" />
+      <xsd:enumeration value="latex" />
+      <xsd:enumeration value="rtf" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxParamListKind">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="param" />
+      <xsd:enumeration value="retval" />
+      <xsd:enumeration value="exception" />
+      <xsd:enumeration value="templateparam" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxCharRange">
+    <xsd:restriction base="xsd:string">
+      <xsd:pattern value="[aeiouncAEIOUNC]" />
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxParamDir">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="in"/>
+      <xsd:enumeration value="out"/>
+      <xsd:enumeration value="inout"/>
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="DoxAccessor">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="retain"/>
+      <xsd:enumeration value="copy"/>
+      <xsd:enumeration value="assign"/>
+      <xsd:enumeration value="weak"/>
+      <xsd:enumeration value="strong"/>
+      <xsd:enumeration value="unretained"/>
+    </xsd:restriction>
+  </xsd:simpleType>
+
+</xsd:schema>
+
diff --git a/wrap/tests/expected-xml-generation/xml/deprecated.xml b/wrap/tests/expected-xml-generation/xml/deprecated.xml
new file mode 100644
index 000000000..9687ded35
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/deprecated.xml
@@ -0,0 +1,15 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="deprecated" kind="page">
+    <compoundname>deprecated</compoundname>
+    <title>Deprecated List</title>
+    <detaileddescription>
+<para><variablelist>
+<varlistentry><term><anchor id="deprecated_1_deprecated000001"/>Member <ref refid="NonlinearFactor_8h_1acd444d887da7c7f7107e8a5caa703b22" kindref="member">ADD_CLONE_NONLINEAR_FACTOR</ref>  (Derived)</term></varlistentry>
+<listitem><para>: will go away shortly - just add the clone function directly  </para></listitem>
+<varlistentry><term><anchor id="deprecated_1_deprecated000002"/>Member <ref refid="classgtsam_1_1NoiseModelFactor_1a16a8a23069ce6ae664f516edc8f36254" kindref="member">gtsam::NoiseModelFactor::get_noiseModel</ref>  () const </term></varlistentry>
+<listitem><para>access to the noise model </para></listitem>
+</variablelist>
+</para>    </detaileddescription>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/dir_59425e443f801f1f2fd8bbe4959a3ccf.xml b/wrap/tests/expected-xml-generation/xml/dir_59425e443f801f1f2fd8bbe4959a3ccf.xml
new file mode 100644
index 000000000..63999b56a
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/dir_59425e443f801f1f2fd8bbe4959a3ccf.xml
@@ -0,0 +1,12 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="dir_59425e443f801f1f2fd8bbe4959a3ccf" kind="dir">
+    <compoundname>tests</compoundname>
+    <innerdir refid="dir_e4787312bc569bb879bb1171628269de">tests/doc-test-files</innerdir>
+    <briefdescription>
+    </briefdescription>
+    <detaileddescription>
+    </detaileddescription>
+    <location file="tests/"/>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/dir_e4787312bc569bb879bb1171628269de.xml b/wrap/tests/expected-xml-generation/xml/dir_e4787312bc569bb879bb1171628269de.xml
new file mode 100644
index 000000000..09382f413
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/dir_e4787312bc569bb879bb1171628269de.xml
@@ -0,0 +1,13 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="dir_e4787312bc569bb879bb1171628269de" kind="dir">
+    <compoundname>tests/doc-test-files</compoundname>
+    <innerfile refid="JacobianFactorQ_8h">JacobianFactorQ.h</innerfile>
+    <innerfile refid="NonlinearFactor_8h">NonlinearFactor.h</innerfile>
+    <briefdescription>
+    </briefdescription>
+    <detaileddescription>
+    </detaileddescription>
+    <location file="tests/doc-test-files/"/>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/index.xml b/wrap/tests/expected-xml-generation/xml/index.xml
new file mode 100644
index 000000000..2913b2e6c
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/index.xml
@@ -0,0 +1,174 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygenindex xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="index.xsd" version="1.8.11">
+  <compound refid="classgtsam_1_1JacobianFactorQ" kind="class"><name>gtsam::JacobianFactorQ</name>
+    <member refid="classgtsam_1_1JacobianFactorQ_1abb244ea915349e3beb40b669fe8317c4" kind="typedef"><name>Base</name></member>
+    <member refid="classgtsam_1_1JacobianFactorQ_1aa64b12e917e0c89d0d0e9782ea9e06a4" kind="typedef"><name>MatrixZD</name></member>
+    <member refid="classgtsam_1_1JacobianFactorQ_1a2dbb8aa1aaf1d46d64181c5aeeb1a06c" kind="typedef"><name>KeyMatrix</name></member>
+    <member refid="classgtsam_1_1JacobianFactorQ_1a4dd3fbba46a6dff75bcf902931a3b769" kind="function"><name>JacobianFactorQ</name></member>
+    <member refid="classgtsam_1_1JacobianFactorQ_1abfb38e58b5b2d1293cf374b7eca2b2d4" kind="function"><name>JacobianFactorQ</name></member>
+    <member refid="classgtsam_1_1JacobianFactorQ_1a549fef1272025c45472f2e4294bd319e" kind="function"><name>JacobianFactorQ</name></member>
+  </compound>
+  <compound refid="classgtsam_1_1NoiseModelFactor" kind="class"><name>gtsam::NoiseModelFactor</name>
+    <member refid="classgtsam_1_1NoiseModelFactor_1a9ac7f77c3ae2d5bfd14d25f4bdc3deaa" kind="typedef"><name>Base</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1a44e242fb0945cf6c60d30c36c3f229af" kind="typedef"><name>This</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1a57d98d52fb6f91ebc54d74746aeb90e4" kind="typedef"><name>shared_ptr</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1a2263eabbefe0eede8c9d10486f61bb4d" kind="variable"><name>noiseModel_</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1ac98d07dd8f7b70e16ccb9a01abf56b9c" kind="friend"><name>boost::serialization::access</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1ad6e8ef3348e042d598617d1d55f20300" kind="function"><name>NoiseModelFactor</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1ae1ed5e0fa6c471b57172f7ebaf552187" kind="function"><name>~NoiseModelFactor</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1a02cea97db30d27b51feaf1f0444d743c" kind="function"><name>NoiseModelFactor</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1a682e0bbaf0c9638b7d0d96148082d1d7" kind="function"><name>print</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1a616d0335db512e9c333cab83b1923399" kind="function"><name>equals</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1af4f62b52fff1a0527ecdb279de023728" kind="function"><name>dim</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1a123e5729d70b4acd6d7e1f70b1a7ced4" kind="function"><name>noiseModel</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1a16a8a23069ce6ae664f516edc8f36254" kind="function"><name>get_noiseModel</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1afd61874915a8076bf83f9d8311fc0e26" kind="function"><name>unwhitenedError</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1a1b3ea51380315bfd63f60d8d9195b0c1" kind="function"><name>whitenedError</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1af6147aff3a33f734435601f6dcafeaf9" kind="function"><name>error</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1a0c83582a0bd04e112e4c0a17cd4ed324" kind="function"><name>linearize</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1a4605224417029e9dc58c5267efa9a46d" kind="function"><name>NoiseModelFactor</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor_1abb60fdb39cb6939b08744605713e153e" kind="function"><name>serialize</name></member>
+  </compound>
+  <compound refid="classgtsam_1_1NoiseModelFactor1" kind="class"><name>gtsam::NoiseModelFactor1</name>
+    <member refid="classgtsam_1_1NoiseModelFactor1_1a37d19c88478e96cf1dfc065229734ece" kind="typedef"><name>X</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor1_1a8ed3a4fbfc8843df8a2f21a805e93ef0" kind="typedef"><name>Base</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor1_1a22cb5d89cc9ec25e2d94004917d890fc" kind="typedef"><name>This</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor1_1ac98d07dd8f7b70e16ccb9a01abf56b9c" kind="friend"><name>boost::serialization::access</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor1_1a85c649f81b216f5b5d7e321febc7762c" kind="function"><name>NoiseModelFactor1</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor1_1aecf0afe14b2af551d62a6e62ee8a8d27" kind="function"><name>~NoiseModelFactor1</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor1_1af7fe05db115c50d7f3bc2aaa1d6c7950" kind="function"><name>key</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor1_1a70a0b32dd5feba3631815614d8386c86" kind="function"><name>NoiseModelFactor1</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor1_1a886bb13688a62b2680c10cdaf7999918" kind="function"><name>unwhitenedError</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor1_1a42ed690bfeafe79ab396126886b05820" kind="function"><name>evaluateError</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor1_1a7f153cb8f238a3be2260979af5b590ab" kind="function"><name>serialize</name></member>
+  </compound>
+  <compound refid="classgtsam_1_1NoiseModelFactor2" kind="class"><name>gtsam::NoiseModelFactor2</name>
+    <member refid="classgtsam_1_1NoiseModelFactor2_1a5fb0fe8b056bad2171f84922c5184ef1" kind="typedef"><name>X1</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor2_1a3b3777f359b34892f33784d73cb62234" kind="typedef"><name>X2</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor2_1a8c2961991a7f2b253874612fc8124c09" kind="typedef"><name>Base</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor2_1a2743c4c486e80127e8eca591ee3ffbed" kind="typedef"><name>This</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor2_1ac98d07dd8f7b70e16ccb9a01abf56b9c" kind="friend"><name>boost::serialization::access</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor2_1a04f5873776c784cafd55b4ac05b4b757" kind="function"><name>NoiseModelFactor2</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor2_1a35453b054509f5679a29a06b59340cf1" kind="function"><name>NoiseModelFactor2</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor2_1ab629a5dc71461efe61d68faaf5a659f3" kind="function"><name>~NoiseModelFactor2</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor2_1ae3b88b40b4ee6e9af9eefcde77bf9768" kind="function"><name>key1</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor2_1a4d27f5d0beaa35e1e4a5f0ef8974c5a6" kind="function"><name>key2</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor2_1a9640bbbb433383f0b40b5c2d9439415e" kind="function"><name>unwhitenedError</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor2_1a9aa6dcd7db6734479e2212213b021ec1" kind="function"><name>evaluateError</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor2_1ae600c15f59be175aca5b059e488d7166" kind="function"><name>serialize</name></member>
+  </compound>
+  <compound refid="classgtsam_1_1NoiseModelFactor3" kind="class"><name>gtsam::NoiseModelFactor3</name>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1a85e11c8ae6f70e0bdf17f9b6405e6501" kind="typedef"><name>X1</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1aaa8424af6ceb0ce3458efe56d52d5187" kind="typedef"><name>X2</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1a9924ba92623c9a10a286ddd1a8b33f2e" kind="typedef"><name>X3</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1a45c5f50294af1756fac4d4f59b7e31e5" kind="typedef"><name>Base</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1a91a0dfdc66913a71ba5daea0ac681745" kind="typedef"><name>This</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1ac98d07dd8f7b70e16ccb9a01abf56b9c" kind="friend"><name>boost::serialization::access</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1a0bf4383cea2cdb0d263e570e98499ac7" kind="function"><name>NoiseModelFactor3</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1ac2a8e8247e7d55768915fac27a4c078c" kind="function"><name>NoiseModelFactor3</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1a4b4dcb0572c11a2f920b67d0ca7ac8b7" kind="function"><name>~NoiseModelFactor3</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1a535f2bec61538509696513b2ab6bbf0b" kind="function"><name>key1</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1af049e3934c4588d78810051272bdd995" kind="function"><name>key2</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1ae8722af2eb7ac282ba82e49fca1264e1" kind="function"><name>key3</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1aab8e3316ef77cecc1bd1909a2c0eed33" kind="function"><name>unwhitenedError</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1a78613881d348f96be1a158b9bb812950" kind="function"><name>evaluateError</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor3_1a61eb4f72c087d840f4c66db44772437b" kind="function"><name>serialize</name></member>
+  </compound>
+  <compound refid="classgtsam_1_1NoiseModelFactor4" kind="class"><name>gtsam::NoiseModelFactor4</name>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1ae43138cc390ef4b2956e334f160df42e" kind="typedef"><name>X1</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1aafd5bc6e588c7e1084897814831713c5" kind="typedef"><name>X2</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1aa07132c1d7f42154fafd026c702ca240" kind="typedef"><name>X3</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1a6ca1a804d790e17f108c8e14df2c5077" kind="typedef"><name>X4</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1a846f05b113e5553fe7b42a9a49906f98" kind="typedef"><name>Base</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1a372757bbf75dee8eaf33b479b31f06a1" kind="typedef"><name>This</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1ac98d07dd8f7b70e16ccb9a01abf56b9c" kind="friend"><name>boost::serialization::access</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1a65adc8d1272df63390168b26f4f7bc70" kind="function"><name>NoiseModelFactor4</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1a8ce3daa05897e12db9e38a9c96043ce3" kind="function"><name>NoiseModelFactor4</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1a3e53af328261232767713e26fa0d35d5" kind="function"><name>~NoiseModelFactor4</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1a4339880be87fa2a533aa07324481f7b6" kind="function"><name>key1</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1acb1d3940d9b5ee1aa7c9c3c3c1a21ed9" kind="function"><name>key2</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1ac1111c7becb0904f141dc07eff1a4e7c" kind="function"><name>key3</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1a6d9ee65e09dfe61a663c60d97f01b341" kind="function"><name>key4</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1a6a79ae22fc40447c8dc183a951243440" kind="function"><name>unwhitenedError</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1a5e4197ee6a113e77d2abc1dbbebb0857" kind="function"><name>evaluateError</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor4_1a51fda42bdf2b1b442b5a8ed204386816" kind="function"><name>serialize</name></member>
+  </compound>
+  <compound refid="classgtsam_1_1NoiseModelFactor5" kind="class"><name>gtsam::NoiseModelFactor5</name>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1a514869f1357a9e05118bfab0c6fa97e2" kind="typedef"><name>X1</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1ad9181205117e8efb56ebe4f297645d3e" kind="typedef"><name>X2</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1aeb3efb9dbda7a2fef53c55a4513ab4ec" kind="typedef"><name>X3</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1a4e272b4742d0477f444995ec73e097c7" kind="typedef"><name>X4</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1afaf024c990892895e5f5ed0b34758f27" kind="typedef"><name>X5</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1a594617cffc3d1a9985cd64e34936a7f5" kind="typedef"><name>Base</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1a19020d6d909366475872911b6dfc16df" kind="typedef"><name>This</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1ac98d07dd8f7b70e16ccb9a01abf56b9c" kind="friend"><name>boost::serialization::access</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1a424c4633f6c1907d0dd3381cffbed82a" kind="function"><name>NoiseModelFactor5</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1a01d2e8a5253a67ceac02320a719c5280" kind="function"><name>NoiseModelFactor5</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1ae16654ff529ddcfedd1b394f7716412d" kind="function"><name>~NoiseModelFactor5</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1a23886196ba086677b1e2168e17a34280" kind="function"><name>key1</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1a464749bd22a65ee29f99452f194eaf54" kind="function"><name>key2</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1a028c4fbddfe09a66ac3780e23bd25fcd" kind="function"><name>key3</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1aac1933a0fa685709867f12cb636c98d3" kind="function"><name>key4</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1a529d24a558fe3e6bd1dc4839c9778f0c" kind="function"><name>key5</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1a701b7a764f18310b4eea14ee023c922b" kind="function"><name>unwhitenedError</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1af0adaca3d551e42404aaf98c3053b009" kind="function"><name>evaluateError</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor5_1a02db3a5d618d3738c5ec277626a2b598" kind="function"><name>serialize</name></member>
+  </compound>
+  <compound refid="classgtsam_1_1NoiseModelFactor6" kind="class"><name>gtsam::NoiseModelFactor6</name>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1a369d9accd53012d84bbbaf87b176222b" kind="typedef"><name>X1</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1a6107bc96fff5d12783429158e79b9390" kind="typedef"><name>X2</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1a76c477f40c2b911a9f28d456ea9164a2" kind="typedef"><name>X3</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1a2a90aff22e4bd88dae81bf824956a113" kind="typedef"><name>X4</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1ae13e19a4b3a7a331d04d5b460e73216f" kind="typedef"><name>X5</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1af44204ef6947a65b973dee5707495601" kind="typedef"><name>X6</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1ad4a36f4c5b39c0ea03806592fc1c3474" kind="typedef"><name>Base</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1a6939c52138ed37f395a57db8b50c71ec" kind="typedef"><name>This</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1ac98d07dd8f7b70e16ccb9a01abf56b9c" kind="friend"><name>boost::serialization::access</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1a15bcca054e397e486fb9dda0d0436cdb" kind="function"><name>NoiseModelFactor6</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1ad3d89a85f0c70d7a424f2060d514cab9" kind="function"><name>NoiseModelFactor6</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1ad62b1867b67dee83e8733392a0aa579b" kind="function"><name>~NoiseModelFactor6</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1a587ce14373742350ff30a78c856ecaa0" kind="function"><name>key1</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1a57f3ba7d334871bcb688727788ac95d3" kind="function"><name>key2</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1a37bbec5754c7c6bd7cee9b3e94d9cbb1" kind="function"><name>key3</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1a675e3df6fed6189dd9b8977907348960" kind="function"><name>key4</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1a926065f63458a5673888e0b53054061d" kind="function"><name>key5</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1a16a7857c857694bc66e19f455ad90a57" kind="function"><name>key6</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1a945d269f9a722d585e3332be23030450" kind="function"><name>unwhitenedError</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1a896a223b88b621bfe9f45bf8e2cd9960" kind="function"><name>evaluateError</name></member>
+    <member refid="classgtsam_1_1NoiseModelFactor6_1ac8f738f856920913a79e5017c1ced402" kind="function"><name>serialize</name></member>
+  </compound>
+  <compound refid="classgtsam_1_1NonlinearFactor" kind="class"><name>gtsam::NonlinearFactor</name>
+    <member refid="classgtsam_1_1NonlinearFactor_1a7c719ae67f222f66569ccb3b526cb9cb" kind="function"><name>NonlinearFactor</name></member>
+    <member refid="classgtsam_1_1NonlinearFactor_1a8a7610dba7e9a6f878268c2769ea0e04" kind="function"><name>NonlinearFactor</name></member>
+    <member refid="classgtsam_1_1NonlinearFactor_1a758285fe17213cbf9c23b9f42fb960e6" kind="function"><name>print</name></member>
+    <member refid="classgtsam_1_1NonlinearFactor_1a38cdabb3302973815e357bd00573734a" kind="function"><name>equals</name></member>
+    <member refid="classgtsam_1_1NonlinearFactor_1ac457810c504de58732d51d61f715c2de" kind="function"><name>~NonlinearFactor</name></member>
+    <member refid="classgtsam_1_1NonlinearFactor_1a8e67e58ff0e4f9d58f07e5eb227adc52" kind="function"><name>error</name></member>
+    <member refid="classgtsam_1_1NonlinearFactor_1a5f0fcf1c93ded207d4a31e8ede07d797" kind="function"><name>dim</name></member>
+    <member refid="classgtsam_1_1NonlinearFactor_1a1968f471dc3ea33aaf0226ff7be5ae8c" kind="function"><name>active</name></member>
+    <member refid="classgtsam_1_1NonlinearFactor_1af2cc20b4ec180610dbf224f2165fa50a" kind="function"><name>linearize</name></member>
+    <member refid="classgtsam_1_1NonlinearFactor_1aa98a30a78ade8489214ce2e5b8302777" kind="function"><name>clone</name></member>
+    <member refid="classgtsam_1_1NonlinearFactor_1a95f4d57e8646b6df634496aaabb683b5" kind="function"><name>rekey</name></member>
+    <member refid="classgtsam_1_1NonlinearFactor_1a5f760600e8608ef15c825a0f8d1d1855" kind="function"><name>rekey</name></member>
+    <member refid="classgtsam_1_1NonlinearFactor_1aa76bae0d7aeb6591a824daa7c5dc2ac6" kind="typedef"><name>Base</name></member>
+    <member refid="classgtsam_1_1NonlinearFactor_1a15c628be5e7433ee4ece0c5fd12d049e" kind="typedef"><name>This</name></member>
+    <member refid="classgtsam_1_1NonlinearFactor_1a3bf72f88f6e726577c89f894bf481515" kind="typedef"><name>shared_ptr</name></member>
+  </compound>
+  <compound refid="structgtsam_1_1traits_3_01JacobianFactorQ_3_01D_00_01ZDim_01_4_01_4" kind="struct"><name>gtsam::traits&lt; JacobianFactorQ&lt; D, ZDim &gt; &gt;</name>
+  </compound>
+  <compound refid="structgtsam_1_1traits_3_01NonlinearFactor_01_4" kind="struct"><name>gtsam::traits&lt; NonlinearFactor &gt;</name>
+  </compound>
+  <compound refid="namespacegtsam" kind="namespace"><name>gtsam</name>
+  </compound>
+  <compound refid="JacobianFactorQ_8h" kind="file"><name>JacobianFactorQ.h</name>
+  </compound>
+  <compound refid="NonlinearFactor_8h" kind="file"><name>NonlinearFactor.h</name>
+    <member refid="NonlinearFactor_8h_1acd444d887da7c7f7107e8a5caa703b22" kind="define"><name>ADD_CLONE_NONLINEAR_FACTOR</name></member>
+  </compound>
+  <compound refid="deprecated" kind="page"><name>deprecated</name>
+  </compound>
+  <compound refid="dir_e4787312bc569bb879bb1171628269de" kind="dir"><name>tests/doc-test-files</name>
+  </compound>
+  <compound refid="dir_59425e443f801f1f2fd8bbe4959a3ccf" kind="dir"><name>tests</name>
+  </compound>
+</doxygenindex>
diff --git a/wrap/tests/expected-xml-generation/xml/index.xsd b/wrap/tests/expected-xml-generation/xml/index.xsd
new file mode 100644
index 000000000..d7ab2a906
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/index.xsd
@@ -0,0 +1,66 @@
+<?xml version='1.0' encoding='utf-8' ?>
+<xsd:schema xmlns:xsd="http://www.w3.org/2001/XMLSchema">
+  <xsd:element name="doxygenindex" type="DoxygenType"/>
+
+  <xsd:complexType name="DoxygenType">
+    <xsd:sequence>
+      <xsd:element name="compound" type="CompoundType" minOccurs="0" maxOccurs="unbounded"/>
+    </xsd:sequence>
+    <xsd:attribute name="version" type="xsd:string" use="required"/>
+  </xsd:complexType>
+
+  <xsd:complexType name="CompoundType">
+    <xsd:sequence>
+      <xsd:element name="name" type="xsd:string"/>
+      <xsd:element name="member" type="MemberType" minOccurs="0" maxOccurs="unbounded"/>
+    </xsd:sequence>
+    <xsd:attribute name="refid" type="xsd:string" use="required"/>
+    <xsd:attribute name="kind" type="CompoundKind" use="required"/>
+  </xsd:complexType>
+
+  <xsd:complexType name="MemberType">
+    <xsd:sequence>
+      <xsd:element name="name" type="xsd:string"/>
+    </xsd:sequence>
+    <xsd:attribute name="refid" type="xsd:string" use="required"/>
+    <xsd:attribute name="kind" type="MemberKind" use="required"/>
+  </xsd:complexType>
+  
+  <xsd:simpleType name="CompoundKind">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="class"/>
+      <xsd:enumeration value="struct"/>
+      <xsd:enumeration value="union"/>
+      <xsd:enumeration value="interface"/>
+      <xsd:enumeration value="protocol"/>
+      <xsd:enumeration value="category"/>
+      <xsd:enumeration value="exception"/>
+      <xsd:enumeration value="file"/>
+      <xsd:enumeration value="namespace"/>
+      <xsd:enumeration value="group"/>
+      <xsd:enumeration value="page"/>
+      <xsd:enumeration value="example"/>
+      <xsd:enumeration value="dir"/>
+    </xsd:restriction>
+  </xsd:simpleType>
+
+  <xsd:simpleType name="MemberKind">
+    <xsd:restriction base="xsd:string">
+      <xsd:enumeration value="define"/>
+      <xsd:enumeration value="property"/>
+      <xsd:enumeration value="event"/>
+      <xsd:enumeration value="variable"/>
+      <xsd:enumeration value="typedef"/>
+      <xsd:enumeration value="enum"/>
+      <xsd:enumeration value="enumvalue"/>
+      <xsd:enumeration value="function"/>
+      <xsd:enumeration value="signal"/>
+      <xsd:enumeration value="prototype"/>
+      <xsd:enumeration value="friend"/>
+      <xsd:enumeration value="dcop"/>
+      <xsd:enumeration value="slot"/>
+    </xsd:restriction>
+  </xsd:simpleType>
+
+</xsd:schema>
+
diff --git a/wrap/tests/expected-xml-generation/xml/namespacegtsam.xml b/wrap/tests/expected-xml-generation/xml/namespacegtsam.xml
new file mode 100644
index 000000000..1eca44ab2
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/namespacegtsam.xml
@@ -0,0 +1,22 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="namespacegtsam" kind="namespace" language="C++">
+    <compoundname>gtsam</compoundname>
+    <innerclass refid="classgtsam_1_1JacobianFactorQ" prot="public">gtsam::JacobianFactorQ</innerclass>
+    <innerclass refid="classgtsam_1_1NoiseModelFactor" prot="public">gtsam::NoiseModelFactor</innerclass>
+    <innerclass refid="classgtsam_1_1NoiseModelFactor1" prot="public">gtsam::NoiseModelFactor1</innerclass>
+    <innerclass refid="classgtsam_1_1NoiseModelFactor2" prot="public">gtsam::NoiseModelFactor2</innerclass>
+    <innerclass refid="classgtsam_1_1NoiseModelFactor3" prot="public">gtsam::NoiseModelFactor3</innerclass>
+    <innerclass refid="classgtsam_1_1NoiseModelFactor4" prot="public">gtsam::NoiseModelFactor4</innerclass>
+    <innerclass refid="classgtsam_1_1NoiseModelFactor5" prot="public">gtsam::NoiseModelFactor5</innerclass>
+    <innerclass refid="classgtsam_1_1NoiseModelFactor6" prot="public">gtsam::NoiseModelFactor6</innerclass>
+    <innerclass refid="classgtsam_1_1NonlinearFactor" prot="public">gtsam::NonlinearFactor</innerclass>
+    <innerclass refid="structgtsam_1_1traits_3_01JacobianFactorQ_3_01D_00_01ZDim_01_4_01_4" prot="public">gtsam::traits&lt; JacobianFactorQ&lt; D, ZDim &gt; &gt;</innerclass>
+    <innerclass refid="structgtsam_1_1traits_3_01NonlinearFactor_01_4" prot="public">gtsam::traits&lt; NonlinearFactor &gt;</innerclass>
+    <briefdescription>
+    </briefdescription>
+    <detaileddescription>
+    </detaileddescription>
+    <location file="tests/doc-test-files/JacobianFactorQ.h" line="22" column="1"/>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/structgtsam_1_1traits_3_01JacobianFactorQ_3_01D_00_01ZDim_01_4_01_4.xml b/wrap/tests/expected-xml-generation/xml/structgtsam_1_1traits_3_01JacobianFactorQ_3_01D_00_01ZDim_01_4_01_4.xml
new file mode 100644
index 000000000..4e83d4b34
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/structgtsam_1_1traits_3_01JacobianFactorQ_3_01D_00_01ZDim_01_4_01_4.xml
@@ -0,0 +1,48 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="structgtsam_1_1traits_3_01JacobianFactorQ_3_01D_00_01ZDim_01_4_01_4" kind="struct" language="C++" prot="public">
+    <compoundname>gtsam::traits&lt; JacobianFactorQ&lt; D, ZDim &gt; &gt;</compoundname>
+    <basecompoundref prot="public" virt="non-virtual">Testable&lt; JacobianFactorQ&lt; D, ZDim &gt; &gt;</basecompoundref>
+    <templateparamlist>
+      <param>
+        <type>size_t</type>
+        <declname>D</declname>
+        <defname>D</defname>
+      </param>
+      <param>
+        <type>size_t</type>
+        <declname>ZDim</declname>
+        <defname>ZDim</defname>
+      </param>
+    </templateparamlist>
+    <briefdescription>
+    </briefdescription>
+    <detaileddescription>
+    </detaileddescription>
+    <inheritancegraph>
+      <node id="159">
+        <label>Testable&lt; JacobianFactorQ&lt; D, ZDim &gt; &gt;</label>
+      </node>
+      <node id="158">
+        <label>gtsam::traits&lt; JacobianFactorQ&lt; D, ZDim &gt; &gt;</label>
+        <link refid="structgtsam_1_1traits_3_01JacobianFactorQ_3_01D_00_01ZDim_01_4_01_4"/>
+        <childnode refid="159" relation="public-inheritance">
+        </childnode>
+      </node>
+    </inheritancegraph>
+    <collaborationgraph>
+      <node id="161">
+        <label>Testable&lt; JacobianFactorQ&lt; D, ZDim &gt; &gt;</label>
+      </node>
+      <node id="160">
+        <label>gtsam::traits&lt; JacobianFactorQ&lt; D, ZDim &gt; &gt;</label>
+        <link refid="structgtsam_1_1traits_3_01JacobianFactorQ_3_01D_00_01ZDim_01_4_01_4"/>
+        <childnode refid="161" relation="public-inheritance">
+        </childnode>
+      </node>
+    </collaborationgraph>
+    <location file="tests/doc-test-files/JacobianFactorQ.h" line="78" column="1" bodyfile="tests/doc-test-files/JacobianFactorQ.h" bodystart="77" bodyend="79"/>
+    <listofallmembers>
+    </listofallmembers>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected-xml-generation/xml/structgtsam_1_1traits_3_01NonlinearFactor_01_4.xml b/wrap/tests/expected-xml-generation/xml/structgtsam_1_1traits_3_01NonlinearFactor_01_4.xml
new file mode 100644
index 000000000..83d7bb22f
--- /dev/null
+++ b/wrap/tests/expected-xml-generation/xml/structgtsam_1_1traits_3_01NonlinearFactor_01_4.xml
@@ -0,0 +1,39 @@
+<?xml version='1.0' encoding='UTF-8' standalone='no'?>
+<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
+  <compounddef id="structgtsam_1_1traits_3_01NonlinearFactor_01_4" kind="struct" language="C++" prot="public">
+    <compoundname>gtsam::traits&lt; NonlinearFactor &gt;</compoundname>
+    <basecompoundref prot="public" virt="non-virtual">Testable&lt; NonlinearFactor &gt;</basecompoundref>
+    <includes refid="NonlinearFactor_8h" local="no">NonlinearFactor.h</includes>
+    <templateparamlist>
+    </templateparamlist>
+    <briefdescription>
+<para>traits </para>    </briefdescription>
+    <detaileddescription>
+    </detaileddescription>
+    <inheritancegraph>
+      <node id="163">
+        <label>Testable&lt; NonlinearFactor &gt;</label>
+      </node>
+      <node id="162">
+        <label>gtsam::traits&lt; NonlinearFactor &gt;</label>
+        <link refid="structgtsam_1_1traits_3_01NonlinearFactor_01_4"/>
+        <childnode refid="163" relation="public-inheritance">
+        </childnode>
+      </node>
+    </inheritancegraph>
+    <collaborationgraph>
+      <node id="165">
+        <label>Testable&lt; NonlinearFactor &gt;</label>
+      </node>
+      <node id="164">
+        <label>gtsam::traits&lt; NonlinearFactor &gt;</label>
+        <link refid="structgtsam_1_1traits_3_01NonlinearFactor_01_4"/>
+        <childnode refid="165" relation="public-inheritance">
+        </childnode>
+      </node>
+    </collaborationgraph>
+    <location file="tests/doc-test-files/NonlinearFactor.h" line="149" column="1" bodyfile="tests/doc-test-files/NonlinearFactor.h" bodystart="149" bodyend="150"/>
+    <listofallmembers>
+    </listofallmembers>
+  </compounddef>
+</doxygen>
diff --git a/wrap/tests/expected/.gitignore b/wrap/tests/expected/.gitignore
deleted file mode 100644
index 6d725d9bc..000000000
--- a/wrap/tests/expected/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-*.m~
diff --git a/wrap/tests/expected/MyFactorPosePoint2.m b/wrap/tests/expected/MyFactorPosePoint2.m
deleted file mode 100644
index b55488b58..000000000
--- a/wrap/tests/expected/MyFactorPosePoint2.m
+++ /dev/null
@@ -1,36 +0,0 @@
-%class MyFactorPosePoint2, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%MyFactorPosePoint2(size_t key1, size_t key2, double measured, Base noiseModel)
-%
-classdef MyFactorPosePoint2 < handle
-  properties
-    ptr_MyFactorPosePoint2 = 0
-  end
-  methods
-    function obj = MyFactorPosePoint2(varargin)
-      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        my_ptr = varargin{2};
-        geometry_wrapper(82, my_ptr);
-      elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base')
-        my_ptr = geometry_wrapper(83, varargin{1}, varargin{2}, varargin{3}, varargin{4});
-      else
-        error('Arguments do not match any overload of MyFactorPosePoint2 constructor');
-      end
-      obj.ptr_MyFactorPosePoint2 = my_ptr;
-    end
-
-    function delete(obj)
-      geometry_wrapper(84, obj.ptr_MyFactorPosePoint2);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected/MyTemplateMatrix.m b/wrap/tests/expected/MyTemplateMatrix.m
deleted file mode 100644
index b17bbdbe7..000000000
--- a/wrap/tests/expected/MyTemplateMatrix.m
+++ /dev/null
@@ -1,156 +0,0 @@
-%class MyTemplateMatrix, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%MyTemplateMatrix()
-%
-%-------Methods-------
-%accept_T(Matrix value) : returns void
-%accept_Tptr(Matrix value) : returns void
-%create_MixedPtrs() : returns pair< Matrix, Matrix >
-%create_ptrs() : returns pair< Matrix, Matrix >
-%return_T(Matrix value) : returns Matrix
-%return_Tptr(Matrix value) : returns Matrix
-%return_ptrs(Matrix p1, Matrix p2) : returns pair< Matrix, Matrix >
-%templatedMethodMatrix(Matrix t) : returns Matrix
-%templatedMethodPoint2(Point2 t) : returns gtsam::Point2
-%templatedMethodPoint3(Point3 t) : returns gtsam::Point3
-%templatedMethodVector(Vector t) : returns Vector
-%
-classdef MyTemplateMatrix < MyBase
-  properties
-    ptr_MyTemplateMatrix = 0
-  end
-  methods
-    function obj = MyTemplateMatrix(varargin)
-      if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        if nargin == 2
-          my_ptr = varargin{2};
-        else
-          my_ptr = geometry_wrapper(62, varargin{2});
-        end
-        base_ptr = geometry_wrapper(61, my_ptr);
-      elseif nargin == 0
-        [ my_ptr, base_ptr ] = geometry_wrapper(63);
-      else
-        error('Arguments do not match any overload of MyTemplateMatrix constructor');
-      end
-      obj = obj@MyBase(uint64(5139824614673773682), base_ptr);
-      obj.ptr_MyTemplateMatrix = my_ptr;
-    end
-
-    function delete(obj)
-      geometry_wrapper(64, obj.ptr_MyTemplateMatrix);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-    function varargout = accept_T(this, varargin)
-      % ACCEPT_T usage: accept_T(Matrix value) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double')
-        geometry_wrapper(65, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.accept_T');
-      end
-    end
-
-    function varargout = accept_Tptr(this, varargin)
-      % ACCEPT_TPTR usage: accept_Tptr(Matrix value) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double')
-        geometry_wrapper(66, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.accept_Tptr');
-      end
-    end
-
-    function varargout = create_MixedPtrs(this, varargin)
-      % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Matrix, Matrix >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(67, this, varargin{:});
-    end
-
-    function varargout = create_ptrs(this, varargin)
-      % CREATE_PTRS usage: create_ptrs() : returns pair< Matrix, Matrix >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(68, this, varargin{:});
-    end
-
-    function varargout = return_T(this, varargin)
-      % RETURN_T usage: return_T(Matrix value) : returns Matrix
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(69, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.return_T');
-      end
-    end
-
-    function varargout = return_Tptr(this, varargin)
-      % RETURN_TPTR usage: return_Tptr(Matrix value) : returns Matrix
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(70, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.return_Tptr');
-      end
-    end
-
-    function varargout = return_ptrs(this, varargin)
-      % RETURN_PTRS usage: return_ptrs(Matrix p1, Matrix p2) : returns pair< Matrix, Matrix >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
-        [ varargout{1} varargout{2} ] = geometry_wrapper(71, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.return_ptrs');
-      end
-    end
-
-    function varargout = templatedMethodMatrix(this, varargin)
-      % TEMPLATEDMETHODMATRIX usage: templatedMethodMatrix(Matrix t) : returns Matrix
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(72, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
-      end
-    end
-
-    function varargout = templatedMethodPoint2(this, varargin)
-      % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns gtsam::Point2
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
-        varargout{1} = geometry_wrapper(73, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
-      end
-    end
-
-    function varargout = templatedMethodPoint3(this, varargin)
-      % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns gtsam::Point3
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
-        varargout{1} = geometry_wrapper(74, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
-      end
-    end
-
-    function varargout = templatedMethodVector(this, varargin)
-      % TEMPLATEDMETHODVECTOR usage: templatedMethodVector(Vector t) : returns Vector
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
-        varargout{1} = geometry_wrapper(75, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
-      end
-    end
-
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected/MyTemplatePoint2.m b/wrap/tests/expected/MyTemplatePoint2.m
deleted file mode 100644
index d908bd365..000000000
--- a/wrap/tests/expected/MyTemplatePoint2.m
+++ /dev/null
@@ -1,156 +0,0 @@
-%class MyTemplatePoint2, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%MyTemplatePoint2()
-%
-%-------Methods-------
-%accept_T(Point2 value) : returns void
-%accept_Tptr(Point2 value) : returns void
-%create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 >
-%create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 >
-%return_T(Point2 value) : returns gtsam::Point2
-%return_Tptr(Point2 value) : returns gtsam::Point2
-%return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 >
-%templatedMethodMatrix(Matrix t) : returns Matrix
-%templatedMethodPoint2(Point2 t) : returns gtsam::Point2
-%templatedMethodPoint3(Point3 t) : returns gtsam::Point3
-%templatedMethodVector(Vector t) : returns Vector
-%
-classdef MyTemplatePoint2 < MyBase
-  properties
-    ptr_MyTemplatePoint2 = 0
-  end
-  methods
-    function obj = MyTemplatePoint2(varargin)
-      if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        if nargin == 2
-          my_ptr = varargin{2};
-        else
-          my_ptr = geometry_wrapper(47, varargin{2});
-        end
-        base_ptr = geometry_wrapper(46, my_ptr);
-      elseif nargin == 0
-        [ my_ptr, base_ptr ] = geometry_wrapper(48);
-      else
-        error('Arguments do not match any overload of MyTemplatePoint2 constructor');
-      end
-      obj = obj@MyBase(uint64(5139824614673773682), base_ptr);
-      obj.ptr_MyTemplatePoint2 = my_ptr;
-    end
-
-    function delete(obj)
-      geometry_wrapper(49, obj.ptr_MyTemplatePoint2);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-    function varargout = accept_T(this, varargin)
-      % ACCEPT_T usage: accept_T(Point2 value) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
-        geometry_wrapper(50, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.accept_T');
-      end
-    end
-
-    function varargout = accept_Tptr(this, varargin)
-      % ACCEPT_TPTR usage: accept_Tptr(Point2 value) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
-        geometry_wrapper(51, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.accept_Tptr');
-      end
-    end
-
-    function varargout = create_MixedPtrs(this, varargin)
-      % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(52, this, varargin{:});
-    end
-
-    function varargout = create_ptrs(this, varargin)
-      % CREATE_PTRS usage: create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(53, this, varargin{:});
-    end
-
-    function varargout = return_T(this, varargin)
-      % RETURN_T usage: return_T(Point2 value) : returns gtsam::Point2
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
-        varargout{1} = geometry_wrapper(54, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.return_T');
-      end
-    end
-
-    function varargout = return_Tptr(this, varargin)
-      % RETURN_TPTR usage: return_Tptr(Point2 value) : returns gtsam::Point2
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
-        varargout{1} = geometry_wrapper(55, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.return_Tptr');
-      end
-    end
-
-    function varargout = return_ptrs(this, varargin)
-      % RETURN_PTRS usage: return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 2 && isa(varargin{1},'gtsam.Point2') && isa(varargin{2},'gtsam.Point2')
-        [ varargout{1} varargout{2} ] = geometry_wrapper(56, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.return_ptrs');
-      end
-    end
-
-    function varargout = templatedMethodMatrix(this, varargin)
-      % TEMPLATEDMETHODMATRIX usage: templatedMethodMatrix(Matrix t) : returns Matrix
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(57, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
-      end
-    end
-
-    function varargout = templatedMethodPoint2(this, varargin)
-      % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns gtsam::Point2
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
-        varargout{1} = geometry_wrapper(58, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
-      end
-    end
-
-    function varargout = templatedMethodPoint3(this, varargin)
-      % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns gtsam::Point3
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
-        varargout{1} = geometry_wrapper(59, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
-      end
-    end
-
-    function varargout = templatedMethodVector(this, varargin)
-      % TEMPLATEDMETHODVECTOR usage: templatedMethodVector(Vector t) : returns Vector
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
-        varargout{1} = geometry_wrapper(60, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
-      end
-    end
-
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected/MyVector12.m b/wrap/tests/expected/MyVector12.m
deleted file mode 100644
index 9d32c285a..000000000
--- a/wrap/tests/expected/MyVector12.m
+++ /dev/null
@@ -1,36 +0,0 @@
-%class MyVector12, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%MyVector12()
-%
-classdef MyVector12 < handle
-  properties
-    ptr_MyVector12 = 0
-  end
-  methods
-    function obj = MyVector12(varargin)
-      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        my_ptr = varargin{2};
-        geometry_wrapper(79, my_ptr);
-      elseif nargin == 0
-        my_ptr = geometry_wrapper(80);
-      else
-        error('Arguments do not match any overload of MyVector12 constructor');
-      end
-      obj.ptr_MyVector12 = my_ptr;
-    end
-
-    function delete(obj)
-      geometry_wrapper(81, obj.ptr_MyVector12);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected/MyVector3.m b/wrap/tests/expected/MyVector3.m
deleted file mode 100644
index ef4b90a00..000000000
--- a/wrap/tests/expected/MyVector3.m
+++ /dev/null
@@ -1,36 +0,0 @@
-%class MyVector3, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%MyVector3()
-%
-classdef MyVector3 < handle
-  properties
-    ptr_MyVector3 = 0
-  end
-  methods
-    function obj = MyVector3(varargin)
-      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        my_ptr = varargin{2};
-        geometry_wrapper(76, my_ptr);
-      elseif nargin == 0
-        my_ptr = geometry_wrapper(77);
-      else
-        error('Arguments do not match any overload of MyVector3 constructor');
-      end
-      obj.ptr_MyVector3 = my_ptr;
-    end
-
-    function delete(obj)
-      geometry_wrapper(78, obj.ptr_MyVector3);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected/Point2.m b/wrap/tests/expected/Point2.m
deleted file mode 100644
index 9f64f2d10..000000000
--- a/wrap/tests/expected/Point2.m
+++ /dev/null
@@ -1,90 +0,0 @@
-%class Point2, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%Point2()
-%Point2(double x, double y)
-%
-%-------Methods-------
-%argChar(char a) : returns void
-%argUChar(unsigned char a) : returns void
-%dim() : returns int
-%returnChar() : returns char
-%vectorConfusion() : returns VectorNotEigen
-%x() : returns double
-%y() : returns double
-%
-classdef Point2 < handle
-  properties
-    ptr_Point2 = 0
-  end
-  methods
-    function obj = Point2(varargin)
-      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        my_ptr = varargin{2};
-        geometry_wrapper(0, my_ptr);
-      elseif nargin == 0
-        my_ptr = geometry_wrapper(1);
-      elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
-        my_ptr = geometry_wrapper(2, varargin{1}, varargin{2});
-      else
-        error('Arguments do not match any overload of Point2 constructor');
-      end
-      obj.ptr_Point2 = my_ptr;
-    end
-
-    function delete(obj)
-      geometry_wrapper(3, obj.ptr_Point2);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-    function varargout = argChar(this, varargin)
-      % ARGCHAR usage: argChar(char a) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      geometry_wrapper(4, this, varargin{:});
-    end
-
-    function varargout = argUChar(this, varargin)
-      % ARGUCHAR usage: argUChar(unsigned char a) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      geometry_wrapper(5, this, varargin{:});
-    end
-
-    function varargout = dim(this, varargin)
-      % DIM usage: dim() : returns int
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(6, this, varargin{:});
-    end
-
-    function varargout = returnChar(this, varargin)
-      % RETURNCHAR usage: returnChar() : returns char
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(7, this, varargin{:});
-    end
-
-    function varargout = vectorConfusion(this, varargin)
-      % VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(8, this, varargin{:});
-    end
-
-    function varargout = x(this, varargin)
-      % X usage: x() : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(9, this, varargin{:});
-    end
-
-    function varargout = y(this, varargin)
-      % Y usage: y() : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(10, this, varargin{:});
-    end
-
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected/Point3.m b/wrap/tests/expected/Point3.m
deleted file mode 100644
index 8a43987b9..000000000
--- a/wrap/tests/expected/Point3.m
+++ /dev/null
@@ -1,107 +0,0 @@
-%class Point3, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%Point3(double x, double y, double z)
-%
-%-------Methods-------
-%norm() : returns double
-%
-%-------Static Methods-------
-%StaticFunctionRet(double z) : returns Point3
-%staticFunction() : returns double
-%
-%-------Serialization Interface-------
-%string_serialize() : returns string
-%string_deserialize(string serialized) : returns Point3
-%
-classdef Point3 < handle
-  properties
-    ptr_Point3 = 0
-  end
-  methods
-    function obj = Point3(varargin)
-      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        my_ptr = varargin{2};
-        geometry_wrapper(11, my_ptr);
-      elseif nargin == 3 && isa(varargin{1},'double') && isa(varargin{2},'double') && isa(varargin{3},'double')
-        my_ptr = geometry_wrapper(12, varargin{1}, varargin{2}, varargin{3});
-      else
-        error('Arguments do not match any overload of Point3 constructor');
-      end
-      obj.ptr_Point3 = my_ptr;
-    end
-
-    function delete(obj)
-      geometry_wrapper(13, obj.ptr_Point3);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-    function varargout = norm(this, varargin)
-      % NORM usage: norm() : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(14, this, varargin{:});
-    end
-
-    function varargout = string_serialize(this, varargin)
-      % STRING_SERIALIZE usage: string_serialize() : returns string
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 0
-        varargout{1} = geometry_wrapper(15, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Point3.string_serialize');
-      end
-    end
-
-    function sobj = saveobj(obj)
-      % SAVEOBJ Saves the object to a matlab-readable format
-      sobj = obj.string_serialize();
-    end
-  end
-
-  methods(Static = true)
-    function varargout = StaticFunctionRet(varargin)
-      % STATICFUNCTIONRET usage: StaticFunctionRet(double z) : returns Point3
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      % 
-      % Usage
-      % STATICFUNCTIONRET(double z)
-      if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(16, varargin{:});
-      else
-        error('Arguments do not match any overload of function Point3.StaticFunctionRet');
-      end
-    end
-
-    function varargout = StaticFunction(varargin)
-      % STATICFUNCTION usage: staticFunction() : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      % 
-      % Usage
-      % STATICFUNCTION()
-      if length(varargin) == 0
-        varargout{1} = geometry_wrapper(17, varargin{:});
-      else
-        error('Arguments do not match any overload of function Point3.StaticFunction');
-      end
-    end
-
-    function varargout = string_deserialize(varargin)
-      % STRING_DESERIALIZE usage: string_deserialize() : returns Point3
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1
-        varargout{1} = geometry_wrapper(18, varargin{:});
-      else
-        error('Arguments do not match any overload of function Point3.string_deserialize');
-      end
-    end
-
-    function obj = loadobj(sobj)
-      % LOADOBJ Saves the object to a matlab-readable format
-      obj = Point3.string_deserialize(sobj);
-    end
-  end
-end
diff --git a/wrap/tests/expected2/+gtsam/Point2.m b/wrap/tests/expected2/+gtsam/Point2.m
deleted file mode 100644
index 05ca8b9a0..000000000
--- a/wrap/tests/expected2/+gtsam/Point2.m
+++ /dev/null
@@ -1,101 +0,0 @@
-%class Point2, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%Point2()
-%Point2(double x, double y)
-%
-%-------Methods-------
-%argChar(char a) : returns void
-%argUChar(unsigned char a) : returns void
-%dim() : returns int
-%eigenArguments(Vector v, Matrix m) : returns void
-%returnChar() : returns char
-%vectorConfusion() : returns VectorNotEigen
-%x() : returns double
-%y() : returns double
-%
-classdef Point2 < handle
-  properties
-    ptr_gtsamPoint2 = 0
-  end
-  methods
-    function obj = Point2(varargin)
-      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        my_ptr = varargin{2};
-        geometry_wrapper(0, my_ptr);
-      elseif nargin == 0
-        my_ptr = geometry_wrapper(1);
-      elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
-        my_ptr = geometry_wrapper(2, varargin{1}, varargin{2});
-      else
-        error('Arguments do not match any overload of gtsam.Point2 constructor');
-      end
-      obj.ptr_gtsamPoint2 = my_ptr;
-    end
-
-    function delete(obj)
-      geometry_wrapper(3, obj.ptr_gtsamPoint2);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-    function varargout = argChar(this, varargin)
-      % ARGCHAR usage: argChar(char a) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      geometry_wrapper(4, this, varargin{:});
-    end
-
-    function varargout = argUChar(this, varargin)
-      % ARGUCHAR usage: argUChar(unsigned char a) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      geometry_wrapper(5, this, varargin{:});
-    end
-
-    function varargout = dim(this, varargin)
-      % DIM usage: dim() : returns int
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(6, this, varargin{:});
-    end
-
-    function varargout = eigenArguments(this, varargin)
-      % EIGENARGUMENTS usage: eigenArguments(Vector v, Matrix m) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},2)==1 && isa(varargin{2},'double')
-        geometry_wrapper(7, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function gtsam.Point2.eigenArguments');
-      end
-    end
-
-    function varargout = returnChar(this, varargin)
-      % RETURNCHAR usage: returnChar() : returns char
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(8, this, varargin{:});
-    end
-
-    function varargout = vectorConfusion(this, varargin)
-      % VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(9, this, varargin{:});
-    end
-
-    function varargout = x(this, varargin)
-      % X usage: x() : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(10, this, varargin{:});
-    end
-
-    function varargout = y(this, varargin)
-      % Y usage: y() : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(11, this, varargin{:});
-    end
-
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected2/+gtsam/Point3.m b/wrap/tests/expected2/+gtsam/Point3.m
deleted file mode 100644
index acdc7239d..000000000
--- a/wrap/tests/expected2/+gtsam/Point3.m
+++ /dev/null
@@ -1,61 +0,0 @@
-%class Point3, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%Point3(double x, double y, double z)
-%
-%-------Methods-------
-%norm() : returns double
-%
-%-------Static Methods-------
-%StaticFunctionRet(double z) : returns gtsam::Point3
-%staticFunction() : returns double
-%
-classdef Point3 < handle
-  properties
-    ptr_gtsamPoint3 = 0
-  end
-  methods
-    function obj = Point3(varargin)
-      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        my_ptr = varargin{2};
-        geometry_wrapper(12, my_ptr);
-      elseif nargin == 3 && isa(varargin{1},'double') && isa(varargin{2},'double') && isa(varargin{3},'double')
-        my_ptr = geometry_wrapper(13, varargin{1}, varargin{2}, varargin{3});
-      else
-        error('Arguments do not match any overload of gtsam.Point3 constructor');
-      end
-      obj.ptr_gtsamPoint3 = my_ptr;
-    end
-
-    function delete(obj)
-      geometry_wrapper(14, obj.ptr_gtsamPoint3);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-    function varargout = norm(this, varargin)
-      % NORM usage: norm() : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(15, this, varargin{:});
-    end
-
-  end
-
-  methods(Static = true)
-    function varargout = StaticFunctionRet(varargin)
-      % STATICFUNCTIONRET usage: StaticFunctionRet(double z) : returns gtsam::Point3
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(16, varargin{:});
-    end
-
-    function varargout = StaticFunction(varargin)
-      % STATICFUNCTION usage: staticFunction() : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(17, varargin{:});
-    end
-
-  end
-end
diff --git a/wrap/tests/expected2/MyBase.m b/wrap/tests/expected2/MyBase.m
deleted file mode 100644
index 98eab005b..000000000
--- a/wrap/tests/expected2/MyBase.m
+++ /dev/null
@@ -1,35 +0,0 @@
-%class MyBase, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-classdef MyBase < handle
-  properties
-    ptr_MyBase = 0
-  end
-  methods
-    function obj = MyBase(varargin)
-      if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        if nargin == 2
-          my_ptr = varargin{2};
-        else
-          my_ptr = geometry_wrapper(42, varargin{2});
-        end
-        geometry_wrapper(41, my_ptr);
-      else
-        error('Arguments do not match any overload of MyBase constructor');
-      end
-      obj.ptr_MyBase = my_ptr;
-    end
-
-    function delete(obj)
-      geometry_wrapper(43, obj.ptr_MyBase);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected2/MyTemplatePoint2.m b/wrap/tests/expected2/MyTemplatePoint2.m
deleted file mode 100644
index 235957963..000000000
--- a/wrap/tests/expected2/MyTemplatePoint2.m
+++ /dev/null
@@ -1,156 +0,0 @@
-%class MyTemplatePoint2, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%MyTemplatePoint2()
-%
-%-------Methods-------
-%accept_T(Point2 value) : returns void
-%accept_Tptr(Point2 value) : returns void
-%create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 >
-%create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 >
-%return_T(Point2 value) : returns gtsam::Point2
-%return_Tptr(Point2 value) : returns gtsam::Point2
-%return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 >
-%templatedMethodMatrix(Matrix t) : returns Matrix
-%templatedMethodPoint2(Point2 t) : returns gtsam::Point2
-%templatedMethodPoint3(Point3 t) : returns gtsam::Point3
-%templatedMethodVector(Vector t) : returns Vector
-%
-classdef MyTemplatePoint2 < MyBase
-  properties
-    ptr_MyTemplatePoint2 = 0
-  end
-  methods
-    function obj = MyTemplatePoint2(varargin)
-      if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        if nargin == 2
-          my_ptr = varargin{2};
-        else
-          my_ptr = geometry_wrapper(45, varargin{2});
-        end
-        base_ptr = geometry_wrapper(44, my_ptr);
-      elseif nargin == 0
-        [ my_ptr, base_ptr ] = geometry_wrapper(46);
-      else
-        error('Arguments do not match any overload of MyTemplatePoint2 constructor');
-      end
-      obj = obj@MyBase(uint64(5139824614673773682), base_ptr);
-      obj.ptr_MyTemplatePoint2 = my_ptr;
-    end
-
-    function delete(obj)
-      geometry_wrapper(47, obj.ptr_MyTemplatePoint2);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-    function varargout = accept_T(this, varargin)
-      % ACCEPT_T usage: accept_T(Point2 value) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
-        geometry_wrapper(48, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.accept_T');
-      end
-    end
-
-    function varargout = accept_Tptr(this, varargin)
-      % ACCEPT_TPTR usage: accept_Tptr(Point2 value) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
-        geometry_wrapper(49, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.accept_Tptr');
-      end
-    end
-
-    function varargout = create_MixedPtrs(this, varargin)
-      % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(50, this, varargin{:});
-    end
-
-    function varargout = create_ptrs(this, varargin)
-      % CREATE_PTRS usage: create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(51, this, varargin{:});
-    end
-
-    function varargout = return_T(this, varargin)
-      % RETURN_T usage: return_T(Point2 value) : returns gtsam::Point2
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
-        varargout{1} = geometry_wrapper(52, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.return_T');
-      end
-    end
-
-    function varargout = return_Tptr(this, varargin)
-      % RETURN_TPTR usage: return_Tptr(Point2 value) : returns gtsam::Point2
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
-        varargout{1} = geometry_wrapper(53, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.return_Tptr');
-      end
-    end
-
-    function varargout = return_ptrs(this, varargin)
-      % RETURN_PTRS usage: return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 2 && isa(varargin{1},'gtsam.Point2') && isa(varargin{2},'gtsam.Point2')
-        [ varargout{1} varargout{2} ] = geometry_wrapper(54, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.return_ptrs');
-      end
-    end
-
-    function varargout = templatedMethodMatrix(this, varargin)
-      % TEMPLATEDMETHODMATRIX usage: templatedMethodMatrix(Matrix t) : returns Matrix
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(55, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
-      end
-    end
-
-    function varargout = templatedMethodPoint2(this, varargin)
-      % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns gtsam::Point2
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
-        varargout{1} = geometry_wrapper(56, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
-      end
-    end
-
-    function varargout = templatedMethodPoint3(this, varargin)
-      % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns gtsam::Point3
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
-        varargout{1} = geometry_wrapper(57, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
-      end
-    end
-
-    function varargout = templatedMethodVector(this, varargin)
-      % TEMPLATEDMETHODVECTOR usage: templatedMethodVector(Vector t) : returns Vector
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
-        varargout{1} = geometry_wrapper(58, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
-      end
-    end
-
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected2/Test.m b/wrap/tests/expected2/Test.m
deleted file mode 100644
index a36cbed48..000000000
--- a/wrap/tests/expected2/Test.m
+++ /dev/null
@@ -1,218 +0,0 @@
-%class Test, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%Test()
-%Test(double a, Matrix b)
-%
-%-------Methods-------
-%arg_EigenConstRef(Matrix value) : returns void
-%create_MixedPtrs() : returns pair< Test, Test >
-%create_ptrs() : returns pair< Test, Test >
-%print() : returns void
-%return_Point2Ptr(bool value) : returns gtsam::Point2
-%return_Test(Test value) : returns Test
-%return_TestPtr(Test value) : returns Test
-%return_bool(bool value) : returns bool
-%return_double(double value) : returns double
-%return_field(Test t) : returns bool
-%return_int(int value) : returns int
-%return_matrix1(Matrix value) : returns Matrix
-%return_matrix2(Matrix value) : returns Matrix
-%return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix >
-%return_ptrs(Test p1, Test p2) : returns pair< Test, Test >
-%return_size_t(size_t value) : returns size_t
-%return_string(string value) : returns string
-%return_vector1(Vector value) : returns Vector
-%return_vector2(Vector value) : returns Vector
-%
-classdef Test < handle
-  properties
-    ptr_Test = 0
-  end
-  methods
-    function obj = Test(varargin)
-      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        my_ptr = varargin{2};
-        geometry_wrapper(18, my_ptr);
-      elseif nargin == 0
-        my_ptr = geometry_wrapper(19);
-      elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
-        my_ptr = geometry_wrapper(20, varargin{1}, varargin{2});
-      else
-        error('Arguments do not match any overload of Test constructor');
-      end
-      obj.ptr_Test = my_ptr;
-    end
-
-    function delete(obj)
-      geometry_wrapper(21, obj.ptr_Test);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-    function varargout = arg_EigenConstRef(this, varargin)
-      % ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double')
-        geometry_wrapper(22, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.arg_EigenConstRef');
-      end
-    end
-
-    function varargout = create_MixedPtrs(this, varargin)
-      % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, Test >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(23, this, varargin{:});
-    end
-
-    function varargout = create_ptrs(this, varargin)
-      % CREATE_PTRS usage: create_ptrs() : returns pair< Test, Test >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:});
-    end
-
-    function varargout = print(this, varargin)
-      % PRINT usage: print() : returns void
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      geometry_wrapper(25, this, varargin{:});
-    end
-
-    function varargout = return_Point2Ptr(this, varargin)
-      % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns gtsam::Point2
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(26, this, varargin{:});
-    end
-
-    function varargout = return_Test(this, varargin)
-      % RETURN_TEST usage: return_Test(Test value) : returns Test
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'Test')
-        varargout{1} = geometry_wrapper(27, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_Test');
-      end
-    end
-
-    function varargout = return_TestPtr(this, varargin)
-      % RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'Test')
-        varargout{1} = geometry_wrapper(28, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_TestPtr');
-      end
-    end
-
-    function varargout = return_bool(this, varargin)
-      % RETURN_BOOL usage: return_bool(bool value) : returns bool
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(29, this, varargin{:});
-    end
-
-    function varargout = return_double(this, varargin)
-      % RETURN_DOUBLE usage: return_double(double value) : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(30, this, varargin{:});
-    end
-
-    function varargout = return_field(this, varargin)
-      % RETURN_FIELD usage: return_field(Test t) : returns bool
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'Test')
-        varargout{1} = geometry_wrapper(31, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_field');
-      end
-    end
-
-    function varargout = return_int(this, varargin)
-      % RETURN_INT usage: return_int(int value) : returns int
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(32, this, varargin{:});
-    end
-
-    function varargout = return_matrix1(this, varargin)
-      % RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(33, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_matrix1');
-      end
-    end
-
-    function varargout = return_matrix2(this, varargin)
-      % RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(34, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_matrix2');
-      end
-    end
-
-    function varargout = return_pair(this, varargin)
-      % RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},2)==1 && isa(varargin{2},'double')
-        [ varargout{1} varargout{2} ] = geometry_wrapper(35, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_pair');
-      end
-    end
-
-    function varargout = return_ptrs(this, varargin)
-      % RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< Test, Test >
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test')
-        [ varargout{1} varargout{2} ] = geometry_wrapper(36, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_ptrs');
-      end
-    end
-
-    function varargout = return_size_t(this, varargin)
-      % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = geometry_wrapper(37, this, varargin{:});
-    end
-
-    function varargout = return_string(this, varargin)
-      % RETURN_STRING usage: return_string(string value) : returns string
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'char')
-        varargout{1} = geometry_wrapper(38, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_string');
-      end
-    end
-
-    function varargout = return_vector1(this, varargin)
-      % RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
-        varargout{1} = geometry_wrapper(39, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_vector1');
-      end
-    end
-
-    function varargout = return_vector2(this, varargin)
-      % RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
-        varargout{1} = geometry_wrapper(40, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function Test.return_vector2');
-      end
-    end
-
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected2/aGlobalFunction.m b/wrap/tests/expected2/aGlobalFunction.m
deleted file mode 100644
index 7246b478b..000000000
--- a/wrap/tests/expected2/aGlobalFunction.m
+++ /dev/null
@@ -1,6 +0,0 @@
-function varargout = aGlobalFunction(varargin)
-      if length(varargin) == 0
-        varargout{1} = geometry_wrapper(83, varargin{:});
-      else
-        error('Arguments do not match any overload of function aGlobalFunction');
-      end
diff --git a/wrap/tests/expected2/geometry_wrapper.cpp b/wrap/tests/expected2/geometry_wrapper.cpp
deleted file mode 100644
index 3251735ff..000000000
--- a/wrap/tests/expected2/geometry_wrapper.cpp
+++ /dev/null
@@ -1,1230 +0,0 @@
-#include <wrap/matlab.h>
-#include <map>
-
-#include <folder/path/to/Test.h>
-#include <gtsam/geometry/Point2.h>
-#include <gtsam/geometry/Point3.h>
-
-typedef MyTemplate<gtsam::Point2> MyTemplatePoint2;
-typedef MyTemplate<Matrix> MyTemplateMatrix;
-typedef MyVector<3> MyVector3;
-typedef MyVector<12> MyVector12;
-typedef MyFactor<gtsam::Pose2, Matrix> MyFactorPosePoint2;
-
-typedef std::set<boost::shared_ptr<gtsam::Point2>*> Collector_gtsamPoint2;
-static Collector_gtsamPoint2 collector_gtsamPoint2;
-typedef std::set<boost::shared_ptr<gtsam::Point3>*> Collector_gtsamPoint3;
-static Collector_gtsamPoint3 collector_gtsamPoint3;
-typedef std::set<boost::shared_ptr<Test>*> Collector_Test;
-static Collector_Test collector_Test;
-typedef std::set<boost::shared_ptr<MyBase>*> Collector_MyBase;
-static Collector_MyBase collector_MyBase;
-typedef std::set<boost::shared_ptr<MyTemplatePoint2>*> Collector_MyTemplatePoint2;
-static Collector_MyTemplatePoint2 collector_MyTemplatePoint2;
-typedef std::set<boost::shared_ptr<MyTemplateMatrix>*> Collector_MyTemplateMatrix;
-static Collector_MyTemplateMatrix collector_MyTemplateMatrix;
-typedef std::set<boost::shared_ptr<MyVector3>*> Collector_MyVector3;
-static Collector_MyVector3 collector_MyVector3;
-typedef std::set<boost::shared_ptr<MyVector12>*> Collector_MyVector12;
-static Collector_MyVector12 collector_MyVector12;
-typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
-static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
-
-void _deleteAllObjects()
-{
-  mstream mout;
-  std::streambuf *outbuf = std::cout.rdbuf(&mout);
-
-  bool anyDeleted = false;
-  { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin();
-      iter != collector_gtsamPoint2.end(); ) {
-    delete *iter;
-    collector_gtsamPoint2.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_gtsamPoint3::iterator iter = collector_gtsamPoint3.begin();
-      iter != collector_gtsamPoint3.end(); ) {
-    delete *iter;
-    collector_gtsamPoint3.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_Test::iterator iter = collector_Test.begin();
-      iter != collector_Test.end(); ) {
-    delete *iter;
-    collector_Test.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyBase::iterator iter = collector_MyBase.begin();
-      iter != collector_MyBase.end(); ) {
-    delete *iter;
-    collector_MyBase.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyTemplatePoint2::iterator iter = collector_MyTemplatePoint2.begin();
-      iter != collector_MyTemplatePoint2.end(); ) {
-    delete *iter;
-    collector_MyTemplatePoint2.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyTemplateMatrix::iterator iter = collector_MyTemplateMatrix.begin();
-      iter != collector_MyTemplateMatrix.end(); ) {
-    delete *iter;
-    collector_MyTemplateMatrix.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin();
-      iter != collector_MyVector3.end(); ) {
-    delete *iter;
-    collector_MyVector3.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin();
-      iter != collector_MyVector12.end(); ) {
-    delete *iter;
-    collector_MyVector12.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
-      iter != collector_MyFactorPosePoint2.end(); ) {
-    delete *iter;
-    collector_MyFactorPosePoint2.erase(iter++);
-    anyDeleted = true;
-  } }
-  if(anyDeleted)
-    cout <<
-      "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\n"
-      "calling destructors, call 'clear all' again if you plan to now recompile a wrap\n"
-      "module, so that your recompiled module is used instead of the old one." << endl;
-  std::cout.rdbuf(outbuf);
-}
-
-void _geometry_RTTIRegister() {
-  const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_geometry_rttiRegistry_created");
-  if(!alreadyCreated) {
-    std::map<std::string, std::string> types;
-    types.insert(std::make_pair(typeid(MyBase).name(), "MyBase"));
-    types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2"));
-    types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), "MyTemplateMatrix"));
-
-    mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry");
-    if(!registry)
-      registry = mxCreateStructMatrix(1, 1, 0, NULL);
-    typedef std::pair<std::string, std::string> StringPair;
-    for(const StringPair& rtti_matlab: types) {
-      int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
-      if(fieldId < 0)
-        mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
-      mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());
-      mxSetFieldByNumber(registry, 0, fieldId, matlabName);
-    }
-    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0)
-      mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
-    mxDestroyArray(registry);
-    
-    mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);
-    if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0)
-      mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
-    mxDestroyArray(newAlreadyCreated);
-  }
-}
-
-void gtsamPoint2_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_gtsamPoint2.insert(self);
-}
-
-void gtsamPoint2_constructor_1(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
-
-  Shared *self = new Shared(new gtsam::Point2());
-  collector_gtsamPoint2.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
-void gtsamPoint2_constructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
-
-  double x = unwrap< double >(in[0]);
-  double y = unwrap< double >(in[1]);
-  Shared *self = new Shared(new gtsam::Point2(x,y));
-  collector_gtsamPoint2.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
-void gtsamPoint2_deconstructor_3(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
-  checkArguments("delete_gtsamPoint2",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_gtsamPoint2::iterator item;
-  item = collector_gtsamPoint2.find(self);
-  if(item != collector_gtsamPoint2.end()) {
-    delete self;
-    collector_gtsamPoint2.erase(item);
-  }
-}
-
-void gtsamPoint2_argChar_4(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("argChar",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
-  char a = unwrap< char >(in[1]);
-  obj->argChar(a);
-}
-
-void gtsamPoint2_argUChar_5(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("argUChar",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
-  unsigned char a = unwrap< unsigned char >(in[1]);
-  obj->argUChar(a);
-}
-
-void gtsamPoint2_dim_6(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("dim",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
-  out[0] = wrap< int >(obj->dim());
-}
-
-void gtsamPoint2_eigenArguments_7(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("eigenArguments",nargout,nargin-1,2);
-  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
-  Vector v = unwrap< Vector >(in[1]);
-  Matrix m = unwrap< Matrix >(in[2]);
-  obj->eigenArguments(v,m);
-}
-
-void gtsamPoint2_returnChar_8(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("returnChar",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
-  out[0] = wrap< char >(obj->returnChar());
-}
-
-void gtsamPoint2_vectorConfusion_9(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("vectorConfusion",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(boost::make_shared<VectorNotEigen>(obj->vectorConfusion()),"VectorNotEigen", false);
-}
-
-void gtsamPoint2_x_10(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("x",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
-  out[0] = wrap< double >(obj->x());
-}
-
-void gtsamPoint2_y_11(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("y",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
-  out[0] = wrap< double >(obj->y());
-}
-
-void gtsamPoint3_collectorInsertAndMakeBase_12(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<gtsam::Point3> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_gtsamPoint3.insert(self);
-}
-
-void gtsamPoint3_constructor_13(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<gtsam::Point3> Shared;
-
-  double x = unwrap< double >(in[0]);
-  double y = unwrap< double >(in[1]);
-  double z = unwrap< double >(in[2]);
-  Shared *self = new Shared(new gtsam::Point3(x,y,z));
-  collector_gtsamPoint3.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
-void gtsamPoint3_deconstructor_14(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<gtsam::Point3> Shared;
-  checkArguments("delete_gtsamPoint3",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_gtsamPoint3::iterator item;
-  item = collector_gtsamPoint3.find(self);
-  if(item != collector_gtsamPoint3.end()) {
-    delete self;
-    collector_gtsamPoint3.erase(item);
-  }
-}
-
-void gtsamPoint3_norm_15(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("norm",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<gtsam::Point3>(in[0], "ptr_gtsamPoint3");
-  out[0] = wrap< double >(obj->norm());
-}
-
-void gtsamPoint3_StaticFunctionRet_16(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("gtsamPoint3.StaticFunctionRet",nargout,nargin,1);
-  double z = unwrap< double >(in[0]);
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point3>(gtsam::Point3::StaticFunctionRet(z)),"gtsam.Point3", false);
-}
-
-void gtsamPoint3_staticFunction_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("gtsamPoint3.staticFunction",nargout,nargin,0);
-  out[0] = wrap< double >(gtsam::Point3::staticFunction());
-}
-
-void Test_collectorInsertAndMakeBase_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<Test> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_Test.insert(self);
-}
-
-void Test_constructor_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<Test> Shared;
-
-  Shared *self = new Shared(new Test());
-  collector_Test.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
-void Test_constructor_20(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<Test> Shared;
-
-  double a = unwrap< double >(in[0]);
-  Matrix b = unwrap< Matrix >(in[1]);
-  Shared *self = new Shared(new Test(a,b));
-  collector_Test.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
-void Test_deconstructor_21(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<Test> Shared;
-  checkArguments("delete_Test",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_Test::iterator item;
-  item = collector_Test.find(self);
-  if(item != collector_Test.end()) {
-    delete self;
-    collector_Test.erase(item);
-  }
-}
-
-void Test_arg_EigenConstRef_22(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("arg_EigenConstRef",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  Matrix value = unwrap< Matrix >(in[1]);
-  obj->arg_EigenConstRef(value);
-}
-
-void Test_create_MixedPtrs_23(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("create_MixedPtrs",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  auto pairResult = obj->create_MixedPtrs();
-  out[0] = wrap_shared_ptr(boost::make_shared<Test>(pairResult.first),"Test", false);
-  out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
-}
-
-void Test_create_ptrs_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("create_ptrs",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  auto pairResult = obj->create_ptrs();
-  out[0] = wrap_shared_ptr(pairResult.first,"Test", false);
-  out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
-}
-
-void Test_print_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("print",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  obj->print();
-}
-
-void Test_return_Point2Ptr_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_Point2Ptr",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  bool value = unwrap< bool >(in[1]);
-  out[0] = wrap_shared_ptr(obj->return_Point2Ptr(value),"gtsam.Point2", false);
-}
-
-void Test_return_Test_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_Test",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  boost::shared_ptr<Test> value = unwrap_shared_ptr< Test >(in[1], "ptr_Test");
-  out[0] = wrap_shared_ptr(boost::make_shared<Test>(obj->return_Test(value)),"Test", false);
-}
-
-void Test_return_TestPtr_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_TestPtr",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  boost::shared_ptr<Test> value = unwrap_shared_ptr< Test >(in[1], "ptr_Test");
-  out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false);
-}
-
-void Test_return_bool_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_bool",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  bool value = unwrap< bool >(in[1]);
-  out[0] = wrap< bool >(obj->return_bool(value));
-}
-
-void Test_return_double_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_double",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  double value = unwrap< double >(in[1]);
-  out[0] = wrap< double >(obj->return_double(value));
-}
-
-void Test_return_field_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_field",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  Test& t = *unwrap_shared_ptr< Test >(in[1], "ptr_Test");
-  out[0] = wrap< bool >(obj->return_field(t));
-}
-
-void Test_return_int_32(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_int",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  int value = unwrap< int >(in[1]);
-  out[0] = wrap< int >(obj->return_int(value));
-}
-
-void Test_return_matrix1_33(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_matrix1",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  Matrix value = unwrap< Matrix >(in[1]);
-  out[0] = wrap< Matrix >(obj->return_matrix1(value));
-}
-
-void Test_return_matrix2_34(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_matrix2",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  Matrix value = unwrap< Matrix >(in[1]);
-  out[0] = wrap< Matrix >(obj->return_matrix2(value));
-}
-
-void Test_return_pair_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_pair",nargout,nargin-1,2);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  Vector v = unwrap< Vector >(in[1]);
-  Matrix A = unwrap< Matrix >(in[2]);
-  auto pairResult = obj->return_pair(v,A);
-  out[0] = wrap< Vector >(pairResult.first);
-  out[1] = wrap< Matrix >(pairResult.second);
-}
-
-void Test_return_ptrs_36(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_ptrs",nargout,nargin-1,2);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  boost::shared_ptr<Test> p1 = unwrap_shared_ptr< Test >(in[1], "ptr_Test");
-  boost::shared_ptr<Test> p2 = unwrap_shared_ptr< Test >(in[2], "ptr_Test");
-  auto pairResult = obj->return_ptrs(p1,p2);
-  out[0] = wrap_shared_ptr(pairResult.first,"Test", false);
-  out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
-}
-
-void Test_return_size_t_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_size_t",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  size_t value = unwrap< size_t >(in[1]);
-  out[0] = wrap< size_t >(obj->return_size_t(value));
-}
-
-void Test_return_string_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_string",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  string value = unwrap< string >(in[1]);
-  out[0] = wrap< string >(obj->return_string(value));
-}
-
-void Test_return_vector1_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_vector1",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  Vector value = unwrap< Vector >(in[1]);
-  out[0] = wrap< Vector >(obj->return_vector1(value));
-}
-
-void Test_return_vector2_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_vector2",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  Vector value = unwrap< Vector >(in[1]);
-  out[0] = wrap< Vector >(obj->return_vector2(value));
-}
-
-void MyBase_collectorInsertAndMakeBase_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyBase> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_MyBase.insert(self);
-}
-
-void MyBase_upcastFromVoid_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyBase> Shared;
-  boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  Shared *self = new Shared(boost::static_pointer_cast<MyBase>(*asVoid));
-  *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
-}
-
-void MyBase_deconstructor_43(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<MyBase> Shared;
-  checkArguments("delete_MyBase",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_MyBase::iterator item;
-  item = collector_MyBase.find(self);
-  if(item != collector_MyBase.end()) {
-    delete self;
-    collector_MyBase.erase(item);
-  }
-}
-
-void MyTemplatePoint2_collectorInsertAndMakeBase_44(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_MyTemplatePoint2.insert(self);
-
-  typedef boost::shared_ptr<MyBase> SharedBase;
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
-}
-
-void MyTemplatePoint2_upcastFromVoid_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
-  boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  Shared *self = new Shared(boost::static_pointer_cast<MyTemplatePoint2>(*asVoid));
-  *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
-}
-
-void MyTemplatePoint2_constructor_46(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
-
-  Shared *self = new Shared(new MyTemplatePoint2());
-  collector_MyTemplatePoint2.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-
-  typedef boost::shared_ptr<MyBase> SharedBase;
-  out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);
-}
-
-void MyTemplatePoint2_deconstructor_47(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
-  checkArguments("delete_MyTemplatePoint2",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_MyTemplatePoint2::iterator item;
-  item = collector_MyTemplatePoint2.find(self);
-  if(item != collector_MyTemplatePoint2.end()) {
-    delete self;
-    collector_MyTemplatePoint2.erase(item);
-  }
-}
-
-void MyTemplatePoint2_accept_T_48(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("accept_T",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  gtsam::Point2& value = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  obj->accept_T(value);
-}
-
-void MyTemplatePoint2_accept_Tptr_49(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("accept_Tptr",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  boost::shared_ptr<gtsam::Point2> value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  obj->accept_Tptr(value);
-}
-
-void MyTemplatePoint2_create_MixedPtrs_50(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("create_MixedPtrs",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  auto pairResult = obj->create_MixedPtrs();
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(pairResult.first),"gtsam.Point2", false);
-  out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false);
-}
-
-void MyTemplatePoint2_create_ptrs_51(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("create_ptrs",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  auto pairResult = obj->create_ptrs();
-  out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false);
-  out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false);
-}
-
-void MyTemplatePoint2_return_T_52(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_T",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  boost::shared_ptr<gtsam::Point2> value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(obj->return_T(value)),"gtsam.Point2", false);
-}
-
-void MyTemplatePoint2_return_Tptr_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_Tptr",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  boost::shared_ptr<gtsam::Point2> value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(obj->return_Tptr(value),"gtsam.Point2", false);
-}
-
-void MyTemplatePoint2_return_ptrs_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_ptrs",nargout,nargin-1,2);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  boost::shared_ptr<gtsam::Point2> p1 = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  boost::shared_ptr<gtsam::Point2> p2 = unwrap_shared_ptr< gtsam::Point2 >(in[2], "ptr_gtsamPoint2");
-  auto pairResult = obj->return_ptrs(p1,p2);
-  out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false);
-  out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false);
-}
-
-void MyTemplatePoint2_templatedMethod_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  Matrix t = unwrap< Matrix >(in[1]);
-  out[0] = wrap< Matrix >(obj->templatedMethod<Matrix>(t));
-}
-
-void MyTemplatePoint2_templatedMethod_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(obj->templatedMethod<gtsam::Point2>(t)),"gtsam.Point2", false);
-}
-
-void MyTemplatePoint2_templatedMethod_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point3>(obj->templatedMethod<gtsam::Point3>(t)),"gtsam.Point3", false);
-}
-
-void MyTemplatePoint2_templatedMethod_58(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("templatedMethodVector",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  Vector t = unwrap< Vector >(in[1]);
-  out[0] = wrap< Vector >(obj->templatedMethod<Vector>(t));
-}
-
-void MyTemplateMatrix_collectorInsertAndMakeBase_59(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_MyTemplateMatrix.insert(self);
-
-  typedef boost::shared_ptr<MyBase> SharedBase;
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
-}
-
-void MyTemplateMatrix_upcastFromVoid_60(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
-  boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  Shared *self = new Shared(boost::static_pointer_cast<MyTemplateMatrix>(*asVoid));
-  *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
-}
-
-void MyTemplateMatrix_constructor_61(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
-
-  Shared *self = new Shared(new MyTemplateMatrix());
-  collector_MyTemplateMatrix.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-
-  typedef boost::shared_ptr<MyBase> SharedBase;
-  out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);
-}
-
-void MyTemplateMatrix_deconstructor_62(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
-  checkArguments("delete_MyTemplateMatrix",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_MyTemplateMatrix::iterator item;
-  item = collector_MyTemplateMatrix.find(self);
-  if(item != collector_MyTemplateMatrix.end()) {
-    delete self;
-    collector_MyTemplateMatrix.erase(item);
-  }
-}
-
-void MyTemplateMatrix_accept_T_63(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("accept_T",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  Matrix value = unwrap< Matrix >(in[1]);
-  obj->accept_T(value);
-}
-
-void MyTemplateMatrix_accept_Tptr_64(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("accept_Tptr",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  Matrix value = unwrap< Matrix >(in[1]);
-  obj->accept_Tptr(value);
-}
-
-void MyTemplateMatrix_create_MixedPtrs_65(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("create_MixedPtrs",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  auto pairResult = obj->create_MixedPtrs();
-  out[0] = wrap< Matrix >(pairResult.first);
-  {
-  boost::shared_ptr<Matrix> shared(pairResult.second);
-  out[1] = wrap_shared_ptr(shared,"Matrix");
-  }
-}
-
-void MyTemplateMatrix_create_ptrs_66(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("create_ptrs",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  auto pairResult = obj->create_ptrs();
-  {
-  boost::shared_ptr<Matrix> shared(pairResult.first);
-  out[0] = wrap_shared_ptr(shared,"Matrix");
-  }
-  {
-  boost::shared_ptr<Matrix> shared(pairResult.second);
-  out[1] = wrap_shared_ptr(shared,"Matrix");
-  }
-}
-
-void MyTemplateMatrix_return_T_67(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_T",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  Matrix value = unwrap< Matrix >(in[1]);
-  out[0] = wrap< Matrix >(obj->return_T(value));
-}
-
-void MyTemplateMatrix_return_Tptr_68(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_Tptr",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  Matrix value = unwrap< Matrix >(in[1]);
-  {
-  boost::shared_ptr<Matrix> shared(obj->return_Tptr(value));
-  out[0] = wrap_shared_ptr(shared,"Matrix");
-  }
-}
-
-void MyTemplateMatrix_return_ptrs_69(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("return_ptrs",nargout,nargin-1,2);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  Matrix p1 = unwrap< Matrix >(in[1]);
-  Matrix p2 = unwrap< Matrix >(in[2]);
-  auto pairResult = obj->return_ptrs(p1,p2);
-  {
-  boost::shared_ptr<Matrix> shared(pairResult.first);
-  out[0] = wrap_shared_ptr(shared,"Matrix");
-  }
-  {
-  boost::shared_ptr<Matrix> shared(pairResult.second);
-  out[1] = wrap_shared_ptr(shared,"Matrix");
-  }
-}
-
-void MyTemplateMatrix_templatedMethod_70(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  Matrix t = unwrap< Matrix >(in[1]);
-  out[0] = wrap< Matrix >(obj->templatedMethod<Matrix>(t));
-}
-
-void MyTemplateMatrix_templatedMethod_71(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(obj->templatedMethod<gtsam::Point2>(t)),"gtsam.Point2", false);
-}
-
-void MyTemplateMatrix_templatedMethod_72(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point3>(obj->templatedMethod<gtsam::Point3>(t)),"gtsam.Point3", false);
-}
-
-void MyTemplateMatrix_templatedMethod_73(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("templatedMethodVector",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  Vector t = unwrap< Vector >(in[1]);
-  out[0] = wrap< Vector >(obj->templatedMethod<Vector>(t));
-}
-
-void MyVector3_collectorInsertAndMakeBase_74(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyVector3> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_MyVector3.insert(self);
-}
-
-void MyVector3_constructor_75(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyVector3> Shared;
-
-  Shared *self = new Shared(new MyVector3());
-  collector_MyVector3.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
-void MyVector3_deconstructor_76(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<MyVector3> Shared;
-  checkArguments("delete_MyVector3",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_MyVector3::iterator item;
-  item = collector_MyVector3.find(self);
-  if(item != collector_MyVector3.end()) {
-    delete self;
-    collector_MyVector3.erase(item);
-  }
-}
-
-void MyVector12_collectorInsertAndMakeBase_77(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyVector12> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_MyVector12.insert(self);
-}
-
-void MyVector12_constructor_78(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyVector12> Shared;
-
-  Shared *self = new Shared(new MyVector12());
-  collector_MyVector12.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
-void MyVector12_deconstructor_79(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<MyVector12> Shared;
-  checkArguments("delete_MyVector12",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_MyVector12::iterator item;
-  item = collector_MyVector12.find(self);
-  if(item != collector_MyVector12.end()) {
-    delete self;
-    collector_MyVector12.erase(item);
-  }
-}
-
-void MyFactorPosePoint2_collectorInsertAndMakeBase_80(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyFactorPosePoint2> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_MyFactorPosePoint2.insert(self);
-}
-
-void MyFactorPosePoint2_constructor_81(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<MyFactorPosePoint2> Shared;
-
-  size_t key1 = unwrap< size_t >(in[0]);
-  size_t key2 = unwrap< size_t >(in[1]);
-  double measured = unwrap< double >(in[2]);
-  boost::shared_ptr<gtsam::noiseModel::Base> noiseModel = unwrap_shared_ptr< gtsam::noiseModel::Base >(in[3], "ptr_gtsamnoiseModelBase");
-  Shared *self = new Shared(new MyFactorPosePoint2(key1,key2,measured,noiseModel));
-  collector_MyFactorPosePoint2.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
-void MyFactorPosePoint2_deconstructor_82(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<MyFactorPosePoint2> Shared;
-  checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_MyFactorPosePoint2::iterator item;
-  item = collector_MyFactorPosePoint2.find(self);
-  if(item != collector_MyFactorPosePoint2.end()) {
-    delete self;
-    collector_MyFactorPosePoint2.erase(item);
-  }
-}
-
-void aGlobalFunction_83(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("aGlobalFunction",nargout,nargin,0);
-  out[0] = wrap< Vector >(aGlobalFunction());
-}
-void overloadedGlobalFunction_84(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("overloadedGlobalFunction",nargout,nargin,1);
-  int a = unwrap< int >(in[0]);
-  out[0] = wrap< Vector >(overloadedGlobalFunction(a));
-}
-void overloadedGlobalFunction_85(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("overloadedGlobalFunction",nargout,nargin,2);
-  int a = unwrap< int >(in[0]);
-  double b = unwrap< double >(in[1]);
-  out[0] = wrap< Vector >(overloadedGlobalFunction(a,b));
-}
-
-void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mstream mout;
-  std::streambuf *outbuf = std::cout.rdbuf(&mout);
-
-  _geometry_RTTIRegister();
-
-  int id = unwrap<int>(in[0]);
-
-  try {
-    switch(id) {
-    case 0:
-      gtsamPoint2_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1);
-      break;
-    case 1:
-      gtsamPoint2_constructor_1(nargout, out, nargin-1, in+1);
-      break;
-    case 2:
-      gtsamPoint2_constructor_2(nargout, out, nargin-1, in+1);
-      break;
-    case 3:
-      gtsamPoint2_deconstructor_3(nargout, out, nargin-1, in+1);
-      break;
-    case 4:
-      gtsamPoint2_argChar_4(nargout, out, nargin-1, in+1);
-      break;
-    case 5:
-      gtsamPoint2_argUChar_5(nargout, out, nargin-1, in+1);
-      break;
-    case 6:
-      gtsamPoint2_dim_6(nargout, out, nargin-1, in+1);
-      break;
-    case 7:
-      gtsamPoint2_eigenArguments_7(nargout, out, nargin-1, in+1);
-      break;
-    case 8:
-      gtsamPoint2_returnChar_8(nargout, out, nargin-1, in+1);
-      break;
-    case 9:
-      gtsamPoint2_vectorConfusion_9(nargout, out, nargin-1, in+1);
-      break;
-    case 10:
-      gtsamPoint2_x_10(nargout, out, nargin-1, in+1);
-      break;
-    case 11:
-      gtsamPoint2_y_11(nargout, out, nargin-1, in+1);
-      break;
-    case 12:
-      gtsamPoint3_collectorInsertAndMakeBase_12(nargout, out, nargin-1, in+1);
-      break;
-    case 13:
-      gtsamPoint3_constructor_13(nargout, out, nargin-1, in+1);
-      break;
-    case 14:
-      gtsamPoint3_deconstructor_14(nargout, out, nargin-1, in+1);
-      break;
-    case 15:
-      gtsamPoint3_norm_15(nargout, out, nargin-1, in+1);
-      break;
-    case 16:
-      gtsamPoint3_StaticFunctionRet_16(nargout, out, nargin-1, in+1);
-      break;
-    case 17:
-      gtsamPoint3_staticFunction_17(nargout, out, nargin-1, in+1);
-      break;
-    case 18:
-      Test_collectorInsertAndMakeBase_18(nargout, out, nargin-1, in+1);
-      break;
-    case 19:
-      Test_constructor_19(nargout, out, nargin-1, in+1);
-      break;
-    case 20:
-      Test_constructor_20(nargout, out, nargin-1, in+1);
-      break;
-    case 21:
-      Test_deconstructor_21(nargout, out, nargin-1, in+1);
-      break;
-    case 22:
-      Test_arg_EigenConstRef_22(nargout, out, nargin-1, in+1);
-      break;
-    case 23:
-      Test_create_MixedPtrs_23(nargout, out, nargin-1, in+1);
-      break;
-    case 24:
-      Test_create_ptrs_24(nargout, out, nargin-1, in+1);
-      break;
-    case 25:
-      Test_print_25(nargout, out, nargin-1, in+1);
-      break;
-    case 26:
-      Test_return_Point2Ptr_26(nargout, out, nargin-1, in+1);
-      break;
-    case 27:
-      Test_return_Test_27(nargout, out, nargin-1, in+1);
-      break;
-    case 28:
-      Test_return_TestPtr_28(nargout, out, nargin-1, in+1);
-      break;
-    case 29:
-      Test_return_bool_29(nargout, out, nargin-1, in+1);
-      break;
-    case 30:
-      Test_return_double_30(nargout, out, nargin-1, in+1);
-      break;
-    case 31:
-      Test_return_field_31(nargout, out, nargin-1, in+1);
-      break;
-    case 32:
-      Test_return_int_32(nargout, out, nargin-1, in+1);
-      break;
-    case 33:
-      Test_return_matrix1_33(nargout, out, nargin-1, in+1);
-      break;
-    case 34:
-      Test_return_matrix2_34(nargout, out, nargin-1, in+1);
-      break;
-    case 35:
-      Test_return_pair_35(nargout, out, nargin-1, in+1);
-      break;
-    case 36:
-      Test_return_ptrs_36(nargout, out, nargin-1, in+1);
-      break;
-    case 37:
-      Test_return_size_t_37(nargout, out, nargin-1, in+1);
-      break;
-    case 38:
-      Test_return_string_38(nargout, out, nargin-1, in+1);
-      break;
-    case 39:
-      Test_return_vector1_39(nargout, out, nargin-1, in+1);
-      break;
-    case 40:
-      Test_return_vector2_40(nargout, out, nargin-1, in+1);
-      break;
-    case 41:
-      MyBase_collectorInsertAndMakeBase_41(nargout, out, nargin-1, in+1);
-      break;
-    case 42:
-      MyBase_upcastFromVoid_42(nargout, out, nargin-1, in+1);
-      break;
-    case 43:
-      MyBase_deconstructor_43(nargout, out, nargin-1, in+1);
-      break;
-    case 44:
-      MyTemplatePoint2_collectorInsertAndMakeBase_44(nargout, out, nargin-1, in+1);
-      break;
-    case 45:
-      MyTemplatePoint2_upcastFromVoid_45(nargout, out, nargin-1, in+1);
-      break;
-    case 46:
-      MyTemplatePoint2_constructor_46(nargout, out, nargin-1, in+1);
-      break;
-    case 47:
-      MyTemplatePoint2_deconstructor_47(nargout, out, nargin-1, in+1);
-      break;
-    case 48:
-      MyTemplatePoint2_accept_T_48(nargout, out, nargin-1, in+1);
-      break;
-    case 49:
-      MyTemplatePoint2_accept_Tptr_49(nargout, out, nargin-1, in+1);
-      break;
-    case 50:
-      MyTemplatePoint2_create_MixedPtrs_50(nargout, out, nargin-1, in+1);
-      break;
-    case 51:
-      MyTemplatePoint2_create_ptrs_51(nargout, out, nargin-1, in+1);
-      break;
-    case 52:
-      MyTemplatePoint2_return_T_52(nargout, out, nargin-1, in+1);
-      break;
-    case 53:
-      MyTemplatePoint2_return_Tptr_53(nargout, out, nargin-1, in+1);
-      break;
-    case 54:
-      MyTemplatePoint2_return_ptrs_54(nargout, out, nargin-1, in+1);
-      break;
-    case 55:
-      MyTemplatePoint2_templatedMethod_55(nargout, out, nargin-1, in+1);
-      break;
-    case 56:
-      MyTemplatePoint2_templatedMethod_56(nargout, out, nargin-1, in+1);
-      break;
-    case 57:
-      MyTemplatePoint2_templatedMethod_57(nargout, out, nargin-1, in+1);
-      break;
-    case 58:
-      MyTemplatePoint2_templatedMethod_58(nargout, out, nargin-1, in+1);
-      break;
-    case 59:
-      MyTemplateMatrix_collectorInsertAndMakeBase_59(nargout, out, nargin-1, in+1);
-      break;
-    case 60:
-      MyTemplateMatrix_upcastFromVoid_60(nargout, out, nargin-1, in+1);
-      break;
-    case 61:
-      MyTemplateMatrix_constructor_61(nargout, out, nargin-1, in+1);
-      break;
-    case 62:
-      MyTemplateMatrix_deconstructor_62(nargout, out, nargin-1, in+1);
-      break;
-    case 63:
-      MyTemplateMatrix_accept_T_63(nargout, out, nargin-1, in+1);
-      break;
-    case 64:
-      MyTemplateMatrix_accept_Tptr_64(nargout, out, nargin-1, in+1);
-      break;
-    case 65:
-      MyTemplateMatrix_create_MixedPtrs_65(nargout, out, nargin-1, in+1);
-      break;
-    case 66:
-      MyTemplateMatrix_create_ptrs_66(nargout, out, nargin-1, in+1);
-      break;
-    case 67:
-      MyTemplateMatrix_return_T_67(nargout, out, nargin-1, in+1);
-      break;
-    case 68:
-      MyTemplateMatrix_return_Tptr_68(nargout, out, nargin-1, in+1);
-      break;
-    case 69:
-      MyTemplateMatrix_return_ptrs_69(nargout, out, nargin-1, in+1);
-      break;
-    case 70:
-      MyTemplateMatrix_templatedMethod_70(nargout, out, nargin-1, in+1);
-      break;
-    case 71:
-      MyTemplateMatrix_templatedMethod_71(nargout, out, nargin-1, in+1);
-      break;
-    case 72:
-      MyTemplateMatrix_templatedMethod_72(nargout, out, nargin-1, in+1);
-      break;
-    case 73:
-      MyTemplateMatrix_templatedMethod_73(nargout, out, nargin-1, in+1);
-      break;
-    case 74:
-      MyVector3_collectorInsertAndMakeBase_74(nargout, out, nargin-1, in+1);
-      break;
-    case 75:
-      MyVector3_constructor_75(nargout, out, nargin-1, in+1);
-      break;
-    case 76:
-      MyVector3_deconstructor_76(nargout, out, nargin-1, in+1);
-      break;
-    case 77:
-      MyVector12_collectorInsertAndMakeBase_77(nargout, out, nargin-1, in+1);
-      break;
-    case 78:
-      MyVector12_constructor_78(nargout, out, nargin-1, in+1);
-      break;
-    case 79:
-      MyVector12_deconstructor_79(nargout, out, nargin-1, in+1);
-      break;
-    case 80:
-      MyFactorPosePoint2_collectorInsertAndMakeBase_80(nargout, out, nargin-1, in+1);
-      break;
-    case 81:
-      MyFactorPosePoint2_constructor_81(nargout, out, nargin-1, in+1);
-      break;
-    case 82:
-      MyFactorPosePoint2_deconstructor_82(nargout, out, nargin-1, in+1);
-      break;
-    case 83:
-      aGlobalFunction_83(nargout, out, nargin-1, in+1);
-      break;
-    case 84:
-      overloadedGlobalFunction_84(nargout, out, nargin-1, in+1);
-      break;
-    case 85:
-      overloadedGlobalFunction_85(nargout, out, nargin-1, in+1);
-      break;
-    }
-  } catch(const std::exception& e) {
-    mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str());
-  }
-
-  std::cout.rdbuf(outbuf);
-}
diff --git a/wrap/tests/expected2/overloadedGlobalFunction.m b/wrap/tests/expected2/overloadedGlobalFunction.m
deleted file mode 100644
index e5b4b21a9..000000000
--- a/wrap/tests/expected2/overloadedGlobalFunction.m
+++ /dev/null
@@ -1,8 +0,0 @@
-function varargout = overloadedGlobalFunction(varargin)
-      if length(varargin) == 1 && isa(varargin{1},'numeric')
-        varargout{1} = geometry_wrapper(84, varargin{:});
-      elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double')
-        varargout{1} = geometry_wrapper(85, varargin{:});
-      else
-        error('Arguments do not match any overload of function overloadedGlobalFunction');
-      end
diff --git a/wrap/tests/expected_namespaces/+ns1/ClassA.m b/wrap/tests/expected_namespaces/+ns1/ClassA.m
deleted file mode 100644
index f7dce6031..000000000
--- a/wrap/tests/expected_namespaces/+ns1/ClassA.m
+++ /dev/null
@@ -1,36 +0,0 @@
-%class ClassA, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%ClassA()
-%
-classdef ClassA < handle
-  properties
-    ptr_ns1ClassA = 0
-  end
-  methods
-    function obj = ClassA(varargin)
-      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        my_ptr = varargin{2};
-        testNamespaces_wrapper(0, my_ptr);
-      elseif nargin == 0
-        my_ptr = testNamespaces_wrapper(1);
-      else
-        error('Arguments do not match any overload of ns1.ClassA constructor');
-      end
-      obj.ptr_ns1ClassA = my_ptr;
-    end
-
-    function delete(obj)
-      testNamespaces_wrapper(2, obj.ptr_ns1ClassA);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected_namespaces/+ns1/ClassB.m b/wrap/tests/expected_namespaces/+ns1/ClassB.m
deleted file mode 100644
index df956a179..000000000
--- a/wrap/tests/expected_namespaces/+ns1/ClassB.m
+++ /dev/null
@@ -1,36 +0,0 @@
-%class ClassB, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%ClassB()
-%
-classdef ClassB < handle
-  properties
-    ptr_ns1ClassB = 0
-  end
-  methods
-    function obj = ClassB(varargin)
-      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        my_ptr = varargin{2};
-        testNamespaces_wrapper(3, my_ptr);
-      elseif nargin == 0
-        my_ptr = testNamespaces_wrapper(4);
-      else
-        error('Arguments do not match any overload of ns1.ClassB constructor');
-      end
-      obj.ptr_ns1ClassB = my_ptr;
-    end
-
-    function delete(obj)
-      testNamespaces_wrapper(5, obj.ptr_ns1ClassB);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected_namespaces/+ns1/aGlobalFunction.m b/wrap/tests/expected_namespaces/+ns1/aGlobalFunction.m
deleted file mode 100644
index a82518ad6..000000000
--- a/wrap/tests/expected_namespaces/+ns1/aGlobalFunction.m
+++ /dev/null
@@ -1,6 +0,0 @@
-function varargout = aGlobalFunction(varargin)
-if length(varargin) == 0
-    varargout{1} = testNamespaces_wrapper(22, varargin{:});
-else
-    error('Arguments do not match any overload of function ns1.aGlobalFunction');
-end
diff --git a/wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m b/wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m
deleted file mode 100644
index 23c3dbacb..000000000
--- a/wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m
+++ /dev/null
@@ -1,36 +0,0 @@
-%class ClassB, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%ClassB()
-%
-classdef ClassB < handle
-  properties
-    ptr_ns2ns3ClassB = 0
-  end
-  methods
-    function obj = ClassB(varargin)
-      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        my_ptr = varargin{2};
-        testNamespaces_wrapper(13, my_ptr);
-      elseif nargin == 0
-        my_ptr = testNamespaces_wrapper(14);
-      else
-        error('Arguments do not match any overload of ns2.ns3.ClassB constructor');
-      end
-      obj.ptr_ns2ns3ClassB = my_ptr;
-    end
-
-    function delete(obj)
-      testNamespaces_wrapper(15, obj.ptr_ns2ns3ClassB);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected_namespaces/+ns2/ClassA.m b/wrap/tests/expected_namespaces/+ns2/ClassA.m
deleted file mode 100644
index 14095899c..000000000
--- a/wrap/tests/expected_namespaces/+ns2/ClassA.m
+++ /dev/null
@@ -1,72 +0,0 @@
-%class ClassA, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%ClassA()
-%
-%-------Methods-------
-%memberFunction() : returns double
-%nsArg(ClassB arg) : returns int
-%nsReturn(double q) : returns ns2::ns3::ClassB
-%
-%-------Static Methods-------
-%afunction() : returns double
-%
-classdef ClassA < handle
-  properties
-    ptr_ns2ClassA = 0
-  end
-  methods
-    function obj = ClassA(varargin)
-      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        my_ptr = varargin{2};
-        testNamespaces_wrapper(6, my_ptr);
-      elseif nargin == 0
-        my_ptr = testNamespaces_wrapper(7);
-      else
-        error('Arguments do not match any overload of ns2.ClassA constructor');
-      end
-      obj.ptr_ns2ClassA = my_ptr;
-    end
-
-    function delete(obj)
-      testNamespaces_wrapper(8, obj.ptr_ns2ClassA);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-    function varargout = memberFunction(this, varargin)
-      % MEMBERFUNCTION usage: memberFunction() : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = testNamespaces_wrapper(9, this, varargin{:});
-    end
-
-    function varargout = nsArg(this, varargin)
-      % NSARG usage: nsArg(ClassB arg) : returns int
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'ns1.ClassB')
-        varargout{1} = testNamespaces_wrapper(10, this, varargin{:});
-      else
-        error('Arguments do not match any overload of function ns2.ClassA.nsArg');
-      end
-    end
-
-    function varargout = nsReturn(this, varargin)
-      % NSRETURN usage: nsReturn(double q) : returns ns2::ns3::ClassB
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = testNamespaces_wrapper(11, this, varargin{:});
-    end
-
-  end
-
-  methods(Static = true)
-    function varargout = Afunction(varargin)
-      % AFUNCTION usage: afunction() : returns double
-      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      varargout{1} = testNamespaces_wrapper(12, varargin{:});
-    end
-
-  end
-end
diff --git a/wrap/tests/expected_namespaces/+ns2/ClassC.m b/wrap/tests/expected_namespaces/+ns2/ClassC.m
deleted file mode 100644
index 19b709f5c..000000000
--- a/wrap/tests/expected_namespaces/+ns2/ClassC.m
+++ /dev/null
@@ -1,36 +0,0 @@
-%class ClassC, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%ClassC()
-%
-classdef ClassC < handle
-  properties
-    ptr_ns2ClassC = 0
-  end
-  methods
-    function obj = ClassC(varargin)
-      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        my_ptr = varargin{2};
-        testNamespaces_wrapper(16, my_ptr);
-      elseif nargin == 0
-        my_ptr = testNamespaces_wrapper(17);
-      else
-        error('Arguments do not match any overload of ns2.ClassC constructor');
-      end
-      obj.ptr_ns2ClassC = my_ptr;
-    end
-
-    function delete(obj)
-      testNamespaces_wrapper(18, obj.ptr_ns2ClassC);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected_namespaces/+ns2/aGlobalFunction.m b/wrap/tests/expected_namespaces/+ns2/aGlobalFunction.m
deleted file mode 100644
index 8b6a5993f..000000000
--- a/wrap/tests/expected_namespaces/+ns2/aGlobalFunction.m
+++ /dev/null
@@ -1,6 +0,0 @@
-function varargout = aGlobalFunction(varargin)
-if length(varargin) == 0
-    varargout{1} = testNamespaces_wrapper(23, varargin{:});
-else
-    error('Arguments do not match any overload of function ns2.aGlobalFunction');
-end
diff --git a/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m b/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m
deleted file mode 100644
index 84f3b8f47..000000000
--- a/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m
+++ /dev/null
@@ -1,8 +0,0 @@
-function varargout = overloadedGlobalFunction(varargin)
-      if length(varargin) == 1 && isa(varargin{1},'ns1.ClassA')
-        varargout{1} = testNamespaces_wrapper(24, varargin{:});
-      elseif length(varargin) == 2 && isa(varargin{1},'ns1.ClassA') && isa(varargin{2},'double')
-        varargout{1} = testNamespaces_wrapper(25, varargin{:});
-      else
-        error('Arguments do not match any overload of function ns2.overloadedGlobalFunction');
-      end
diff --git a/wrap/tests/expected_namespaces/ClassD.m b/wrap/tests/expected_namespaces/ClassD.m
deleted file mode 100644
index 453025961..000000000
--- a/wrap/tests/expected_namespaces/ClassD.m
+++ /dev/null
@@ -1,36 +0,0 @@
-%class ClassD, see Doxygen page for details
-%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-%
-%-------Constructors-------
-%ClassD()
-%
-classdef ClassD < handle
-  properties
-    ptr_ClassD = 0
-  end
-  methods
-    function obj = ClassD(varargin)
-      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
-        my_ptr = varargin{2};
-        testNamespaces_wrapper(19, my_ptr);
-      elseif nargin == 0
-        my_ptr = testNamespaces_wrapper(20);
-      else
-        error('Arguments do not match any overload of ClassD constructor');
-      end
-      obj.ptr_ClassD = my_ptr;
-    end
-
-    function delete(obj)
-      testNamespaces_wrapper(21, obj.ptr_ClassD);
-    end
-
-    function display(obj), obj.print(''); end
-    %DISPLAY Calls print on the object
-    function disp(obj), obj.display; end
-    %DISP Calls print on the object
-  end
-
-  methods(Static = true)
-  end
-end
diff --git a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp
deleted file mode 100644
index 6e0c5670d..000000000
--- a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp
+++ /dev/null
@@ -1,448 +0,0 @@
-#include <wrap/matlab.h>
-#include <map>
-
-#include <path/to/ns1.h>
-#include <path/to/ns1/ClassB.h>
-#include <path/to/ns2.h>
-#include <path/to/ns2/ClassA.h>
-#include <path/to/ns3.h>
-
-
-typedef std::set<boost::shared_ptr<ns1::ClassA>*> Collector_ns1ClassA;
-static Collector_ns1ClassA collector_ns1ClassA;
-typedef std::set<boost::shared_ptr<ns1::ClassB>*> Collector_ns1ClassB;
-static Collector_ns1ClassB collector_ns1ClassB;
-typedef std::set<boost::shared_ptr<ns2::ClassA>*> Collector_ns2ClassA;
-static Collector_ns2ClassA collector_ns2ClassA;
-typedef std::set<boost::shared_ptr<ns2::ns3::ClassB>*> Collector_ns2ns3ClassB;
-static Collector_ns2ns3ClassB collector_ns2ns3ClassB;
-typedef std::set<boost::shared_ptr<ns2::ClassC>*> Collector_ns2ClassC;
-static Collector_ns2ClassC collector_ns2ClassC;
-typedef std::set<boost::shared_ptr<ClassD>*> Collector_ClassD;
-static Collector_ClassD collector_ClassD;
-
-void _deleteAllObjects()
-{
-  mstream mout;
-  std::streambuf *outbuf = std::cout.rdbuf(&mout);
-
-  bool anyDeleted = false;
-  { for(Collector_ns1ClassA::iterator iter = collector_ns1ClassA.begin();
-      iter != collector_ns1ClassA.end(); ) {
-    delete *iter;
-    collector_ns1ClassA.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ns1ClassB::iterator iter = collector_ns1ClassB.begin();
-      iter != collector_ns1ClassB.end(); ) {
-    delete *iter;
-    collector_ns1ClassB.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ns2ClassA::iterator iter = collector_ns2ClassA.begin();
-      iter != collector_ns2ClassA.end(); ) {
-    delete *iter;
-    collector_ns2ClassA.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ns2ns3ClassB::iterator iter = collector_ns2ns3ClassB.begin();
-      iter != collector_ns2ns3ClassB.end(); ) {
-    delete *iter;
-    collector_ns2ns3ClassB.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ns2ClassC::iterator iter = collector_ns2ClassC.begin();
-      iter != collector_ns2ClassC.end(); ) {
-    delete *iter;
-    collector_ns2ClassC.erase(iter++);
-    anyDeleted = true;
-  } }
-  { for(Collector_ClassD::iterator iter = collector_ClassD.begin();
-      iter != collector_ClassD.end(); ) {
-    delete *iter;
-    collector_ClassD.erase(iter++);
-    anyDeleted = true;
-  } }
-  if(anyDeleted)
-    cout <<
-      "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\n"
-      "calling destructors, call 'clear all' again if you plan to now recompile a wrap\n"
-      "module, so that your recompiled module is used instead of the old one." << endl;
-  std::cout.rdbuf(outbuf);
-}
-
-void _testNamespaces_RTTIRegister() {
-  const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_testNamespaces_rttiRegistry_created");
-  if(!alreadyCreated) {
-    std::map<std::string, std::string> types;
-
-    mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry");
-    if(!registry)
-      registry = mxCreateStructMatrix(1, 1, 0, NULL);
-    typedef std::pair<std::string, std::string> StringPair;
-    for(const StringPair& rtti_matlab: types) {
-      int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
-      if(fieldId < 0)
-        mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
-      mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());
-      mxSetFieldByNumber(registry, 0, fieldId, matlabName);
-    }
-    if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0)
-      mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
-    mxDestroyArray(registry);
-    
-    mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);
-    if(mexPutVariable("global", "gtsam_testNamespaces_rttiRegistry_created", newAlreadyCreated) != 0)
-      mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
-    mxDestroyArray(newAlreadyCreated);
-  }
-}
-
-void ns1ClassA_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<ns1::ClassA> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_ns1ClassA.insert(self);
-}
-
-void ns1ClassA_constructor_1(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<ns1::ClassA> Shared;
-
-  Shared *self = new Shared(new ns1::ClassA());
-  collector_ns1ClassA.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
-void ns1ClassA_deconstructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<ns1::ClassA> Shared;
-  checkArguments("delete_ns1ClassA",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_ns1ClassA::iterator item;
-  item = collector_ns1ClassA.find(self);
-  if(item != collector_ns1ClassA.end()) {
-    delete self;
-    collector_ns1ClassA.erase(item);
-  }
-}
-
-void ns1ClassB_collectorInsertAndMakeBase_3(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<ns1::ClassB> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_ns1ClassB.insert(self);
-}
-
-void ns1ClassB_constructor_4(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<ns1::ClassB> Shared;
-
-  Shared *self = new Shared(new ns1::ClassB());
-  collector_ns1ClassB.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
-void ns1ClassB_deconstructor_5(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<ns1::ClassB> Shared;
-  checkArguments("delete_ns1ClassB",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_ns1ClassB::iterator item;
-  item = collector_ns1ClassB.find(self);
-  if(item != collector_ns1ClassB.end()) {
-    delete self;
-    collector_ns1ClassB.erase(item);
-  }
-}
-
-void ns2ClassA_collectorInsertAndMakeBase_6(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<ns2::ClassA> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_ns2ClassA.insert(self);
-}
-
-void ns2ClassA_constructor_7(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<ns2::ClassA> Shared;
-
-  Shared *self = new Shared(new ns2::ClassA());
-  collector_ns2ClassA.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
-void ns2ClassA_deconstructor_8(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<ns2::ClassA> Shared;
-  checkArguments("delete_ns2ClassA",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_ns2ClassA::iterator item;
-  item = collector_ns2ClassA.find(self);
-  if(item != collector_ns2ClassA.end()) {
-    delete self;
-    collector_ns2ClassA.erase(item);
-  }
-}
-
-void ns2ClassA_memberFunction_9(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("memberFunction",nargout,nargin-1,0);
-  auto obj = unwrap_shared_ptr<ns2::ClassA>(in[0], "ptr_ns2ClassA");
-  out[0] = wrap< double >(obj->memberFunction());
-}
-
-void ns2ClassA_nsArg_10(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("nsArg",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<ns2::ClassA>(in[0], "ptr_ns2ClassA");
-  ns1::ClassB& arg = *unwrap_shared_ptr< ns1::ClassB >(in[1], "ptr_ns1ClassB");
-  out[0] = wrap< int >(obj->nsArg(arg));
-}
-
-void ns2ClassA_nsReturn_11(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("nsReturn",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<ns2::ClassA>(in[0], "ptr_ns2ClassA");
-  double q = unwrap< double >(in[1]);
-  out[0] = wrap_shared_ptr(boost::make_shared<ns2::ns3::ClassB>(obj->nsReturn(q)),"ns2.ns3.ClassB", false);
-}
-
-void ns2ClassA_afunction_12(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("ns2ClassA.afunction",nargout,nargin,0);
-  out[0] = wrap< double >(ns2::ClassA::afunction());
-}
-
-void ns2ns3ClassB_collectorInsertAndMakeBase_13(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<ns2::ns3::ClassB> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_ns2ns3ClassB.insert(self);
-}
-
-void ns2ns3ClassB_constructor_14(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<ns2::ns3::ClassB> Shared;
-
-  Shared *self = new Shared(new ns2::ns3::ClassB());
-  collector_ns2ns3ClassB.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
-void ns2ns3ClassB_deconstructor_15(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<ns2::ns3::ClassB> Shared;
-  checkArguments("delete_ns2ns3ClassB",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_ns2ns3ClassB::iterator item;
-  item = collector_ns2ns3ClassB.find(self);
-  if(item != collector_ns2ns3ClassB.end()) {
-    delete self;
-    collector_ns2ns3ClassB.erase(item);
-  }
-}
-
-void ns2ClassC_collectorInsertAndMakeBase_16(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<ns2::ClassC> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_ns2ClassC.insert(self);
-}
-
-void ns2ClassC_constructor_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<ns2::ClassC> Shared;
-
-  Shared *self = new Shared(new ns2::ClassC());
-  collector_ns2ClassC.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
-void ns2ClassC_deconstructor_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<ns2::ClassC> Shared;
-  checkArguments("delete_ns2ClassC",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_ns2ClassC::iterator item;
-  item = collector_ns2ClassC.find(self);
-  if(item != collector_ns2ClassC.end()) {
-    delete self;
-    collector_ns2ClassC.erase(item);
-  }
-}
-
-void ClassD_collectorInsertAndMakeBase_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<ClassD> Shared;
-
-  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_ClassD.insert(self);
-}
-
-void ClassD_constructor_20(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mexAtExit(&_deleteAllObjects);
-  typedef boost::shared_ptr<ClassD> Shared;
-
-  Shared *self = new Shared(new ClassD());
-  collector_ClassD.insert(self);
-  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
-}
-
-void ClassD_deconstructor_21(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  typedef boost::shared_ptr<ClassD> Shared;
-  checkArguments("delete_ClassD",nargout,nargin,1);
-  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
-  Collector_ClassD::iterator item;
-  item = collector_ClassD.find(self);
-  if(item != collector_ClassD.end()) {
-    delete self;
-    collector_ClassD.erase(item);
-  }
-}
-
-void ns1aGlobalFunction_22(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("ns1aGlobalFunction",nargout,nargin,0);
-  out[0] = wrap< Vector >(ns1::aGlobalFunction());
-}
-
-void ns2aGlobalFunction_23(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("ns2aGlobalFunction",nargout,nargin,0);
-  out[0] = wrap< Vector >(ns2::aGlobalFunction());
-}
-void ns2overloadedGlobalFunction_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("ns2overloadedGlobalFunction",nargout,nargin,1);
-  ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA");
-  out[0] = wrap_shared_ptr(boost::make_shared<ns1::ClassA>(ns2::overloadedGlobalFunction(a)),"ns1.ClassA", false);
-}
-void ns2overloadedGlobalFunction_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("ns2overloadedGlobalFunction",nargout,nargin,2);
-  ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA");
-  double b = unwrap< double >(in[1]);
-  out[0] = wrap_shared_ptr(boost::make_shared<ns1::ClassA>(ns2::overloadedGlobalFunction(a,b)),"ns1.ClassA", false);
-}
-
-void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  mstream mout;
-  std::streambuf *outbuf = std::cout.rdbuf(&mout);
-
-  _testNamespaces_RTTIRegister();
-
-  int id = unwrap<int>(in[0]);
-
-  try {
-    switch(id) {
-    case 0:
-      ns1ClassA_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1);
-      break;
-    case 1:
-      ns1ClassA_constructor_1(nargout, out, nargin-1, in+1);
-      break;
-    case 2:
-      ns1ClassA_deconstructor_2(nargout, out, nargin-1, in+1);
-      break;
-    case 3:
-      ns1ClassB_collectorInsertAndMakeBase_3(nargout, out, nargin-1, in+1);
-      break;
-    case 4:
-      ns1ClassB_constructor_4(nargout, out, nargin-1, in+1);
-      break;
-    case 5:
-      ns1ClassB_deconstructor_5(nargout, out, nargin-1, in+1);
-      break;
-    case 6:
-      ns2ClassA_collectorInsertAndMakeBase_6(nargout, out, nargin-1, in+1);
-      break;
-    case 7:
-      ns2ClassA_constructor_7(nargout, out, nargin-1, in+1);
-      break;
-    case 8:
-      ns2ClassA_deconstructor_8(nargout, out, nargin-1, in+1);
-      break;
-    case 9:
-      ns2ClassA_memberFunction_9(nargout, out, nargin-1, in+1);
-      break;
-    case 10:
-      ns2ClassA_nsArg_10(nargout, out, nargin-1, in+1);
-      break;
-    case 11:
-      ns2ClassA_nsReturn_11(nargout, out, nargin-1, in+1);
-      break;
-    case 12:
-      ns2ClassA_afunction_12(nargout, out, nargin-1, in+1);
-      break;
-    case 13:
-      ns2ns3ClassB_collectorInsertAndMakeBase_13(nargout, out, nargin-1, in+1);
-      break;
-    case 14:
-      ns2ns3ClassB_constructor_14(nargout, out, nargin-1, in+1);
-      break;
-    case 15:
-      ns2ns3ClassB_deconstructor_15(nargout, out, nargin-1, in+1);
-      break;
-    case 16:
-      ns2ClassC_collectorInsertAndMakeBase_16(nargout, out, nargin-1, in+1);
-      break;
-    case 17:
-      ns2ClassC_constructor_17(nargout, out, nargin-1, in+1);
-      break;
-    case 18:
-      ns2ClassC_deconstructor_18(nargout, out, nargin-1, in+1);
-      break;
-    case 19:
-      ClassD_collectorInsertAndMakeBase_19(nargout, out, nargin-1, in+1);
-      break;
-    case 20:
-      ClassD_constructor_20(nargout, out, nargin-1, in+1);
-      break;
-    case 21:
-      ClassD_deconstructor_21(nargout, out, nargin-1, in+1);
-      break;
-    case 22:
-      ns1aGlobalFunction_22(nargout, out, nargin-1, in+1);
-      break;
-    case 23:
-      ns2aGlobalFunction_23(nargout, out, nargin-1, in+1);
-      break;
-    case 24:
-      ns2overloadedGlobalFunction_24(nargout, out, nargin-1, in+1);
-      break;
-    case 25:
-      ns2overloadedGlobalFunction_25(nargout, out, nargin-1, in+1);
-      break;
-    }
-  } catch(const std::exception& e) {
-    mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str());
-  }
-
-  std::cout.rdbuf(outbuf);
-}
diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h
index 74db80c81..40d878c9f 100644
--- a/wrap/tests/geometry.h
+++ b/wrap/tests/geometry.h
@@ -2,6 +2,9 @@
 
 class VectorNotEigen;
 virtual class ns::OtherClass;
+class gtsam::NonlinearFactorGraph;
+class gtsam::Values;
+class gtsam::noiseModel::Diagonal;
 
 namespace gtsam {
 
@@ -53,6 +56,7 @@ class Test {
   Test();
 
   pair<Vector,Matrix> return_pair (Vector v, Matrix A) const; // intentionally the first method
+  pair<Vector,Matrix> return_pair (Vector v) const; // overload
 
   bool   return_bool   (bool   value) const; // comment after a line!
   size_t return_size_t (size_t value) const;
@@ -90,6 +94,9 @@ class Test {
   // even more comments at the end!
 };
 
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename, Test* model, int maxID, bool addNoise, bool smart);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename, gtsam::noiseModel::Diagonal* model);
 
 Vector aGlobalFunction();
 
@@ -118,6 +125,8 @@ virtual class MyTemplate : MyBase {
   pair<T*,T*> create_ptrs () const;
   pair<T ,T*> create_MixedPtrs () const;
   pair<T*,T*> return_ptrs (T* p1, T* p2) const;
+
+  static This Level(const T& K);
 };
 
 // A doubly templated class
@@ -129,6 +138,13 @@ class MyFactor {
 // and a typedef specializing it
 typedef MyFactor<gtsam::Pose2, Matrix> MyFactorPosePoint2;
 
+template<T = {double}>
+class PrimitiveRef {
+  PrimitiveRef();
+
+  static This Brutal(const T& t);
+};
+
 // A class with integer template arguments
 template<N = {3,12}>
 class MyVector {
diff --git a/wrap/tests/interface_parser_test.py b/wrap/tests/interface_parser_test.py
new file mode 100644
index 000000000..3e989a519
--- /dev/null
+++ b/wrap/tests/interface_parser_test.py
@@ -0,0 +1,258 @@
+# TODO(duy): make them proper tests!!!
+import unittest
+
+import sys, os
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+from interface_parser import *
+
+
+class TestPyparsing(unittest.TestCase):
+    def test_argument_list(self):
+        arg_string = "int a, C1 c1, C2& c2, C3* c3, "\
+            "const C4 c4, const C5& c5,"\
+            "const C6* c6"
+        args = ArgumentList.rule.parseString(arg_string)
+        print(ArgumentList(args))
+
+
+empty_args = ArgumentList.rule.parseString("")[0]
+print(empty_args)
+
+arg_string = "int a, C1 c1, C2& c2, C3* c3, "\
+    "const C4 c4, const C5& c5,"\
+    "const C6* c6"
+args = ArgumentList.rule.parseString(arg_string)[0]
+print(args)
+
+# Test ReturnType
+ReturnType.rule.parseString("pair<fdsa, rewcds>")[0]
+ReturnType.rule.parseString("cdwdc")[0]
+
+# expect throw
+# ReturnType.parseString("int&")
+# ReturnType.parseString("const int")
+
+ret = Class.rule.parseString("""
+virtual class SymbolicFactorGraph {
+  SymbolicFactorGraph();
+  SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet);
+  SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree);
+
+  // From FactorGraph.
+  void push_back(gtsam::SymbolicFactor* factor);
+  void print(string s) const;
+  bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const;
+  size_t size() const;
+  bool exists(size_t idx) const;
+
+  // Standard interface
+  gtsam::KeySet keys() const;
+  void push_back(const gtsam::SymbolicFactorGraph& graph);
+  void push_back(const gtsam::SymbolicBayesNet& bayesNet);
+  void push_back(const gtsam::SymbolicBayesTree& bayesTree);
+
+  /* Advanced interface */
+  void push_factor(size_t key);
+  void push_factor(size_t key1, size_t key2);
+  void push_factor(size_t key1, size_t key2, size_t key3);
+  void push_factor(size_t key1, size_t key2, size_t key3, size_t key4);
+
+  gtsam::SymbolicBayesNet* eliminateSequential();
+  gtsam::SymbolicBayesNet* eliminateSequential(
+      const gtsam::Ordering& ordering);
+  gtsam::SymbolicBayesTree* eliminateMultifrontal();
+  gtsam::SymbolicBayesTree* eliminateMultifrontal(
+      const gtsam::Ordering& ordering);
+  pair<gtsam::SymbolicBayesNet*, gtsam::SymbolicFactorGraph*>
+      eliminatePartialSequential(const gtsam::Ordering& ordering);
+  pair<gtsam::SymbolicBayesNet*, gtsam::SymbolicFactorGraph*>
+      eliminatePartialSequential(const gtsam::KeyVector& keys);
+  pair<gtsam::SymbolicBayesTree*, gtsam::SymbolicFactorGraph*>
+      eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
+  gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(
+      const gtsam::Ordering& ordering);
+  gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(
+      const gtsam::KeyVector& key_vector,
+      const gtsam::Ordering& marginalizedVariableOrdering);
+  gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector);
+};
+""")[0]
+
+ret = Class.rule.parseString("""
+virtual class Base {
+};
+""")[0]
+
+ret = Class.rule.parseString("""
+virtual class Null: gtsam::noiseModel::mEstimator::Base {
+  Null();
+  void print(string s) const;
+  static gtsam::noiseModel::mEstimator::Null* Create();
+
+  // enabling serialization functionality
+  void serializable() const;
+};
+""")[0]
+
+retFactorIndices = Class.rule.parseString("""
+class FactorIndices {};
+""")[0]
+
+retIsam2 = Class.rule.parseString("""
+class ISAM2 {
+  ISAM2();
+  ISAM2(const gtsam::ISAM2Params& params);
+  ISAM2(const gtsam::ISAM2& other);
+
+  bool equals(const gtsam::ISAM2& other, double tol) const;
+  void print(string s) const;
+  void printStats() const;
+  void saveGraph(string s) const;
+
+  gtsam::ISAM2Result update();
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
+    const gtsam::Values& newTheta);
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
+    const gtsam::Values& newTheta, const gtsam::FactorIndices&
+    removeFactorIndices);
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
+    const gtsam::Values& newTheta,
+    const gtsam::FactorIndices& removeFactorIndices,
+    const gtsam::KeyGroupMap& constrainedKeys);
+
+  gtsam::Values getLinearizationPoint() const;
+  gtsam::Values calculateEstimate() const;
+  template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
+                     gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
+                     gtsam::Cal3Bundler, gtsam::EssentialMatrix,
+                     gtsam::SimpleCamera, Vector, Matrix}>
+  VALUE calculateEstimate(size_t key) const;
+  gtsam::Values calculateBestEstimate() const;
+  Matrix marginalCovariance(size_t key) const;
+  gtsam::VectorValues getDelta() const;
+  gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
+  gtsam::VariableIndex getVariableIndex() const;
+  gtsam::ISAM2Params params() const;
+};
+""")[0]
+# if __name__ == '__main__':
+#     unittest.main()
+
+typename = Typename.rule.parseString("rew")[0]
+ret = ReturnType.rule.parseString("pair<fdsa, rewcds>")[0]
+ret1 = Method.rule.parseString(
+    "int f(const int x, const Class& c, Class* t) const;")[0]
+ret = Method.rule.parseString("int f() const;")[0]
+
+ret1 = StaticMethod.rule.parseString(
+    "static int f(const int x, const Class& c, Class* t);")[0]
+ret = StaticMethod.rule.parseString("static int f();")[0]
+ret1 = Constructor.rule.parseString(
+    "f(const int x, const Class& c, Class* t);")[0]
+ret = Constructor.rule.parseString("f();")[0]
+
+typedef = TypedefTemplateInstantiation.rule.parseString("""
+typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2>
+    BearingFactor2D;
+""")[0]
+
+include = Include.rule.parseString("#include <gtsam/slam/PriorFactor.h>")[0]
+print(include)
+
+fwd = ForwardDeclaration.rule.parseString(
+    "virtual class Test:gtsam::Point3;")[0]
+
+func = GlobalFunction.rule.parseString("""
+gtsam::Values localToWorld(const gtsam::Values& local,
+    const gtsam::Pose2& base, const gtsam::KeyVector& keys);
+""")[0]
+print(func)
+
+try:
+    namespace = Namespace.rule.parseString("""
+namespace gtsam {
+#include <gtsam/geometry/Point2.h>
+class Point2 {
+Point2();
+Point2(double x, double y);
+double x() const;
+double y() const;
+int dim() const;
+char returnChar() const;
+void argChar(char a) const;
+void argUChar(unsigned char a) const;
+void eigenArguments(Vector v, Matrix m) const;
+VectorNotEigen vectorConfusion();
+};
+
+#include <gtsam/geometry/Point3.h>
+class Point3 {
+Point3(double x, double y, double z);
+double norm() const;
+
+// static functions - use static keyword and uppercase
+static double staticFunction();
+static gtsam::Point3 StaticFunctionRet(double z);
+
+// enabling serialization functionality
+void serialize() const; // Just triggers a flag internally
+};
+
+}
+    """)
+except ParseException as pe:
+    print(pe.markInputline())
+
+# filename = "tools/workspace/pybind_wrapper/gtsam.h"
+# with open(filename, "r") as f:
+#     content = f.read()
+# module = Module.parseString(content)
+
+module = Module.parseString("""
+namespace one {
+    namespace two {
+        namespace three {
+            class Class123 {
+            };
+        }
+        class Class12a {
+        };
+    }
+    namespace two_dummy {
+        namespace three_dummy{
+
+        }
+        namespace fourth_dummy{
+
+        }
+    }
+    namespace two {
+        class Class12b {
+
+        };
+    }
+}
+
+class Global{
+};
+""")
+
+print("module: ", module)
+
+sub_namespace = find_sub_namespace(module, ['one', 'two', 'three'])
+print("Found namespace:", sub_namespace[0].name)
+print(find_sub_namespace(module, ['one', 'two_test', 'three']))
+print(find_sub_namespace(module, ['one', 'two']))
+
+found_class = module.find_class(
+    Typename(namespaces_name=['one', 'two', 'three', 'Class123']))
+print(found_class)
+
+found_class = module.find_class(
+    Typename(namespaces_name=['one', 'two', 'Class12b']))
+print(found_class.name)
+
+found_class = module.find_class(
+    Typename(namespaces_name=['one', 'two', 'Class12a']))
+print(found_class.name)
diff --git a/wrap/tests/pybind_wrapper.tpl b/wrap/tests/pybind_wrapper.tpl
new file mode 100644
index 000000000..2674824b8
--- /dev/null
+++ b/wrap/tests/pybind_wrapper.tpl
@@ -0,0 +1,28 @@
+{include_boost}
+
+#include <pybind11/eigen.h>
+#include <pybind11/stl_bind.h>
+#include <pybind11/pybind11.h>
+#include "gtsam/nonlinear/utilities.h"  // for RedirectCout.
+
+{includes}
+#include "wrap/serialization.h"
+#include <boost/serialization/export.hpp>
+
+{boost_class_export}
+
+{hoder_type}
+
+using namespace std;
+
+namespace py = pybind11;
+
+PYBIND11_MODULE({module_name}, m_) {{
+    m_.doc() = "pybind11 wrapper of {module_name}";
+
+{wrapped_namespace}
+
+#include "python/specializations.h"
+
+}}
+
diff --git a/wrap/tests/pybind_wrapper_test.cc b/wrap/tests/pybind_wrapper_test.cc
new file mode 100644
index 000000000..aa216aeb0
--- /dev/null
+++ b/wrap/tests/pybind_wrapper_test.cc
@@ -0,0 +1,23 @@
+#include "tools/workspace/pybind_wrapper/test/pybind_wrapper_test.h"
+
+namespace anzu {
+
+namespace sub {
+double Point2::sum() const { return x_ + y_; }
+}  // namespace sub
+
+double Point3::sum() const { return x_ + y_ + z_; }
+
+double global_func_on_base(const std::shared_ptr<PointBase>& point) {
+  return point->sum();
+}
+
+}  // namespace anzu
+
+double global_func_overloads(const std::shared_ptr<anzu::sub::Point2>& point2) {
+  return point2->sum();
+}
+
+double global_func_overloads(const std::shared_ptr<anzu::Point3>& point3) {
+  return point3->sum();
+}
diff --git a/wrap/tests/pybind_wrapper_test.gth b/wrap/tests/pybind_wrapper_test.gth
new file mode 100644
index 000000000..52d715bc3
--- /dev/null
+++ b/wrap/tests/pybind_wrapper_test.gth
@@ -0,0 +1,82 @@
+#include <tools/workspace/pybind_wrapper/test/pybind_wrapper_test.h>
+
+namespace anzu {
+
+virtual class PointBase {};
+
+namespace sub {
+class Point2 : anzu::PointBase {
+  Point2(double x);  // use the default value.
+  Point2(double x, double y);
+  double x() const;
+  double y() const;
+  double sum() const;
+  double func_with_default_args(double a) const;  // use the default value.
+  double func_with_default_args(double a, double b) const;
+  void print(string s) const;
+};
+}  // namespace sub
+
+class Point3 : anzu::PointBase {
+  Point3(double x, double y, double z);
+  double x() const;
+  double x(double to_add) const;
+  double y() const;
+  double z() const;
+  double sum() const;
+};
+
+template <POINT = {anzu::sub::Point2, anzu::Point3}>
+class Template {
+  Template(const POINT& point);
+  Template(const POINT& point, double a);
+  Template(const This& other);
+
+  POINT point() const;
+
+  double overload() const;
+  double overload(const POINT& point) const;
+  double overload(const This& other) const;
+
+  POINT method_on_template_type(const POINT& point) const;
+  This method_on_this(const POINT& point) const;
+
+  static This static_method(const This& other, double dummy);
+
+  template <OTHER_POINT = {anzu::sub::Point2, anzu::Point3}>
+  double template_method(const OTHER_POINT& other) const;
+};
+
+template <T1, T2>
+class Template2 {
+  Template2(const T1& t1, const T2& t2);
+  double sum_x() const;
+  double sum_x(const T1& other1) const;
+  double sum_x(T2* other2) const;
+  double sum_x(const T1& other1, T2* other2) const;
+  T1 property_t1;
+};
+
+typedef anzu::Template2<anzu::sub::Point2, anzu::Point3>
+    Template2Point2Point3Instantiation;
+
+class Ignore {
+  Ignore(int x);
+};
+
+namespace sub2 {
+class Point4 {
+  Point4(const anzu::sub::Point2& p_in, double z_in, double w_in);
+  double sum();
+  const anzu::sub::Point2 p;
+  double z;
+  double w;
+};
+}  // namespace sub2
+
+double global_func_on_base(anzu::PointBase* point);
+
+}  // namespace anzu
+
+double global_func_overloads(anzu::sub::Point2* point2);
+double global_func_overloads(anzu::Point3* point3);
diff --git a/wrap/tests/pybind_wrapper_test.h b/wrap/tests/pybind_wrapper_test.h
new file mode 100644
index 000000000..9307b7dbd
--- /dev/null
+++ b/wrap/tests/pybind_wrapper_test.h
@@ -0,0 +1,133 @@
+#pragma once
+
+#include <iostream>
+#include <memory>
+#include <string>
+
+namespace anzu {
+
+class PointBase {
+ public:
+  virtual double sum() const = 0;
+  virtual ~PointBase() {}
+};
+
+namespace sub {
+
+class Point2 : public PointBase {
+ public:
+  explicit Point2(double x, double y = 10.0) : x_{x}, y_{y} {}
+  double x() const { return x_; }
+  double y() const { return y_; }
+  double sum() const override;
+  double func_with_default_args(double a, double b = 20.0) const {
+    return a + b;
+  }
+  void print(const std::string& s) const { std::cout << s << std::endl; }
+
+ private:
+  double x_, y_;
+};
+
+}  // namespace sub
+
+class Point3 : public PointBase {
+ public:
+  Point3(double x, double y, double z) : x_{x}, y_{y}, z_{z} {}
+  double x() const { return x_; }
+  // Overload method.
+  double x(double to_add) const { return x_ + to_add; }
+  double y() const { return y_; }
+  double z() const { return z_; }
+  double sum() const override;
+
+ private:
+  double x_, y_, z_;
+};
+
+template <class POINT>
+class Template {
+ public:
+  explicit Template(const POINT& point, double a = 10) : point_{point} {}
+  Template(const Template<POINT>& other) : point_{other.point_} {}
+
+  double overload() const { return point_.sum() + point_.x(); }
+  double overload(const POINT& point) const {
+    return point_.sum() + point.sum();
+  }
+  double overload(const Template<POINT>& other) const {
+    return point_.sum() + other.overload();
+  }
+
+  POINT point() const { return point_; }
+
+  POINT method_on_template_type(const POINT& point) const { return point; }
+  Template<POINT> method_on_this(const POINT& point) const { return *this; }
+
+  static Template<POINT> static_method(const Template<POINT>& other,
+                                       double dummy) {
+    return other.method_on_this(other.point());
+  }
+
+  template <typename OTHER_POINT>
+  double template_method(const OTHER_POINT& other) const {
+    return point_.x() + other.x();
+  }
+
+ private:
+  POINT point_;
+};
+
+template <class T1, class T2>
+class Template2 {
+ public:
+  Template2(const T1& t1, const T2& t2) : t1_(t1), t2_(t2) {}
+
+  double sum_x() const { return t1_.x() + t2_.x(); }
+
+  double sum_x(const T1& other1) const {
+    return t1_.x() + t2_.x() + other1.x();
+  }
+
+  double sum_x(const std::shared_ptr<T2>& other2) const {
+    return t1_.x() + t2_.x() + other2->x();
+  }
+
+  double sum_x(const T1& other1, const std::shared_ptr<T2>& other2) const {
+    return t1_.x() + t2_.x() + other1.x() + other2->x();
+  }
+
+ private:
+  T1 t1_;
+  T2 t2_;
+
+ public:
+  T1 property_t1{10};
+};
+
+class Ignore {
+ public:
+  explicit Ignore(int x) {}
+};
+
+namespace sub2 {
+class Point4 {
+ public:
+  Point4(const sub::Point2& p_in, double z_in, double w_in)
+      : p(p_in), z(z_in), w(w_in) {}
+  double sum() { return p.sum() + z + w; }
+
+  const anzu::sub::Point2 p;
+  double z;
+  double w;
+};
+}  // namespace sub2
+
+// A function on the base class.
+double global_func_on_base(const std::shared_ptr<PointBase>& point);
+
+}  // namespace anzu
+
+// Overload functions.
+double global_func_overloads(const std::shared_ptr<anzu::sub::Point2>& point2);
+double global_func_overloads(const std::shared_ptr<anzu::Point3>& point3);
diff --git a/wrap/tests/pybind_wrapper_test_script.py b/wrap/tests/pybind_wrapper_test_script.py
new file mode 100644
index 000000000..667f2337f
--- /dev/null
+++ b/wrap/tests/pybind_wrapper_test_script.py
@@ -0,0 +1,62 @@
+import pybind_wrapper_test as pwt
+
+p2 = pwt.sub.Point2(10, 20)
+print(p2.sum())
+print(p2.func_with_default_args(10))
+print(p2.func_with_default_args(10, 10))
+p2.print_("test print")
+
+p22 = pwt.sub.Point2(10)
+print(p22.y())
+
+p3 = pwt.Point3(10, 20, 30)
+print(p3.sum())
+print(p3.x(to_add=100))
+
+print(pwt.global_func_on_base(p2))
+print(pwt.global_func_on_base(p3))
+
+# Test template class.
+# Construct with POINT
+template_p2 = pwt.TemplatePoint2(p2)
+print(template_p2.overload())
+
+template_p3 = pwt.TemplatePoint3(p3)
+print(template_p3.overload())
+print(template_p3.overload(p3))
+
+# Construct with This
+template_p3_copy = pwt.TemplatePoint3(template_p3)
+print(template_p3_copy.overload())
+print(template_p3.overload(template_p3_copy))
+
+# Function of template type.
+ret_p3 = template_p3.method_on_template_type(p3)
+print(ret_p3.z())
+
+# Function of This class type.
+this = template_p3.method_on_this(p3)
+print(this.method_on_template_type(p3).sum())
+
+# Static function.
+another_this = pwt.TemplatePoint2.static_method(other=template_p2, dummy=0.0)
+
+# Template function of other POINT type.
+print(another_this.template_methodPoint3(p3))
+print(another_this.template_methodPoint2(p2))
+
+# Typedef template instantiation.
+inst = pwt.Template2Point2Point3Instantiation(p2, p3)
+inst.property_t1 = pwt.sub.Point2(100)
+print("inst overload: ", inst.sum_x())
+print(inst.sum_x(p2))
+print(inst.sum_x(p3))
+print(inst.sum_x(p2, p3))
+print(inst.property_t1.sum())
+
+# Properties
+p4 = pwt.sub2.Point4(p2, 30, 40)
+print(p4.p.sum())
+print(p4.sum())
+p4.z = 40
+print(p4.sum())
diff --git a/wrap/tests/testArgument.cpp b/wrap/tests/testArgument.cpp
deleted file mode 100644
index 555401309..000000000
--- a/wrap/tests/testArgument.cpp
+++ /dev/null
@@ -1,131 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 testArgument.cpp
- * @brief Unit test for Argument class
- * @author Frank Dellaert
- * @date Nov 12, 2014
- **/
-
-#include <wrap/Argument.h>
-#include <CppUnitLite/TestHarness.h>
-#include <iostream>
-
-using namespace std;
-using namespace wrap;
-
-//******************************************************************************
-TEST( Argument, grammar ) {
-
-  using classic::space_p;
-
-  // Create type grammar that will place result in actual
-  Argument actual, arg0;
-  ArgumentGrammar g(actual);
-
-  EXPECT(parse("const gtsam::Point2& p4", g, space_p).full);
-  EXPECT(actual.type==Qualified("gtsam","Point2",Qualified::CLASS));
-  EXPECT(actual.name=="p4");
-  EXPECT(actual.is_const);
-  EXPECT(actual.is_ref);
-  EXPECT(!actual.is_ptr);
-  actual = arg0;
-
-  EXPECT(parse("Point2& p", g, space_p).full);
-  EXPECT(actual.type==Qualified("Point2",Qualified::CLASS));
-  EXPECT(actual.name=="p");
-  EXPECT(!actual.is_const);
-  EXPECT(actual.is_ref);
-  EXPECT(!actual.is_ptr);
-  actual = arg0;
-
-  EXPECT(parse("gtsam::Point2* p3", g, space_p).full);
-  EXPECT(actual.type==Qualified("gtsam","Point2",Qualified::CLASS));
-  EXPECT(actual.name=="p3");
-  EXPECT(!actual.is_const);
-  EXPECT(!actual.is_ref);
-  EXPECT(actual.is_ptr);
-  actual = arg0;
-
-  EXPECT(parse("char a", g, space_p).full);
-  EXPECT(actual==Argument(Qualified("char",Qualified::BASIS),"a"));
-  actual = arg0;
-
-  EXPECT(parse("unsigned char a", g, space_p).full);
-  EXPECT(actual==Argument(Qualified("unsigned char",Qualified::BASIS),"a"));
-  actual = arg0;
-
-  EXPECT(parse("Vector v", g, space_p).full);
-  EXPECT(actual==Argument(Qualified("Vector",Qualified::EIGEN),"v"));
-  actual = arg0;
-
-  EXPECT(parse("const Matrix& m", g, space_p).full);
-  EXPECT(actual.type==Qualified("Matrix",Qualified::EIGEN));
-  EXPECT(actual.name=="m");
-  EXPECT(actual.is_const);
-  EXPECT(actual.is_ref);
-  EXPECT(!actual.is_ptr);
-  actual = arg0;
-}
-
-//******************************************************************************
-TEST( ArgumentList, grammar ) {
-
-  using classic::space_p;
-
-  // Create type grammar that will place result in actual
-  ArgumentList actual;
-  ArgumentListGrammar g(actual);
-
-  EXPECT(parse("(const gtsam::Point2& p4)", g, space_p).full);
-  EXPECT_LONGS_EQUAL(1, actual.size());
-  actual.clear();
-
-  EXPECT(parse("()", g, space_p).full);
-  EXPECT_LONGS_EQUAL(0, actual.size());
-  actual.clear();
-
-  EXPECT(parse("(char a)", g, space_p).full);
-  EXPECT_LONGS_EQUAL(1, actual.size());
-  actual.clear();
-
-  EXPECT(parse("(unsigned char a)", g, space_p).full);
-  EXPECT_LONGS_EQUAL(1, actual.size());
-  actual.clear();
-
-  EXPECT(parse("(Vector v, Matrix m)", g, space_p).full);
-  EXPECT_LONGS_EQUAL(2, actual.size());
-  EXPECT(actual[0]==Argument(Qualified("Vector",Qualified::EIGEN),"v"));
-  EXPECT(actual[1]==Argument(Qualified("Matrix",Qualified::EIGEN),"m"));
-  actual.clear();
-
-  EXPECT(parse("(Point2 p)", g, space_p).full);
-  EXPECT_LONGS_EQUAL(1, actual.size());
-  actual.clear();
-
-  EXPECT(parse("(Point2 p1, Point3 p2)", g, space_p).full);
-  EXPECT_LONGS_EQUAL(2, actual.size());
-  EXPECT(actual[0]==Argument(Qualified("Point2"),"p1"));
-  EXPECT(actual[1]==Argument(Qualified("Point3"),"p2"));
-  actual.clear();
-
-  EXPECT(parse("(gtsam::Point2 p3)", g, space_p).full);
-  EXPECT_LONGS_EQUAL(1, actual.size());
-  actual.clear();
-}
-
-/* ************************************************************************* */
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-/* ************************************************************************* */
diff --git a/wrap/tests/testClass.cpp b/wrap/tests/testClass.cpp
deleted file mode 100644
index dbf2e8e7a..000000000
--- a/wrap/tests/testClass.cpp
+++ /dev/null
@@ -1,285 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 testClass.cpp
- * @brief Unit test for Class class
- * @author Frank Dellaert
- * @date Nov 12, 2014
- **/
-
-#include <wrap/Class.h>
-#include <CppUnitLite/TestHarness.h>
-#include <iostream>
-
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-// Constructor
-TEST( Class, Constructor ) {
-  Class cls;
-}
-
-/* ************************************************************************* */
-// test method overloading
-TEST( Class, OverloadingMethod ) {
-  Class cls;
-  const string name = "method1";
-  EXPECT(!cls.exists(name));
-
-  bool verbose = true, is_const = true;
-  ArgumentList args;
-  const ReturnValue retVal;
-  const Template tmplate;
-  cls.addMethod(verbose, is_const, name, args, retVal, tmplate);
-  EXPECT_LONGS_EQUAL(1, cls.nrMethods());
-  EXPECT(cls.exists(name));
-  EXPECT_LONGS_EQUAL(1, cls.method(name).nrOverloads());
-
-  // add an overload w different argument list
-  args.push_back(Argument(Qualified("Vector", Qualified::EIGEN), "v"));
-  cls.addMethod(verbose, is_const, name, args, retVal, tmplate);
-  EXPECT_LONGS_EQUAL(1, cls.nrMethods());
-  EXPECT_LONGS_EQUAL(2, cls.method(name).nrOverloads());
-
-  // add with non-trivial template list, will create two different functions
-  boost::optional<Template> t = //
-      CreateTemplate("template<T = {Point2, Point3}>");
-  CHECK(t);
-  cls.addMethod(verbose, is_const, name, args, retVal, *t);
-  EXPECT_LONGS_EQUAL(3, cls.nrMethods());
-  EXPECT_LONGS_EQUAL(2, cls.method(name).nrOverloads());
-}
-
-/* ************************************************************************* */
-// test templated methods
-TEST( Class, TemplatedMethods ) {
-  Class cls;
-  const string name = "method";
-  EXPECT(!cls.exists(name));
-
-  bool verbose = true, is_const = true;
-  ArgumentList args;
-  Argument arg;
-  arg.type.name_ = "T";
-  args.push_back(arg);
-  const ReturnValue retVal(ReturnType("T"));
-  boost::optional<Template> tmplate = //
-      CreateTemplate("template<T = {Point2, Point3}>");
-  CHECK(tmplate);
-  cls.addMethod(verbose, is_const, name, args, retVal, *tmplate);
-  EXPECT_LONGS_EQUAL(2, cls.nrMethods());
-  EXPECT(cls.exists(name+"Point2"));
-  EXPECT(cls.exists(name+"Point3"));
-}
-
-//******************************************************************************
-TEST( Class, Grammar ) {
-
-  using classic::space_p;
-
-  // Create type grammar that will place result in cls
-  Class cls;
-  Template t;
-  ClassGrammar g(cls, t);
-
-  EXPECT(parse("class Point2 {\n};", g, space_p).full);
-
-  string markup(
-      string("class Point2 {                \n")
-          + string(" double x() const;            \n") // Method 1
-          + string(" Matrix returnMatrix() const;   \n") // Method 2
-          + string(" Point2 returnPoint2() const; \n") // Method 3
-          + string(" static Vector returnVector(); \n") // Static Method 1
-          + string("};"));
-
-  EXPECT(parse(markup.c_str(), g, space_p).full);
-
-  // check return types
-  EXPECT(assert_equal("Point2", cls.name()));
-  EXPECT(!cls.isVirtual);
-  EXPECT(cls.namespaces().empty());
-  LONGS_EQUAL(3, cls.nrMethods());
-  LONGS_EQUAL(1, cls.static_methods.size());
-
-  // Method 1
-  Method m1 = cls.method("x");
-  EXPECT(assert_equal("x", m1.name()));
-  EXPECT(m1.isConst());
-  LONGS_EQUAL(1, m1.nrOverloads());
-
-  ReturnValue rv1 = m1.returnValue(0);
-  EXPECT(!rv1.isPair);
-  EXPECT(!rv1.type1.isPtr);
-  EXPECT(assert_equal("double", rv1.type1.name()));
-  EXPECT_LONGS_EQUAL(ReturnType::BASIS, rv1.type1.category);
-
-  // Method 2
-  Method m2 = cls.method("returnMatrix");
-  EXPECT(assert_equal("returnMatrix", m2.name()));
-  EXPECT(m2.isConst());
-  LONGS_EQUAL(1, m2.nrOverloads());
-
-  ReturnValue rv2 = m2.returnValue(0);
-  EXPECT(!rv2.isPair);
-  EXPECT(!rv2.type1.isPtr);
-  EXPECT(assert_equal("Matrix", rv2.type1.name()));
-  EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv2.type1.category);
-
-  // Method 3
-  Method m3 = cls.method("returnPoint2");
-  EXPECT(assert_equal("returnPoint2", m3.name()));
-  EXPECT(m3.isConst());
-  LONGS_EQUAL(1, m3.nrOverloads());
-
-  ReturnValue rv3 = m3.returnValue(0);
-  EXPECT(!rv3.isPair);
-  EXPECT(!rv3.type1.isPtr);
-  EXPECT(assert_equal("Point2", rv3.type1.name()));
-  EXPECT_LONGS_EQUAL(ReturnType::CLASS, rv3.type1.category);
-
-  // Static Method 1
-  // static Vector returnVector();
-  StaticMethod sm1 = cls.static_methods.at("returnVector");
-  EXPECT(assert_equal("returnVector", sm1.name()));
-  LONGS_EQUAL(1, sm1.nrOverloads());
-
-  ReturnValue rv4 = sm1.returnValue(0);
-  EXPECT(!rv4.isPair);
-  EXPECT(!rv4.type1.isPtr);
-  EXPECT(assert_equal("Vector", rv4.type1.name()));
-  EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv4.type1.category);
-}
-
-//******************************************************************************
-TEST( Class, TemplateSubstition ) {
-
-  using classic::space_p;
-
-  // Create type grammar that will place result in cls
-  Class cls;
-  Template t;
-  ClassGrammar g(cls, t);
-
-  string markup(string("template<T = {void, double, Matrix, Point3}>"
-      "class Point2 {"
-      " T myMethod(const T& t) const;"
-      "};"));
-
-  EXPECT(parse(markup.c_str(), g, space_p).full);
-
-  // Method 2
-  Method m2 = cls.method("myMethod");
-  EXPECT(assert_equal("myMethod", m2.name()));
-  EXPECT(m2.isConst());
-  LONGS_EQUAL(1, m2.nrOverloads());
-
-  ReturnValue rv2 = m2.returnValue(0);
-  EXPECT(!rv2.isPair);
-  EXPECT(!rv2.type1.isPtr);
-  EXPECT(assert_equal("T", rv2.type1.name()));
-  EXPECT_LONGS_EQUAL(ReturnType::CLASS, rv2.type1.category);
-
-  // Generate some expected values for qualified types
-  Qualified q_void("void", Qualified::VOID);
-  Qualified q_double("double", Qualified::BASIS);
-  Qualified q_Matrix("Matrix", Qualified::EIGEN);
-  Qualified q_Point3("Point3", Qualified::CLASS);
-
-  EXPECT_LONGS_EQUAL(4, t.nrValues());
-  EXPECT(t.argName()=="T");
-  EXPECT(t[0]==q_void);
-  EXPECT(t[1]==q_double);
-  EXPECT(t[2]==q_Matrix);
-  EXPECT(t[3]==q_Point3);
-
-  vector<Class> classes = cls.expandTemplate(t.argName(), t.argValues());
-
-  // check the number of new classes is four
-  EXPECT_LONGS_EQUAL(4, classes.size());
-
-  // check return types
-  EXPECT( classes[0].method("myMethod").returnValue(0).type1 == q_void);
-  EXPECT( classes[1].method("myMethod").returnValue(0).type1 == q_double);
-  EXPECT( classes[2].method("myMethod").returnValue(0).type1 == q_Matrix);
-  EXPECT( classes[3].method("myMethod").returnValue(0).type1 == q_Point3);
-
-  // check the argument types
-  EXPECT( classes[0].method("myMethod").argumentList(0)[0].type == q_void);
-  EXPECT( classes[1].method("myMethod").argumentList(0)[0].type == q_double);
-  EXPECT( classes[2].method("myMethod").argumentList(0)[0].type == q_Matrix);
-  EXPECT( classes[3].method("myMethod").argumentList(0)[0].type == q_Point3);
-
-}
-
-//******************************************************************************
-TEST( Class, TemplateSubstitionIntList ) {
-
-  using classic::space_p;
-
-  // Create type grammar that will place result in cls
-  Class cls;
-  Template t;
-  ClassGrammar g(cls, t);
-
-  string markup(string("template<N = {3,4}>"
-      "class Point2 {"
-      " void myMethod(Matrix A) const;"
-      "};"));
-
-  EXPECT(parse(markup.c_str(), g, space_p).full);
-
-  vector<Class> classes = cls.expandTemplate(t.argName(), t.intList());
-
-  // check the number of new classes is 2
-  EXPECT_LONGS_EQUAL(2, classes.size());
-}
-
-//******************************************************************************
-TEST(Class, Template) {
-
-  using classic::space_p;
-
-  // Create type grammar that will place result in cls
-  Class cls;
-  Template t;
-  ClassGrammar g(cls, t);
-
-  string markup(
-      string(
-          "template<T = {Vector, Matrix}>"
-              " virtual class PriorFactor : gtsam::NoiseModelFactor {"
-              "   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); "
-              "   T prior() const; "
-              "  void serialize() const; "
-              "};"));
-
-  EXPECT(parse(markup.c_str(), g, space_p).full);
-
-}
-
-//******************************************************************************
-TEST( Class, Virtualness ) {
-  using classic::space_p;
-  Class cls;
-  Template t;
-  ClassGrammar g(cls, t);
-  string markup("virtual class Point3 {};");
-  EXPECT(parse(markup.c_str(), g, space_p).full);
-  EXPECT(cls.isVirtual);
-}
-//******************************************************************************
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-//******************************************************************************
diff --git a/wrap/tests/testGlobalFunction.cpp b/wrap/tests/testGlobalFunction.cpp
deleted file mode 100644
index c336afd32..000000000
--- a/wrap/tests/testGlobalFunction.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 testMethod.cpp
- * @brief Unit test for GlobalFunction class
- * @author Frank Dellaert
- * @date Nov 12, 2014
- **/
-
-#include <wrap/GlobalFunction.h>
-#include <CppUnitLite/TestHarness.h>
-#include <iostream>
-
-using namespace std;
-using namespace wrap;
-
-//******************************************************************************
-// Constructor
-TEST( GlobalFunction, Constructor ) {
-  GlobalFunction f;
-}
-
-//******************************************************************************
-TEST( GlobalFunction, Grammar ) {
-
-  using classic::space_p;
-
-  // Create type grammar that will place result in actual
-  GlobalFunctions actual;
-  vector<string> namespaces;
-  std::string includeFile;
-  GlobalFunctionGrammar g(actual,namespaces,includeFile);
-
-  // a class type with namespaces
-  EXPECT(parse("Vector aGlobalFunction();", g, space_p).full);
-  EXPECT(parse("Vector overloadedGlobalFunction(int a);", g, space_p).full);
-  EXPECT(parse("Vector overloadedGlobalFunction(int a, double b);", g, space_p).full);
-  LONGS_EQUAL(2,actual.size());
-}
-
-//******************************************************************************
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-//******************************************************************************
diff --git a/wrap/tests/testMethod.cpp b/wrap/tests/testMethod.cpp
deleted file mode 100644
index f45869728..000000000
--- a/wrap/tests/testMethod.cpp
+++ /dev/null
@@ -1,63 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 testMethod.cpp
- * @brief Unit test for Method class
- * @author Frank Dellaert
- * @date Nov 12, 2014
- **/
-
-#include <wrap/Method.h>
-#include <CppUnitLite/TestHarness.h>
-#include <iostream>
-
-using namespace std;
-using namespace wrap;
-
-//******************************************************************************
-// Constructor
-TEST( Method, Constructor ) {
-  Method method;
-}
-
-//******************************************************************************
-// addOverload
-TEST( Method, addOverload ) {
-  Method method;
-  ArgumentList args;
-  bool is_const = true;
-  const ReturnValue retVal1(ReturnType("return_type1"));
-  method.addOverload("myName", args, retVal1, is_const);
-  const ReturnValue retVal2(ReturnType("return_type2"));
-  method.addOverload("myName", args, retVal2, is_const);
-  EXPECT_LONGS_EQUAL(2, method.nrOverloads());
-}
-
-////******************************************************************************
-//TEST( Method, grammar ) {
-//
-//  using classic::space_p;
-//
-//  // Create type grammar that will place result in actual
-//  Method actual;
-//  method_grammar method_g(actual);
-//
-//  // a class type with namespaces
-//  EXPECT(parse("double x() const;", method_g, space_p).full);
-//}
-
-//******************************************************************************
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-//******************************************************************************
diff --git a/wrap/tests/testReturnValue.cpp b/wrap/tests/testReturnValue.cpp
deleted file mode 100644
index 62a8ff0c7..000000000
--- a/wrap/tests/testReturnValue.cpp
+++ /dev/null
@@ -1,119 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 testReturnValue.cpp
- * @brief Unit test for ReturnValue class & parser
- * @author Frank Dellaert
- * @date Nov 30, 2014
- **/
-
-#include <wrap/ReturnValue.h>
-#include <CppUnitLite/TestHarness.h>
-#include <iostream>
-
-using namespace std;
-using namespace wrap;
-
-//******************************************************************************
-TEST( ReturnType, Constructor1 ) {
-  ReturnType actual("Point2");
-  EXPECT(actual.namespaces().empty());
-  EXPECT(actual.name()=="Point2");
-  EXPECT(actual.category==Qualified::CLASS);
-  EXPECT(!actual.isPtr);
-}
-
-//******************************************************************************
-TEST( ReturnType, Constructor2 ) {
-  ReturnType actual("Point3", Qualified::CLASS, true);
-  EXPECT(actual.namespaces().empty());
-  EXPECT(actual.name()=="Point3");
-  EXPECT(actual.category==Qualified::CLASS);
-  EXPECT(actual.isPtr);
-}
-
-//******************************************************************************
-TEST( ReturnType, grammar ) {
-
-  using classic::space_p;
-
-  // Create type grammar that will place result in actual
-  ReturnType actual;
-  ReturnTypeGrammar g(actual);
-
-  EXPECT(parse("Point3", g, space_p).full);
-  EXPECT( actual==ReturnType("Point3"));
-  actual.clear();
-
-  EXPECT(parse("Test*", g, space_p).full);
-  EXPECT( actual==ReturnType("Test",Qualified::CLASS,true));
-  actual.clear();
-
-  EXPECT(parse("VectorNotEigen", g, space_p).full);
-  EXPECT(actual==ReturnType("VectorNotEigen"));
-  actual.clear();
-
-  EXPECT(parse("double", g, space_p).full);
-  EXPECT(actual==ReturnType("double",Qualified::BASIS));
-  actual.clear();
-}
-
-//******************************************************************************
-TEST( ReturnValue, Constructor ) {
-  ReturnValue actual(ReturnType("Point2"), ReturnType("Point3"));
-  EXPECT(actual.type1==Qualified("Point2"));
-  EXPECT(actual.type2==Qualified("Point3"));
-  EXPECT(actual.isPair);
-}
-
-//******************************************************************************
-TEST( ReturnValue, grammar ) {
-
-  using classic::space_p;
-
-  // Create type grammar that will place result in actual
-  ReturnValue actual;
-  ReturnValueGrammar g(actual);
-
-  EXPECT(parse("pair<Point2,Point3>", g, space_p).full);
-  EXPECT( actual==ReturnValue(ReturnType("Point2"),ReturnType("Point3")));
-  actual.clear();
-
-  EXPECT(parse("pair<Test*,Test>", g, space_p).full);
-  EXPECT( actual==ReturnValue( //
-      ReturnType("Test",Qualified::CLASS,true),ReturnType("Test")));
-  actual.clear();
-
-  EXPECT(parse("pair<Test,Test*>", g, space_p).full);
-  EXPECT( actual==ReturnValue(ReturnType("Test"), //
-      ReturnType("Test",Qualified::CLASS,true)));
-  actual.clear();
-
-  EXPECT(parse("VectorNotEigen", g, space_p).full);
-  EXPECT(actual==ReturnValue(ReturnType("VectorNotEigen")));
-  actual.clear();
-
-  EXPECT(parse("double", g, space_p).full);
-  EXPECT(actual==ReturnValue(ReturnType("double",Qualified::BASIS)));
-  actual.clear();
-
-  EXPECT(parse("void", g, space_p).full);
-  EXPECT(actual==ReturnValue(ReturnType("void",Qualified::VOID)));
-  actual.clear();
-}
-
-//******************************************************************************
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-//******************************************************************************
diff --git a/wrap/tests/testSpirit.cpp b/wrap/tests/testSpirit.cpp
deleted file mode 100644
index b9e01a17e..000000000
--- a/wrap/tests/testSpirit.cpp
+++ /dev/null
@@ -1,151 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 testSpirit.cpp
- * @brief Unit test for Boost's awesome Spirit parser
- * @author Frank Dellaert
- **/
-
-#include <boost/spirit/include/classic_core.hpp>
-#include <boost/spirit/include/classic_push_back_actor.hpp>
-#include <CppUnitLite/TestHarness.h>
-#include <wrap/utilities.h>
-
-using namespace std;
-using namespace BOOST_SPIRIT_CLASSIC_NS;
-
-typedef rule<BOOST_SPIRIT_CLASSIC_NS::phrase_scanner_t> Rule;
-
-/* ************************************************************************* */
-// lexeme_d turns off white space skipping
-// http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html
-Rule name_p       = lexeme_d[alpha_p >> *(alnum_p | '_')];
-Rule className_p  = lexeme_d[upper_p >> *(alnum_p | '_')];
-Rule methodName_p = lexeme_d[lower_p >> *(alnum_p | '_')];
-
-Rule basisType_p = (str_p("string") | "bool" | "size_t" | "int" | "double" | "Vector" | "Matrix");
-
-/* ************************************************************************* */
-TEST( spirit, real ) {
-  // check if we can parse 8.99 as a real
-  EXPECT(parse("8.99", real_p, space_p).full);
-  // make sure parsing fails on this one
-  EXPECT(!parse("zztop", real_p, space_p).full);
-}
-
-/* ************************************************************************* */
-TEST( spirit, string ) {
-  // check if we can parse a string
-  EXPECT(parse("double", str_p("double"), space_p).full);
-}
-
-/* ************************************************************************* */
-TEST( spirit, sequence ) {
-  // check that we skip white space
-  EXPECT(parse("int int", str_p("int") >> *str_p("int"), space_p).full);
-  EXPECT(parse("int --- - -- -", str_p("int") >> *ch_p('-'), space_p).full);
-  EXPECT(parse("const \t string", str_p("const") >> str_p("string"), space_p).full);
-
-  // note that (see spirit FAQ) the vanilla rule<> does not deal with whitespace
-  rule<>vanilla_p = str_p("const") >> str_p("string");
-  EXPECT(!parse("const \t string", vanilla_p, space_p).full);
-
-  // to fix it, we need to use <phrase_scanner_t>
-  rule<phrase_scanner_t>phrase_level_p = str_p("const") >> str_p("string");
-  EXPECT(parse("const \t string", phrase_level_p, space_p).full);
-}
-
-/* ************************************************************************* */
-// parser for interface files
-
-// const string reference reference
-Rule constStringRef_p =
-  str_p("const") >> "string" >> '&';
-
-// class reference
-Rule classRef_p = className_p >> '&';
-
-// const class reference
-Rule constClassRef_p = str_p("const") >> classRef_p;
-
-// method parsers
-Rule constMethod_p = basisType_p >> methodName_p >> '(' >> ')' >> "const" >> ';';
-
-/* ************************************************************************* */
-TEST( spirit, basisType_p ) {
-  EXPECT(!parse("Point3", basisType_p, space_p).full);
-  EXPECT(parse("string", basisType_p, space_p).full);
-}
-
-/* ************************************************************************* */
-TEST( spirit, className_p ) {
-  EXPECT(parse("Point3", className_p, space_p).full);
-}
-
-/* ************************************************************************* */
-TEST( spirit, classRef_p ) {
-  EXPECT(parse("Point3 &", classRef_p, space_p).full);
-  EXPECT(parse("Point3&", classRef_p, space_p).full);
-}
-
-/* ************************************************************************* */
-TEST( spirit, constMethod_p ) {
-  EXPECT(parse("double norm() const;", constMethod_p, space_p).full);
-}
-
-
-/* ************************************************************************* */
-/* See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=56665
-   GCC compiler issues with -O2 and -fno-strict-aliasing results in undefined
-   behaviour when spirit uses assign_a with a literal.
-   GCC versions 4.7.2 -> 5.4 inclusive */
-
-TEST( spirit, return_value_p ) {
-  static const bool T = true;
-  static const bool F = false;
-
-  bool isEigen = T;
-
-  string actual_return_type;
-  string actual_function_name;
-
-  Rule basisType_p =
-    (str_p("string") | "bool" | "size_t" | "int" | "double");
-
-  Rule eigenType_p =
-    (str_p("Vector") | "Matrix");
-
-  Rule className_p  = lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - basisType_p;
-
-  Rule funcName_p  = lexeme_d[lower_p >> *(alnum_p | '_')];
-
-  Rule returnType_p =
-      (basisType_p[assign_a(actual_return_type)][assign_a(isEigen, T)]) |
-      (className_p[assign_a(actual_return_type)][assign_a(isEigen, F)]) |
-      (eigenType_p[assign_a(actual_return_type)][assign_a(isEigen, T)]);
-
-  Rule testFunc_p = returnType_p >> funcName_p[assign_a(actual_function_name)] >> str_p("();");
-
-  EXPECT(parse("VectorNotEigen doesNotReturnAnEigenVector();", testFunc_p, space_p).full);
-  EXPECT(!isEigen);
-  EXPECT(actual_return_type == "VectorNotEigen");
-  EXPECT(actual_function_name == "doesNotReturnAnEigenVector");
-
-  EXPECT(parse("Vector actuallyAVector();", testFunc_p, space_p).full);
-  EXPECT(isEigen);
-  EXPECT(actual_return_type == "Vector");
-  EXPECT(actual_function_name == "actuallyAVector");
-}
-
-/* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
-/* ************************************************************************* */
diff --git a/wrap/tests/testTemplate.cpp b/wrap/tests/testTemplate.cpp
deleted file mode 100644
index 1c735f3f2..000000000
--- a/wrap/tests/testTemplate.cpp
+++ /dev/null
@@ -1,78 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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   testTemplate.cpp
- * @brief  unit test for Template class
- * @author Frank Dellaert
- * @date   Dec 1, 2014
- **/
-
-#include <wrap/Template.h>
-#include <CppUnitLite/TestHarness.h>
-
-using namespace std;
-using namespace wrap;
-
-//******************************************************************************
-TEST( Template, grammar ) {
-
-  using classic::space_p;
-
-  // Create type grammar that will place result in actual
-  Template actual;
-  TemplateGrammar g(actual);
-
-  EXPECT(parse("template<T = {gtsam::Point2, Matrix}>", g, space_p).full);
-  EXPECT_LONGS_EQUAL(2, actual.nrValues());
-  EXPECT(actual.argName()=="T");
-  EXPECT(actual[0]==Qualified("gtsam","Point2",Qualified::CLASS));
-  EXPECT(actual[1]==Qualified("Matrix",Qualified::EIGEN));
-  actual.clear();
-
-  EXPECT(
-      parse("template<ARG = {gtsam::Point2, gtsam::Point3, Vector, Matrix}>", g, space_p).full);
-  EXPECT_LONGS_EQUAL(4, actual.nrValues());
-  EXPECT(actual.argName()=="ARG");
-  EXPECT(actual[0]==Qualified("gtsam","Point2",Qualified::CLASS));
-  EXPECT(actual[1]==Qualified("gtsam","Point3",Qualified::CLASS));
-  EXPECT(actual[2]==Qualified("Vector",Qualified::EIGEN));
-  EXPECT(actual[3]==Qualified("Matrix",Qualified::EIGEN));
-  actual.clear();
-
-  EXPECT(parse("template<N = {1,2,3,4}>", g, space_p).full);
-  EXPECT_LONGS_EQUAL(4, actual.intList().size());
-  EXPECT(actual.argName()=="N");
-  EXPECT_LONGS_EQUAL(1,actual.intList()[0]);
-  EXPECT_LONGS_EQUAL(2,actual.intList()[1]);
-  EXPECT_LONGS_EQUAL(3,actual.intList()[2]);
-  EXPECT_LONGS_EQUAL(4,actual.intList()[3]);
-  actual.clear();
-}
-
-//******************************************************************************
-TEST( Template, CreateTemplate ) {
-  boost::optional<Template> actual = //
-      CreateTemplate("template<T = {gtsam::Point2, Matrix}>");
-  CHECK(actual);
-  // aborts if not true
-  EXPECT_LONGS_EQUAL(2, actual->nrValues());
-  EXPECT(actual->argName()=="T");
-  EXPECT((*actual)[0]==Qualified("gtsam","Point2",Qualified::CLASS));
-  EXPECT((*actual)[1]==Qualified("Matrix",Qualified::EIGEN));
-}
-
-//******************************************************************************
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-//******************************************************************************
diff --git a/wrap/tests/testType.cpp b/wrap/tests/testType.cpp
deleted file mode 100644
index 229b79728..000000000
--- a/wrap/tests/testType.cpp
+++ /dev/null
@@ -1,137 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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   testType.cpp
- * @brief  unit test for parsing a fully qualified type
- * @author Frank Dellaert
- * @date   Nov 30, 2014
- **/
-
-#include <wrap/Qualified.h>
-#include <CppUnitLite/TestHarness.h>
-
-using namespace std;
-using namespace wrap;
-
-//******************************************************************************
-TEST( Type, Constructor1 ) {
-  Qualified actual("Point2");
-  EXPECT(actual.namespaces().empty());
-  EXPECT(actual.name()=="Point2");
-  EXPECT(actual.category==Qualified::CLASS);
-}
-
-//******************************************************************************
-TEST( Type, Constructor2 ) {
-  Qualified actual("Point3",Qualified::CLASS);
-  EXPECT(actual.namespaces().empty());
-  EXPECT(actual.name()=="Point3");
-  EXPECT(actual.category==Qualified::CLASS);
-}
-
-//******************************************************************************
-TEST( Type, grammar ) {
-
-  using classic::space_p;
-
-  // Create type grammar that will place result in actual
-  Qualified actual;
-  TypeGrammar type_g(actual);
-
-  // a class type with 2 namespaces
-  EXPECT(parse("gtsam::internal::Point2", type_g, space_p).full);
-  EXPECT(actual==Qualified("gtsam","internal","Point2",Qualified::CLASS));
-  actual.clear();
-
-  // a class type with 1 namespace
-  EXPECT(parse("gtsam::Point2", type_g, space_p).full);
-  EXPECT(actual==Qualified("gtsam","Point2",Qualified::CLASS));
-  actual.clear();
-
-  // a class type with no namespaces
-  EXPECT(parse("Point2", type_g, space_p).full);
-  EXPECT(actual==Qualified("Point2",Qualified::CLASS));
-  actual.clear();
-
-  // a class type with no namespaces
-  EXPECT(parse("VectorNotEigen", type_g, space_p).full);
-  EXPECT(actual==Qualified("VectorNotEigen",Qualified::CLASS));
-  actual.clear();
-
-  // an Eigen type
-  EXPECT(parse("Vector", type_g, space_p).full);
-  EXPECT(actual==Qualified("Vector",Qualified::EIGEN));
-  actual.clear();
-
-  // a basic type
-  EXPECT(parse("double", type_g, space_p).full);
-  EXPECT(actual==Qualified("double",Qualified::BASIS));
-  actual.clear();
-
-  // void
-  EXPECT(parse("void", type_g, space_p).full);
-  EXPECT(actual==Qualified("void",Qualified::VOID));
-  actual.clear();
-}
-
-//******************************************************************************
-TEST( TypeList, grammar ) {
-
-  using classic::space_p;
-
-  // Create type grammar that will place result in actual
-  vector<Qualified> actual;
-  TypeListGrammar<'{','}'> g(actual);
-
-  EXPECT(parse("{gtsam::Point2}", g, space_p).full);
-  EXPECT_LONGS_EQUAL(1, actual.size());
-  actual.clear();
-
-  EXPECT(parse("{}", g, space_p).full);
-  EXPECT_LONGS_EQUAL(0, actual.size());
-  actual.clear();
-
-  EXPECT(parse("{char}", g, space_p).full);
-  EXPECT_LONGS_EQUAL(1, actual.size());
-  actual.clear();
-
-  EXPECT(parse("{unsigned char}", g, space_p).full);
-  EXPECT_LONGS_EQUAL(1, actual.size());
-  actual.clear();
-
-  EXPECT(parse("{Vector, Matrix}", g, space_p).full);
-  EXPECT_LONGS_EQUAL(2, actual.size());
-  EXPECT(actual[0]==Qualified("Vector",Qualified::EIGEN));
-  EXPECT(actual[1]==Qualified("Matrix",Qualified::EIGEN));
-  actual.clear();
-
-  EXPECT(parse("{Point2}", g, space_p).full);
-  EXPECT_LONGS_EQUAL(1, actual.size());
-  actual.clear();
-
-  EXPECT(parse("{Point2, Point3}", g, space_p).full);
-  EXPECT_LONGS_EQUAL(2, actual.size());
-  EXPECT(actual[0]==Qualified("Point2"));
-  EXPECT(actual[1]==Qualified("Point3"));
-  actual.clear();
-
-  EXPECT(parse("{gtsam::Point2}", g, space_p).full);
-  EXPECT_LONGS_EQUAL(1, actual.size());
-  actual.clear();
-}
-
-//******************************************************************************
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-//******************************************************************************
diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp
deleted file mode 100644
index 64f28db83..000000000
--- a/wrap/tests/testWrap.cpp
+++ /dev/null
@@ -1,505 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 testWrap.cpp
- * @brief Unit test for wrap.c
- * @author Frank Dellaert
- **/
-
-#include <wrap/utilities.h>
-#include <wrap/Module.h>
-
-#include <CppUnitLite/TestHarness.h>
-
-#include <boost/assign/std/vector.hpp>
-#include <boost/filesystem.hpp>
-
-#include <stdlib.h>
-#include <iostream>
-#include <fstream>
-#include <sstream>
-
-using namespace std;
-using namespace boost::assign;
-using namespace wrap;
-namespace fs = boost::filesystem;
-static bool enable_verbose = false;
-#ifdef TOPSRCDIR
-static string topdir = TOPSRCDIR;
-#else
-static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not defined, we error
-#endif
-
-typedef vector<string> strvec;
-
-/* ************************************************************************* */
-TEST( wrap, ArgumentList ) {
-  ArgumentList args;
-  Argument arg1; arg1.type.name_ = "double"; arg1.name = "x";
-  Argument arg2; arg2.type.name_ = "double"; arg2.name = "y";
-  Argument arg3; arg3.type.name_ = "double"; arg3.name = "z";
-  args.push_back(arg1);
-  args.push_back(arg2);
-  args.push_back(arg3);
-  EXPECT(assert_equal("ddd", args.signature()));
-  EXPECT(assert_equal("double,double,double", args.types()));
-  EXPECT(assert_equal("x,y,z", args.names()));
-}
-
-/* ************************************************************************* */
-TEST( wrap, check_exception ) {
-  THROWS_EXCEPTION(Module("/notarealpath", "geometry",enable_verbose));
-  CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",enable_verbose), CantOpenFile);
-
-//  // TODO: generate_matlab_wrapper does not throw this anymore, so check constructor
-//  fs::remove_all("actual_deps"); // clean out previous generated code
-//  string path = topdir + "/wrap/tests";
-//  Module module(path.c_str(), "testDependencies",enable_verbose);
-//  CHECK_EXCEPTION(module.generate_matlab_wrapper("actual_deps"), DependencyMissing);
-}
-
-/* ************************************************************************* */
-TEST( wrap, Small ) {
-  string moduleName("gtsam");
-  Module module(moduleName, true);
-
-  string markup(
-      string("class Point2 {                \n") +
-      string(" double x() const;            \n") +   // Method 1
-      string(" Matrix returnMatrix() const;   \n") + // Method 2
-      string(" Point2 returnPoint2() const; \n") +   // Method 3
-      string(" static Vector returnVector(); \n") +  // Static Method 1
-      string("};\n"));
-  module.parseMarkup(markup);
-
-  // check return types
-  LONGS_EQUAL(1, module.classes.size());
-  Class cls = module.classes.front();
-  EXPECT(assert_equal("Point2", cls.name()));
-  EXPECT(!cls.isVirtual);
-  EXPECT(cls.namespaces().empty());
-  LONGS_EQUAL(3, cls.nrMethods());
-  LONGS_EQUAL(1, cls.static_methods.size());
-
-  // Method 1
-  Method m1 = cls.method("x");
-  EXPECT(assert_equal("x", m1.name()));
-  EXPECT(m1.isConst());
-  LONGS_EQUAL(1, m1.nrOverloads());
-
-  ReturnValue rv1 = m1.returnValue(0);
-  EXPECT(!rv1.isPair);
-  EXPECT(!rv1.type1.isPtr);
-  EXPECT(assert_equal("double", rv1.type1.name()));
-  EXPECT_LONGS_EQUAL(ReturnType::BASIS, rv1.type1.category);
-
-  // Method 2
-  Method m2 = cls.method("returnMatrix");
-  EXPECT(assert_equal("returnMatrix", m2.name()));
-  EXPECT(m2.isConst());
-  LONGS_EQUAL(1, m2.nrOverloads());
-
-  ReturnValue rv2 = m2.returnValue(0);
-  EXPECT(!rv2.isPair);
-  EXPECT(!rv2.type1.isPtr);
-  EXPECT(assert_equal("Matrix", rv2.type1.name()));
-  EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv2.type1.category);
-
-  // Method 3
-  Method m3 = cls.method("returnPoint2");
-  EXPECT(assert_equal("returnPoint2", m3.name()));
-  EXPECT(m3.isConst());
-  LONGS_EQUAL(1, m3.nrOverloads());
-
-  ReturnValue rv3 = m3.returnValue(0);
-  EXPECT(!rv3.isPair);
-  EXPECT(!rv3.type1.isPtr);
-  EXPECT(assert_equal("Point2", rv3.type1.name()));
-  EXPECT_LONGS_EQUAL(ReturnType::CLASS, rv3.type1.category);
-
-  // Static Method 1
-  // static Vector returnVector();
-  StaticMethod sm1 = cls.static_methods.at("returnVector");
-  EXPECT(assert_equal("returnVector", sm1.name()));
-  LONGS_EQUAL(1, sm1.nrOverloads());
-
-  ReturnValue rv4 = sm1.returnValue(0);
-  EXPECT(!rv4.isPair);
-  EXPECT(!rv4.type1.isPtr);
-  EXPECT(assert_equal("Vector", rv4.type1.name()));
-  EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv4.type1.category);
-
-}
-
-/* ************************************************************************* */
-TEST( wrap, Geometry ) {
-  string markup_header_path = topdir + "/wrap/tests";
-  Module module(markup_header_path.c_str(), "geometry",enable_verbose);
-  EXPECT_LONGS_EQUAL(9, module.classes.size());
-
-  // forward declarations
-  LONGS_EQUAL(2, module.forward_declarations.size());
-  EXPECT(assert_equal("VectorNotEigen", module.forward_declarations[0].name()));
-  EXPECT(assert_equal("ns::OtherClass", module.forward_declarations[1].name()));
-
-  // includes
-  strvec exp_includes;
-  exp_includes += "gtsam/geometry/Point2.h";
-  exp_includes += "gtsam/geometry/Point3.h";
-  exp_includes += "folder/path/to/Test.h";
-  EXPECT(assert_equal(exp_includes, module.includes));
-
-  LONGS_EQUAL(9, module.classes.size());
-
-  // Key for ReturnType::return_category
-//  CLASS = 1,
-//  EIGEN = 2,
-//  BASIS = 3,
-//  VOID  = 4,
-
-  {
-    // check first class
-    //  class Point2 {
-    //   Point2();
-    //   Point2(double x, double y);
-    //   double x() const;
-    //   double y() const;
-    //   int dim() const;
-    //   char returnChar() const;
-    //   void argChar(char a) const;
-    //   void argUChar(unsigned char a) const;
-    //   VectorNotEigen vectorConfusion();
-    //  };
-
-    Class cls = module.classes.at(0);
-    EXPECT(assert_equal("Point2", cls.name()));
-    EXPECT_LONGS_EQUAL(2, cls.constructor.nrOverloads());
-    EXPECT_LONGS_EQUAL(8, cls.nrMethods());
-
-    {
-      //   char returnChar() const;
-      CHECK(cls.exists("returnChar"));
-      Method m1 = cls.method("returnChar");
-      LONGS_EQUAL(1, m1.nrOverloads());
-      EXPECT(assert_equal("char", m1.returnValue(0).type1.name()));
-      EXPECT_LONGS_EQUAL(ReturnType::BASIS, m1.returnValue(0).type1.category);
-      EXPECT(!m1.returnValue(0).isPair);
-      EXPECT(assert_equal("returnChar", m1.name()));
-      LONGS_EQUAL(1, m1.nrOverloads());
-      EXPECT_LONGS_EQUAL(0, m1.argumentList(0).size());
-      EXPECT(m1.isConst());
-    }
-
-    {
-      //   VectorNotEigen vectorConfusion();
-      CHECK(cls.exists("vectorConfusion"));
-      Method m1 = cls.method("vectorConfusion");
-      LONGS_EQUAL(1, m1.nrOverloads());
-      EXPECT(assert_equal("VectorNotEigen", m1.returnValue(0).type1.name()));
-      EXPECT_LONGS_EQUAL(ReturnType::CLASS, m1.returnValue(0).type1.category);
-      EXPECT(!m1.returnValue(0).isPair);
-      EXPECT(assert_equal("vectorConfusion", m1.name()));
-      LONGS_EQUAL(1, m1.nrOverloads());
-      EXPECT_LONGS_EQUAL(0, m1.argumentList(0).size());
-      EXPECT(!m1.isConst());
-    }
-
-    EXPECT_LONGS_EQUAL(0, cls.static_methods.size());
-    EXPECT_LONGS_EQUAL(1, cls.namespaces().size());
-
-#ifndef WRAP_DISABLE_SERIALIZE
-    // check serialization flag
-    EXPECT(cls.isSerializable);
-    EXPECT(!cls.hasSerialization);
-#endif
-    }
-
-  // check second class, Point3
-  {
-    Class cls = module.classes.at(1);
-    EXPECT(assert_equal("Point3", cls.name()));
-    EXPECT_LONGS_EQUAL(1, cls.constructor.nrOverloads());
-    EXPECT_LONGS_EQUAL(1, cls.nrMethods());
-    EXPECT_LONGS_EQUAL(2, cls.static_methods.size());
-    EXPECT_LONGS_EQUAL(1, cls.namespaces().size());
-
-    // first constructor takes 3 doubles
-    ArgumentList c1 = cls.constructor.argumentList(0);
-    EXPECT_LONGS_EQUAL(3, c1.size());
-
-    // check first double argument
-    Argument a1 = c1.front();
-    EXPECT(!a1.is_const);
-    EXPECT(assert_equal("double", a1.type.name_));
-    EXPECT(!a1.is_ref);
-    EXPECT(assert_equal("x", a1.name));
-
-    // check method
-    CHECK(cls.exists("norm"));
-    Method m1 = cls.method("norm");
-    LONGS_EQUAL(1, m1.nrOverloads());
-    EXPECT(assert_equal("double", m1.returnValue(0).type1.name()));
-    EXPECT_LONGS_EQUAL(ReturnType::BASIS, m1.returnValue(0).type1.category);
-    EXPECT(assert_equal("norm", m1.name()));
-    LONGS_EQUAL(1, m1.nrOverloads());
-    EXPECT_LONGS_EQUAL(0, m1.argumentList(0).size());
-    EXPECT(m1.isConst());
-
-#ifndef WRAP_DISABLE_SERIALIZE
-    // check serialization flag
-    EXPECT(cls.isSerializable);
-    EXPECT(cls.hasSerialization);
-#endif
-    }
-
-  // Test class is the third one
-  {
-    Class testCls = module.classes.at(2);
-    EXPECT_LONGS_EQUAL( 2, testCls.constructor.nrOverloads());
-    EXPECT_LONGS_EQUAL(19, testCls.nrMethods());
-    EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size());
-    EXPECT_LONGS_EQUAL( 0, testCls.namespaces().size());
-
-    // function to parse: pair<Vector,Matrix> return_pair (Vector v, Matrix A) const;
-    CHECK(testCls.exists("return_pair"));
-    Method m2 = testCls.method("return_pair");
-    LONGS_EQUAL(1, m2.nrOverloads());
-    EXPECT(m2.returnValue(0).isPair);
-    EXPECT_LONGS_EQUAL(ReturnType::EIGEN, m2.returnValue(0).type1.category);
-    EXPECT(assert_equal("Vector", m2.returnValue(0).type1.name()));
-    EXPECT_LONGS_EQUAL(ReturnType::EIGEN, m2.returnValue(0).type2.category);
-    EXPECT(assert_equal("Matrix", m2.returnValue(0).type2.name()));
-
-    // checking pointer args and return values
-//    pair<Test*,Test*> return_ptrs (Test* p1, Test* p2) const;
-    CHECK(testCls.exists("return_ptrs"));
-    Method m3 = testCls.method("return_ptrs");
-    LONGS_EQUAL(1, m3.nrOverloads());
-    ArgumentList args = m3.argumentList(0);
-    LONGS_EQUAL(2, args.size());
-
-    Argument arg1 = args.at(0);
-    EXPECT(arg1.is_ptr);
-    EXPECT(!arg1.is_const);
-    EXPECT(!arg1.is_ref);
-    EXPECT(assert_equal("Test", arg1.type.name_));
-    EXPECT(assert_equal("p1", arg1.name));
-    EXPECT(arg1.type.namespaces_.empty());
-
-    Argument arg2 = args.at(1);
-    EXPECT(arg2.is_ptr);
-    EXPECT(!arg2.is_const);
-    EXPECT(!arg2.is_ref);
-    EXPECT(assert_equal("Test", arg2.type.name_));
-    EXPECT(assert_equal("p2", arg2.name));
-    EXPECT(arg2.type.namespaces_.empty());
-  }
-
-  // evaluate global functions
-  //  Vector aGlobalFunction();
-  LONGS_EQUAL(2, module.global_functions.size());
-  CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end());
-  {
-    GlobalFunction gfunc = module.global_functions.at("aGlobalFunction");
-    EXPECT(assert_equal("aGlobalFunction", gfunc.name()));
-    LONGS_EQUAL(1, gfunc.nrOverloads());
-    EXPECT(assert_equal("Vector", gfunc.returnValue(0).type1.name()));
-    EXPECT_LONGS_EQUAL(1, gfunc.nrOverloads());
-    LONGS_EQUAL(1, gfunc.overloads.size());
-    EXPECT(gfunc.overloads.front().namespaces().empty());
-  }
-}
-
-/* ************************************************************************* */
-TEST( wrap, parse_namespaces ) {
-  string header_path = topdir + "/wrap/tests";
-  Module module(header_path.c_str(), "testNamespaces",enable_verbose);
-  EXPECT_LONGS_EQUAL(6, module.classes.size());
-
-  {
-    strvec module_exp_includes;
-    module_exp_includes += "path/to/ns1.h";
-    module_exp_includes += "path/to/ns1/ClassB.h";
-    module_exp_includes += "path/to/ns2.h";
-    module_exp_includes += "path/to/ns2/ClassA.h";
-    module_exp_includes += "path/to/ns3.h";
-    EXPECT(assert_equal(module_exp_includes, module.includes));
-  }
-
-  {
-    Class cls = module.classes.at(0);
-    EXPECT(assert_equal("ClassA", cls.name()));
-    strvec exp_namespaces; exp_namespaces += "ns1";
-    EXPECT(assert_equal(exp_namespaces, cls.namespaces()));
-  }
-
-  {
-    Class cls = module.classes.at(1);
-    EXPECT(assert_equal("ClassB", cls.name()));
-    strvec exp_namespaces; exp_namespaces += "ns1";
-    EXPECT(assert_equal(exp_namespaces, cls.namespaces()));
-  }
-
-  {
-    Class cls = module.classes.at(2);
-    EXPECT(assert_equal("ClassA", cls.name()));
-    strvec exp_namespaces; exp_namespaces += "ns2";
-    EXPECT(assert_equal(exp_namespaces, cls.namespaces()));
-  }
-
-  {
-    Class cls = module.classes.at(3);
-    EXPECT(assert_equal("ClassB", cls.name()));
-    strvec exp_namespaces; exp_namespaces += "ns2", "ns3";
-    EXPECT(assert_equal(exp_namespaces, cls.namespaces()));
-  }
-
-  {
-    Class cls = module.classes.at(4);
-    EXPECT(assert_equal("ClassC", cls.name()));
-    strvec exp_namespaces; exp_namespaces += "ns2";
-    EXPECT(assert_equal(exp_namespaces, cls.namespaces()));
-  }
-
-  {
-    Class cls = module.classes.at(5);
-    EXPECT(assert_equal("ClassD", cls.name()));
-    strvec exp_namespaces;
-    EXPECT(assert_equal(exp_namespaces, cls.namespaces()));
-  }
-
-  // evaluate global functions
-//  Vector ns1::aGlobalFunction();
-//  Vector ns2::aGlobalFunction();
-  LONGS_EQUAL(2, module.global_functions.size());
-  CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end());
-  {
-    GlobalFunction gfunc = module.global_functions.at("aGlobalFunction");
-    EXPECT(assert_equal("aGlobalFunction", gfunc.name()));
-    LONGS_EQUAL(2, gfunc.nrOverloads());
-    EXPECT(assert_equal("Vector", gfunc.returnValue(0).type1.name()));
-
-    // check namespaces
-    LONGS_EQUAL(2, gfunc.overloads.size());
-    strvec exp_namespaces1; exp_namespaces1 += "ns1";
-    EXPECT(assert_equal(exp_namespaces1, gfunc.overloads.at(0).namespaces()));
-
-    strvec exp_namespaces2; exp_namespaces2 += "ns2";
-    EXPECT(assert_equal(exp_namespaces2, gfunc.overloads.at(1).namespaces()));
-  }
-}
-
-/* ************************************************************************* */
-TEST( wrap, matlab_code_namespaces ) {
-  string header_path = topdir + "/wrap/tests";
-  Module module(header_path.c_str(), "testNamespaces",enable_verbose);
-  EXPECT_LONGS_EQUAL(6, module.classes.size());
-  string path = topdir + "/wrap";
-
-  // clean out previous generated code
-  fs::remove_all("actual_namespaces");
-
-  // emit MATLAB code
-  string exp_path = path + "/tests/expected_namespaces/";
-  string act_path = "actual_namespaces/";
-  module.generate_matlab_wrapper("actual_namespaces");
-
-
-  EXPECT(files_equal(exp_path + "ClassD.m", act_path + "ClassD.m" ));
-  EXPECT(files_equal(exp_path + "+ns1/ClassA.m", act_path + "+ns1/ClassA.m" ));
-  EXPECT(files_equal(exp_path + "+ns1/ClassB.m", act_path + "+ns1/ClassB.m" ));
-  EXPECT(files_equal(exp_path + "+ns2/ClassA.m", act_path + "+ns2/ClassA.m" ));
-  EXPECT(files_equal(exp_path + "+ns2/ClassC.m", act_path + "+ns2/ClassC.m" ));
-  EXPECT(
-      files_equal(exp_path + "+ns2/overloadedGlobalFunction.m", exp_path + "+ns2/overloadedGlobalFunction.m" ));
-  EXPECT(
-      files_equal(exp_path + "+ns2/+ns3/ClassB.m", act_path + "+ns2/+ns3/ClassB.m" ));
-  EXPECT(
-      files_equal(exp_path + "testNamespaces_wrapper.cpp", act_path + "testNamespaces_wrapper.cpp" ));
-}
-
-/* ************************************************************************* */
-TEST( wrap, matlab_code_geometry ) {
-  // Parse into class object
-  string header_path = topdir + "/wrap/tests";
-  Module module(header_path,"geometry",enable_verbose);
-  string path = topdir + "/wrap";
-
-  // clean out previous generated code
-  fs::remove_all("actual");
-
-  // emit MATLAB code
-  // make_geometry will not compile, use make testwrap to generate real make
-  module.generate_matlab_wrapper("actual");
-#ifndef WRAP_DISABLE_SERIALIZE
-  string epath = path + "/tests/expected/";
-#else
-  string epath = path + "/tests/expected2/";
-#endif
-  string apath = "actual/";
-
-  EXPECT(files_equal(epath + "geometry_wrapper.cpp" , apath + "geometry_wrapper.cpp" ));
-  EXPECT(files_equal(epath + "+gtsam/Point2.m"      , apath + "+gtsam/Point2.m"      ));
-  EXPECT(files_equal(epath + "+gtsam/Point3.m"      , apath + "+gtsam/Point3.m"      ));
-  EXPECT(files_equal(epath + "Test.m"               , apath + "Test.m"               ));
-  EXPECT(files_equal(epath + "MyBase.m"             , apath + "MyBase.m"             ));
-  EXPECT(files_equal(epath + "MyVector3.m"          , apath + "MyVector3.m"          ));
-  EXPECT(files_equal(epath + "MyVector12.m"         , apath + "MyVector12.m"         ));
-  EXPECT(files_equal(epath + "MyTemplatePoint2.m"   , apath + "MyTemplatePoint2.m"   ));
-  EXPECT(files_equal(epath + "MyTemplateMatrix.m"   , apath + "MyTemplateMatrix.m"   ));
-  EXPECT(files_equal(epath + "MyFactorPosePoint2.m" , apath + "MyFactorPosePoint2.m" ));
-  EXPECT(files_equal(epath + "aGlobalFunction.m"    , apath + "aGlobalFunction.m"    ));
-  EXPECT(files_equal(epath + "overloadedGlobalFunction.m", apath + "overloadedGlobalFunction.m"));
-}
-
-/* ************************************************************************* */
-TEST( wrap, python_code_geometry ) {
-  // Parse into class object
-  string header_path = topdir + "/wrap/tests";
-  Module module(header_path,"geometry",enable_verbose);
-  string path = topdir + "/wrap";
-
-  // clean out previous generated code
-  fs::remove_all("actual-python");
-
-  // emit MATLAB code
-  // make_geometry will not compile, use make testwrap to generate real make
-  module.generate_python_wrapper("actual-python");
-  string epath = path + "/tests/expected-python/";
-  string apath = "actual-python/";
-
-  EXPECT(files_equal(epath + "geometry_python.cpp", apath + "geometry_python.cpp" ));
-}
-
-/* ************************************************************************* */
-TEST( wrap, cython_code_geometry ) {
-  // Parse into class object
-  string header_path = topdir + "/wrap/tests";
-  Module module(header_path,"geometry",enable_verbose);
-  string path = topdir + "/wrap";
-
-  // clean out previous generated code
-  fs::remove_all("actual-python");
-
-  // emit MATLAB code
-  // make_geometry will not compile, use make testwrap to generate real make
-  module.generate_cython_wrapper("actual-cython");
-  string epath = path + "/tests/expected-cython/";
-  string apath = "actual-cython/";
-
-  EXPECT(files_equal(epath + "geometry.pxd", apath + "geometry.pxd" ));
-  EXPECT(files_equal(epath + "geometry.pyx", apath + "geometry.pyx" ));
-}
-
-/* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
-/* ************************************************************************* */
diff --git a/wrap/tests/test_docs.py b/wrap/tests/test_docs.py
new file mode 100644
index 000000000..2fe4f2086
--- /dev/null
+++ b/wrap/tests/test_docs.py
@@ -0,0 +1,229 @@
+"""
+Unit test for documentation generation
+Author: Matthew Sklar
+Date: May 2019
+"""
+import filecmp
+import os
+import os.path as path
+import shutil
+import sys
+import unittest
+import xml.etree.ElementTree as ET
+
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+import docs.parser.parse_doxygen_xml as parser
+from docs.docs import ClassDoc, Doc, Docs, FreeDoc
+
+
+tree_root = ET.Element('a')
+tree_left = ET.SubElement(tree_root, 'b')
+tree_right = ET.SubElement(tree_root, 1)
+tree_leaf = ET.SubElement(tree_right, None)
+tree_recursive = ET.SubElement(tree_left, tree_left)
+
+a = ET.Element('a')
+b = ET.SubElement(a, 'b')
+c = ET.SubElement(b, 'c')
+d = ET.SubElement(b, 'd')
+d2 = ET.SubElement(d, 'd')
+
+
+class TestDocument(unittest.TestCase):
+    DIR_NAME = path.dirname(__file__)
+
+    DOC_DIR = 'doc-test-files'
+    OUTPUT_XML_DIR = 'actual-xml-generation'
+    EXPECTED_XML_DIR = 'expected-xml-generation'
+
+    DOC_DIR_PATH = path.abspath(path.join(DIR_NAME, DOC_DIR))
+    OUTPUT_XML_DIR_PATH = path.abspath(path.join(DIR_NAME, OUTPUT_XML_DIR))
+    EXPECTED_XML_DIR_PATH = path.abspath(path.join(DIR_NAME, EXPECTED_XML_DIR))
+
+    def test_generate_xml(self):
+        '''Test parse_xml.generate_xml'''
+        shutil.rmtree(self.OUTPUT_XML_DIR_PATH, ignore_errors=True)
+        parser.generate_xml(
+            self.DOC_DIR_PATH, self.OUTPUT_XML_DIR_PATH, quiet=True)
+
+        self.assertTrue(os.path.isdir(self.OUTPUT_XML_DIR_PATH))
+
+        shutil.rmtree(path.join(self.OUTPUT_XML_DIR_PATH, 'xml'))
+        parser.generate_xml(
+            self.DOC_DIR_PATH, self.OUTPUT_XML_DIR_PATH, quiet=True)
+
+        dircmp = filecmp.dircmp(
+            self.OUTPUT_XML_DIR_PATH, self.EXPECTED_XML_DIR_PATH)
+
+        self.assertTrue(not dircmp.diff_files and not dircmp.funny_files)
+
+    def test_parse(self):
+        docs = parser.ParseDoxygenXML(
+            self.DOC_DIR_PATH, self.OUTPUT_XML_DIR_PATH).run()
+
+        for class_name in docs.get_class_docs_keys_list():
+            actual_tree_root = docs.get_class_docs(
+                class_name).get_tree().getroot()
+            expected_tree_root = ET.parse(class_name).getroot()
+
+            self.assertEqual(
+                ET.tostring(actual_tree_root), ET.tostring(expected_tree_root))
+
+
+class TestDocTemplate(unittest.TestCase):
+    class_doc_root = ClassDoc(tree_root)
+    class_doc_left = ClassDoc(tree_left)
+    class_doc_right = ClassDoc(tree_right)
+    class_doc_leaf = ClassDoc(tree_leaf)
+    class_doc_recursive = ClassDoc(tree_recursive)
+
+    free_doc_root = ClassDoc(tree_root)
+    free_doc_left = ClassDoc(tree_left)
+    free_doc_right = ClassDoc(tree_right)
+    free_doc_leaf = ClassDoc(tree_leaf)
+    free_doc_recursive = ClassDoc(tree_recursive)
+
+    CLASS_DOCS = {
+        'Class_Root': tree_root,
+        'Class_Left': tree_left,
+        'Class_Right': tree_right,
+        'Class_Leaf': tree_leaf,
+        'Class_Recursive': tree_recursive,
+    }
+
+    FREE_DOCS = {
+        'Free_Root': tree_root,
+        'Free_Left': tree_left,
+        'Free_Right': tree_right,
+        'Free_Leaf': tree_leaf,
+        'Free_Recursive': tree_recursive,
+    }
+
+    # ClassDoc
+    def test_class_doc(self):
+        '''Test the constructor in ClassDoc'''
+        self.assertIs(self.class_doc_root.tree, tree_root)
+        self.assertIs(self.class_doc_left.tree, tree_left)
+        self.assertIs(self.class_doc_right.tree, tree_right)
+        self.assertIs(self.class_doc_leaf.tree, tree_leaf)
+        self.assertIs(self.class_doc_recursive.tree, tree_recursive)
+
+    def test_class_doc_get_tree(self):
+        '''Test the get_tree() method is ClassDoc'''
+        self.assertIs(self.class_doc_root.get_tree(), tree_root)
+        self.assertIs(self.class_doc_left.get_tree(), tree_left)
+        self.assertIs(self.class_doc_right.get_tree(), tree_right)
+        self.assertIs(self.class_doc_leaf.get_tree(), tree_leaf)
+        self.assertIs(self.class_doc_recursive.get_tree(), tree_recursive)
+
+    def test_class_doc_eq(self):
+        '''Test ClassDoc.__eq__'''
+        doc1 = ClassDoc(ET.ElementTree(a))
+        doc2 = ClassDoc(ET.ElementTree(d))
+        doc3 = ClassDoc(ET.ElementTree(d2))
+
+        self.assertEqual(doc1, doc1)
+        self.assertEqual(doc2, doc2)
+        self.assertNotEqual(doc2, doc3)
+        self.assertEqual(None, None)
+        self.assertNotEqual(doc1, doc2)
+        self.assertNotEqual(doc1, None)
+
+    # FreeDoc
+    def test_free_doc(self):
+        '''Test the constructor in FreeDoc'''
+        self.assertIs(self.free_doc_root.tree, tree_root)
+        self.assertIs(self.free_doc_left.tree, tree_left)
+        self.assertIs(self.free_doc_right.tree, tree_right)
+        self.assertIs(self.free_doc_leaf.tree, tree_leaf)
+        self.assertIs(self.free_doc_recursive.tree, tree_recursive)
+
+    def test_free_doc_get_tree(self):
+        '''Test the get_tree() method is FreeDoc'''
+        self.assertIs(self.free_doc_root.get_tree(), tree_root)
+        self.assertIs(self.free_doc_left.get_tree(), tree_left)
+        self.assertIs(self.free_doc_right.get_tree(), tree_right)
+        self.assertIs(self.free_doc_leaf.get_tree(), tree_leaf)
+        self.assertIs(self.free_doc_recursive.get_tree(), tree_recursive)
+
+    def test_free_doc_eq(self):
+        '''Test FreeDoc.__eq__'''
+        doc1 = FreeDoc(ET.ElementTree(a))
+        doc2 = FreeDoc(ET.ElementTree(d))
+        doc3 = FreeDoc(ET.ElementTree(d2))
+
+        self.assertEqual(doc1, doc1)
+        self.assertEqual(doc2, doc2)
+        self.assertNotEqual(doc2, doc3)
+        self.assertEqual(None, None)
+        self.assertNotEqual(doc1, doc2)
+        self.assertNotEqual(doc1, None)
+
+    # Docs
+    def test_docs(self):
+        '''Test Docs template constructor'''
+        docs = Docs(self.CLASS_DOCS, self.FREE_DOCS)
+
+        self.assertIs(docs.class_docs, self.CLASS_DOCS)
+        self.assertIs(docs.free_docs, self.FREE_DOCS)
+
+    def test_get_class_docs(self):
+        docs = Docs(self.CLASS_DOCS, self.FREE_DOCS)
+
+        for doc_name in self.CLASS_DOCS.keys():
+            self.assertIs(
+                self.CLASS_DOCS.get(doc_name), docs.get_class_docs(doc_name))
+
+    def test_get_free_docs(self):
+        docs = Docs(self.CLASS_DOCS, self.FREE_DOCS)
+
+        for doc_name in self.FREE_DOCS.keys():
+            self.assertIs(
+                self.FREE_DOCS.get(doc_name), docs.get_free_docs(doc_name))
+
+    def test_get_class_docs_keys_list(self):
+        docs = Docs(self.CLASS_DOCS, self.FREE_DOCS)
+        class_doc_keys = list(self.CLASS_DOCS)
+        docs_list = docs.get_class_docs_keys_list()
+
+        self.assertEqual(len(self.CLASS_DOCS), len(docs_list))
+
+        for key in class_doc_keys:
+            self.assertIn(key, docs_list)
+
+    def test_get_free_docs_keys_list(self):
+        docs = Docs(self.CLASS_DOCS, self.FREE_DOCS)
+        free_doc_keys = list(self.FREE_DOCS)
+        docs_list = docs.get_free_docs_keys_list()
+
+        self.assertEqual(len(self.FREE_DOCS), len(docs_list))
+
+        for key in free_doc_keys:
+            self.assertIn(key, docs_list)
+
+    def test_get_class_docs_values_list(self):
+        docs = Docs(self.CLASS_DOCS, self.FREE_DOCS)
+        docs_list = docs.get_class_docs_values_list()
+
+        self.assertEqual(len(self.CLASS_DOCS), len(docs_list))
+        self.assertIn(tree_root, docs_list)
+        self.assertIn(tree_left, docs_list)
+        self.assertIn(tree_right, docs_list)
+        self.assertIn(tree_leaf, docs_list)
+        self.assertIn(tree_recursive, docs_list)
+
+    def test_get_free_docs_values_list(self):
+        docs = Docs(self.CLASS_DOCS, self.FREE_DOCS)
+        docs_list = docs.get_free_docs_values_list()
+
+        self.assertEqual(len(self.FREE_DOCS), len(docs_list))
+        self.assertIn(tree_root, docs_list)
+        self.assertIn(tree_left, docs_list)
+        self.assertIn(tree_right, docs_list)
+        self.assertIn(tree_leaf, docs_list)
+        self.assertIn(tree_recursive, docs_list)
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/wrap/tests/test_matlab_wrapper.py b/wrap/tests/test_matlab_wrapper.py
new file mode 100644
index 000000000..18dc49baf
--- /dev/null
+++ b/wrap/tests/test_matlab_wrapper.py
@@ -0,0 +1,140 @@
+"""
+Unit test for Matlab wrap program
+Author: Matthew Sklar
+Date: March 2019
+"""
+import os
+import sys
+import unittest
+import filecmp
+
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+import template_instantiator as instantiator
+import interface_parser as parser
+from matlab_wrapper import MatlabWrapper
+
+
+class TestWrap(unittest.TestCase):
+    TEST_DIR = os.path.dirname(os.path.realpath(__file__)) + "/"
+    MATLAB_TEST_DIR = TEST_DIR + "expected-matlab/"
+    MATLAB_ACTUAL_DIR = TEST_DIR + "actual-matlab/"
+
+    def _generate_content(self, cc_content, path=''):
+        """Generate files and folders from matlab wrapper content.
+
+        Keyword arguments:
+        cc_content -- the content to generate formatted as
+            (file_name, file_content) or
+            (folder_name, [(file_name, file_content)])
+        path -- the path to the files parent folder within the main folder
+        """
+        if path == '':
+            path = self.MATLAB_ACTUAL_DIR
+        for c in cc_content:
+            if type(c) == list:
+                if len(c) == 0:
+                    continue
+                import sys
+                print("c object: {}".format(c[0][0]), file=sys.stderr)
+                path_to_folder = path + '/' + c[0][0]
+
+                if not os.path.isdir(path_to_folder):
+                    try:
+                        os.makedirs(path_to_folder, exist_ok=True)
+                    except OSError:
+                        pass
+
+                for sub_content in c:
+                    import sys
+                    print("sub object: {}".format(sub_content[1][0][0]), file=sys.stderr)
+                    self._generate_content(sub_content[1], path_to_folder)
+            elif type(c[1]) == list:
+                path_to_folder = path + '/' + c[0]
+
+                import sys
+                print("[generate_content_global]: {}".format(path_to_folder), file=sys.stderr)
+                if not os.path.isdir(path_to_folder):
+                    try:
+                        os.makedirs(path_to_folder, exist_ok=True)
+                    except OSError:
+                        pass
+                for sub_content in c[1]:
+                    import sys
+                    path_to_file = path_to_folder + '/' + sub_content[0]
+                    print("[generate_global_method]: {}".format(path_to_file), file=sys.stderr)
+                    with open(path_to_file, 'w') as f:
+                        f.write(sub_content[1])
+            else:
+                path_to_file = path + '/' + c[0]
+
+                import sys
+                print("[generate_content]: {}".format(path_to_file), file=sys.stderr)
+                if not os.path.isdir(path_to_file):
+                    try:
+                        os.mkdir(path)
+                    except OSError:
+                        pass
+
+                with open(path_to_file, 'w') as f:
+                    f.write(c[1])
+
+    def test_geometry_matlab(self):
+        """ Check generation of matlab geometry wrapper.
+        python3 wrap/matlab_wrapper.py --src wrap/tests/geometry.h
+            --module_name geometry --out wrap/tests/actual-matlab
+        """
+        with open(self.TEST_DIR + 'geometry.h', 'r') as f:
+            content = f.read()
+
+        if not os.path.exists(self.MATLAB_ACTUAL_DIR):
+            os.mkdir(self.MATLAB_ACTUAL_DIR)
+
+        module = parser.Module.parseString(content)
+
+        instantiator.instantiate_namespace_inplace(module)
+
+        # Create MATLAB wrapper instance
+        wrapper = MatlabWrapper(
+            module=module,
+            module_name='geometry',
+            top_module_namespace=['gtsam'],
+            ignore_classes=[''],
+        )
+
+        cc_content = wrapper.wrap()
+
+        self._generate_content(cc_content)
+
+        def compare_and_diff(file):
+            output = self.MATLAB_ACTUAL_DIR + file
+            expected = self.MATLAB_TEST_DIR + file
+            success = filecmp.cmp(output, expected)
+            if not success:
+                print("Differ in file: {}".format(file))
+                os.system("diff {} {}".format(output, expected))
+            self.assertTrue(success)
+
+        self.assertTrue(os.path.isdir(self.MATLAB_ACTUAL_DIR + '+gtsam'))
+
+        files = [
+            '+gtsam/Point2.m',
+            '+gtsam/Point3.m',
+            'Test.m',
+            'MyBase.m',
+            'load2D.m',
+            'MyTemplatePoint2.m',
+            'MyTemplateMatrix.m',
+            'MyVector3.m',
+            'MyVector12.m',
+            'MyFactorPosePoint2.m',
+            'aGlobalFunction.m',
+            'overloadedGlobalFunction.m',
+            'geometry_wrapper.cpp'
+        ]
+
+        for file in files:
+            compare_and_diff(file)
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/wrap/tests/test_pybind_wrapper.py b/wrap/tests/test_pybind_wrapper.py
new file mode 100644
index 000000000..80032cbcd
--- /dev/null
+++ b/wrap/tests/test_pybind_wrapper.py
@@ -0,0 +1,71 @@
+"""
+Unit test for Pybind wrap program
+Author: Matthew Sklar
+Date: February 2019
+"""
+
+import os
+import sys
+import unittest
+import filecmp
+
+import os.path as path
+
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+sys.path.append(os.path.normpath(os.path.abspath(os.path.join(__file__, '../../../build/wrap'))))
+
+from pybind_wrapper import PybindWrapper
+import interface_parser as parser
+import template_instantiator as instantiator
+
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+
+class TestWrap(unittest.TestCase):
+    TEST_DIR = os.path.dirname(os.path.realpath(__file__)) + "/"
+
+    def test_geometry_python(self):
+        """
+        Check generation of python geometry wrapper.
+        python3 ../pybind_wrapper.py --src geometry.h --module_name
+            geometry_py --out output/geometry_py.cc"
+        """
+        with open(os.path.join(self.TEST_DIR, 'geometry.h'), 'r') as f:
+            content = f.read()
+
+        module = parser.Module.parseString(content)
+
+        instantiator.instantiate_namespace_inplace(module)
+
+        with open(self.TEST_DIR + "pybind_wrapper.tpl") as template_file:
+            module_template = template_file.read()
+        
+        # Create Pybind wrapper instance
+        wrapper = PybindWrapper(
+            module=module,
+            module_name='geometry_py',
+            use_boost=False,
+            top_module_namespaces=[''],
+            ignore_classes=[''],
+            module_template=module_template
+        )
+
+        cc_content = wrapper.wrap()
+
+        output = path.join(self.TEST_DIR, 'actual-python/geometry_py.cpp')
+
+        if not path.exists(path.join(self.TEST_DIR, 'actual-python')):
+            os.mkdir(path.join(self.TEST_DIR, 'actual-python'))
+
+        with open(output, 'w') as f:
+            f.write(cc_content)
+
+        expected = path.join(self.TEST_DIR, 'expected-python/geometry_pybind.cpp')
+        success = filecmp.cmp(output, expected)
+
+        if not success:
+            os.system("diff {} {}".format(output, expected))
+        self.assertTrue(success)
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp
deleted file mode 100644
index 1127fa77b..000000000
--- a/wrap/utilities.cpp
+++ /dev/null
@@ -1,148 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 utilities.ccp
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#include <iostream>
-#include <cstdlib>
-
-#include <boost/filesystem.hpp>
-
-#include "utilities.h"
-
-namespace wrap {
-
-using namespace std;
-
-/* ************************************************************************* */
-string file_contents(const string& filename, bool skipheader) {
-  ifstream ifs(filename.c_str());
-  if(!ifs) throw CantOpenFile(filename);
-
-  // read file into stringstream
-  stringstream ss;
-  if (skipheader) ifs.ignore(256,'\n');
-  ss << ifs.rdbuf();
-  ifs.close();
-
-  // return string
-  return ss.str();
-}
-
-/* ************************************************************************* */
-bool assert_equal(const string& expected, const string& actual) {
-  if (expected == actual)
-    return true;
-  printf("Not equal:\n");
-  cout << "expected: [" << expected << "]\n";
-  cout << "actual: [" << actual << "]" << endl;
-  return false;
-}
-
-/* ************************************************************************* */
-bool assert_equal(const vector<string>& expected, const vector<string>& actual) {
-  bool match = true;
-  if (expected.size() != actual.size())
-    match = false;
-  vector<string>::const_iterator
-    itExp = expected.begin(),
-    itAct = actual.begin();
-  if(match) {
-    for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) {
-      if (*itExp != *itAct) {
-        match = false;
-        break;
-      }
-    }
-  }
-  if(!match) {
-    cout << "expected: " << endl;
-    for(const vector<string>::value_type& a: expected) { cout << "["  << a << "] "; }
-    cout << "\nactual: " << endl;
-    for(const vector<string>::value_type& a: actual) { cout << "["  << a << "] "; }
-    cout << endl;
-    return false;
-  }
-  return true;
-
-}
-
-/* ************************************************************************* */
-bool files_equal(const string& expected, const string& actual, bool skipheader) {
-  try {
-    string expected_contents = file_contents(expected, skipheader);
-    string actual_contents   = file_contents(actual, skipheader);
-    bool equal = actual_contents == expected_contents;
-    if (!equal) {
-      cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n";
-      stringstream command;
-      command << "diff --ignore-all-space " << expected << " " << actual << endl;
-      if (system(command.str().c_str())<0)
-        throw "command '" + command.str() + "' failed";
-      cerr << ">>>\n";
-    }
-    return equal;
-  }
-  catch (const string& reason) {
-    cerr << "expection: " << reason << endl;
-    return false;
-  }
-  catch (CantOpenFile& e) {
-    cerr << "file opening error: " << e.what() << endl;
-    return false;
-  }
-  return true;
-}
-
-/* ************************************************************************* */
-string maybe_shared_ptr(bool add, const string& qtype, const string& type) {
-  string str = add? "Shared" : "";
-  if (add) str += type;
-  else str += qtype;
-  return str;
-}
-
-/* ************************************************************************* */
-string qualifiedName(const string& separator, const vector<string>& names) {
-  string result;
-  if (!names.empty()) {
-    for (size_t i = 0; i < names.size() - 1; ++i)
-      result += (names[i] + separator);
-    result += names.back();
-  }
-  return result;
-}
-
-/* ************************************************************************* */
-void createNamespaceStructure(const std::vector<std::string>& namespaces,
-    const std::string& toolboxPath) {
-  using namespace boost::filesystem;
-  path curPath = toolboxPath;
-  for(const string& subdir: namespaces) {
-//    curPath /= "+" + subdir; // original - resulted in valgrind error
-    curPath = curPath / string(string("+") + subdir);
-    if(!is_directory(curPath)) {
-      if(exists("+" + subdir))
-        throw OutputError("Need to write files to directory " + curPath.string() + ", which already exists as a file but is not a directory");
-      else
-        boost::filesystem::create_directory(curPath);
-    }
-  }
-}
-
-/* ************************************************************************* */
-
-} // \namespace wrap
diff --git a/wrap/utilities.h b/wrap/utilities.h
deleted file mode 100644
index 8c6f9a0b2..000000000
--- a/wrap/utilities.h
+++ /dev/null
@@ -1,136 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 utilities.h
- * @author Frank Dellaert
- * @author Andrew Melim
- * @author Richard Roberts
- **/
-
-#pragma once
-
-#include "FileWriter.h"
-
-#include <boost/format.hpp>
-
-#include <string>
-#include <vector>
-#include <exception>
-#include <fstream>
-#include <sstream>
-
-//#include <cstdint> // on Linux GCC: fails with error regarding needing C++0x std flags
-//#include <cinttypes>  // same failure as above
-#include <stdint.h> // works on Linux GCC
-
-namespace wrap {
-
-class CantOpenFile : public std::exception {
- private:
-  const std::string what_;
- public:
- CantOpenFile(const std::string& filename) : what_("Can't open file " + filename) {}
-  ~CantOpenFile() throw() {}
-  const char* what() const noexcept override { return what_.c_str(); }
-};
-
-class OutputError : public std::exception {
-private:
-  const std::string what_;
-public:
-  OutputError(const std::string& what) : what_(what) {}
-  ~OutputError() throw() {}
-  const char* what() const noexcept override { return what_.c_str(); }
-};
-
-class ParseFailed : public std::exception {
- private:
-  const std::string what_;
- public:
-   ParseFailed(int length) : what_((boost::format("Parse failed at character [%d]")%(length-1)).str()) {}
-   ~ParseFailed() throw() {}
-   const char* what() const noexcept override { return what_.c_str(); }
-};
-
-class DependencyMissing : public std::exception {
-private:
-  const std::string what_;
-public:
-  DependencyMissing(const std::string& dep, const std::string& loc) :
-    what_("Missing dependency '" + dep + "' in " + loc) {}
-  ~DependencyMissing() throw() {}
-  const char* what() const noexcept override { return what_.c_str(); }
-};
-
-class DuplicateDefinition : public std::exception {
-private:
-  const std::string what_;
-public:
-  DuplicateDefinition(const std::string& name) :
-    what_("Duplicate definition of " + name) {}
-  ~DuplicateDefinition() throw() {}
-  const char* what() const noexcept override { return what_.c_str(); }
-};
-
-class AttributeError : public std::exception {
-private:
-  const std::string what_;
-public:
-  AttributeError(const std::string& name, const std::string& problem) :
-    what_("Class " + name + ": " + problem) {}
-  ~AttributeError() throw() {}
-  const char* what() const noexcept override { return what_.c_str(); }
-};
-
-// "Unique" key to signal calling the matlab object constructor with a raw pointer
-// to a shared pointer of the same C++ object type as the MATLAB type.
-// Also present in matlab.h
-static const uint64_t ptr_constructor_key =
-  (uint64_t('G') << 56) |
-  (uint64_t('T') << 48) |
-  (uint64_t('S') << 40) |
-  (uint64_t('A') << 32) |
-  (uint64_t('M') << 24) |
-  (uint64_t('p') << 16) |
-  (uint64_t('t') << 8) |
-  (uint64_t('r'));
-
-/**
- * read contents of a file into a std::string
- */
-std::string file_contents(const std::string& filename, bool skipheader=false);
-
-/**
- * Check whether two files are equal
- * By default, skips the first line of actual so header is not generated
- */
-bool files_equal(const std::string& expected, const std::string& actual, bool skipheader=false);
-
-/**
- * Compare strings for unit tests
- */
-bool assert_equal(const std::string& expected, const std::string& actual);
-bool assert_equal(const std::vector<std::string>& expected, const std::vector<std::string>& actual);
-
-// auxiliary function to wrap an argument into a shared_ptr template
-std::string maybe_shared_ptr(bool add, const std::string& qtype, const std::string& type);
-
-/**
- * Return a qualified name
- */
-std::string qualifiedName(const std::string& separator, const std::vector<std::string>& names);
-
-/** creates the necessary folders for namespaces, as specified by a namespace stack */
-void createNamespaceStructure(const std::vector<std::string>& namespaces,
-    const std::string& toolboxPath);
-
-} // \namespace wrap
diff --git a/wrap/utilities/update.sh b/wrap/utilities/update.sh
new file mode 100644
index 000000000..200c03066
--- /dev/null
+++ b/wrap/utilities/update.sh
@@ -0,0 +1,13 @@
+#!/bin/bash
+REF=${1-master} # branch or tag; defaults to 'master' if parameter 1 not present
+REMOTE=pybind11 # just a name to identify the remote
+REPO=git@github.com:pybind/pybind11.git # replace this with your repository URL
+FOLDER=pybind11 # where to mount the subtree
+
+git remote add $REMOTE --no-tags $REPO
+if [[ -d $FOLDER ]]; then # update the existing subtree
+    git subtree pull $REMOTE $REF --prefix=$FOLDER --squash -m "Merging '$REF' into '$FOLDER'"
+else # add the subtree
+    git subtree add  $REMOTE $REF --prefix=$FOLDER --squash -m "Merging '$REF' into '$FOLDER'"
+fi
+git remote remove $REMOTE
\ No newline at end of file
diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp
deleted file mode 100644
index 5874f52d0..000000000
--- a/wrap/wrap.cpp
+++ /dev/null
@@ -1,89 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 wrap.cpp
- * @brief wraps functions
- * @author Frank Dellaert
- **/
-
-#include <stdio.h>
-#include <iostream>
-
-#include "Module.h"
-
-using namespace std;
-
-/** Displays usage information */
-void usage() {
-  cerr << "wrap parses an interface file and produces a MATLAB or Cython toolbox" << endl;
-  cerr << "usage: wrap [--matlab|--cython] interfacePath moduleName toolboxPath cythonImports" << endl;
-  cerr << "  interfacePath : *absolute* path to directory of module interface file" << endl;
-  cerr << "  moduleName    : the name of the module, interface file must be called moduleName.h" << endl;
-  cerr << "  toolboxPath   : the directory in which to generate the wrappers" << endl;
-  cerr << "  cythonImports : extra imports for Cython pxd header file" << endl;
-}
-
-/**
- * Top-level function to wrap a module
- * @param language can be "--matlab" or "--cython"
- * @param interfacePath path to where interface file lives, e.g., borg/gtsam
- * @param moduleName name of the module to be generated e.g. gtsam
- * @param toolboxPath path where the toolbox should be generated, e.g. borg/gtsam/build
- * @param headerPath is the path to matlab.h
- * @param cythonImports additional imports to include in the generated Cython pxd header file
- */
-void generate_toolbox(
-           const string& language,
-           const string& interfacePath,
-           const string& moduleName,
-           const string& toolboxPath,
-           const string& cythonImports)
-{
-  // Parse interface file into class object
-  // This recursively creates Class objects, Method objects, etc...
-  wrap::Module module(interfacePath, moduleName, false);
-
-  if (language == "--matlab")
-  // Then emit MATLAB code
-    module.generate_matlab_wrapper(toolboxPath);
-  else if (language == "--cython") {
-    module.generate_cython_wrapper(toolboxPath, cythonImports);
-  }
-  else {
-      cerr << "First argument invalid" << endl;
-      cerr << endl;
-      usage();
-  }
-}
-
-/**
- * main parses arguments and calls generate_matlab_toolbox above
- * Typically called from "make all" using appropriate arguments
- */
-int main(int argc, const char* argv[]) {
-  if (argc != 6) {
-    cerr << "Invalid arguments:\n";
-    for (int i=0; i<argc; ++i)
-      cerr << argv[i] << endl;
-    cerr << endl;
-    usage();
-  }
-  else {
-    try {
-        generate_toolbox(argv[1], argv[2],argv[3],argv[4],argv[5]);
-    } catch(std::exception& e) {
-      cerr << e.what() << endl;
-      return 1;
-    }
-  }
-  return 0;
-}