diff --git a/CMakeLists.txt b/CMakeLists.txt
index b19ece0e6..6fc456609 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -60,17 +60,6 @@ include(cmake/HandleGlobalBuildFlags.cmake) # Build flags
 # Build CppUnitLite
 add_subdirectory(CppUnitLite)
 
-# This is the new wrapper
-if(GTSAM_BUILD_PYTHON)
-    # Need to set this for the wrap package so we don't use the default value.
-    set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION}
-        CACHE STRING "The Python version to use for wrapping")
-
-    add_subdirectory(wrap)
-    list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")
-    add_subdirectory(python)
-endif()
-
 # Build GTSAM library
 add_subdirectory(gtsam)
 
@@ -88,8 +77,22 @@ if (GTSAM_BUILD_UNSTABLE)
     add_subdirectory(gtsam_unstable)
 endif()
 
+# This is the new wrapper
+if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
+    # Need to set this for the wrap package so we don't use the default value.
+    set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION}
+        CACHE STRING "The Python version to use for wrapping")
+    add_subdirectory(wrap)
+    list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")
+endif()
+
+# Python toolbox
+if(GTSAM_BUILD_PYTHON)
+    add_subdirectory(python)
+endif()
+
 # Matlab toolbox
-if (GTSAM_INSTALL_MATLAB_TOOLBOX)
+if(GTSAM_INSTALL_MATLAB_TOOLBOX)
     add_subdirectory(matlab)
 endif()
 
diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt
index 451ca38a4..2c32049a0 100644
--- a/cmake/CMakeLists.txt
+++ b/cmake/CMakeLists.txt
@@ -16,11 +16,8 @@ install(FILES
   dllexport.h.in
   GtsamBuildTypes.cmake
   GtsamMakeConfigFile.cmake
-  GtsamMatlabWrap.cmake
   GtsamTesting.cmake
   GtsamPrinting.cmake
   FindNumPy.cmake
   README.html
   DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools")
-
-
diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake
deleted file mode 100644
index 993af47c4..000000000
--- a/cmake/GtsamMatlabWrap.cmake
+++ /dev/null
@@ -1,448 +0,0 @@
-# Check / set dependent variables for MATLAB wrapper
-if(GTSAM_INSTALL_MATLAB_TOOLBOX)
-    find_package(Matlab COMPONENTS MEX_COMPILER REQUIRED)
-    if(NOT Matlab_MEX_COMPILER)
-        message(FATAL_ERROR "Cannot find MEX compiler binary. Please check your Matlab installation and ensure MEX in installed as well.")
-        endif()
-
-    if(GTSAM_BUILD_TYPE_POSTFIXES)
-        set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
-    endif()
-
-    if(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.")
-    endif()
-endif()
-
-# Fixup the Python paths
-if(GTWRAP_DIR)
-	# packaged
-	set(GTWRAP_PACKAGE_DIR ${GTWRAP_DIR})
-else()
-	set(GTWRAP_PACKAGE_DIR ${CMAKE_SOURCE_DIR}/wrap)
-endif()
-
-# Set up cache options
-option(GTSAM_MEX_BUILD_STATIC_MODULE "Build MATLAB wrapper statically (increases build time)" OFF)
-set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation")
-set(GTSAM_TOOLBOX_INSTALL_PATH "" CACHE PATH "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox")
-if(NOT GTSAM_TOOLBOX_INSTALL_PATH)
-    set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox")
-endif()
-
-# GTSAM_MEX_BUILD_STATIC_MODULE is not for Windows - on Windows any static
-# are already compiled into the library by the linker
-if(GTSAM_MEX_BUILD_STATIC_MODULE AND WIN32)
-    message(FATAL_ERROR "GTSAM_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries. To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).")
-endif()
-
-set(MEX_COMMAND ${Matlab_MEX_COMPILER} CACHE PATH "Path to MATLAB MEX compiler")
-set(MATLAB_ROOT ${Matlab_ROOT_DIR} CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
-
-# Try to automatically configure mex path from provided custom `bin` path.
-if(GTSAM_CUSTOM_MATLAB_PATH)
-    set(matlab_bin_directory ${GTSAM_CUSTOM_MATLAB_PATH})
-
-    if(WIN32)
-        set(mex_program_name "mex.bat")
-    else()
-        set(mex_program_name "mex")
-    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
-    # on the system path.
-    find_program(MEX_COMMAND ${mex_program_name}
-       PATHS ${matlab_bin_directory} ENV PATH
-       NO_DEFAULT_PATH)
-
-    mark_as_advanced(FORCE MEX_COMMAND)
-    # Now that we have mex, trace back to find the Matlab installation root
-    get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
-    get_filename_component(mex_path "${MEX_COMMAND}" PATH)
-    if(mex_path MATCHES ".*/win64$")
-       get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE)
-    else()
-       get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
-    endif()
-endif()
-
-
-# User-friendly wrapping function.  Builds a mex module from the provided
-# interfaceHeader.  For example, for the interface header gtsam.h,
-# this will build the wrap module 'gtsam'.
-#
-# Arguments:
-#
-# interfaceHeader:  The relative path to the wrapper interface definition file.
-# linkLibraries:    Any *additional* libraries to link.  Your project library
-#                   (e.g. `lba`), libraries it depends on, and any necessary
-#                   MATLAB libraries will be linked automatically.  So normally,
-#                   leave this empty.
-# extraIncludeDirs: Any *additional* include paths required by dependent
-#                   libraries that have not already been added by
-#                   include_directories.  Again, normally, leave this empty.
-# extraMexFlags:    Any *additional* flags to pass to the compiler when building
-#                   the wrap code.  Normally, leave this empty.
-function(wrap_and_install_library interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)
-	wrap_library_internal("${interfaceHeader}" "${linkLibraries}" "${extraIncludeDirs}" "${mexFlags}")
-	install_wrapped_library_internal("${interfaceHeader}")
-endfunction()
-
-
-# Internal function that wraps a library and compiles the wrapper
-function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)
-	if(UNIX AND NOT APPLE)
-		if(CMAKE_SIZEOF_VOID_P EQUAL 8)
-			set(mexModuleExt mexa64)
-		else()
-			set(mexModuleExt mexglx)
-		endif()
-	elseif(APPLE)
-		set(mexModuleExt mexmaci64)
-	elseif(MSVC)
-		if(CMAKE_CL_64)
-			set(mexModuleExt mexw64)
-		else()
-			set(mexModuleExt mexw32)
-		endif()
-	endif()
-
-	# Wrap codegen interface
-    #usage: wrap interfacePath moduleName toolboxPath headerPath
-    #  interfacePath : *absolute* path to directory of module interface file
-    #  moduleName    : the name of the module, interface file must be called moduleName.h
-    #  toolboxPath   : the directory in which to generate the wrappers
-    #  headerPath    : path to matlab.h
-
-	# Extract module name from interface header file name
-	get_filename_component(interfaceHeader "${interfaceHeader}" ABSOLUTE)
-	get_filename_component(modulePath "${interfaceHeader}" PATH)
-	get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
-
-	# Paths for generated files
-	set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
-	set(generated_cpp_file "${generated_files_path}/${moduleName}_wrapper.cpp")
-	set(compiled_mex_modules_root "${PROJECT_BINARY_DIR}/wrap/${moduleName}_mex")
-
-	message(STATUS "Building wrap module ${moduleName}")
-
-	# Find matlab.h in GTSAM
-	if(("${PROJECT_NAME}" STREQUAL "gtsam") OR
-		("${PROJECT_NAME}" STREQUAL "gtsam_unstable"))
-		set(matlab_h_path "${PROJECT_SOURCE_DIR}")
-	else()
-		if(NOT GTSAM_INCLUDE_DIR)
-			message(FATAL_ERROR "You must call find_package(GTSAM) before using wrap")
-		endif()
-		list(GET GTSAM_INCLUDE_DIR 0 installed_includes_path)
-		set(matlab_h_path "${installed_includes_path}/wrap")
-	endif()
-
-	# If building a static mex module, add all cmake-linked libraries to the
-	# explicit link libraries list so that the next block of code can unpack
-	# any static libraries
-	set(automaticDependencies "")
-	foreach(lib ${moduleName} ${linkLibraries})
-	  #message("MODULE NAME: ${moduleName}")
-		if(TARGET "${lib}")
-            get_target_property(dependentLibraries ${lib} INTERFACE_LINK_LIBRARIES)
-           # message("DEPENDENT LIBRARIES:  ${dependentLibraries}")
-            if(dependentLibraries)
-               list(APPEND automaticDependencies ${dependentLibraries})
-            endif()
-        endif()
-    endforeach()
-
-    ## CHRIS: Temporary fix. On my system the get_target_property above returned Not-found for gtsam module
-    ## This needs to be fixed!!
-    if(UNIX AND NOT APPLE)
-      list(APPEND automaticDependencies ${Boost_SERIALIZATION_LIBRARY_RELEASE} ${Boost_FILESYSTEM_LIBRARY_RELEASE}
-        ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE})
-      if(Boost_TIMER_LIBRARY_RELEASE AND NOT GTSAM_DISABLE_NEW_TIMERS) # Only present in Boost >= 1.48.0
-        list(APPEND automaticDependencies ${Boost_TIMER_LIBRARY_RELEASE} ${Boost_CHRONO_LIBRARY_RELEASE})
-        if(GTSAM_MEX_BUILD_STATIC_MODULE)
-          #list(APPEND automaticDependencies -Wl,--no-as-needed -lrt)
-        endif()
-      endif()
-    endif()
-
-    #message("AUTOMATIC DEPENDENCIES:  ${automaticDependencies}")
-    ## CHRIS: End temporary fix
-
-	# Separate dependencies
-	set(correctedOtherLibraries "")
-	set(otherLibraryTargets "")
-    set(otherLibraryNontargets "")
-    set(otherSourcesAndObjects "")
-	foreach(lib ${moduleName} ${linkLibraries} ${automaticDependencies})
-		if(TARGET "${lib}")
-		    if(GTSAM_MEX_BUILD_STATIC_MODULE)
-		    	get_target_property(target_sources ${lib} SOURCES)
-		    	list(APPEND otherSourcesAndObjects ${target_sources})
-		    else()
-				list(APPEND correctedOtherLibraries ${lib})
-				list(APPEND otherLibraryTargets ${lib})
-		    endif()
-		else()
-			get_filename_component(file_extension "${lib}" EXT)
-			get_filename_component(lib_name "${lib}" NAME_WE)
-			if(file_extension STREQUAL ".a" AND GTSAM_MEX_BUILD_STATIC_MODULE)
-				# For building a static MEX module, unpack the static library
-				# and compile its object files into our module
-				file(MAKE_DIRECTORY "${generated_files_path}/${lib_name}_objects")
-				execute_process(COMMAND ar -x "${lib}"
-				                WORKING_DIRECTORY "${generated_files_path}/${lib_name}_objects"
-				                RESULT_VARIABLE ar_result)
-				if(NOT ar_result EQUAL 0)
-					message(FATAL_ERROR "Failed extracting ${lib}")
-				endif()
-
-				# Get list of object files
-				execute_process(COMMAND ar -t "${lib}"
-								OUTPUT_VARIABLE object_files
-				                RESULT_VARIABLE ar_result)
-				if(NOT ar_result EQUAL 0)
-					message(FATAL_ERROR "Failed listing ${lib}")
-				endif()
-
-				# Add directory to object files
-				string(REPLACE "\n" ";" object_files_list "${object_files}")
-				foreach(object_file ${object_files_list})
-					get_filename_component(file_extension "${object_file}" EXT)
-					if(file_extension STREQUAL ".o")
-						list(APPEND otherSourcesAndObjects "${generated_files_path}/${lib_name}_objects/${object_file}")
-					endif()
-				endforeach()
-			else()
-				list(APPEND correctedOtherLibraries ${lib})
-	            list(APPEND otherLibraryNontargets ${lib})
-            endif()
-		endif()
-	endforeach()
-
-    # Check libraries for conflicting versions built-in to MATLAB
-    set(dependentLibraries "")
-    if(NOT "${otherLibraryTargets}" STREQUAL "")
-        foreach(target ${otherLibraryTargets})
-            get_target_property(dependentLibrariesOne ${target} INTERFACE_LINK_LIBRARIES)
-            list(APPEND dependentLibraries ${dependentLibrariesOne})
-        endforeach()
-    endif()
-    list(APPEND dependentLibraries ${otherLibraryNontargets})
-    check_conflicting_libraries_internal("${dependentLibraries}")
-
-	# Set up generation of module source file
-	file(MAKE_DIRECTORY "${generated_files_path}")
-
-    find_package(PythonInterp
-            ${GTSAM_PYTHON_VERSION}
-            EXACT
-            REQUIRED)
-    find_package(PythonLibs
-            ${GTSAM_PYTHON_VERSION}
-            EXACT
-            REQUIRED)
-
-
-	set(_ignore gtsam::Point2
-			gtsam::Point3
-			gtsam::BearingRangeFactor
-			gtsam::BearingRangeFactor2D
-			gtsam::BearingRangeFactorPose2)
-
-    # set the matlab wrapping script variable
-    set(MATLAB_WRAP_SCRIPT "${GTSAM_SOURCE_DIR}/wrap/scripts/matlab_wrap.py")
-
-    add_custom_command(
-		OUTPUT ${generated_cpp_file}
-		DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects}
-        COMMAND ${CMAKE_COMMAND} -E env "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}"
-			${PYTHON_EXECUTABLE}
-			${MATLAB_WRAP_SCRIPT}
-            --src ${interfaceHeader}
-			--module_name ${moduleName}
-            --out ${generated_files_path}
-			--top_module_namespaces ${moduleName}
-			--ignore ${_ignore}
-		VERBATIM
-		WORKING_DIRECTORY ${generated_files_path})
-
-	# Set up building of mex module
-	string(REPLACE ";" " " extraMexFlagsSpaced "${extraMexFlags}")
-	string(REPLACE ";" " " mexFlagsSpaced "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
-	add_library(${moduleName}_matlab_wrapper MODULE ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects})
-	target_link_libraries(${moduleName}_matlab_wrapper ${correctedOtherLibraries})
-	target_link_libraries(${moduleName}_matlab_wrapper ${moduleName})
-	set_target_properties(${moduleName}_matlab_wrapper PROPERTIES
-		OUTPUT_NAME              "${moduleName}_wrapper"
-		PREFIX                   ""
-		SUFFIX                   ".${mexModuleExt}"
-		LIBRARY_OUTPUT_DIRECTORY "${compiled_mex_modules_root}"
-		ARCHIVE_OUTPUT_DIRECTORY "${compiled_mex_modules_root}"
-		RUNTIME_OUTPUT_DIRECTORY "${compiled_mex_modules_root}"
-		CLEAN_DIRECT_OUTPUT 1)
-	set_property(TARGET ${moduleName}_matlab_wrapper APPEND_STRING PROPERTY COMPILE_FLAGS " ${extraMexFlagsSpaced} ${mexFlagsSpaced} \"-I${MATLAB_ROOT}/extern/include\" -DMATLAB_MEX_FILE -DMX_COMPAT_32")
-	set_property(TARGET ${moduleName}_matlab_wrapper APPEND PROPERTY INCLUDE_DIRECTORIES ${extraIncludeDirs})
-	# Disable build type postfixes for the mex module - we install in different directories for each build type instead
-	foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
-		string(TOUPPER "${build_type}" build_type_upper)
-		set_target_properties(${moduleName}_matlab_wrapper PROPERTIES ${build_type_upper}_POSTFIX "")
-	endforeach()
-	# Set up platform-specific flags
-	if(MSVC)
-		if(CMAKE_CL_64)
-			set(mxLibPath "${MATLAB_ROOT}/extern/lib/win64/microsoft")
-		else()
-			set(mxLibPath "${MATLAB_ROOT}/extern/lib/win32/microsoft")
-		endif()
-		target_link_libraries(${moduleName}_matlab_wrapper "${mxLibPath}/libmex.lib" "${mxLibPath}/libmx.lib" "${mxLibPath}/libmat.lib")
-		set_target_properties(${moduleName}_matlab_wrapper PROPERTIES LINK_FLAGS "/export:mexFunction")
-		set_property(SOURCE "${generated_cpp_file}" APPEND PROPERTY COMPILE_FLAGS "/bigobj")
-	elseif(APPLE)
-		set(mxLibPath "${MATLAB_ROOT}/bin/maci64")
-		target_link_libraries(${moduleName}_matlab_wrapper "${mxLibPath}/libmex.dylib" "${mxLibPath}/libmx.dylib" "${mxLibPath}/libmat.dylib")
-	endif()
-
-    # Hacking around output issue with custom command
-    # Deletes generated build folder
-    add_custom_target(wrap_${moduleName}_matlab_distclean
-	    COMMAND cmake -E remove_directory ${generated_files_path}
-		COMMAND cmake -E remove_directory ${compiled_mex_modules_root})
-endfunction()
-
-# Internal function that installs a wrap toolbox
-function(install_wrapped_library_internal interfaceHeader)
-	get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
-	set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
-
-    # NOTE: only installs .m and mex binary files (not .cpp) - the trailing slash on the directory name
-	# here prevents creating the top-level module name directory in the destination.
-	message(STATUS "Installing Matlab Toolbox to ${GTSAM_TOOLBOX_INSTALL_PATH}")
-	if(GTSAM_BUILD_TYPE_POSTFIXES)
-		foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
-			string(TOUPPER "${build_type}" build_type_upper)
-			if(${build_type_upper} STREQUAL "RELEASE")
-				set(build_type_tag "") # Don't create release mode tag on installed directory
-			else()
-				set(build_type_tag "${build_type}")
-			endif()
-			# Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one
-			get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH)
-			get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME)
-			install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING PATTERN "*.m")
-			install(TARGETS ${moduleName}_matlab_wrapper
-				LIBRARY DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}"
-				RUNTIME DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}")
-		endforeach()
-	else()
-		install(DIRECTORY "${generated_files_path}/" DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH} FILES_MATCHING PATTERN "*.m")
-		install(TARGETS ${moduleName}_matlab_wrapper
-			LIBRARY DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}
-			RUNTIME DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH})
-	endif()
-endfunction()
-
-# Internal function to check for libraries installed with MATLAB that may conflict
-# and prints a warning to move them if problems occur.
-function(check_conflicting_libraries_internal libraries)
-    if(UNIX)
-        # Set path for matlab's built-in libraries
-        if(APPLE)
-            set(mxLibPath "${MATLAB_ROOT}/bin/maci64")
-        else()
-            if(CMAKE_CL_64)
-                set(mxLibPath "${MATLAB_ROOT}/bin/glnxa64")
-            else()
-                set(mxLibPath "${MATLAB_ROOT}/bin/glnx86")
-            endif()
-        endif()
-
-        # List matlab's built-in libraries
-        file(GLOB matlabLibs RELATIVE "${mxLibPath}" "${mxLibPath}/lib*")
-
-        # Convert to base names
-        set(matlabLibNames "")
-        foreach(lib ${matlabLibs})
-            get_filename_component(libName "${lib}" NAME_WE)
-            list(APPEND matlabLibNames "${libName}")
-        endforeach()
-
-        # Get names of link libraries
-        set(linkLibNames "")
-        foreach(lib ${libraries})
-            string(FIND "${lib}" "/" slashPos)
-            if(NOT slashPos EQUAL -1)
-                # If the name is a path, just get the library name
-                get_filename_component(libName "${lib}" NAME_WE)
-                list(APPEND linkLibNames "${libName}")
-            else()
-                # It's not a path, so see if it looks like a filename
-                get_filename_component(ext "${lib}" EXT)
-                if(NOT "${ext}" STREQUAL "")
-                    # It's a filename, so get the base name
-                    get_filename_component(libName "${lib}" NAME_WE)
-                    list(APPEND linkLibNames "${libName}")
-                else()
-                    # It's not a filename so it must be a short name, add the "lib" prefix
-                    list(APPEND linkLibNames "lib${lib}")
-                endif()
-            endif()
-        endforeach()
-
-        # Remove duplicates
-        list(REMOVE_DUPLICATES linkLibNames)
-
-        set(conflictingLibs "")
-        foreach(lib ${linkLibNames})
-            list(FIND matlabLibNames "${lib}" libPos)
-            if(NOT libPos EQUAL -1)
-                if(NOT conflictingLibs STREQUAL "")
-                    set(conflictingLibs "${conflictingLibs}, ")
-                endif()
-                set(conflictingLibs "${conflictingLibs}${lib}")
-            endif()
-        endforeach()
-
-        if(NOT "${conflictingLibs}" STREQUAL "")
-            message(WARNING "GTSAM links to the libraries [ ${conflictingLibs} ] on your system, but "
-                "MATLAB is distributed with its own versions of these libraries which may conflict. "
-                "If you get strange errors or crashes with the GTSAM MATLAB wrapper, move these "
-                "libraries out of MATLAB's built-in library directory, which is ${mxLibPath} on "
-                "your system.  MATLAB will usually still work with these libraries moved away, but "
-                "if not, you'll have to compile the static GTSAM MATLAB wrapper module.")
-        endif()
-    endif()
-endfunction()
-
-# Helper function to install MATLAB scripts and handle multiple build types where the scripts
-# should be installed to all build type toolboxes
-function(install_matlab_scripts source_directory patterns)
-	set(patterns_args "")
-	set(exclude_patterns "")
-	if(NOT GTSAM_WRAP_SERIALIZATION)
-	    set(exclude_patterns "testSerialization.m")
-	endif()
-
-	foreach(pattern ${patterns})
-		list(APPEND patterns_args PATTERN "${pattern}")
-	endforeach()
-	if(GTSAM_BUILD_TYPE_POSTFIXES)
-		foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
-			string(TOUPPER "${build_type}" build_type_upper)
-			if(${build_type_upper} STREQUAL "RELEASE")
-				set(build_type_tag "") # Don't create release mode tag on installed directory
-			else()
-				set(build_type_tag "${build_type}")
-			endif()
-			# Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one
-			get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH)
-			get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" 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 "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
-	endif()
-
-endfunction()
diff --git a/cmake/README.md b/cmake/README.md
index 7f38bbcf2..569a401b1 100644
--- a/cmake/README.md
+++ b/cmake/README.md
@@ -67,32 +67,6 @@ Defines two useful functions for creating CTest unit tests.  Also immediately cr
                        an empty string "" if nothing needs to be excluded.
         linkLibraries: The list of libraries to link to.
 
-## GtsamMatlabWrap
-
-    include(GtsamMatlabWrap)
-    
-Defines functions for generating MATLAB wrappers.  Also immediately creates several CMake options for configuring the wrapper.
-
-*   `wrap_and_install_library(interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)` Generates wrap code and compiles the wrapper.
-
-    Usage example:
-    
-        `wrap_and_install_library("lba.h" "" "" "")`
-    
-    Arguments:
-    
-        interfaceHeader:  The relative or absolute path to the wrapper interface
-                          definition file.
-        linkLibraries:    Any *additional* libraries to link.  Your project library
-                          (e.g. `lba`), libraries it depends on, and any necessary
-                          MATLAB libraries will be linked automatically.  So normally,
-                          leave this empty.
-        extraIncludeDirs: Any *additional* include paths required by dependent
-                          libraries that have not already been added by
-                          include_directories.  Again, normally, leave this empty.
-        extraMexFlags:    Any *additional* flags to pass to the compiler when building
-                          the wrap code.  Normally, leave this empty.
-
 ## GtsamMakeConfigFile
 
     include(GtsamMakeConfigFile)
diff --git a/cmake/example_project/CMakeLists.txt b/cmake/example_project/CMakeLists.txt
deleted file mode 100644
index 620ad4811..000000000
--- a/cmake/example_project/CMakeLists.txt
+++ /dev/null
@@ -1,49 +0,0 @@
-# This file should be used as a template for creating new projects using the CMake tools
-# This project has the following features
-#    - GTSAM linking
-#    - Unit tests via CppUnitLite
-#    - Scripts
-#    - Automatic MATLAB wrapper generation
-
-###################################################################################
-# To create your own project, replace "example" with the actual name of your project
-cmake_minimum_required(VERSION 3.0)
-project(example CXX C)
-
-# Include GTSAM CMake tools
-find_package(GTSAMCMakeTools)
-include(GtsamBuildTypes) # Load build type flags and default to Debug mode
-include(GtsamTesting)    # Easy functions for creating unit tests and scripts
-include(GtsamMatlabWrap) # Automatic MATLAB wrapper generation
-
-# Ensure that local folder is searched before library folders
-include_directories(BEFORE "${PROJECT_SOURCE_DIR}")
-
-###################################################################################
-# Find GTSAM components
-find_package(GTSAM REQUIRED) # Uses installed package
-# Note: Since Jan-2019, GTSAMConfig.cmake defines exported CMake targets
-# that automatically do include the include_directories() without the need
-# to call include_directories(), just target_link_libraries(NAME gtsam)
-#include_directories(${GTSAM_INCLUDE_DIR})
-
-###################################################################################
-# Build static library from common sources
-set(CONVENIENCE_LIB_NAME ${PROJECT_NAME})
-add_library(${CONVENIENCE_LIB_NAME} SHARED example/PrintExamples.h example/PrintExamples.cpp)
-target_link_libraries(${CONVENIENCE_LIB_NAME} gtsam)
-
-# Install library
-install(TARGETS ${CONVENIENCE_LIB_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
-
-###################################################################################
-# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library)
-gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}")
-
-###################################################################################
-# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library)
-gtsamAddExamplesGlob("*.cpp" "" "${CONVENIENCE_LIB_NAME}")
-
-###################################################################################
-# Build MATLAB wrapper (CMake tracks the dependecy to link with GTSAM through our project's static library)
-wrap_and_install_library("example.h" "${CONVENIENCE_LIB_NAME}" "" "")
diff --git a/cmake/example_project/README.md b/cmake/example_project/README.md
deleted file mode 100644
index a1269cf19..000000000
--- a/cmake/example_project/README.md
+++ /dev/null
@@ -1,32 +0,0 @@
-# MATLAB Wrapper Example Project
-
-This project serves as a lightweight example for demonstrating how to wrap C++ code in MATLAB using GTSAM.
-
-## Compiling
-
-We follow the regular build procedure inside the `example_project` directory:
-
-```sh
-mkdir build && cd build
-cmake ..
-make -j8
-sudo make install
-
-sudo ldconfig  # ensures the shared object file generated is correctly loaded
-```
-
-## Usage
-
-Now you can open MATLAB and add the `gtsam_toolbox` to the MATLAB path
-
-```matlab
-addpath('/usr/local/gtsam_toolbox')
-```
-
-At this point you are ready to run the example project. Starting from the `example_project` directory inside MATLAB, simply run code like regular MATLAB, e.g.
-
-```matlab
-pe = example.PrintExamples();
-pe.sayHello();
-pe.sayGoodbye();
-```
\ No newline at end of file
diff --git a/cmake/example_project/SayGoodbye.cpp b/cmake/example_project/SayGoodbye.cpp
deleted file mode 100644
index be1165ef6..000000000
--- a/cmake/example_project/SayGoodbye.cpp
+++ /dev/null
@@ -1,23 +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     SayGoodbye.cpp
- * @brief    Example script for example project
- * @author   Richard Roberts
- */
-
-#include <example/PrintExamples.h>
-
-int main(int argc, char *argv[]) {
-  example::PrintExamples().sayGoodbye();
-  return 0;
-}
diff --git a/cmake/example_project/SayHello.cpp b/cmake/example_project/SayHello.cpp
deleted file mode 100644
index 2da06ab32..000000000
--- a/cmake/example_project/SayHello.cpp
+++ /dev/null
@@ -1,23 +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     SayHello.cpp
- * @brief    Example script for example project
- * @author   Richard Roberts
- */
-
-#include <example/PrintExamples.h>
-
-int main(int argc, char *argv[]) {
-  example::PrintExamples().sayHello();
-  return 0;
-}
diff --git a/cmake/example_project/example.h b/cmake/example_project/example.h
deleted file mode 100644
index b0d732e14..000000000
--- a/cmake/example_project/example.h
+++ /dev/null
@@ -1,31 +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     example.h
- * @brief    Example wrapper interface file
- * @author   Richard Roberts
- */
-
-// This is an interface file for automatic MATLAB wrapper generation.  See
-// gtsam.h for full documentation and more examples.
-
-#include <example/PrintExamples.h>
-
-namespace example {
-
-class PrintExamples {
-    PrintExamples();
-    void sayHello() const;
-    void sayGoodbye() const;
-};
-
-}
diff --git a/cmake/example_project/example/PrintExamples.cpp b/cmake/example_project/example/PrintExamples.cpp
deleted file mode 100644
index 1e9f10713..000000000
--- a/cmake/example_project/example/PrintExamples.cpp
+++ /dev/null
@@ -1,44 +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     print_examples.cpp
- * @brief    Example library file
- * @author   Richard Roberts
- */
-
-#include <iostream>
-
-#include <example/PrintExamples.h>
-
-namespace example {
-
-void PrintExamples::sayHello() const {
-  std::cout << internal::getHelloString() << std::endl;
-}
-
-void PrintExamples::sayGoodbye() const {
-  std::cout << internal::getGoodbyeString() << std::endl;
-}
-
-namespace internal {
-
-std::string getHelloString() {
-  return "Hello!";
-}
-
-std::string getGoodbyeString() {
-  return "See you soon!";
-}
-
-}  // namespace internal
-
-}  // namespace example
diff --git a/cmake/example_project/example/PrintExamples.h b/cmake/example_project/example/PrintExamples.h
deleted file mode 100644
index 25d4dd8cb..000000000
--- a/cmake/example_project/example/PrintExamples.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     print_examples.h
- * @brief    Example library file
- * @author   Richard Roberts
- */
-
-#pragma once
-
-#include <iostream>
-#include <string>
-
-namespace example {
-
-class PrintExamples {
-public:
-  /// Print a greeting
-  void sayHello() const;
-
-  /// Print a farewell
-  void sayGoodbye() const;
-};
-
-namespace internal {
-
-std::string getHelloString();
-
-std::string getGoodbyeString();
-
-}  // namespace internal
-
-}  // namespace example
diff --git a/cmake/example_project/tests/testExample.cpp b/cmake/example_project/tests/testExample.cpp
deleted file mode 100644
index c2a5a173b..000000000
--- a/cmake/example_project/tests/testExample.cpp
+++ /dev/null
@@ -1,43 +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     testExample.cpp
- * @brief    Unit tests for example
- * @author   Richard Roberts
- */
-
-#include <CppUnitLite/TestHarness.h>
-
-#include <gtsam/base/TestableAssertions.h>
-
-#include <example/PrintExamples.h>
-
-using namespace gtsam;
-
-TEST(Example, HelloString) {
-  const std::string expectedString = "Hello!";
-  EXPECT(assert_equal(expectedString, example::internal::getHelloString()));
-}
-
-TEST(Example, GoodbyeString) {
-  const std::string expectedString = "See you soon!";
-  EXPECT(assert_equal(expectedString, example::internal::getGoodbyeString()));
-}
-
-/* ************************************************************************* */
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-/* ************************************************************************* */
-
-
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 11ae113b9..71daf0653 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -204,18 +204,3 @@ else()
     set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-Wno-error")
   endif()
 endif()
-
-# Create the matlab toolbox for the gtsam library
-if (GTSAM_INSTALL_MATLAB_TOOLBOX)
-    # Set up codegen
-    include(GtsamMatlabWrap)
-
-    # Generate, build and install toolbox
-    set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
-        if(NOT BUILD_SHARED_LIBS)
-                list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
-        endif()
-
-    # Wrap
-    wrap_and_install_library(gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
-endif ()
diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 714910b7b..38653ea9c 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -5,120 +5,7 @@
  * These are the current classes available through the matlab and python wrappers,
  * add more functions/classes as they are available.
  *
- * IMPORTANT: the python wrapper supports keyword arguments for functions/methods. Hence, the
- *            argument names matter. An implementation restriction is that in overloaded methods
- *            or functions, arguments of different types *have* to have different names.
- *
- * Requirements:
- *   Classes must start with an uppercase letter
- *      - Can wrap a typedef
- *   Only one Method/Constructor per line, though methods/constructors can extend across multiple lines
- *   Methods can return
- *     - Eigen types:       Matrix, Vector
- *     - C/C++ basic types: string, bool, size_t, int, double, char, unsigned char
- *     - void
- *     - Any class with which be copied with boost::make_shared()
- *     - boost::shared_ptr of any object type
- *   Constructors
- *     - Overloads are supported, but arguments of different types *have* to have different names
- *     - A class with no constructors can be returned from other functions but not allocated directly in MATLAB
- *   Methods
- *     - Constness has no effect
- *     - Specify by-value (not reference) return types, even if C++ method returns reference
- *     - Must start with a letter (upper or lowercase)
- *     - Overloads are supported
- *   Static methods
- *     - Must start with a letter (upper or lowercase) and use the "static" keyword
- *     - The first letter will be made uppercase in the generated MATLAB interface
- *     - Overloads are supported, but arguments of different types *have* to have different names
- *   Arguments to functions any of
- *      - Eigen types:       Matrix, Vector
- *      - Eigen types and classes as an optionally const reference
- *     - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char
- *     - Any class with which be copied with boost::make_shared() (except Eigen)
- *     - boost::shared_ptr of any object type (except Eigen)
- *   Comments can use either C++ or C style, with multiple lines
- *   Namespace definitions
- *     - Names of namespaces must start with a lowercase letter
- *      - start a namespace with "namespace {"
- *      - end a namespace with exactly "}"
- *      - Namespaces can be nested
- *   Namespace usage
- *      - Namespaces can be specified for classes in arguments and return values
- *      - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName"
- *   Includes in C++ wrappers
- *     - All includes will be collected and added in a single file
- *     - All namespaces must have angle brackets: <path>
- *     - No default includes will be added
- *   Global/Namespace functions
- *     - Functions specified outside of a class are global
- *     - Can be overloaded with different arguments
- *     - Can have multiple functions of the same name in different namespaces
- *   Using classes defined in other modules
- *     - If you are using a class 'OtherClass' not wrapped in this definition file, add "class OtherClass;" to avoid a dependency error
- *   Virtual inheritance
- *     - Specify fully-qualified base classes, i.e. "virtual class Derived : ns::Base {" where "ns" is the namespace
- *     - Mark with 'virtual' keyword, e.g. "virtual class Base {", and also "virtual class Derived : ns::Base {"
- *     - Forward declarations must also be marked virtual, e.g. "virtual class ns::Base;" and
- *       also "virtual class ns::Derived;"
- *     - Pure virtual (abstract) classes should list no constructors in this interface file
- *     - Virtual classes must have a clone() function in C++ (though it does not have to be included
- *       in the MATLAB interface).  clone() will be called whenever an object copy is needed, instead
- *       of using the copy constructor (which is used for non-virtual objects).
- *     - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree
- *           virtual boost::shared_ptr<CLASS_NAME> clone() const;
- *   Class Templates
- *     - Basic templates are supported either with an explicit list of types to instantiate,
- *       e.g. template<T = {gtsam::Pose2, gtsam::Rot2, gtsam::Point3}> class Class1 { ... };
- *       or with typedefs, e.g.
- *       template<T, U> class Class2 { ... };
- *       typedef Class2<Type1, Type2> MyInstantiatedClass;
- *     - In the class definition, appearances of the template argument(s) will be replaced with their
- *       instantiated types, e.g. 'void setValue(const T& value);'.
- *     - To refer to the instantiation of the template class itself, use 'This', i.e. 'static This Create();'
- *     - To create new instantiations in other modules, you must copy-and-paste the whole class definition
- *       into the new module, but use only your new instantiation types.
- *     - When forward-declaring template instantiations, use the generated/typedefed name, e.g.
- *       class gtsam::Class1Pose2;
- *       class gtsam::MyInstantiatedClass;
- *   Boost.serialization within Matlab:
- *     - you need to mark classes as being serializable in the markup file (see this file for an example).
- *     - There are two options currently, depending on the class.  To "mark" a class as serializable,
- *       add a function with a particular signature so that wrap will catch it.
- *        - Add "void serialize()" to a class to create serialization functions for a class.
- *          Adding this flag subsumes the serializable() flag below. Requirements:
- *             - A default constructor must be publicly accessible
- *             - Must not be an abstract base class
- *             - The class must have an actual boost.serialization serialize() function.
- *        - Add "void serializable()" to a class if you only want the class to be serialized as a
- *          part of a container (such as noisemodel). This version does not require a publicly
- *          accessible default constructor.
- *   Forward declarations and class definitions for Pybind:
- *     - Need to specify the base class (both this forward class and base class are declared in an external Pybind header)
- *       This is so Pybind can generate proper inheritance.
- *       Example when wrapping a gtsam-based project:
- *          // forward declarations
- *          virtual class gtsam::NonlinearFactor
- *          virtual class gtsam::NoiseModelFactor : gtsam::NonlinearFactor
- *          // class definition
- *          #include <MyFactor.h>
- *          virtual class MyFactor : gtsam::NoiseModelFactor {...};
- *    - *DO NOT* re-define overriden function already declared in the external (forward-declared) base class
- *        - This will cause an ambiguity problem in Pybind header file
- *   Pickle support in Python:
- *    - Add "void pickle()" to a class to enable pickling via gtwrap. In the current implementation, "void serialize()"
- *      and a public constructor with no-arguments in needed for successful build.
- */
-
-/**
- * Status:
- *  - TODO: default values for arguments
- *    - WORKAROUND: make multiple versions of the same function for different configurations of default arguments
- *  - TODO: Handle gtsam::Rot3M conversions to quaternions
- *  - TODO: Parse return of const ref arguments
- *  - TODO: Parse std::string variants and convert directly to special string
- *  - TODO: Add enum support
- *  - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load
+ * Please refer to the wrapping docs: https://github.com/borglab/wrap/blob/master/README.md
  */
 
 namespace gtsam {
@@ -1910,6 +1797,8 @@ virtual class GaussianBayesNet {
   gtsam::KeySet keys() const;
   bool exists(size_t idx) const;
 
+  void saveGraph(const string& s) const;
+
   gtsam::GaussianConditional* front() const;
   gtsam::GaussianConditional* back() const;
   void push_back(gtsam::GaussianConditional* conditional);
@@ -2758,7 +2647,7 @@ virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
       const BEARING& measuredBearing, const RANGE& measuredRange,
       const gtsam::noiseModel::Base* noiseModel);
 
-  BearingRange<POSE, POINT, BEARING, RANGE> measured() const;
+  gtsam::BearingRange<POSE, POINT, BEARING, RANGE> measured() const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -2898,8 +2787,8 @@ class SfmTrack {
   double r;
   double g;
   double b;
-  // TODO Need to close wrap#10 to allow this to work.
-  // std::vector<pair<size_t, gtsam::Point2>> measurements;
+
+  std::vector<pair<size_t, gtsam::Point2>> measurements;
 
   size_t number_measurements() const;
   pair<size_t, gtsam::Point2> measurement(size_t idx) const;
diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp
index 04094d593..1e790d0f1 100644
--- a/gtsam/linear/GaussianBayesNet.cpp
+++ b/gtsam/linear/GaussianBayesNet.cpp
@@ -12,15 +12,16 @@
 /**
  * @file   GaussianBayesNet.cpp
  * @brief  Chordal Bayes Net, the result of eliminating a factor graph
- * @author Frank Dellaert
+ * @author Frank Dellaert, Varun Agrawal
  */
 
+#include <gtsam/base/timing.h>
+#include <gtsam/inference/FactorGraph-inst.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/inference/FactorGraph-inst.h>
-#include <gtsam/base/timing.h>
 
 #include <boost/range/adaptor/reversed.hpp>
+#include <fstream>
 
 using namespace std;
 using namespace gtsam;
@@ -204,5 +205,23 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
+  void GaussianBayesNet::saveGraph(const std::string& s,
+                                   const KeyFormatter& keyFormatter) const {
+    std::ofstream of(s.c_str());
+    of << "digraph G{\n";
+
+    for (auto conditional : boost::adaptors::reverse(*this)) {
+      typename GaussianConditional::Frontals frontals = conditional->frontals();
+      Key me = frontals.front();
+      typename GaussianConditional::Parents parents = conditional->parents();
+      for (Key p : parents)
+        of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl;
+    }
+
+    of << "}";
+    of.close();
+  }
+
+  /* ************************************************************************* */
 
 } // namespace gtsam
diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h
index 3f6d69e91..06782c3cf 100644
--- a/gtsam/linear/GaussianBayesNet.h
+++ b/gtsam/linear/GaussianBayesNet.h
@@ -177,6 +177,16 @@ namespace gtsam {
      */
     VectorValues backSubstituteTranspose(const VectorValues& gx) const;
 
+    /**
+     * @brief Save the GaussianBayesNet as an image. Requires `dot` to be
+     * installed.
+     *
+     * @param s The name of the figure.
+     * @param keyFormatter Formatter to use for styling keys in the graph.
+     */
+    void saveGraph(const std::string& s, const KeyFormatter& keyFormatter =
+                                             DefaultKeyFormatter) const;
+
     /// @}
 
   private:
diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt
index ec161baa8..13c061b9b 100644
--- a/gtsam_unstable/CMakeLists.txt
+++ b/gtsam_unstable/CMakeLists.txt
@@ -107,22 +107,6 @@ install(
 list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
 
-# Wrap version for gtsam_unstable
-if (GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
-    # Set up codegen
-    include(GtsamMatlabWrap)
-
-    # Generate, build and install toolbox
-    set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
-        if(NOT BUILD_SHARED_LIBS)
-                list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
-        endif()
-
-    # Wrap
-    wrap_and_install_library(gtsam_unstable.i "gtsam" "" "${mexFlags}")
-endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
-
-
 # Build examples
 add_subdirectory(examples)
 
diff --git a/matlab/+gtsam/plotBayesNet.m b/matlab/+gtsam/plotBayesNet.m
index dea9b04ad..446ffffac 100644
--- a/matlab/+gtsam/plotBayesNet.m
+++ b/matlab/+gtsam/plotBayesNet.m
@@ -5,4 +5,6 @@ function plotBayesNet(bayesNet)
 bayesNet.saveGraph('/tmp/bayesNet.dot')
 !dot -Tpng -o /tmp/dotImage.png /tmp/bayesNet.dot
 dotImage=imread('/tmp/dotImage.png');
-imshow(dotImage)
\ No newline at end of file
+imshow(dotImage)
+
+end
\ No newline at end of file
diff --git a/matlab/+gtsam/plotBayesTree.m b/matlab/+gtsam/plotBayesTree.m
index 94628e8a5..91d0925a9 100644
--- a/matlab/+gtsam/plotBayesTree.m
+++ b/matlab/+gtsam/plotBayesTree.m
@@ -5,4 +5,6 @@ function plotBayesTree(bayesTree)
 bayesTree.saveGraph('/tmp/bayesTree.dot')
 !dot -Tpng -o /tmp/dotImage.png /tmp/bayesTree.dot
 dotImage=imread('/tmp/dotImage.png');
-imshow(dotImage)
\ No newline at end of file
+imshow(dotImage)
+
+end
\ No newline at end of file
diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt
index 52d56a2b5..c8d6d197f 100644
--- a/matlab/CMakeLists.txt
+++ b/matlab/CMakeLists.txt
@@ -1,44 +1,132 @@
 # Install matlab components
 
-include(GtsamMatlabWrap)
+# Check if flag is enabled
+if(NOT GTSAM_INSTALL_MATLAB_TOOLBOX)
+  return()
+endif()
+
+# Create the matlab toolbox for the gtsam library
+
+# Set the wrapping script variable
+set(MATLAB_WRAP_SCRIPT "${GTSAM_SOURCE_DIR}/wrap/scripts/matlab_wrap.py")
+
+# Set up cache options
+option(GTSAM_MEX_BUILD_STATIC_MODULE
+       "Build MATLAB wrapper statically (increases build time)" OFF)
+set(GTSAM_BUILD_MEX_BINARY_FLAGS
+    ""
+    CACHE STRING "Extra flags for running Matlab MEX compilation")
+set(GTSAM_TOOLBOX_INSTALL_PATH
+    ""
+    CACHE
+      PATH
+      "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox"
+)
+if(NOT GTSAM_TOOLBOX_INSTALL_PATH)
+  set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox")
+endif()
+
+set(WRAP_MEX_BUILD_STATIC_MODULE ${GTSAM_MEX_BUILD_STATIC_MODULE})
+set(WRAP_BUILD_MEX_BINARY_FLAGS ${GTSAM_BUILD_MEX_BINARY_FLAGS})
+set(WRAP_TOOLBOX_INSTALL_PATH ${GTSAM_TOOLBOX_INSTALL_PATH})
+set(WRAP_CUSTOM_MATLAB_PATH ${GTSAM_CUSTOM_MATLAB_PATH})
+set(WRAP_BUILD_TYPE_POSTFIXES ${GTSAM_BUILD_TYPE_POSTFIXES})
+
+# Fixup the Python paths
+if(GTWRAP_DIR)
+  # packaged
+  set(GTWRAP_PACKAGE_DIR ${GTWRAP_DIR})
+else()
+  set(GTWRAP_PACKAGE_DIR ${GTSAM_SOURCE_DIR}/wrap)
+endif()
+
+include(MatlabWrap)
+
+if(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."
+  )
+endif()
+
+# ##############################################################################
+# Generate, build and install toolbox
+set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
+if(NOT BUILD_SHARED_LIBS)
+  list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
+endif()
+
+# ignoring the non-concrete types (type aliases)
+set(ignore
+    gtsam::Point2
+    gtsam::Point3)
+
+# Wrap
+matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}"
+            "" "${mexFlags}" "${ignore}")
+
+# Wrap version for gtsam_unstable
+if(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
+  # Generate, build and install toolbox
+  set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
+  if(NOT BUILD_SHARED_LIBS)
+    list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
+  endif()
+
+  # Wrap
+  matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i "gtsam" ""
+              "${mexFlags}")
+endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
 
 # Record the root dir for gtsam - needed during external builds, e.g., ROS
 set(GTSAM_SOURCE_ROOT_DIR ${GTSAM_SOURCE_DIR})
 message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]")
 
-# Tests
-#message(STATUS "Installing Matlab Toolbox")
+# Tests message(STATUS "Installing Matlab Toolbox")
 install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig")
-install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "README-gtsam-toolbox.txt")
+install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/"
+                       "README-gtsam-toolbox.txt")
 
-# Examples
-#message(STATUS "Installing Matlab Toolbox Examples")
-# Matlab files: *.m and *.fig
-#install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" "*.m;*.fig")
+# Examples message(STATUS "Installing Matlab Toolbox Examples") Matlab files:
+# *.m and *.fig
+# install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples"
+# "*.m;*.fig")
 
-# Utilities
-#message(STATUS "Installing Matlab Toolbox Utilities")
-#install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" "*.m")
+# Utilities message(STATUS "Installing Matlab Toolbox Utilities")
+# install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" "*.m")
 
-#message(STATUS "Installing Matlab Toolbox Example Data")
-# Data files: *.graph, *.mat, and *.txt
-file(GLOB matlab_examples_data_graph "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph")
-file(GLOB matlab_examples_data_mat "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.mat")
-file(GLOB matlab_examples_data_txt "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt")
-set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_mat} ${matlab_examples_data_txt})
+# message(STATUS "Installing Matlab Toolbox Example Data") Data files: *.graph,
+# *.mat, and *.txt
+file(GLOB matlab_examples_data_graph
+     "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph")
+file(GLOB matlab_examples_data_mat
+     "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.mat")
+file(GLOB matlab_examples_data_txt
+     "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt")
+set(matlab_examples_data
+    ${matlab_examples_data_graph} ${matlab_examples_data_mat}
+    ${matlab_examples_data_txt})
 if(GTSAM_BUILD_TYPE_POSTFIXES)
-	foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
-		string(TOUPPER "${build_type}" build_type_upper)
-		if(${build_type_upper} STREQUAL "RELEASE")
-			set(build_type_tag "") # Don't create release mode tag on installed directory
-		else()
-			set(build_type_tag "${build_type}")
-		endif()
-		# Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one
-		get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH)
-		get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME)
-		install(FILES ${matlab_examples_data} DESTINATION "${location}/${name}${build_type_tag}/gtsam_examples/Data" CONFIGURATIONS "${build_type}")
-	endforeach()
+  foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
+    string(TOUPPER "${build_type}" build_type_upper)
+    if(${build_type_upper} STREQUAL "RELEASE")
+      set(build_type_tag "") # Don't create release mode tag on installed
+      # directory
+    else()
+      set(build_type_tag "${build_type}")
+    endif()
+    # Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if
+    # there is one
+    get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH)
+    get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME)
+    install(
+      FILES ${matlab_examples_data}
+      DESTINATION "${location}/${name}${build_type_tag}/gtsam_examples/Data"
+      CONFIGURATIONS "${build_type}")
+  endforeach()
 else()
-	install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data)
+  install(FILES ${matlab_examples_data}
+          DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data)
 endif()
diff --git a/matlab/gtsam_tests/testThinBayesTree.m b/matlab/gtsam_tests/testThinBayesTree.m
index 034fc38de..747852146 100644
--- a/matlab/gtsam_tests/testThinBayesTree.m
+++ b/matlab/gtsam_tests/testThinBayesTree.m
@@ -20,4 +20,4 @@
 %% Run the tests
 import gtsam.*
 bayesTree = thinBayesTree(3,2);
-EQUALITY('7 = bayesTree.size', 7, bayesTree.size);
\ No newline at end of file
+EQUALITY('7 = bayesTree.size', 4, bayesTree.size);
\ No newline at end of file
diff --git a/matlab/gtsam_tests/testTriangulation.m b/matlab/gtsam_tests/testTriangulation.m
index 7116d3838..c623e7b2b 100644
--- a/matlab/gtsam_tests/testTriangulation.m
+++ b/matlab/gtsam_tests/testTriangulation.m
@@ -44,15 +44,15 @@ optimize = true;
 rank_tol = 1e-9;
 
 triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize);
-CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9));
+CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-9);
 
 %% 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
 measurements = Point2Vector;
-measurements.push_back(z1.retract([0.1;0.5]));
-measurements.push_back(z2.retract([-0.2;0.3]));
+measurements.push_back(z1 + [0.1;0.5]);
+measurements.push_back(z2 + [-0.2;0.3]);
 
 triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize);
-CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-2));
+CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-2);
 
 %% two Poses with Bundler Calibration
 bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480);
@@ -67,4 +67,4 @@ measurements.push_back(z1);
 measurements.push_back(z2);
 
 triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize);
-CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9));
+CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-9);
diff --git a/matlab/gtsam_tests/thinBayesTree.m b/matlab/gtsam_tests/thinBayesTree.m
index 6c2f496c6..61f89cead 100644
--- a/matlab/gtsam_tests/thinBayesTree.m
+++ b/matlab/gtsam_tests/thinBayesTree.m
@@ -1,5 +1,6 @@
 function bayesTree = thinBayesTree(depth, width)
     import gtsam.*
     bayesNet = thinTreeBayesNet(depth, width);
-    bayesTree = GaussianBayesTree(bayesNet);
+    fg = GaussianFactorGraph(bayesNet);
+    bayesTree = fg.eliminateMultifrontal();
 end
\ No newline at end of file
diff --git a/matlab/gtsam_tests/thinTreeBayesNet.m b/matlab/gtsam_tests/thinTreeBayesNet.m
index d993c3fd3..b91a83ba8 100644
--- a/matlab/gtsam_tests/thinTreeBayesNet.m
+++ b/matlab/gtsam_tests/thinTreeBayesNet.m
@@ -6,8 +6,9 @@ bayesNet = GaussianBayesNet;
 tree = thinTree(depth,width);
 
 % Add root to the Bayes net
-gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), 3*rand(1));
-bayesNet.push_front(gc);
+model = noiseModel.Isotropic.Sigma(1, 3*rand(1));
+gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), model);
+bayesNet.push_back(gc);
 
 n=tree.getNumberOfElements();
 for i=2:n
@@ -17,13 +18,15 @@ for i=2:n
   % Create and link the corresponding GaussianConditionals
   if tree.getW == 1 || di == 2
     % Creation of single-parent GaussianConditional
-    gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), 5*rand(1));
+    model = noiseModel.Isotropic.Sigma(1, 5*rand(1));
+    gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), model);
   elseif tree.getW == 2 || di == 3
     % GaussianConditionalj associated with the second parent
-    gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), n-parents(2), 5*rand(1), 5*rand(1));
+    model = noiseModel.Isotropic.Sigma(1, 5*rand(1));
+    gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), n-parents(2), 5*rand(1), model);
   end
   % Add conditional to the Bayes net
-  bayesNet.push_front(gc);
+  bayesNet.push_back(gc);
 end
 
 end
\ No newline at end of file
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 8d1c07d7b..fb4d89148 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -13,6 +13,8 @@ configure_file(${PROJECT_SOURCE_DIR}/python/setup.py.in
 file(COPY ${PROJECT_SOURCE_DIR}/python/MANIFEST.in
      DESTINATION ${GTSAM_PYTHON_BUILD_DIRECTORY})
 
+set(WRAP_BUILD_TYPE_POSTFIXES ${GTSAM_BUILD_TYPE_POSTFIXES})
+
 include(PybindWrap)
 
 ############################################################
diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl
index 634a81e90..60b3d1fd0 100644
--- a/python/gtsam/gtsam.tpl
+++ b/python/gtsam/gtsam.tpl
@@ -25,7 +25,7 @@
 {boost_class_export}
 
 // Holder type for pybind11
-{hoder_type}
+{holder_type}
 
 // Preamble for STL classes
 // TODO(fan): make this automatic
diff --git a/python/gtsam_unstable/gtsam_unstable.tpl b/python/gtsam_unstable/gtsam_unstable.tpl
index 1d9dfaa40..111e46d5e 100644
--- a/python/gtsam_unstable/gtsam_unstable.tpl
+++ b/python/gtsam_unstable/gtsam_unstable.tpl
@@ -22,7 +22,7 @@
 
 {boost_class_export}
 
-{hoder_type}
+{holder_type}
 
 #include "python/gtsam_unstable/preamble.h"
 
diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt
index 5ffb66ad0..163165d98 100644
--- a/wrap/CMakeLists.txt
+++ b/wrap/CMakeLists.txt
@@ -21,20 +21,27 @@ else()
   set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/cmake")
 endif()
 
-# Install scripts to the standard CMake script directory.
-install(FILES cmake/gtwrapConfig.cmake cmake/PybindWrap.cmake
-              cmake/GtwrapUtils.cmake
+# Install CMake scripts to the standard CMake script directory.
+install(FILES cmake/gtwrapConfig.cmake cmake/MatlabWrap.cmake
+              cmake/PybindWrap.cmake cmake/GtwrapUtils.cmake
         DESTINATION "${SCRIPT_INSTALL_DIR}/gtwrap")
 
+# Needed for the CMAKE_INSTALL_X variables used below.
 include(GNUInstallDirs)
-# Install the gtwrap python package as a directory so it can be found for
-# wrapping.
+
+# Install the gtwrap python package as a directory so it can be found  by CMake
+# for wrapping.
 install(DIRECTORY gtwrap DESTINATION "${CMAKE_INSTALL_DATADIR}/gtwrap")
 
 # Install wrapping scripts as binaries to `CMAKE_INSTALL_PREFIX/bin` so they can
-# be invoked for wrapping.
-install(PROGRAMS scripts/pybind_wrap.py scripts/matlab_wrap.py TYPE BIN)
+# be invoked for wrapping. We use DESTINATION (instead of TYPE) so we can
+# support older CMake versions.
+install(PROGRAMS scripts/pybind_wrap.py scripts/matlab_wrap.py
+        DESTINATION ${CMAKE_INSTALL_BINDIR})
 
-# Install pybind11 directory to `CMAKE_INSTALL_PREFIX/lib/pybind11` This will
-# allow the gtwrapConfig.cmake file to load it later.
+# Install pybind11 directory to `CMAKE_INSTALL_PREFIX/lib/gtwrap/pybind11` This
+# will allow the gtwrapConfig.cmake file to load it later.
 install(DIRECTORY pybind11 DESTINATION "${CMAKE_INSTALL_LIBDIR}/gtwrap")
+
+# Install the matlab.h file to `CMAKE_INSTALL_PREFIX/lib/gtwrap/matlab.h`.
+install(FILES matlab.h DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/gtwrap")
diff --git a/wrap/DOCS.md b/wrap/DOCS.md
new file mode 100644
index 000000000..3acb7df4f
--- /dev/null
+++ b/wrap/DOCS.md
@@ -0,0 +1,167 @@
+## Wrap Module Definition
+
+### Important
+
+The python wrapper supports keyword arguments for functions/methods. Hence, the argument names matter. An implementation restriction is that in overloaded methods or functions, arguments of different types *have* to have different names.
+
+### Requirements
+
+- Classes must start with an uppercase letter.
+    - The wrapper can wrap a typedef, e.g. `typedef TemplatedClass<Arg> EasyName;`.
+
+- Only one Method/Constructor per line, though methods/constructors can extend across multiple lines.
+
+- Methods can return
+    - Eigen types: `Matrix`, `Vector`.
+    - C/C++ basic types: `string`, `bool`, `size_t`, `int`, `double`, `char`, `unsigned char`.
+    - `void`
+    - Any class with which be copied with `boost::make_shared()`.
+    - `boost::shared_ptr` of any object type.
+
+- Constructors
+    - Overloads are supported, but arguments of different types *have* to have different names.
+    - A class with no constructors can be returned from other functions but not allocated directly in MATLAB.
+
+- Methods
+    - Constness has no effect.
+    - Specify by-value (not reference) return types, even if C++ method returns reference.
+    - Must start with a letter (upper or lowercase).
+    - Overloads are supported.
+
+- Static methods
+    - Must start with a letter (upper or lowercase) and use the "static" keyword, e.g. `static void func()`.
+    - The first letter will be made uppercase in the generated MATLAB interface.
+    - Overloads are supported, but arguments of different types *have* to have different names.
+
+- Arguments to functions can be any of
+    - Eigen types: `Matrix`, `Vector`.
+    - Eigen types and classes as an optionally const reference.
+    - C/C++ basic types: `string`, `bool`, `size_t`, `size_t`, `double`, `char`, `unsigned char`.
+    - Any class with which be copied with `boost::make_shared()` (except Eigen).
+    - `boost::shared_ptr` of any object type (except Eigen).
+
+- Properties or Variables
+    - You can specify class variables in the interface file as long as they are in the `public` scope, e.g.
+
+    ```cpp
+    class Sample {
+        double seed;
+    };
+    ```
+
+    - Class variables are read-write so they can be updated directly in Python.
+
+- Pointer types
+    - To declare a pointer type (including shared pointers), simply add an asterisk (i.e. `*`) to the class name.
+    - E.g. `gtsam::noiseModel::Base*` to define the wrapping for the `Base` noise model shared pointer.
+
+- Comments can use either C++ or C style, with multiple lines.
+
+- Namespace definitions
+    - Names of namespaces must start with a lowercase letter.
+    - Start a namespace with `namespace example_ns {`, where `example_ns` is the namespace name.
+    - End a namespace with exactly `}`
+    - Namespaces can be nested.
+
+- Namespace usage
+     - Namespaces can be specified for classes in arguments and return values.
+     - In each case, the namespace must be fully specified, e.g., `namespace1::namespace2::ClassName`.
+
+- Includes in C++ wrappers
+    - All includes will be collected and added in a single file.
+    - All namespaces must have angle brackets, e.g. `#include <path>`.
+    - No default includes will be added.
+
+- Global/Namespace functions
+    - Functions specified outside of a class are **global**.
+    - Can be overloaded with different arguments.
+    - Can have multiple functions of the same name in different namespaces.
+
+- Using classes defined in other modules
+    - If you are using a class `OtherClass` not wrapped in an interface file, add `class OtherClass;` as a forward declaration to avoid a dependency error.
+
+- Virtual inheritance
+    - Specify fully-qualified base classes, i.e. `virtual class Derived : ns::Base {` where `ns` is the namespace.
+    - Mark with `virtual` keyword, e.g. `virtual class Base {`, and also `virtual class Derived : ns::Base {`.
+    - Forward declarations must also be marked virtual, e.g. `virtual class ns::Base;` and
+      also `virtual class ns::Derived;`.
+    - Pure virtual (abstract) classes should list no constructors in the interface file.
+    - Virtual classes must have a `clone()` function in C++ (though it does not have to be included
+      in the interface file). `clone()` will be called whenever an object copy is needed, instead
+      of using the copy constructor (which is used for non-virtual objects).
+    - Signature of clone function - `clone()` will be called virtually, so must appear at least at the top of the inheritance tree
+
+        ```cpp
+        virtual boost::shared_ptr<CLASS_NAME> clone() const;
+        ```
+
+- Class Templates
+    - Basic templates are supported either with an explicit list of types to instantiate,
+      e.g.
+
+      ```cpp
+      template<T = {gtsam::Pose2, gtsam::Rot2, gtsam::Point3}> class Class1 { ... };
+      ```
+
+      or with typedefs, e.g.
+
+      ```cpp
+      template<T, U> class Class2 { ... };
+      typedef Class2<Type1, Type2> MyInstantiatedClass;
+      ```
+
+    - In the class definition, appearances of the template argument(s) will be replaced with their
+      instantiated types, e.g. `void setValue(const T& value);`.
+    - To refer to the instantiation of the template class itself, use `This`, i.e. `static This Create();`.
+    - To create new instantiations in other modules, you must copy-and-paste the whole class definition
+      into the new module, but use only your new instantiation types.
+    - When forward-declaring template instantiations, use the generated/typedefed name, e.g.
+
+      ```cpp
+      class gtsam::Class1Pose2;
+      class gtsam::MyInstantiatedClass;
+      ```
+
+- `Boost.serialization` within the wrapper:
+    - You need to mark classes as being serializable in the markup file (see `gtsam.i` for examples).
+    - There are two options currently, depending on the class.  To "mark" a class as serializable,
+      add a function with a particular signature so that `wrap` will catch it.
+       - Add `void serialize()` to a class to create serialization functions for a class.
+         Adding this flag subsumes the `serializable()` flag below.
+         
+         Requirements:
+            - A default constructor must be publicly accessible.
+            - Must not be an abstract base class.
+            - The class must have an actual boost.serialization `serialize()` function.
+
+       - Add `void serializable()` to a class if you only want the class to be serialized as a
+         part of a container (such as `noiseModel`). This version does not require a publicly
+         accessible default constructor.
+
+- Forward declarations and class definitions for **Pybind**:
+    - Need to specify the base class (both this forward class and base class are declared in an external Pybind header)
+      This is so that Pybind can generate proper inheritance.
+
+      Example when wrapping a gtsam-based project:
+
+        ```cpp
+         // forward declarations
+         virtual class gtsam::NonlinearFactor
+         virtual class gtsam::NoiseModelFactor : gtsam::NonlinearFactor
+         // class definition
+         #include <MyFactor.h>
+         virtual class MyFactor : gtsam::NoiseModelFactor {...};
+         ```
+
+   - **DO NOT** re-define an overriden function already declared in the external (forward-declared) base class
+       - This will cause an ambiguity problem in Pybind header file.
+
+
+### TODO
+- Default values for arguments.
+    - WORKAROUND: make multiple versions of the same function for different configurations of default arguments.
+- Handle `gtsam::Rot3M` conversions to quaternions.
+- Parse return of const ref arguments.
+- Parse `std::string` variants and convert directly to special string.
+- Add enum support.
+- Add generalized serialization support via `boost.serialization` with hooks to MATLAB save/load.
diff --git a/wrap/README.md b/wrap/README.md
index 5b5cbb902..2f5689db7 100644
--- a/wrap/README.md
+++ b/wrap/README.md
@@ -1,15 +1,16 @@
-
 # WRAP
 
 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.
 
-## Prerequisites: Pybind11 and pyparsing
+## 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.
 
-```
+```sh
 python3 -m pip install pyparsing
 ```
 
@@ -43,36 +44,68 @@ pybind_wrap(${PROJECT_NAME}_py # target
 
 For more information, please follow our [tutorial](https://github.com/borglab/gtsam-project-python).
 
-## GTSAM Python wrapper
+## Documentation
+
+Documentation for wrapping C++ code can be found [here](https://github.com/borglab/wrap/blob/master/DOCS.md).
+
+## 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. Just run python then import GTSAM and play around:
+
+      ```python
+      import gtsam
+      gtsam.__dir__()
+      ```
+
+   1. Run the unittests:
+      ```sh
+      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. 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.
+   1. Run the unittests:
+      ```sh
+      python setup.py test
+      ```
+   2. Install `gtsam` to your current Python environment.
+      ```sh
+      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.
+
+## Matlab Wrapper
+
+In the CMake, simply include the `MatlabWrap.cmake` file.
+
+```cmake
+include(MatlabWrap)
+```
+
+This cmake file defines functions for generating MATLAB wrappers.
+
+- `wrap_and_install_library(interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)` Generates wrap code and compiles the wrapper.
+
+Usage example:
+
+    `wrap_and_install_library("lba.h" "" "" "")`
+
+Arguments:
+
+- `interfaceHeader`: The relative or absolute path to the wrapper interface definition file.
+- `linkLibraries`: Any _additional_ libraries to link. Your project library
+  (e.g. `lba`), libraries it depends on, and any necessary
+  MATLAB libraries will be linked automatically. So normally,
+  leave this empty.
+- `extraIncludeDirs`: Any _additional_ include paths required by dependent
+  libraries that have not already been added by
+  include_directories. Again, normally, leave this empty.
+- `extraMexFlags`: Any _additional_ flags to pass to the compiler when building
+  the wrap code. Normally, leave this empty.
diff --git a/wrap/cmake/MatlabWrap.cmake b/wrap/cmake/MatlabWrap.cmake
new file mode 100644
index 000000000..75654238a
--- /dev/null
+++ b/wrap/cmake/MatlabWrap.cmake
@@ -0,0 +1,477 @@
+find_package(
+  Matlab
+  COMPONENTS MEX_COMPILER
+  REQUIRED)
+
+if(NOT Matlab_MEX_COMPILER)
+  message(
+    FATAL_ERROR
+      "Cannot find MEX compiler binary. Please check your Matlab installation and ensure MEX in installed as well."
+  )
+endif()
+
+if(WRAP_BUILD_TYPE_POSTFIXES)
+    set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
+endif()
+
+# WRAP_MEX_BUILD_STATIC_MODULE is not for Windows - on Windows any static
+# are already compiled into the library by the linker
+if(WRAP_MEX_BUILD_STATIC_MODULE AND WIN32)
+    message(FATAL_ERROR "WRAP_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries. To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).")
+endif()
+
+set(MEX_COMMAND ${Matlab_MEX_COMPILER} CACHE PATH "Path to MATLAB MEX compiler")
+set(MATLAB_ROOT ${Matlab_ROOT_DIR} CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
+
+# Try to automatically configure mex path from provided custom `bin` path.
+if(WRAP_CUSTOM_MATLAB_PATH)
+  set(matlab_bin_directory ${WRAP_CUSTOM_MATLAB_PATH})
+
+  if(WIN32)
+    set(mex_program_name "mex.bat")
+  else()
+    set(mex_program_name "mex")
+  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
+  # on the system path.
+  find_program(MEX_COMMAND ${mex_program_name}
+     PATHS ${matlab_bin_directory} ENV PATH
+     NO_DEFAULT_PATH)
+  mark_as_advanced(FORCE MEX_COMMAND)
+  # Now that we have mex, trace back to find the Matlab installation root
+  get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
+  get_filename_component(mex_path "${MEX_COMMAND}" PATH)
+  if(mex_path MATCHES ".*/win64$")
+     get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE)
+  else()
+     get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
+  endif()
+endif()
+
+# Consistent and user-friendly wrap function
+function(matlab_wrap interfaceHeader linkLibraries
+         extraIncludeDirs extraMexFlags ignore_classes)
+  wrap_and_install_library("${interfaceHeader}" "${linkLibraries}"
+                           "${extraIncludeDirs}" "${extraMexFlags}"
+                           "${ignore_classes}")
+endfunction()
+
+# Wrapping function.  Builds a mex module from the provided
+# interfaceHeader. For example, for the interface header gtsam.h, this will
+# build the wrap module 'gtsam'.
+#
+# Arguments:
+#
+# interfaceHeader:  The relative path to the wrapper interface definition file.
+# linkLibraries:    Any *additional* libraries to link.  Your project library
+# (e.g. `lba`), libraries it depends on, and any necessary MATLAB libraries will
+# be linked automatically.  So normally, leave this empty.
+# extraIncludeDirs: Any *additional* include paths required by dependent libraries that have not
+# already been added by include_directories.  Again, normally, leave this empty.
+# extraMexFlags:    Any *additional* flags to pass to the compiler when building
+# the wrap code.  Normally, leave this empty.
+# ignore_classes:  List of classes to ignore in the wrapping.
+function(wrap_and_install_library interfaceHeader linkLibraries
+         extraIncludeDirs extraMexFlags ignore_classes)
+  wrap_library_internal("${interfaceHeader}" "${linkLibraries}"
+                        "${extraIncludeDirs}" "${mexFlags}")
+  install_wrapped_library_internal("${interfaceHeader}")
+endfunction()
+
+# Internal function that wraps a library and compiles the wrapper
+function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
+         extraMexFlags)
+  if(UNIX AND NOT APPLE)
+    if(CMAKE_SIZEOF_VOID_P EQUAL 8)
+      set(mexModuleExt mexa64)
+    else()
+      set(mexModuleExt mexglx)
+    endif()
+  elseif(APPLE)
+    set(mexModuleExt mexmaci64)
+  elseif(MSVC)
+    if(CMAKE_CL_64)
+      set(mexModuleExt mexw64)
+    else()
+      set(mexModuleExt mexw32)
+    endif()
+  endif()
+
+  # Wrap codegen interface usage: wrap interfacePath moduleName toolboxPath
+  # headerPath interfacePath : *absolute* path to directory of module interface
+  # file moduleName    : the name of the module, interface file must be called
+  # moduleName.h toolboxPath : the directory in which to generate the wrappers
+  # headerPath    : path to matlab.h
+
+  # Extract module name from interface header file name
+  get_filename_component(interfaceHeader "${interfaceHeader}" ABSOLUTE)
+  get_filename_component(modulePath "${interfaceHeader}" PATH)
+  get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
+
+  # Paths for generated files
+  set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
+  set(generated_cpp_file "${generated_files_path}/${moduleName}_wrapper.cpp")
+  set(compiled_mex_modules_root "${PROJECT_BINARY_DIR}/wrap/${moduleName}_mex")
+
+  message(STATUS "Building wrap module ${moduleName}")
+
+  # Set matlab.h in project
+  set(matlab_h_path "${CMAKE_SOURCE_DIR}")
+
+  # If building a static mex module, add all cmake-linked libraries to the
+  # explicit link libraries list so that the next block of code can unpack any
+  # static libraries
+  set(automaticDependencies "")
+  foreach(lib ${moduleName} ${linkLibraries})
+    # message("MODULE NAME: ${moduleName}")
+    if(TARGET "${lib}")
+      get_target_property(dependentLibraries ${lib} INTERFACE_LINK_LIBRARIES)
+      # message("DEPENDENT LIBRARIES:  ${dependentLibraries}")
+      if(dependentLibraries)
+        list(APPEND automaticDependencies ${dependentLibraries})
+      endif()
+    endif()
+  endforeach()
+
+  # CHRIS: Temporary fix. On my system the get_target_property above returned
+  # Not-found for gtsam module This needs to be fixed!!
+  if(UNIX AND NOT APPLE)
+    list(
+      APPEND
+      automaticDependencies
+      ${Boost_SERIALIZATION_LIBRARY_RELEASE}
+      ${Boost_FILESYSTEM_LIBRARY_RELEASE}
+      ${Boost_SYSTEM_LIBRARY_RELEASE}
+      ${Boost_THREAD_LIBRARY_RELEASE}
+      ${Boost_DATE_TIME_LIBRARY_RELEASE})
+    # Only present in Boost >= 1.48.0
+    if(Boost_TIMER_LIBRARY_RELEASE)
+      list(APPEND automaticDependencies ${Boost_TIMER_LIBRARY_RELEASE}
+           ${Boost_CHRONO_LIBRARY_RELEASE})
+      if(WRAP_MEX_BUILD_STATIC_MODULE)
+        # list(APPEND automaticDependencies -Wl,--no-as-needed -lrt)
+      endif()
+    endif()
+  endif()
+
+  # message("AUTOMATIC DEPENDENCIES:  ${automaticDependencies}") CHRIS: End
+  # temporary fix
+
+  # Separate dependencies
+  set(correctedOtherLibraries "")
+  set(otherLibraryTargets "")
+  set(otherLibraryNontargets "")
+  set(otherSourcesAndObjects "")
+  foreach(lib ${moduleName} ${linkLibraries} ${automaticDependencies})
+    if(TARGET "${lib}")
+      if(WRAP_MEX_BUILD_STATIC_MODULE)
+        get_target_property(target_sources ${lib} SOURCES)
+        list(APPEND otherSourcesAndObjects ${target_sources})
+      else()
+        list(APPEND correctedOtherLibraries ${lib})
+        list(APPEND otherLibraryTargets ${lib})
+      endif()
+    else()
+      get_filename_component(file_extension "${lib}" EXT)
+      get_filename_component(lib_name "${lib}" NAME_WE)
+      if(file_extension STREQUAL ".a" AND WRAP_MEX_BUILD_STATIC_MODULE)
+        # For building a static MEX module, unpack the static library and
+        # compile its object files into our module
+        file(MAKE_DIRECTORY "${generated_files_path}/${lib_name}_objects")
+        execute_process(
+          COMMAND ar -x "${lib}"
+          WORKING_DIRECTORY "${generated_files_path}/${lib_name}_objects"
+          RESULT_VARIABLE ar_result)
+        if(NOT ar_result EQUAL 0)
+          message(FATAL_ERROR "Failed extracting ${lib}")
+        endif()
+
+        # Get list of object files
+        execute_process(
+          COMMAND ar -t "${lib}"
+          OUTPUT_VARIABLE object_files
+          RESULT_VARIABLE ar_result)
+        if(NOT ar_result EQUAL 0)
+          message(FATAL_ERROR "Failed listing ${lib}")
+        endif()
+
+        # Add directory to object files
+        string(REPLACE "\n" ";" object_files_list "${object_files}")
+        foreach(object_file ${object_files_list})
+          get_filename_component(file_extension "${object_file}" EXT)
+          if(file_extension STREQUAL ".o")
+            list(APPEND otherSourcesAndObjects
+                 "${generated_files_path}/${lib_name}_objects/${object_file}")
+          endif()
+        endforeach()
+      else()
+        list(APPEND correctedOtherLibraries ${lib})
+        list(APPEND otherLibraryNontargets ${lib})
+      endif()
+    endif()
+  endforeach()
+
+  # Check libraries for conflicting versions built-in to MATLAB
+  set(dependentLibraries "")
+  if(NOT "${otherLibraryTargets}" STREQUAL "")
+    foreach(target ${otherLibraryTargets})
+      get_target_property(dependentLibrariesOne ${target}
+                          INTERFACE_LINK_LIBRARIES)
+      list(APPEND dependentLibraries ${dependentLibrariesOne})
+    endforeach()
+  endif()
+  list(APPEND dependentLibraries ${otherLibraryNontargets})
+  check_conflicting_libraries_internal("${dependentLibraries}")
+
+  # Set up generation of module source file
+  file(MAKE_DIRECTORY "${generated_files_path}")
+
+  find_package(PythonInterp ${WRAP_PYTHON_VERSION} EXACT)
+  find_package(PythonLibs ${WRAP_PYTHON_VERSION} EXACT)
+
+  add_custom_command(
+    OUTPUT ${generated_cpp_file}
+    DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets}
+            ${otherSourcesAndObjects}
+    COMMAND
+      ${CMAKE_COMMAND} -E env
+      "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}"
+      ${PYTHON_EXECUTABLE} ${MATLAB_WRAP_SCRIPT} --src ${interfaceHeader}
+      --module_name ${moduleName} --out ${generated_files_path}
+      --top_module_namespaces ${moduleName} --ignore ${ignore_classes}
+    VERBATIM
+    WORKING_DIRECTORY ${generated_files_path})
+
+  # Set up building of mex module
+  string(REPLACE ";" " " extraMexFlagsSpaced "${extraMexFlags}")
+  string(REPLACE ";" " " mexFlagsSpaced "${WRAP_BUILD_MEX_BINARY_FLAGS}")
+  add_library(
+    ${moduleName}_matlab_wrapper MODULE
+    ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects})
+  target_link_libraries(${moduleName}_matlab_wrapper ${correctedOtherLibraries})
+  target_link_libraries(${moduleName}_matlab_wrapper ${moduleName})
+  set_target_properties(
+    ${moduleName}_matlab_wrapper
+    PROPERTIES OUTPUT_NAME "${moduleName}_wrapper"
+               PREFIX ""
+               SUFFIX ".${mexModuleExt}"
+               LIBRARY_OUTPUT_DIRECTORY "${compiled_mex_modules_root}"
+               ARCHIVE_OUTPUT_DIRECTORY "${compiled_mex_modules_root}"
+               RUNTIME_OUTPUT_DIRECTORY "${compiled_mex_modules_root}"
+               CLEAN_DIRECT_OUTPUT 1)
+  set_property(
+    TARGET ${moduleName}_matlab_wrapper
+    APPEND_STRING
+    PROPERTY
+      COMPILE_FLAGS
+      " ${extraMexFlagsSpaced} ${mexFlagsSpaced} \"-I${MATLAB_ROOT}/extern/include\" -DMATLAB_MEX_FILE -DMX_COMPAT_32"
+  )
+  set_property(
+    TARGET ${moduleName}_matlab_wrapper
+    APPEND
+    PROPERTY INCLUDE_DIRECTORIES ${extraIncludeDirs})
+  # Disable build type postfixes for the mex module - we install in different
+  # directories for each build type instead
+  foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
+    string(TOUPPER "${build_type}" build_type_upper)
+    set_target_properties(${moduleName}_matlab_wrapper
+                          PROPERTIES ${build_type_upper}_POSTFIX "")
+  endforeach()
+  # Set up platform-specific flags
+  if(MSVC)
+    if(CMAKE_CL_64)
+      set(mxLibPath "${MATLAB_ROOT}/extern/lib/win64/microsoft")
+    else()
+      set(mxLibPath "${MATLAB_ROOT}/extern/lib/win32/microsoft")
+    endif()
+    target_link_libraries(
+      ${moduleName}_matlab_wrapper "${mxLibPath}/libmex.lib"
+      "${mxLibPath}/libmx.lib" "${mxLibPath}/libmat.lib")
+    set_target_properties(${moduleName}_matlab_wrapper
+                          PROPERTIES LINK_FLAGS "/export:mexFunction")
+    set_property(
+      SOURCE "${generated_cpp_file}"
+      APPEND
+      PROPERTY COMPILE_FLAGS "/bigobj")
+  elseif(APPLE)
+    set(mxLibPath "${MATLAB_ROOT}/bin/maci64")
+    target_link_libraries(
+      ${moduleName}_matlab_wrapper "${mxLibPath}/libmex.dylib"
+      "${mxLibPath}/libmx.dylib" "${mxLibPath}/libmat.dylib")
+  endif()
+
+  # Hacking around output issue with custom command Deletes generated build
+  # folder
+  add_custom_target(
+    wrap_${moduleName}_matlab_distclean
+    COMMAND cmake -E remove_directory ${generated_files_path}
+    COMMAND cmake -E remove_directory ${compiled_mex_modules_root})
+endfunction()
+
+# Internal function that installs a wrap toolbox
+function(install_wrapped_library_internal interfaceHeader)
+  get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
+  set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
+
+  # NOTE: only installs .m and mex binary files (not .cpp) - the trailing slash
+  # on the directory name here prevents creating the top-level module name
+  # directory in the destination.
+  message(STATUS "Installing Matlab Toolbox to ${WRAP_TOOLBOX_INSTALL_PATH}")
+  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 "${build_type}")
+      endif()
+      # Split up filename to strip trailing '/' in WRAP_TOOLBOX_INSTALL_PATH if
+      # there is one
+      get_filename_component(location "${WRAP_TOOLBOX_INSTALL_PATH}" PATH)
+      get_filename_component(name "${WRAP_TOOLBOX_INSTALL_PATH}" NAME)
+      install(
+        DIRECTORY "${generated_files_path}/"
+        DESTINATION "${location}/${name}${build_type_tag}"
+        CONFIGURATIONS "${build_type}"
+        FILES_MATCHING
+        PATTERN "*.m")
+      install(
+        TARGETS ${moduleName}_matlab_wrapper
+        LIBRARY DESTINATION "${location}/${name}${build_type_tag}"
+                CONFIGURATIONS "${build_type}"
+        RUNTIME DESTINATION "${location}/${name}${build_type_tag}"
+                CONFIGURATIONS "${build_type}")
+    endforeach()
+  else()
+    install(
+      DIRECTORY "${generated_files_path}/"
+      DESTINATION ${WRAP_TOOLBOX_INSTALL_PATH}
+      FILES_MATCHING
+      PATTERN "*.m")
+    install(
+      TARGETS ${moduleName}_matlab_wrapper
+      LIBRARY DESTINATION ${WRAP_TOOLBOX_INSTALL_PATH}
+      RUNTIME DESTINATION ${WRAP_TOOLBOX_INSTALL_PATH})
+  endif()
+endfunction()
+
+# Internal function to check for libraries installed with MATLAB that may
+# conflict and prints a warning to move them if problems occur.
+function(check_conflicting_libraries_internal libraries)
+  if(UNIX)
+    # Set path for matlab's built-in libraries
+    if(APPLE)
+      set(mxLibPath "${MATLAB_ROOT}/bin/maci64")
+    else()
+      if(CMAKE_CL_64)
+        set(mxLibPath "${MATLAB_ROOT}/bin/glnxa64")
+      else()
+        set(mxLibPath "${MATLAB_ROOT}/bin/glnx86")
+      endif()
+    endif()
+
+    # List matlab's built-in libraries
+    file(
+      GLOB matlabLibs
+      RELATIVE "${mxLibPath}"
+      "${mxLibPath}/lib*")
+
+    # Convert to base names
+    set(matlabLibNames "")
+    foreach(lib ${matlabLibs})
+      get_filename_component(libName "${lib}" NAME_WE)
+      list(APPEND matlabLibNames "${libName}")
+    endforeach()
+
+    # Get names of link libraries
+    set(linkLibNames "")
+    foreach(lib ${libraries})
+      string(FIND "${lib}" "/" slashPos)
+      if(NOT slashPos EQUAL -1)
+        # If the name is a path, just get the library name
+        get_filename_component(libName "${lib}" NAME_WE)
+        list(APPEND linkLibNames "${libName}")
+      else()
+        # It's not a path, so see if it looks like a filename
+        get_filename_component(ext "${lib}" EXT)
+        if(NOT "${ext}" STREQUAL "")
+          # It's a filename, so get the base name
+          get_filename_component(libName "${lib}" NAME_WE)
+          list(APPEND linkLibNames "${libName}")
+        else()
+          # It's not a filename so it must be a short name, add the "lib" prefix
+          list(APPEND linkLibNames "lib${lib}")
+        endif()
+      endif()
+    endforeach()
+
+    # Remove duplicates
+    list(REMOVE_DUPLICATES linkLibNames)
+
+    set(conflictingLibs "")
+    foreach(lib ${linkLibNames})
+      list(FIND matlabLibNames "${lib}" libPos)
+      if(NOT libPos EQUAL -1)
+        if(NOT conflictingLibs STREQUAL "")
+          set(conflictingLibs "${conflictingLibs}, ")
+        endif()
+        set(conflictingLibs "${conflictingLibs}${lib}")
+      endif()
+    endforeach()
+
+    if(NOT "${conflictingLibs}" STREQUAL "")
+      message(
+        WARNING
+          "The project links to the libraries [ ${conflictingLibs} ] on your system, but "
+          "MATLAB is distributed with its own versions of these libraries which may conflict. "
+          "If you get strange errors or crashes with the MATLAB wrapper, move these "
+          "libraries out of MATLAB's built-in library directory, which is ${mxLibPath} on "
+          "your system. MATLAB will usually still work with these libraries moved away, but "
+          "if not, you'll have to compile the static MATLAB wrapper module."
+      )
+    endif()
+  endif()
+endfunction()
+
+# Helper function to install MATLAB scripts and handle multiple build types
+# where the scripts should be installed to all build type toolboxes
+function(install_matlab_scripts source_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 "${build_type}")
+      endif()
+      # Split up filename to strip trailing '/' in WRAP_TOOLBOX_INSTALL_PATH if
+      # there is one
+      get_filename_component(location "${WRAP_TOOLBOX_INSTALL_PATH}" PATH)
+      get_filename_component(name "${WRAP_TOOLBOX_INSTALL_PATH}" 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 "${WRAP_TOOLBOX_INSTALL_PATH}"
+      FILES_MATCHING ${patterns_args}
+      PATTERN "${exclude_patterns}" EXCLUDE)
+  endif()
+
+endfunction()
diff --git a/wrap/gtwrap/interface_parser.py b/wrap/gtwrap/interface_parser.py
index 73f9ebaa1..b4328327f 100644
--- a/wrap/gtwrap/interface_parser.py
+++ b/wrap/gtwrap/interface_parser.py
@@ -6,37 +6,43 @@ 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
+Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, 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,
-)
+# pylint: disable=unnecessary-lambda, unused-import, expression-not-assigned, no-else-return, protected-access, too-few-public-methods, too-many-arguments
+
+import sys
+import typing
+
+import pyparsing
+from pyparsing import (CharsNotIn, Forward, Group, Keyword, Literal, OneOrMore,
+                       Optional, Or, ParseException, ParserElement, Suppress,
+                       Word, ZeroOrMore, alphanums, alphas, cppStyleComment,
+                       delimitedList, empty, nums, stringEnd)
+
+# Fix deepcopy issue with pyparsing
+# Can remove once https://github.com/pyparsing/pyparsing/issues/208 is resolved.
+if sys.version_info >= (3, 8):
+    def fixed_get_attr(self, item):
+        """
+        Fix for monkey-patching issue with deepcopy in pyparsing.ParseResults
+        """
+        if item == '__deepcopy__':
+            raise AttributeError(item)
+        try:
+            return self[item]
+        except KeyError:
+            return ""
+
+    # apply the monkey-patch
+    pyparsing.ParseResults.__getattr__ = fixed_get_attr
+
 
 ParserElement.enablePackrat()
 
+# rule for identifiers (e.g. variable names)
 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, "<>,=")
@@ -70,9 +76,9 @@ BASIS_TYPES = map(
 )
 
 
-class Typename(object):
+class Typename:
     """
-    Type's name with full namespaces.
+    Type's name with full namespaces, used in Type class.
     """
 
     namespaces_name_rule = delimitedList(IDENT, "::")
@@ -86,34 +92,38 @@ class Typename(object):
         )
     ).setParseAction(lambda t: Typename(t.namespaces_name, t.instantiations))
 
-    def __init__(self, namespaces_name, instantiations=[]):
+    def __init__(self, namespaces_name, instantiations=()):
         self.namespaces = namespaces_name[:-1]
         self.name = namespaces_name[-1]
 
         if instantiations:
-            if not isinstance(instantiations, list):
+            if not isinstance(instantiations, typing.Iterable):
                 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 the typename from the parsed result."""
         return parse_result[0]
 
     def __repr__(self):
         return self.to_cpp()
 
     def instantiated_name(self):
+        """Get the instantiated name of the type."""
         res = self.name
         for instantiation in self.instantiations:
             res += instantiation.instantiated_name()
         return res
 
     def to_cpp(self):
+        """Generate the C++ code for wrapping."""
         idx = 1 if self.namespaces and not self.namespaces[0] else 0
         if self.instantiations:
             cpp_name = self.name + "<{}>".format(
@@ -140,8 +150,11 @@ class Typename(object):
         return not res
 
 
-class Type(object):
-    class _QualifiedType(object):
+class Type:
+    """
+    The type value that is parsed, e.g. void, string, size_t.
+    """
+    class _QualifiedType:
         """
         Type with qualifiers.
         """
@@ -165,7 +178,7 @@ class Type(object):
             self.is_ptr = is_ptr
             self.is_ref = is_ref
 
-    class _BasisType(object):
+    class _BasisType:
         """
         Basis types don't have qualifiers and only allow copy-by-value.
         """
@@ -185,6 +198,7 @@ class Type(object):
 
     @staticmethod
     def from_parse_result(t):
+        """Return the resulting Type from parsing the source."""
         if t.basis:
             return Type(
                 typename=t.basis,
@@ -211,6 +225,8 @@ class Type(object):
 
     def to_cpp(self, use_boost):
         """
+        Generate the C++ code for wrapping.
+
         Treat all pointers as "const shared_ptr<T>&"
         Treat Matrix and Vector as "const Matrix&" and "const Vector&" resp.
         """
@@ -237,7 +253,15 @@ class Type(object):
         )
 
 
-class Argument(object):
+class Argument:
+    """
+    The type and name of a function/method argument.
+
+    E.g.
+    ```
+    void sayHello(/*s is the method argument with type `const string&`*/ const string& s);
+    ```
+    """
     rule = (Type.rule("ctype") + IDENT("name")).setParseAction(
         lambda t: Argument(t.ctype, t.name)
     )
@@ -250,7 +274,10 @@ class Argument(object):
         return '{} {}'.format(self.ctype.__repr__(), self.name)
 
 
-class ArgumentList(object):
+class ArgumentList:
+    """
+    List of Argument objects for all arguments in a function.
+    """
     rule = Optional(delimitedList(Argument.rule)("args_list")).setParseAction(
         lambda t: ArgumentList.from_parse_result(t.args_list)
     )
@@ -262,6 +289,7 @@ class ArgumentList(object):
 
     @staticmethod
     def from_parse_result(parse_result):
+        """Return the result of parsing."""
         if parse_result:
             return ArgumentList(parse_result.asList())
         else:
@@ -271,13 +299,20 @@ class ArgumentList(object):
         return self.args_list.__repr__()
 
     def args_names(self):
+        """Return a list of the names of all the arguments."""
         return [arg.name for arg in self.args_list]
 
     def to_cpp(self, use_boost):
+        """Generate the C++ code for wrapping."""
         return [arg.ctype.to_cpp(use_boost) for arg in self.args_list]
 
 
-class ReturnType(object):
+class ReturnType:
+    """
+    Rule to parse the return type.
+
+    The return type can either be a single type or a pair such as <type1, type2>.
+    """
     _pair = (
         PAIR.suppress()
         + LOPBRACK
@@ -295,6 +330,9 @@ class ReturnType(object):
         self.type2 = type2
 
     def is_void(self):
+        """
+        Check if the return type is void.
+        """
         return self.type1.typename.name == "void" and not self.type2
 
     def __repr__(self):
@@ -303,6 +341,7 @@ class ReturnType(object):
         )
 
     def to_cpp(self):
+        """Generate the C++ code for wrapping."""
         if self.type2:
             return "std::pair<{type1},{type2}>".format(
                 type1=self.type1.to_cpp(), type2=self.type2.to_cpp()
@@ -311,8 +350,20 @@ class ReturnType(object):
             return self.type1.to_cpp()
 
 
-class Template(object):
-    class TypenameAndInstantiations(object):
+class Template:
+    """
+    Rule to parse templated values in the interface file.
+
+    E.g.
+    template<POSE>  // this is the Template.
+    class Camera { ... };
+    """
+    class TypenameAndInstantiations:
+        """
+        Rule to parse the template parameters.
+
+        template<typename POSE>  // POSE is the Instantiation.
+        """
         rule = (
             IDENT("typename")
             + Optional(
@@ -351,8 +402,21 @@ class Template(object):
         self.typenames = [ti.typename for ti in ti_list]
         self.instantiations = [ti.instantiations for ti in ti_list]
 
+    def __repr__(self):
+        return "<{0}>".format(", ".join(self.typenames))
 
-class Method(object):
+
+class Method:
+    """
+    Rule to parse a method in a class.
+
+    E.g.
+    ```
+    class Hello {
+        void sayHello() const;
+    };
+    ```
+    """
     rule = (
         Optional(Template.rule("template"))
         + ReturnType.rule("return_type")
@@ -387,7 +451,17 @@ class Method(object):
         )
 
 
-class StaticMethod(object):
+class StaticMethod:
+    """
+    Rule to parse all the static methods in a class.
+
+    E.g.
+    ```
+    class Hello {
+        static void changeGreeting();
+    };
+    ```
+    """
     rule = (
         STATIC
         + ReturnType.rule("return_type")
@@ -411,10 +485,15 @@ class StaticMethod(object):
         return "static {} {}{}".format(self.return_type, self.name, self.args)
 
     def to_cpp(self):
+        """Generate the C++ code for wrapping."""
         return self.name
 
 
-class Constructor(object):
+class Constructor:
+    """
+    Rule to parse the class constructor.
+    Can have 0 or more arguments.
+    """
     rule = (
         IDENT("name")
         + LPAREN
@@ -433,7 +512,17 @@ class Constructor(object):
         return "Constructor: {}".format(self.name)
 
 
-class Property(object):
+class Property:
+    """
+    Rule to parse the variable members of a class.
+
+    E.g.
+    ```
+    class Hello {
+        string name;  // This is a property.
+    };
+    ````
+    """
     rule = (Type.rule("ctype") + IDENT("name") + SEMI_COLON).setParseAction(
         lambda t: Property(t.ctype, t.name)
     )
@@ -441,10 +530,6 @@ class Property(object):
     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):
@@ -452,6 +537,7 @@ class Property(object):
 
 
 def collect_namespaces(obj):
+    """Get the chain of namespaces from the lowest to highest for the given object."""
     namespaces = []
     ancestor = obj.parent
     while ancestor and ancestor.name:
@@ -460,8 +546,21 @@ def collect_namespaces(obj):
     return [''] + namespaces
 
 
-class Class(object):
-    class MethodsAndProperties(object):
+class Class:
+    """
+    Rule to parse a class defined in the interface file.
+
+    E.g.
+    ```
+    class Hello {
+        ...
+    };
+    ```
+    """
+    class MethodsAndProperties:
+        """
+        Rule for all the methods and properties within a class.
+        """
         rule = ZeroOrMore(
             Constructor.rule ^ StaticMethod.rule ^ Method.rule ^ Property.rule
         ).setParseAction(lambda t: Class.MethodsAndProperties(t.asList()))
@@ -549,10 +648,19 @@ class Class(object):
             _property.parent = self
 
     def namespaces(self):
+        """Get the namespaces which this class is nested under as a list."""
         return collect_namespaces(self)
 
 
-class TypedefTemplateInstantiation(object):
+class TypedefTemplateInstantiation:
+    """
+    Rule for parsing typedefs (with templates) within the interface file.
+
+    E.g.
+    ```
+    typedef SuperComplexName<Arg1, Arg2, Arg3> EasierName;
+    ```
+    """
     rule = (
         TYPEDEF + Typename.rule("typename") + IDENT("new_name") + SEMI_COLON
     ).setParseAction(
@@ -567,7 +675,10 @@ class TypedefTemplateInstantiation(object):
         self.parent = parent
 
 
-class Include(object):
+class Include:
+    """
+    Rule to parse #include directives.
+    """
     rule = (
         INCLUDE + LOPBRACK + CharsNotIn('>')("header") + ROPBRACK
     ).setParseAction(lambda t: Include(t.header))
@@ -580,7 +691,10 @@ class Include(object):
         return "#include <{}>".format(self.header)
 
 
-class ForwardDeclaration(object):
+class ForwardDeclaration:
+    """
+    Rule to parse forward declarations in the interface file.
+    """
     rule = (
         Optional(VIRTUAL("is_virtual"))
         + CLASS
@@ -606,7 +720,10 @@ class ForwardDeclaration(object):
         )
 
 
-class GlobalFunction(object):
+class GlobalFunction:
+    """
+    Rule to parse functions defined in the global scope.
+    """
     rule = (
         ReturnType.rule("return_type")
         + IDENT("name")
@@ -634,10 +751,18 @@ class GlobalFunction(object):
         )
 
     def to_cpp(self):
+        """Generate the C++ code for wrapping."""
         return self.name
 
 
 def find_sub_namespace(namespace, str_namespaces):
+    """
+    Get the namespaces nested under `namespace`, filtered by a list of namespace strings.
+
+    Args:
+        namespace: The top-level namespace under which to find sub-namespaces.
+        str_namespaces: The list of namespace strings to filter against.
+    """
     if not str_namespaces:
         return [namespace]
 
@@ -659,7 +784,8 @@ def find_sub_namespace(namespace, str_namespaces):
     return res
 
 
-class Namespace(object):
+class Namespace:
+    """Rule for parsing a namespace in the interface file."""
     rule = Forward()
     rule << (
         NAMESPACE
@@ -687,6 +813,7 @@ class Namespace(object):
 
     @staticmethod
     def from_parse_result(t):
+        """Return the result of parsing."""
         if t.content:
             content = t.content.asList()
         else:
@@ -717,6 +844,7 @@ class Namespace(object):
             return res[0]
 
     def top_level(self):
+        """Return the top leve namespace."""
         if self.name == '' or self.parent == '':
             return self
         else:
@@ -726,15 +854,23 @@ class Namespace(object):
         return "Namespace: {}\n\t{}".format(self.name, self.content)
 
     def full_namespaces(self):
+        """Get the full namespace list."""
         ancestors = collect_namespaces(self)
         if self.name:
             ancestors.append(self.name)
         return ancestors
 
 
-class Module(object):
+class Module:
     """
     Module is just a global namespace.
+
+    E.g.
+    ```
+    namespace gtsam {
+        ...
+    }
+    ```
     """
 
     rule = (
@@ -752,5 +888,6 @@ class Module(object):
     rule.ignore(cppStyleComment)
 
     @staticmethod
-    def parseString(str):
-        return Module.rule.parseString(str)[0]
+    def parseString(s: str):
+        """Parse the source string and apply the rules."""
+        return Module.rule.parseString(s)[0]
diff --git a/wrap/gtwrap/matlab_wrapper.py b/wrap/gtwrap/matlab_wrapper.py
index fe4ee7e19..aeaf221bd 100755
--- a/wrap/gtwrap/matlab_wrapper.py
+++ b/wrap/gtwrap/matlab_wrapper.py
@@ -591,7 +591,7 @@ class MatlabWrapper(object):
         file_name = self._wrapper_name() + '.cpp'
 
         wrapper_file = textwrap.dedent('''\
-            # include <wrap/matlab.h>
+            # include <gtwrap/matlab.h>
             # include <map>
         ''')
 
@@ -1476,7 +1476,7 @@ class MatlabWrapper(object):
         """Generate the c++ wrapper."""
         # Includes
         wrapper_file = textwrap.dedent('''\
-            #include <wrap/matlab.h>
+            #include <gtwrap/matlab.h>
             #include <map>\n
             #include <boost/archive/text_iarchive.hpp>
             #include <boost/archive/text_oarchive.hpp>
@@ -1485,7 +1485,16 @@ class MatlabWrapper(object):
 
         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'
+        # Check the number of includes.
+        # If no includes, do nothing, if 1 then just append newline.
+        # if more than one, concatenate them with newlines.
+        if len(includes_list) == 0:
+            pass
+        elif len(includes_list) == 1:
+            wrapper_file += (str(includes_list[0]) + '\n')
+        else:
+            wrapper_file += reduce(lambda x, y: str(x) + '\n' + str(y), includes_list)
+        wrapper_file += '\n'
 
         typedef_instances = '\n'
         typedef_collectors = ''
diff --git a/wrap/gtwrap/pybind_wrapper.py b/wrap/gtwrap/pybind_wrapper.py
index a045afcbd..986cdd8b1 100755
--- a/wrap/gtwrap/pybind_wrapper.py
+++ b/wrap/gtwrap/pybind_wrapper.py
@@ -7,8 +7,11 @@ 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
+Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert
 """
+
+# pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument
+
 import re
 import textwrap
 
@@ -16,13 +19,16 @@ import gtwrap.interface_parser as parser
 import gtwrap.template_instantiator as instantiator
 
 
-class PybindWrapper(object):
+class PybindWrapper:
+    """
+    Class to generate binding code for Pybind11 specifically.
+    """
     def __init__(self,
                  module,
                  module_name,
                  top_module_namespaces='',
                  use_boost=False,
-                 ignore_classes=[],
+                 ignore_classes=(),
                  module_template=""):
         self.module = module
         self.module_name = module_name
@@ -34,6 +40,7 @@ class PybindWrapper(object):
         self.python_keywords = ['print', 'lambda']
 
     def _py_args_names(self, args_list):
+        """Set the argument names in Pybind11 format."""
         names = args_list.args_names()
         if names:
             py_args = ['py::arg("{}")'.format(name) for name in names]
@@ -42,6 +49,7 @@ class PybindWrapper(object):
             return ''
 
     def _method_args_signature_with_names(self, args_list):
+        """Define the method signature types with the argument names."""
         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)]
@@ -49,6 +57,7 @@ class PybindWrapper(object):
         return ','.join(types_names)
 
     def wrap_ctors(self, my_class):
+        """Wrap the constructors."""
         res = ""
         for ctor in my_class.ctors:
             res += ('\n' + ' ' * 8 + '.def(py::init<{args_cpp_types}>()'
@@ -115,8 +124,10 @@ class PybindWrapper(object):
                '{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 "",
+                   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 '',
@@ -152,6 +163,7 @@ class PybindWrapper(object):
         return ret
 
     def wrap_methods(self, methods, cpp_class, prefix='\n' + ' ' * 8, suffix=''):
+        """Wrap all the methods in the `cpp_class`."""
         res = ""
         for method in methods:
 
@@ -176,6 +188,7 @@ class PybindWrapper(object):
         return res
 
     def wrap_properties(self, properties, cpp_class, prefix='\n' + ' ' * 8):
+        """Wrap all the properties in the `cpp_class`."""
         res = ""
         for prop in properties:
             res += ('{prefix}.def_{property}("{property_name}", '
@@ -188,50 +201,61 @@ class PybindWrapper(object):
         return res
 
     def wrap_instantiated_class(self, instantiated_class):
+        """Wrap the 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),
-                ))
+        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):
+        """Wrap STL containers."""
         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),
-                ))
+        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))):
@@ -252,6 +276,7 @@ class PybindWrapper(object):
             return name
 
     def wrap_namespace(self, namespace):
+        """Wrap the complete `namespace`."""
         wrapped = ""
         includes = ""
 
@@ -298,7 +323,10 @@ class PybindWrapper(object):
                     wrapped += self.wrap_instantiated_class(element)
 
             # Global functions.
-            all_funcs = [func for func in namespace.content if isinstance(func, parser.GlobalFunction)]
+            all_funcs = [
+                func for func in namespace.content
+                if isinstance(func, parser.GlobalFunction)
+            ]
             wrapped += self.wrap_methods(
                 all_funcs,
                 self._add_namespaces('', namespaces)[:-2],
@@ -308,6 +336,7 @@ class PybindWrapper(object):
         return wrapped, includes
 
     def wrap(self):
+        """Wrap the code in the interface file."""
         wrapped_namespace, includes = self.wrap_namespace(self.module)
 
         # Export classes for serialization.
@@ -323,14 +352,16 @@ class PybindWrapper(object):
                 )
             boost_class_export += "BOOST_CLASS_EXPORT({new_name})\n".format(new_name=new_name, )
 
+        holder_type = "PYBIND11_DECLARE_HOLDER_TYPE(TYPE_PLACEHOLDER_DONOTUSE, " \
+                      "{shared_ptr_type}::shared_ptr<TYPE_PLACEHOLDER_DONOTUSE>);"
+        include_boost = "#include <boost/shared_ptr.hpp>" if self.use_boost else ""
+
         return self.module_template.format(
-            include_boost="#include <boost/shared_ptr.hpp>" if self.use_boost else "",
+            include_boost=include_boost,
             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 "",
+            holder_type=holder_type.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,
         )
-
diff --git a/wrap/gtwrap/template_instantiator.py b/wrap/gtwrap/template_instantiator.py
index 6032beac4..fe2caf025 100644
--- a/wrap/gtwrap/template_instantiator.py
+++ b/wrap/gtwrap/template_instantiator.py
@@ -1,15 +1,45 @@
+"""Code to help instantiate templated classes, methods and functions."""
+
+# pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument, unused-variable
+
+import itertools
+from copy import deepcopy
+from typing import List
+
 import gtwrap.interface_parser as parser
 
 
-def instantiate_type(ctype, template_typenames, instantiations, cpp_typename, instantiated_class=None):
+def instantiate_type(ctype: parser.Type,
+                     template_typenames: List[str],
+                     instantiations: List[parser.Typename],
+                     cpp_typename: parser.Typename,
+                     instantiated_class=None):
     """
     Instantiate template typename for @p ctype.
+
+    Args:
+        instiated_class (InstantiatedClass):
+
     @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.
     """
+    # make a deep copy so that there is no overwriting of original template params
+    ctype = deepcopy(ctype)
+
+    # Check if the return type has template parameters
+    if len(ctype.typename.instantiations) > 0:
+        for idx, instantiation in enumerate(ctype.typename.instantiations):
+            if instantiation.name in template_typenames:
+                template_idx = template_typenames.index(instantiation.name)
+                ctype.typename.instantiations[idx] = instantiations[
+                    template_idx]
+
+        return ctype
+
     str_arg_typename = str(ctype.typename)
+
     if str_arg_typename in template_typenames:
         idx = template_typenames.index(str_arg_typename)
         return parser.Type(
@@ -20,7 +50,6 @@ def instantiate_type(ctype, template_typenames, instantiations, cpp_typename, in
             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()
@@ -29,8 +58,8 @@ def instantiate_type(ctype, template_typenames, instantiations, cpp_typename, in
             #     ctype, instantiations, cpp_typename, instantiated_class.instantiations
             # ), file=sys.stderr)
             cpp_typename = parser.Typename(
-                namespaces_name, instantiations=[inst for inst in instantiated_class.instantiations]
-            )
+                namespaces_name,
+                instantiations=instantiated_class.instantiations)
         return parser.Type(
             typename=cpp_typename,
             is_const=ctype.is_const,
@@ -70,12 +99,18 @@ def instantiate_args_list(args_list, template_typenames, instantiations,
 
 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)
+    """Instantiate the return type."""
+    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)
+        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)
@@ -89,9 +124,14 @@ def instantiate_name(original_name, instantiations):
     namespaces, but I find that too verbose.
     """
     inst_name = ''
+    instantiated_names = []
+    for inst in instantiations:
+        # Ensure the first character of the type is capitalized
+        name = inst.instantiated_name()
+        # Using `capitalize` on the complete causes other caps to be lower case
+        instantiated_names.append(name.replace(name[0], name[0].capitalize()))
 
-    return "{}{}".format(original_name, "".join(
-        [inst.instantiated_name() for inst in instantiations]))
+    return "{}{}".format(original_name, "".join(instantiated_names))
 
 
 class InstantiatedMethod(parser.Method):
@@ -111,6 +151,7 @@ class InstantiatedMethod(parser.Method):
             self.return_type = original.return_type
             self.args = original.args
         else:
+            #TODO(Varun) enable multiple templates for methods
             if len(self.original.template.typenames) > 1:
                 raise ValueError("Can only instantiate template method with "
                                  "single template parameter.")
@@ -133,11 +174,20 @@ class InstantiatedMethod(parser.Method):
             )
             self.args = parser.ArgumentList(instantiated_args)
 
+        super().__init__(self.template,
+                         self.name,
+                         self.return_type,
+                         self.args,
+                         self.is_const,
+                         parent=self.parent)
+
     def to_cpp(self):
+        """Generate the C++ code for wrapping."""
         if self.original.template:
-            return "{}<{}>".format(self.original.name, self.instantiation)
+            ret = "{}<{}>".format(self.original.name, self.instantiation)
         else:
-            return self.original.name
+            ret = self.original.name
+        return ret
 
     def __repr__(self):
         return "Instantiated {}".format(
@@ -146,7 +196,10 @@ class InstantiatedMethod(parser.Method):
 
 
 class InstantiatedClass(parser.Class):
-    def __init__(self, original, instantiations=[], new_name=''):
+    """
+    Instantiate the class defined in the interface file.
+    """
+    def __init__(self, original, instantiations=(), new_name=''):
         """
         Template <T, U>
         Instantiations: [T1, U1]
@@ -190,6 +243,18 @@ class InstantiatedClass(parser.Class):
                 for inst in method.template.instantiations[0]:
                     self.methods.append(InstantiatedMethod(method, inst))
 
+        super().__init__(
+            self.template,
+            self.is_virtual,
+            self.name,
+            [self.parent_class],
+            self.ctors,
+            self.methods,
+            self.static_methods,
+            self.properties,
+            parent=self.parent,
+        )
+
     def __repr__(self):
         return "{virtual} class {name} [{cpp_class}]: {parent_class}\n"\
             "{ctors}\n{static_methods}\n{methods}".format(
@@ -204,6 +269,7 @@ class InstantiatedClass(parser.Class):
             )
 
     def instantiate_ctors(self):
+        """Instantiate the class constructors."""
         instantiated_ctors = []
         for ctor in self.original.ctors:
             instantiated_args = instantiate_args_list(
@@ -220,6 +286,7 @@ class InstantiatedClass(parser.Class):
         return instantiated_ctors
 
     def instantiate_static_methods(self):
+        """Instantiate static methods in the class."""
         instantiated_static_methods = []
         for static_method in self.original.static_methods:
             instantiated_args = instantiate_args_list(
@@ -274,6 +341,7 @@ class InstantiatedClass(parser.Class):
         return class_instantiated_methods
 
     def instantiate_properties(self):
+        """Instantiate the class properties."""
         instantiated_properties = instantiate_args_list(
             self.original.properties,
             self.original.template.typenames,
@@ -283,6 +351,7 @@ class InstantiatedClass(parser.Class):
         return instantiated_properties
 
     def cpp_class(self):
+        """Generate the C++ code for wrapping."""
         return self.cpp_typename().to_cpp()
 
     def cpp_typename(self):
@@ -303,7 +372,10 @@ class InstantiatedClass(parser.Class):
 
 def instantiate_namespace_inplace(namespace):
     """
-    @param[in/out] namespace The namespace which content will be replaced with
+    Instantiate the classes and other elements in the `namespace` content and
+    assign it back to the namespace content attribute.
+
+    @param[in/out] namespace The namespace whose content will be replaced with
         the instantiated content.
     """
     instantiated_content = []
@@ -316,15 +388,14 @@ def instantiate_namespace_inplace(namespace):
                 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]:
+                # Use itertools to get all possible combinations of instantiations
+                # Works even if one template does not have an instantiation list
+                for instantiations in itertools.product(
+                        *original_class.template.instantiations):
                     instantiated_content.append(
-                        InstantiatedClass(original_class, [inst]))
+                        InstantiatedClass(original_class,
+                                          list(instantiations)))
+
         elif isinstance(element, parser.TypedefTemplateInstantiation):
             typedef_inst = element
             original_class = namespace.top_level().find_class(
diff --git a/wrap/pybind_wrapper.tpl.example b/wrap/pybind_wrapper.tpl.example
index 1bdd55140..2260e5406 100644
--- a/wrap/pybind_wrapper.tpl.example
+++ b/wrap/pybind_wrapper.tpl.example
@@ -11,7 +11,7 @@
 
 {boost_class_export}
 
-{hoder_type}
+{holder_type}
 
 #include "python/preamble.h"
 
diff --git a/wrap/scripts/pybind_wrap.py b/wrap/scripts/pybind_wrap.py
index e641cfaaf..26e63d51c 100644
--- a/wrap/scripts/pybind_wrap.py
+++ b/wrap/scripts/pybind_wrap.py
@@ -6,6 +6,8 @@ This script is installed via CMake to the user's binary directory
 and invoked during the wrapping by CMake.
 """
 
+# pylint: disable=import-error
+
 import argparse
 
 import gtwrap.interface_parser as parser
@@ -68,13 +70,16 @@ def main():
     if top_module_namespaces[0]:
         top_module_namespaces = [''] + top_module_namespaces
 
+    # Read in the complete interface (.i) file
     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,
@@ -84,7 +89,10 @@ def main():
         module_template=template_content,
     )
 
+    # Wrap the code and get back the cpp/cc code.
     cc_content = wrapper.wrap()
+
+    # Generate the C++ code which Pybind11 will use.
     with open(args.out, "w") as f:
         f.write(cc_content)
 
diff --git a/wrap/tests/expected-matlab/MultipleTemplatesIntDouble.m b/wrap/tests/expected-matlab/MultipleTemplatesIntDouble.m
new file mode 100644
index 000000000..d5a27b199
--- /dev/null
+++ b/wrap/tests/expected-matlab/MultipleTemplatesIntDouble.m
@@ -0,0 +1,31 @@
+%class MultipleTemplatesIntDouble, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+classdef MultipleTemplatesIntDouble < handle
+  properties
+    ptr_MultipleTemplatesIntDouble = 0
+  end
+  methods
+    function obj = MultipleTemplatesIntDouble(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        geometry_wrapper(89, my_ptr);
+      else
+        error('Arguments do not match any overload of MultipleTemplatesIntDouble constructor');
+      end
+      obj.ptr_MultipleTemplatesIntDouble = my_ptr;
+    end
+
+    function delete(obj)
+      geometry_wrapper(90, obj.ptr_MultipleTemplatesIntDouble);
+    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-matlab/MultipleTemplatesIntFloat.m b/wrap/tests/expected-matlab/MultipleTemplatesIntFloat.m
new file mode 100644
index 000000000..0434c6c79
--- /dev/null
+++ b/wrap/tests/expected-matlab/MultipleTemplatesIntFloat.m
@@ -0,0 +1,31 @@
+%class MultipleTemplatesIntFloat, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+classdef MultipleTemplatesIntFloat < handle
+  properties
+    ptr_MultipleTemplatesIntFloat = 0
+  end
+  methods
+    function obj = MultipleTemplatesIntFloat(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        geometry_wrapper(91, my_ptr);
+      else
+        error('Arguments do not match any overload of MultipleTemplatesIntFloat constructor');
+      end
+      obj.ptr_MultipleTemplatesIntFloat = my_ptr;
+    end
+
+    function delete(obj)
+      geometry_wrapper(92, obj.ptr_MultipleTemplatesIntFloat);
+    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-matlab/MyFactorPosePoint2.m b/wrap/tests/expected-matlab/MyFactorPosePoint2.m
index 04381f1dc..d711c5325 100644
--- a/wrap/tests/expected-matlab/MyFactorPosePoint2.m
+++ b/wrap/tests/expected-matlab/MyFactorPosePoint2.m
@@ -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(89, my_ptr);
+        geometry_wrapper(93, 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(90, varargin{1}, varargin{2}, varargin{3}, varargin{4});
+        my_ptr = geometry_wrapper(94, 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(91, obj.ptr_MyFactorPosePoint2);
+      geometry_wrapper(95, obj.ptr_MyFactorPosePoint2);
     end
 
     function display(obj), obj.print(''); end
diff --git a/wrap/tests/expected-matlab/aGlobalFunction.m b/wrap/tests/expected-matlab/aGlobalFunction.m
index 0f0b225fa..8f1c65821 100644
--- a/wrap/tests/expected-matlab/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(95, varargin{:});
+        varargout{1} = geometry_wrapper(99, varargin{:});
       else
         error('Arguments do not match any overload of function aGlobalFunction');
       end
diff --git a/wrap/tests/expected-matlab/geometry_wrapper.cpp b/wrap/tests/expected-matlab/geometry_wrapper.cpp
index 70f673d25..98d723fab 100644
--- a/wrap/tests/expected-matlab/geometry_wrapper.cpp
+++ b/wrap/tests/expected-matlab/geometry_wrapper.cpp
@@ -1,4 +1,4 @@
-#include <wrap/matlab.h>
+#include <gtwrap/matlab.h>
 #include <map>
 
 #include <boost/archive/text_iarchive.hpp>
@@ -11,9 +11,11 @@
 
 typedef MyTemplate<gtsam::Point2> MyTemplatePoint2;
 typedef MyTemplate<gtsam::Matrix> MyTemplateMatrix;
-typedef PrimitiveRef<double> PrimitiveRefdouble;
+typedef PrimitiveRef<double> PrimitiveRefDouble;
 typedef MyVector<3> MyVector3;
 typedef MyVector<12> MyVector12;
+typedef MultipleTemplates<int, double> MultipleTemplatesIntDouble;
+typedef MultipleTemplates<int, float> MultipleTemplatesIntFloat;
 typedef MyFactor<gtsam::Pose2, gtsam::Matrix> MyFactorPosePoint2;
 
 BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2");
@@ -31,12 +33,16 @@ 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<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;
 static Collector_MyVector12 collector_MyVector12;
+typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_MultipleTemplatesIntDouble;
+static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
+typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
+static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
 
@@ -82,10 +88,10 @@ void _deleteAllObjects()
     collector_MyTemplateMatrix.erase(iter++);
     anyDeleted = true;
   } }
-  { for(Collector_PrimitiveRefdouble::iterator iter = collector_PrimitiveRefdouble.begin();
-      iter != collector_PrimitiveRefdouble.end(); ) {
+  { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin();
+      iter != collector_PrimitiveRefDouble.end(); ) {
     delete *iter;
-    collector_PrimitiveRefdouble.erase(iter++);
+    collector_PrimitiveRefDouble.erase(iter++);
     anyDeleted = true;
   } }
   { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin();
@@ -100,6 +106,18 @@ void _deleteAllObjects()
     collector_MyVector12.erase(iter++);
     anyDeleted = true;
   } }
+  { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin();
+      iter != collector_MultipleTemplatesIntDouble.end(); ) {
+    delete *iter;
+    collector_MultipleTemplatesIntDouble.erase(iter++);
+    anyDeleted = true;
+  } }
+  { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin();
+      iter != collector_MultipleTemplatesIntFloat.end(); ) {
+    delete *iter;
+    collector_MultipleTemplatesIntFloat.erase(iter++);
+    anyDeleted = true;
+  } }
   { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
       iter != collector_MyFactorPosePoint2.end(); ) {
     delete *iter;
@@ -912,42 +930,42 @@ void MyTemplateMatrix_Level_78(int nargout, mxArray *out[], int nargin, const mx
   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[])
+void PrimitiveRefDouble_collectorInsertAndMakeBase_79(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
 
   Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
-  collector_PrimitiveRefdouble.insert(self);
+  collector_PrimitiveRefDouble.insert(self);
 }
 
-void PrimitiveRefdouble_constructor_80(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+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);
+  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[])
+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);
+  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()) {
+  Collector_PrimitiveRefDouble::iterator item;
+  item = collector_PrimitiveRefDouble.find(self);
+  if(item != collector_PrimitiveRefDouble.end()) {
     delete self;
-    collector_PrimitiveRefdouble.erase(item);
+    collector_PrimitiveRefDouble.erase(item);
   }
 }
 
-void PrimitiveRefdouble_Brutal_82(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void PrimitiveRefDouble_Brutal_82(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("PrimitiveRefdouble.Brutal",nargout,nargin,1);
+  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);
 }
@@ -1018,7 +1036,51 @@ void MyVector12_deconstructor_88(int nargout, mxArray *out[], int nargin, const
   }
 }
 
-void MyFactorPosePoint2_collectorInsertAndMakeBase_89(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_89(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<MultipleTemplates<int, double>> Shared;
+
+  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
+  collector_MultipleTemplatesIntDouble.insert(self);
+}
+
+void MultipleTemplatesIntDouble_deconstructor_90(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<MultipleTemplates<int, double>> Shared;
+  checkArguments("delete_MultipleTemplatesIntDouble",nargout,nargin,1);
+  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
+  Collector_MultipleTemplatesIntDouble::iterator item;
+  item = collector_MultipleTemplatesIntDouble.find(self);
+  if(item != collector_MultipleTemplatesIntDouble.end()) {
+    delete self;
+    collector_MultipleTemplatesIntDouble.erase(item);
+  }
+}
+
+void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_91(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<MultipleTemplates<int, float>> Shared;
+
+  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
+  collector_MultipleTemplatesIntFloat.insert(self);
+}
+
+void MultipleTemplatesIntFloat_deconstructor_92(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<MultipleTemplates<int, float>> Shared;
+  checkArguments("delete_MultipleTemplatesIntFloat",nargout,nargin,1);
+  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
+  Collector_MultipleTemplatesIntFloat::iterator item;
+  item = collector_MultipleTemplatesIntFloat.find(self);
+  if(item != collector_MultipleTemplatesIntFloat.end()) {
+    delete self;
+    collector_MultipleTemplatesIntFloat.erase(item);
+  }
+}
+
+void MyFactorPosePoint2_collectorInsertAndMakeBase_93(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@@ -1027,7 +1089,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_89(int nargout, mxArray *out[
   collector_MyFactorPosePoint2.insert(self);
 }
 
-void MyFactorPosePoint2_constructor_90(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_constructor_94(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@@ -1042,7 +1104,7 @@ void MyFactorPosePoint2_constructor_90(int nargout, mxArray *out[], int nargin,
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyFactorPosePoint2_deconstructor_91(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_deconstructor_95(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
   checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1);
@@ -1055,7 +1117,7 @@ void MyFactorPosePoint2_deconstructor_91(int nargout, mxArray *out[], int nargin
   }
 }
 
-void load2D_92(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void load2D_96(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("load2D",nargout,nargin,5);
   string filename = unwrap< string >(in[0]);
@@ -1067,7 +1129,7 @@ void load2D_92(int nargout, mxArray *out[], int nargin, const mxArray *in[])
   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[])
+void load2D_97(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("load2D",nargout,nargin,5);
   string filename = unwrap< string >(in[0]);
@@ -1079,7 +1141,7 @@ void load2D_93(int nargout, mxArray *out[], int nargin, const mxArray *in[])
   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[])
+void load2D_98(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("load2D",nargout,nargin,2);
   string filename = unwrap< string >(in[0]);
@@ -1088,18 +1150,18 @@ void load2D_94(int nargout, mxArray *out[], int nargin, const mxArray *in[])
   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[])
+void aGlobalFunction_99(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("aGlobalFunction",nargout,nargin,0);
   out[0] = wrap< Vector >(aGlobalFunction());
 }
-void overloadedGlobalFunction_96(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void overloadedGlobalFunction_100(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_97(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void overloadedGlobalFunction_101(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("overloadedGlobalFunction",nargout,nargin,2);
   int a = unwrap< int >(in[0]);
@@ -1356,16 +1418,16 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       MyTemplateMatrix_Level_78(nargout, out, nargin-1, in+1);
       break;
     case 79:
-      PrimitiveRefdouble_collectorInsertAndMakeBase_79(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_collectorInsertAndMakeBase_79(nargout, out, nargin-1, in+1);
       break;
     case 80:
-      PrimitiveRefdouble_constructor_80(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_constructor_80(nargout, out, nargin-1, in+1);
       break;
     case 81:
-      PrimitiveRefdouble_deconstructor_81(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_deconstructor_81(nargout, out, nargin-1, in+1);
       break;
     case 82:
-      PrimitiveRefdouble_Brutal_82(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_Brutal_82(nargout, out, nargin-1, in+1);
       break;
     case 83:
       MyVector3_collectorInsertAndMakeBase_83(nargout, out, nargin-1, in+1);
@@ -1386,31 +1448,43 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       MyVector12_deconstructor_88(nargout, out, nargin-1, in+1);
       break;
     case 89:
-      MyFactorPosePoint2_collectorInsertAndMakeBase_89(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntDouble_collectorInsertAndMakeBase_89(nargout, out, nargin-1, in+1);
       break;
     case 90:
-      MyFactorPosePoint2_constructor_90(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntDouble_deconstructor_90(nargout, out, nargin-1, in+1);
       break;
     case 91:
-      MyFactorPosePoint2_deconstructor_91(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntFloat_collectorInsertAndMakeBase_91(nargout, out, nargin-1, in+1);
       break;
     case 92:
-      load2D_92(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntFloat_deconstructor_92(nargout, out, nargin-1, in+1);
       break;
     case 93:
-      load2D_93(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_collectorInsertAndMakeBase_93(nargout, out, nargin-1, in+1);
       break;
     case 94:
-      load2D_94(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_constructor_94(nargout, out, nargin-1, in+1);
       break;
     case 95:
-      aGlobalFunction_95(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_deconstructor_95(nargout, out, nargin-1, in+1);
       break;
     case 96:
-      overloadedGlobalFunction_96(nargout, out, nargin-1, in+1);
+      load2D_96(nargout, out, nargin-1, in+1);
       break;
     case 97:
-      overloadedGlobalFunction_97(nargout, out, nargin-1, in+1);
+      load2D_97(nargout, out, nargin-1, in+1);
+      break;
+    case 98:
+      load2D_98(nargout, out, nargin-1, in+1);
+      break;
+    case 99:
+      aGlobalFunction_99(nargout, out, nargin-1, in+1);
+      break;
+    case 100:
+      overloadedGlobalFunction_100(nargout, out, nargin-1, in+1);
+      break;
+    case 101:
+      overloadedGlobalFunction_101(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
index fec762dc5..bcea4ed76 100644
--- a/wrap/tests/expected-matlab/load2D.m
+++ b/wrap/tests/expected-matlab/load2D.m
@@ -1,10 +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{:});
+        [ varargout{1} varargout{2} ] = geometry_wrapper(96, 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{:});
+        [ varargout{1} varargout{2} ] = geometry_wrapper(97, varargin{:});
       elseif length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.noiseModel.Diagonal')
-        [ varargout{1} varargout{2} ] = geometry_wrapper(94, varargin{:});
+        [ varargout{1} varargout{2} ] = geometry_wrapper(98, varargin{:});
       else
         error('Arguments do not match any overload of function load2D');
       end
diff --git a/wrap/tests/expected-matlab/overloadedGlobalFunction.m b/wrap/tests/expected-matlab/overloadedGlobalFunction.m
index 168c83c67..2fbaa88dc 100644
--- a/wrap/tests/expected-matlab/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(96, varargin{:});
+        varargout{1} = geometry_wrapper(100, varargin{:});
       elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double')
-        varargout{1} = geometry_wrapper(97, varargin{:});
+        varargout{1} = geometry_wrapper(101, 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
index be6482d89..348888f25 100644
--- a/wrap/tests/expected-python/geometry_pybind.cpp
+++ b/wrap/tests/expected-python/geometry_pybind.cpp
@@ -115,7 +115,8 @@ PYBIND11_MODULE(geometry_py, m_) {
                         gtsam::RedirectCout redirect;
                         a.print();
                         return redirect.str();
-                    });
+                    })
+        .def_readwrite("model_ptr", &Test::model_ptr);
 
     py::class_<MyBase, std::shared_ptr<MyBase>>(m_, "MyBase");
 
@@ -149,7 +150,7 @@ PYBIND11_MODULE(geometry_py, m_) {
         .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")
+    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"));
 
@@ -159,6 +160,10 @@ PYBIND11_MODULE(geometry_py, m_) {
     py::class_<MyVector<12>, std::shared_ptr<MyVector<12>>>(m_, "MyVector12")
         .def(py::init<>());
 
+    py::class_<MultipleTemplates<int, double>, std::shared_ptr<MultipleTemplates<int, double>>>(m_, "MultipleTemplatesIntDouble");
+
+    py::class_<MultipleTemplates<int, float>, std::shared_ptr<MultipleTemplates<int, float>>>(m_, "MultipleTemplatesIntFloat");
+
     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"));
 
diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h
index ec5d3b277..716b4307d 100644
--- a/wrap/tests/geometry.h
+++ b/wrap/tests/geometry.h
@@ -61,6 +61,9 @@ class Test {
   // another comment
   Test();
 
+  // Test a shared ptr property
+  gtsam::noiseModel::Base* model_ptr;
+
   pair<Vector,Matrix> return_pair (Vector v, Matrix A) const; // intentionally the first method
   pair<Vector,Matrix> return_pair (Vector v) const; // overload
 
@@ -160,3 +163,7 @@ class MyVector {
 // comments at the end!
 
 // even more comments at the end!
+
+// Class with multiple instantiated templates
+template<T = {int}, U = {double, float}>
+class MultipleTemplates {};
diff --git a/wrap/tests/pybind_wrapper.tpl b/wrap/tests/pybind_wrapper.tpl
index 2674824b8..836075edf 100644
--- a/wrap/tests/pybind_wrapper.tpl
+++ b/wrap/tests/pybind_wrapper.tpl
@@ -11,7 +11,7 @@
 
 {boost_class_export}
 
-{hoder_type}
+{holder_type}
 
 using namespace std;
 
diff --git a/wrap/tests/test_matlab_wrapper.py b/wrap/tests/test_matlab_wrapper.py
index 258f2da8f..6e7699c86 100644
--- a/wrap/tests/test_matlab_wrapper.py
+++ b/wrap/tests/test_matlab_wrapper.py
@@ -1,21 +1,26 @@
 """
-Unit test for Matlab wrap program
-Author: Matthew Sklar
+Unit tests for Matlab wrap program
+Author: Matthew Sklar, Varun Agrawal
 Date: March 2019
 """
+# pylint: disable=import-error, wrong-import-position, too-many-branches
+
+import filecmp
 import os
 import sys
 import unittest
-import filecmp
 
 sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
 
-import gtwrap.template_instantiator as instantiator
 import gtwrap.interface_parser as parser
+import gtwrap.template_instantiator as instantiator
 from gtwrap.matlab_wrapper import MatlabWrapper
 
 
 class TestWrap(unittest.TestCase):
+    """
+    Test the Matlab wrapper
+    """
     TEST_DIR = os.path.dirname(os.path.realpath(__file__)) + "/"
     MATLAB_TEST_DIR = TEST_DIR + "expected-matlab/"
     MATLAB_ACTUAL_DIR = TEST_DIR + "actual-matlab/"
@@ -31,11 +36,11 @@ class TestWrap(unittest.TestCase):
         """
         if path == '':
             path = self.MATLAB_ACTUAL_DIR
+
         for c in cc_content:
-            if type(c) == list:
+            if isinstance(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]
 
@@ -46,13 +51,12 @@ class TestWrap(unittest.TestCase):
                         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:
+
+            elif isinstance(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:
@@ -60,15 +64,14 @@ class TestWrap(unittest.TestCase):
                     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:
@@ -80,7 +83,8 @@ class TestWrap(unittest.TestCase):
                     f.write(c[1])
 
     def test_geometry_matlab(self):
-        """ Check generation of matlab geometry wrapper.
+        """
+        Check generation of matlab geometry wrapper.
         python3 wrap/matlab_wrapper.py --src wrap/tests/geometry.h
             --module_name geometry --out wrap/tests/actual-matlab
         """
diff --git a/wrap/tests/test_pybind_wrapper.py b/wrap/tests/test_pybind_wrapper.py
index d859cc99f..61edc3e78 100644
--- a/wrap/tests/test_pybind_wrapper.py
+++ b/wrap/tests/test_pybind_wrapper.py
@@ -1,27 +1,29 @@
 """
 Unit test for Pybind wrap program
-Author: Matthew Sklar
+Author: Matthew Sklar, Varun Agrawal
 Date: February 2019
 """
 
+# pylint: disable=import-error, wrong-import-position, too-many-branches
+
+import filecmp
 import os
+import os.path as path
 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 gtwrap.pybind_wrapper import PybindWrapper
 import gtwrap.interface_parser as parser
 import gtwrap.template_instantiator as instantiator
+from gtwrap.pybind_wrapper import PybindWrapper
 
 sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
 
 
 class TestWrap(unittest.TestCase):
+    """Tests for Python wrapper based on Pybind11."""
     TEST_DIR = os.path.dirname(os.path.realpath(__file__)) + "/"
 
     def test_geometry_python(self):
@@ -39,7 +41,7 @@ class TestWrap(unittest.TestCase):
 
         with open(self.TEST_DIR + "pybind_wrapper.tpl") as template_file:
             module_template = template_file.read()
-        
+
         # Create Pybind wrapper instance
         wrapper = PybindWrapper(
             module=module,