diff --git a/CMakeLists.txt b/CMakeLists.txt
index d2b9bc75e..44710c977 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -65,6 +65,7 @@ add_custom_target(uninstall
 # Configurable Options
 if(GTSAM_UNSTABLE_AVAILABLE)
     option(GTSAM_BUILD_UNSTABLE              "Enable/Disable libgtsam_unstable"          ON)
+	option(GTSAM_UNSTABLE_BUILD_PYTHON       "Enable/Disable Python wrapper for libgtsam_unstable" ON)
 endif()
 option(BUILD_SHARED_LIBS                 "Build shared gtsam library, instead of static" ON)
 option(GTSAM_USE_QUATERNIONS             "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
@@ -75,6 +76,7 @@ option(GTSAM_WITH_TBB                    "Use Intel Threaded Building Blocks (TB
 option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available" OFF)
 option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
 option(GTSAM_THROW_CHEIRALITY_EXCEPTION  "Throw exception when a triangulated point is behind a camera" ON)
+option(GTSAM_BUILD_PYTHON                "Enable/Disable building & installation of Python module with pybind11" OFF)
 option(GTSAM_ALLOW_DEPRECATED_SINCE_V41  "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
 option(GTSAM_TYPEDEF_POINTS_TO_VECTORS   "Typedef Point2 and Point3 to Eigen::Vector equivalents" OFF)
 option(GTSAM_SUPPORT_NESTED_DISSECTION   "Support Metis-based nested dissection" ON)
@@ -99,37 +101,44 @@ endif()
 # Options relating to MATLAB wrapper
 # TODO: Check for matlab mex binary before handling building of binaries
 option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab toolbox"  OFF)
-option(GTSAM_INSTALL_CYTHON_TOOLBOX      "Enable/Disable installation of Cython toolbox"  OFF)
-option(GTSAM_BUILD_WRAP                  "Enable/Disable building of matlab/cython wrap utility (necessary for matlab/cython interface)" ON)
-set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of python to build the cython wrapper for (or Default)")
+set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.")
 
 # Check / set dependent variables for MATLAB wrapper
-if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND NOT GTSAM_BUILD_WRAP)
-	message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX or GTSAM_INSTALL_CYTHON_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP")
-endif()
-if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND GTSAM_BUILD_TYPE_POSTFIXES)
+if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES)
 		set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
 endif()
-if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
-	message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
-endif()
 
 if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
 	message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries.  If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
 endif()
 
-if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS)
-    message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the MATLAB toolbox cannot deal with this yet.  Please turn one of the two options off.")
-endif()
+if(GTSAM_BUILD_PYTHON)
+	if (NOT GTSAM_TYPEDEF_POINTS_TO_VECTORS)
+		message(FATAL_ERROR "GTSAM_BUILD_PYTHON requires GTSAM_TYPEDEF_POINTS_TO_VECTORS to be enabled but it is not.")
+	endif()
 
-if(GTSAM_INSTALL_CYTHON_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS)
-    message(FATAL_ERROR "GTSAM_INSTALL_CYTHON_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the CYTHON toolbox cannot deal with this yet.  Please turn one of the two options off.")
+	if(GTSAM_UNSTABLE_BUILD_PYTHON)
+		if (NOT GTSAM_BUILD_UNSTABLE)
+			message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.")
+			set(GTSAM_UNSTABLE_BUILD_PYTHON OFF)
+		endif()
+	endif()
+
+	set(GTSAM_PY_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/python")
 endif()
 
 # Flags for choosing default packaging tools
 set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
 set(CPACK_GENERATOR        "TGZ" CACHE STRING "CPack Default Binary Generator")
 
+if (CMAKE_GENERATOR STREQUAL "Ninja" AND
+    ((CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.9) OR
+     (CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 3.5)))
+    # Force colored warnings in Ninja's output, if the compiler has -fdiagnostics-color support.
+    # Rationale in https://github.com/ninja-build/ninja/issues/814
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always")
+endif()
+
 ###############################################################################
 # Find boost
 
@@ -419,14 +428,11 @@ endif()
 # Build CppUnitLite
 add_subdirectory(CppUnitLite)
 
-# Build wrap
-if (GTSAM_BUILD_WRAP)
-    add_subdirectory(wrap)
-	# suppress warning of cython line being too long
-	if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
-		set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation")
-	endif()
-endif(GTSAM_BUILD_WRAP)
+# This is the new wrapper
+if(GTSAM_BUILD_PYTHON)
+	list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")
+	add_subdirectory(python)
+endif()
 
 # Build GTSAM library
 add_subdirectory(gtsam)
@@ -450,20 +456,6 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
 	add_subdirectory(matlab)
 endif()
 
-# Cython wrap
-if (GTSAM_INSTALL_CYTHON_TOOLBOX)
-  set(GTSAM_INSTALL_CYTHON_TOOLBOX 1)
-  # Set up cache options
-  # Cython install path appended with Build type (e.g. cython, cythonDebug, etc).
-  # This does not override custom values set from the command line
-  set(GTSAM_CYTHON_INSTALL_PATH "${PROJECT_BINARY_DIR}/cython${GTSAM_BUILD_TAG}" CACHE PATH "Cython toolbox destination, blank defaults to PROJECT_BINARY_DIR/cython<GTSAM_BUILD_TAG>")
-  set(GTSAM_EIGENCY_INSTALL_PATH ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency)
-  add_subdirectory(cython ${GTSAM_CYTHON_INSTALL_PATH})
-else()
-  set(GTSAM_INSTALL_CYTHON_TOOLBOX 0) # This will go into config.h
-endif()
-
-
 # Install config and export files
 GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in")
 export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake)
@@ -524,7 +516,8 @@ endif()
 print_enabled_config(${BUILD_SHARED_LIBS}                 "Build shared GTSAM libraries")
 print_enabled_config(${GTSAM_BUILD_TYPE_POSTFIXES}        "Put build type in library name")
 if(GTSAM_UNSTABLE_AVAILABLE)
-    print_enabled_config(${GTSAM_BUILD_UNSTABLE}          "Build libgtsam_unstable")
+	print_enabled_config(${GTSAM_BUILD_UNSTABLE}          "Build libgtsam_unstable        ")
+	print_enabled_config(${GTSAM_UNSTABLE_BUILD_PYTHON}   "Build GTSAM unstable Python    ")
 endif()
 
 if(NOT MSVC AND NOT XCODE_VERSION)
@@ -594,20 +587,15 @@ print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V41}  "Allow features deprec
 print_enabled_config(${GTSAM_TYPEDEF_POINTS_TO_VECTORS}   "Point3 is typedef to Vector3    ")
 print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION}   "Metis-based Nested Dissection   ")
 print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION}      "Use tangent-space preintegration")
-print_enabled_config(${GTSAM_BUILD_WRAP}                  "Build Wrap                      ")
 
 message(STATUS "MATLAB toolbox flags")
 print_enabled_config(${GTSAM_INSTALL_MATLAB_TOOLBOX}      "Install MATLAB toolbox          ")
-if (${GTSAM_INSTALL_MATLAB_TOOLBOX})
-    print_config("MATLAB root" "${MATLAB_ROOT}")
-    print_config("MEX binary" "${MEX_COMMAND}")
+message(STATUS "Python toolbox flags                                      ")
+print_enabled_config(${GTSAM_BUILD_PYTHON}                "Build Python module with pybind ")
+if(GTSAM_BUILD_PYTHON)
+	print_config("Python version" ${GTSAM_PYTHON_VERSION})
 endif()
 
-message(STATUS "Cython toolbox flags                                      ")
-print_enabled_config(${GTSAM_INSTALL_CYTHON_TOOLBOX}      "Install Cython toolbox          ")
-if(GTSAM_INSTALL_CYTHON_TOOLBOX)
-    print_config("Python version" "${GTSAM_PYTHON_VERSION}")
-endif()
 message(STATUS "===============================================================")
 
 # Print warnings at the end
diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt
index d612e2fae..9d9ecd48b 100644
--- a/cmake/CMakeLists.txt
+++ b/cmake/CMakeLists.txt
@@ -17,8 +17,6 @@ install(FILES
   GtsamBuildTypes.cmake
   GtsamMakeConfigFile.cmake
   GtsamMatlabWrap.cmake
-  GtsamPythonWrap.cmake
-  GtsamCythonWrap.cmake
   GtsamTesting.cmake
   GtsamPrinting.cmake
   FindCython.cmake
diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake
deleted file mode 100644
index c8f876895..000000000
--- a/cmake/GtsamCythonWrap.cmake
+++ /dev/null
@@ -1,204 +0,0 @@
-# Check Cython version, need to be >=0.25.2
-# Unset these cached variables to avoid surprises when the python/cython
-# in the current environment are different from the cached!
-unset(PYTHON_EXECUTABLE CACHE)
-unset(CYTHON_EXECUTABLE CACHE)
-unset(PYTHON_INCLUDE_DIR CACHE)
-unset(PYTHON_MAJOR_VERSION CACHE)
-unset(PYTHON_LIBRARY CACHE)
-
-# Allow override from command line
-if(NOT DEFINED GTSAM_USE_CUSTOM_PYTHON_LIBRARY)
-  if(GTSAM_PYTHON_VERSION STREQUAL "Default")
-    find_package(PythonInterp REQUIRED)
-    find_package(PythonLibs REQUIRED)
-  else()
-    find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
-    find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
-  endif()
-endif()
-find_package(Cython 0.25.2 REQUIRED)
-
-execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c"
-    "from __future__ import print_function;import sys;print(sys.version[0], end='')"
-    OUTPUT_VARIABLE PYTHON_MAJOR_VERSION
-)
-
-# User-friendly Cython wrapping and installing function.
-# Builds a Cython module from the provided interface_header.
-# For example, for the interface header gtsam.h,
-# this will build the wrap module 'gtsam'.
-#
-# Arguments:
-#
-# interface_header:  The relative path to the wrapper interface definition file.
-# extra_imports: extra header to import in the Cython pxd file.
-#                For example, to use Cython gtsam.pxd in your own module,
-#        use "from gtsam cimport *"
-# install_path: destination to install the library
-# libs: libraries to link with
-# dependencies: Dependencies which need to be built before the wrapper
-function(wrap_and_install_library_cython interface_header extra_imports install_path libs dependencies)
-  # Paths for generated files
-  get_filename_component(module_name "${interface_header}" NAME_WE)
-  set(generated_files_path "${install_path}")
-  wrap_library_cython("${interface_header}" "${generated_files_path}" "${extra_imports}" "${libs}" "${dependencies}")
-endfunction()
-
-function(set_up_required_cython_packages)
-  # Set up building of cython module
-  include_directories(${PYTHON_INCLUDE_DIRS})
-  find_package(NumPy REQUIRED)
-  include_directories(${NUMPY_INCLUDE_DIRS})
-endfunction()
-
-
-# Convert pyx to cpp by executing cython
-# This is the first step to compile cython from the command line
-# as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html
-#
-# Arguments:
-#    - target:  The specified target for this step
-#    - pyx_file:   The input pyx_file in full *absolute* path
-#    - generated_cpp:   The output cpp file in full absolute path
-#    - include_dirs:   Directories to include when executing cython
-function(pyx_to_cpp target pyx_file generated_cpp include_dirs)
-  foreach(dir ${include_dirs})
-    set(includes_for_cython ${includes_for_cython}  -I ${dir})
-  endforeach()
-
-  add_custom_command(
-    OUTPUT ${generated_cpp}
-    COMMAND
-    ${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus -${PYTHON_MAJOR_VERSION} ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
-    VERBATIM)
-  add_custom_target(${target} ALL DEPENDS ${generated_cpp})
-endfunction()
-
-# Build the cpp file generated by converting pyx using cython
-# This is the second step to compile cython from the command line
-# as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html
-#
-# Arguments:
-#    - target:  The specified target for this step
-#    - cpp_file:   The input cpp_file in full *absolute* path
-#    - output_lib_we:   The output lib filename only (without extension)
-#    - output_dir:   The output directory
-function(build_cythonized_cpp target cpp_file output_lib_we output_dir)
-  add_library(${target} MODULE ${cpp_file})
-  
-  if(WIN32)
-    # Use .pyd extension instead of .dll on Windows
-    set_target_properties(${target} PROPERTIES SUFFIX ".pyd")
-
-    # Add full path to the Python library
-    target_link_libraries(${target} ${PYTHON_LIBRARIES})
-  endif()
-  
-  if(APPLE)
-    set(link_flags "-undefined dynamic_lookup")
-  endif()
-  set_target_properties(${target}
-      PROPERTIES COMPILE_FLAGS "-w"
-      LINK_FLAGS "${link_flags}"
-      OUTPUT_NAME ${output_lib_we}
-      PREFIX ""
-      ${CMAKE_BUILD_TYPE_UPPER}_POSTFIX ""
-      LIBRARY_OUTPUT_DIRECTORY ${output_dir})
-endfunction()
-
-# Cythonize a pyx from the command line as described at
-# http://cython.readthedocs.io/en/latest/src/reference/compilation.html
-# Arguments:
-#    - target:        The specified target
-#    - pyx_file:      The input pyx_file in full *absolute* path
-#    - output_lib_we: The output lib filename only (without extension)
-#    - output_dir:    The output directory
-#    - include_dirs:  Directories to include when executing cython
-#    - libs:          Libraries to link with
-#    - interface_header: For dependency. Any update in interface header will re-trigger cythonize
-function(cythonize target pyx_file output_lib_we output_dir include_dirs libs interface_header dependencies)
-  get_filename_component(pyx_path "${pyx_file}" DIRECTORY)
-  get_filename_component(pyx_name "${pyx_file}" NAME_WE)
-  set(generated_cpp "${output_dir}/${pyx_name}.cpp")
-
-  set_up_required_cython_packages()
-  pyx_to_cpp(${target}_pyx2cpp ${pyx_file} ${generated_cpp} "${include_dirs}")
-
-  # Late dependency injection, to make sure this gets called whenever the interface header is updated
-  # See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes
-  add_custom_command(OUTPUT ${generated_cpp} DEPENDS ${interface_header} ${pyx_file} APPEND)
-  if (NOT "${dependencies}" STREQUAL "")
-    add_dependencies(${target}_pyx2cpp "${dependencies}")
-  endif()
-
-  build_cythonized_cpp(${target} ${generated_cpp} ${output_lib_we} ${output_dir})
-  if (NOT "${libs}" STREQUAL "")
-    target_link_libraries(${target} "${libs}")
-  endif()
-  add_dependencies(${target} ${target}_pyx2cpp)
-
-  if(TARGET ${python_install_target})
-    add_dependencies(${python_install_target} ${target})
-  endif()
-endfunction()
-
-# Internal function that wraps a library and compiles the wrapper
-function(wrap_library_cython interface_header generated_files_path extra_imports libs dependencies)
-  # Wrap codegen interface
-  # Extract module path and name from interface header file name
-  # wrap requires interfacePath to be *absolute*
-  get_filename_component(interface_header "${interface_header}" ABSOLUTE)
-  get_filename_component(module_path "${interface_header}" PATH)
-  get_filename_component(module_name "${interface_header}" NAME_WE)
-
-  # Wrap module to Cython pyx
-  message(STATUS "Cython wrapper generating ${generated_files_path}/${module_name}.pyx")
-  set(generated_pyx "${generated_files_path}/${module_name}.pyx")
-  if(NOT EXISTS ${generated_files_path})
-    file(MAKE_DIRECTORY "${generated_files_path}")
-  endif()
-
-  add_custom_command(
-    OUTPUT ${generated_pyx}
-    DEPENDS ${interface_header} wrap
-    COMMAND
-        wrap --cython ${module_path} ${module_name} ${generated_files_path} "${extra_imports}"
-    VERBATIM
-    WORKING_DIRECTORY ${generated_files_path}/../)
-  add_custom_target(cython_wrap_${module_name}_pyx ALL DEPENDS ${generated_pyx})
-  if(NOT "${dependencies}" STREQUAL "")
-    add_dependencies(cython_wrap_${module_name}_pyx ${dependencies})
-  endif()
-
-  message(STATUS "Cythonize and build ${module_name}.pyx")
-  get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES)
-  cythonize(cythonize_${module_name} ${generated_pyx} ${module_name}
-    ${generated_files_path} "${include_dirs}" "${libs}" ${interface_header} cython_wrap_${module_name}_pyx)
-
-  # distclean
-  add_custom_target(wrap_${module_name}_cython_distclean
-      COMMAND cmake -E remove_directory ${generated_files_path})
-endfunction()
-
-# Helper function to install Cython scripts and handle multiple build types where the scripts
-# should be installed to all build type toolboxes
-#
-# Arguments:
-#  source_directory: The source directory to be installed. "The last component of each directory
-#                    name is appended to the destination directory but a trailing slash may be
-#                    used to avoid this because it leaves the last component empty."
-#                    (https://cmake.org/cmake/help/v3.3/command/install.html?highlight=install#installing-directories)
-#  dest_directory: The destination directory to install to.
-#  patterns: list of file patterns to install
-function(install_cython_scripts source_directory dest_directory patterns)
-  set(patterns_args "")
-  set(exclude_patterns "")
-
-  foreach(pattern ${patterns})
-    list(APPEND patterns_args PATTERN "${pattern}")
-  endforeach()
-
-  file(COPY "${source_directory}" DESTINATION "${dest_directory}"
-            FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
-endfunction()
diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake
index 5fc829bf2..111114a7b 100644
--- a/cmake/GtsamMatlabWrap.cmake
+++ b/cmake/GtsamMatlabWrap.cmake
@@ -23,6 +23,11 @@ else()
 	file(GLOB matlab_bin_directories "/usr/local/MATLAB/*/bin")
 	set(mex_program_name "mex")
 endif()
+
+if(GTSAM_CUSTOM_MATLAB_PATH)
+	set(matlab_bin_directories ${GTSAM_CUSTOM_MATLAB_PATH})
+endif()
+
 # Run find_program explicitly putting $PATH after our predefined program
 # directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents
 # finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is
@@ -209,15 +214,34 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
 
 	# Set up generation of module source file
 	file(MAKE_DIRECTORY "${generated_files_path}")
+
+	if(GTSAM_PYTHON_VERSION STREQUAL "Default")
+		find_package(PythonInterp REQUIRED)
+		find_package(PythonLibs REQUIRED)
+	else()
+		find_package(PythonInterp
+				${GTSAM_PYTHON_VERSION}
+				EXACT
+				REQUIRED)
+		find_package(PythonLibs
+				${GTSAM_PYTHON_VERSION}
+				EXACT
+				REQUIRED)
+	endif()
+
+	set(_ignore gtsam::Point2
+			gtsam::Point3)
 	add_custom_command(
 		OUTPUT ${generated_cpp_file}
-		DEPENDS ${interfaceHeader} wrap ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects}
-        COMMAND 
-            wrap --matlab
-            ${modulePath}
-            ${moduleName} 
-            ${generated_files_path} 
-            ${matlab_h_path} 
+		DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects}
+        COMMAND
+			${PYTHON_EXECUTABLE}
+			${CMAKE_SOURCE_DIR}/wrap/matlab_wrapper.py
+            --src ${interfaceHeader}
+			--module_name ${moduleName}
+            --out ${generated_files_path}
+			--top_module_namespaces ${moduleName}
+			--ignore ${_ignore}
 		VERBATIM
 		WORKING_DIRECTORY ${generated_files_path})
 		
diff --git a/cmake/GtsamPythonWrap.cmake b/cmake/GtsamPythonWrap.cmake
deleted file mode 100644
index 714e37488..000000000
--- a/cmake/GtsamPythonWrap.cmake
+++ /dev/null
@@ -1,102 +0,0 @@
-#Setup cache options
-set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version")
-set(GTSAM_BUILD_PYTHON_FLAGS "" CACHE STRING "Extra flags for running Matlab PYTHON compilation")
-set(GTSAM_PYTHON_INSTALL_PATH "" CACHE PATH "Python toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/borg/python")
-if(NOT GTSAM_PYTHON_INSTALL_PATH)
-  set(GTSAM_PYTHON_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/borg/python")
-endif() 
-
-#Author: Paul Furgale Modified by Andrew Melim
-function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY)
-  # # Boost
-  # find_package(Boost COMPONENTS python filesystem system REQUIRED)
-  # include_directories(${Boost_INCLUDE_DIRS})
-
-  # # Find Python
-  # FIND_PACKAGE(PythonLibs 2.7 REQUIRED)
-  # INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
-
-  IF(APPLE)
-    # The apple framework headers don't include the numpy headers for some reason.
-    GET_FILENAME_COMPONENT(REAL_PYTHON_INCLUDE ${PYTHON_INCLUDE_DIRS} REALPATH)
-    IF( ${REAL_PYTHON_INCLUDE} MATCHES Python.framework)
-      message("Trying to find extra headers for numpy from ${REAL_PYTHON_INCLUDE}.")
-      message("Looking in ${REAL_PYTHON_INCLUDE}/../../Extras/lib/python/numpy/core/include/numpy")
-      FIND_PATH(NUMPY_INCLUDE_DIR arrayobject.h
-  ${REAL_PYTHON_INCLUDE}/../../Extras/lib/python/numpy/core/include/numpy
-  ${REAL_PYTHON_INCLUDE}/numpy
-  )
-      IF(${NUMPY_INCLUDE_DIR} MATCHES NOTFOUND)
-  message("Unable to find numpy include directories: ${NUMPY_INCLUDE_DIR}")
-      ELSE()
-  message("Found headers at ${NUMPY_INCLUDE_DIR}")
-  INCLUDE_DIRECTORIES(${NUMPY_INCLUDE_DIR})
-  INCLUDE_DIRECTORIES(${NUMPY_INCLUDE_DIR}/..)
-      ENDIF()
-    ENDIF()
-  ENDIF(APPLE)
-
-  if(MSVC)
-    add_library(${moduleName}_python MODULE ${ARGN})
-    set_target_properties(${moduleName}_python PROPERTIES
-        OUTPUT_NAME         ${moduleName}_python
-        CLEAN_DIRECT_OUTPUT 1
-        VERSION             1
-        SOVERSION           0
-        SUFFIX              ".pyd")
-      target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp
-
-    set(PYLIB_OUTPUT_FILE $<TARGET_FILE:${moduleName}_python>)
-    message(${PYLIB_OUTPUT_FILE})
-    get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
-    set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.pyd)
-
-  ELSE()
-    # Create a shared library
-    add_library(${moduleName}_python SHARED ${generated_cpp_file})
-
-    set_target_properties(${moduleName}_python PROPERTIES
-        OUTPUT_NAME         ${moduleName}_python
-        CLEAN_DIRECT_OUTPUT 1)
-      target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp
-    # On OSX and Linux, the python library must end in the extension .so. Build this
-    # filename here.
-    get_property(PYLIB_OUTPUT_FILE TARGET ${moduleName}_python PROPERTY LOCATION)
-    set(PYLIB_OUTPUT_FILE $<TARGET_FILE:${moduleName}_python>)
-    message(${PYLIB_OUTPUT_FILE})
-    get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
-    set(PYLIB_SO_NAME lib${moduleName}_python.so)
-  ENDIF(MSVC)
-
-  # Installs the library in the gtsam folder, which is used by setup.py to create the gtsam package
-  set(PYTHON_MODULE_DIRECTORY ${CMAKE_SOURCE_DIR}/python/gtsam)
-  # Cause the library to be output in the correct directory.
-  add_custom_command(TARGET ${moduleName}_python
-    POST_BUILD
-    COMMAND cp -v ${PYLIB_OUTPUT_FILE} ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME}
-    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
-    COMMENT "Copying library files to python directory" )
-
-  # Cause the library to be output in the correct directory.
-  add_custom_command(TARGET ${TARGET_NAME}
-    POST_BUILD
-    COMMAND cp -v ${PYLIB_OUTPUT_FILE} ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME}
-    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
-    COMMENT "Copying library files to python directory" )
-
-  get_directory_property(AMCF ADDITIONAL_MAKE_CLEAN_FILES)
-  list(APPEND AMCF ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME})
-  set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${AMCF}") 
-endfunction(wrap_python)
-
-# Macro to get list of subdirectories
-macro(SUBDIRLIST result curdir)
-  file(GLOB children RELATIVE ${curdir} ${curdir}/*)
-  set(dirlist "")
-  foreach(child ${children})
-    if(IS_DIRECTORY ${curdir}/${child})
-        list(APPEND dirlist ${child})
-    endif()
-  endforeach()
-  set(${result} ${dirlist})
-endmacro()
diff --git a/cmake/dllexport.h.in b/cmake/dllexport.h.in
index 9a0a344b7..7d757edea 100644
--- a/cmake/dllexport.h.in
+++ b/cmake/dllexport.h.in
@@ -47,9 +47,14 @@
 #    endif
 #  endif
 #else
+#ifdef __APPLE__
+#  define @library_name@_EXPORT __attribute__((visibility("default")))
+#  define @library_name@_EXTERN_EXPORT extern
+#else
 #  define @library_name@_EXPORT
 #  define @library_name@_EXTERN_EXPORT extern
 #endif
+#endif
 
 #undef BUILD_SHARED_LIBS
 
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 8736a5954..88fb6ba8f 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -212,5 +212,5 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
         endif()
 
     # Wrap
-    wrap_and_install_library(../gtsam.h "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
+    wrap_and_install_library(gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
 endif ()
diff --git a/gtsam.h b/gtsam/gtsam.i
similarity index 99%
rename from gtsam.h
rename to gtsam/gtsam.i
index c5d25b0e9..fb7aae17d 100644
--- a/gtsam.h
+++ b/gtsam/gtsam.i
@@ -2797,16 +2797,16 @@ class SfmData {
 
 string findExampleDataFile(string name);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
-    gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
+    gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
-    gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise);
+    gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
-    gtsam::noiseModel::Diagonal* model, int maxID);
+    gtsam::noiseModel::Diagonal* model, int maxIndex);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
     gtsam::noiseModel::Diagonal* model);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
-    gtsam::noiseModel::Base* model);
+    gtsam::noiseModel::Base* model, int maxIndex);
 void save2D(const gtsam::NonlinearFactorGraph& graph,
     const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
     string filename);
@@ -2816,8 +2816,8 @@ class BetweenFactorPose3s
 {
   BetweenFactorPose3s();
   size_t size() const;
-  gtsam::BetweenFactorPose3* at(size_t i) const;
-  void push_back(const gtsam::BetweenFactorPose3* factor);
+  gtsam::BetweenFactor<gtsam::Pose3>* at(size_t i) const;
+  void push_back(const gtsam::BetweenFactor<gtsam::Pose3>* factor);
 };
 
 #include <gtsam/slam/InitializePose3.h>
@@ -2877,9 +2877,9 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
 #include <gtsam/sfm/ShonanFactor.h>
 
 virtual class ShonanFactor3 : gtsam::NoiseModelFactor {
-  ShonanFactor(size_t key1, size_t key2, const gtsam::Rot3 &R12,
+  ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12,
                           size_t p);
-  ShonanFactor(size_t key1, size_t key2, const gtsam::Rot3 &R12,
+  ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12,
                           size_t p, gtsam::noiseModel::Base *model);
   Vector evaluateError(const gtsam::SOn &Q1, const gtsam::SOn &Q2);
 };
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index d7b067d70..797e778a3 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -570,7 +570,7 @@ GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, size_t maxIndex,
 
 /* ************************************************************************* */
 GraphAndValues load2D_robust(const string &filename,
-                             noiseModel::Base::shared_ptr &model,
+                             const noiseModel::Base::shared_ptr &model,
                              size_t maxIndex) {
   return load2D(filename, model, maxIndex);
 }
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index 53abe55ba..f6752eb34 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -172,7 +172,7 @@ GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
 
 /// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel
 GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
-    noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
+    const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
 
 /** save 2d graph */
 GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt
index 010b32710..ec161baa8 100644
--- a/gtsam_unstable/CMakeLists.txt
+++ b/gtsam_unstable/CMakeLists.txt
@@ -108,7 +108,7 @@ list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
 
 # Wrap version for gtsam_unstable
-if (GTSAM_INSTALL_MATLAB_TOOLBOX)
+if (GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
     # Set up codegen
     include(GtsamMatlabWrap)
 
@@ -119,8 +119,8 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
         endif()
 
     # Wrap
-    wrap_and_install_library(gtsam_unstable.h "gtsam" "" "${mexFlags}")
-endif(GTSAM_INSTALL_MATLAB_TOOLBOX)
+    wrap_and_install_library(gtsam_unstable.i "gtsam" "" "${mexFlags}")
+endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX)
 
 
 # Build examples
diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.i
similarity index 100%
rename from gtsam_unstable/gtsam_unstable.h
rename to gtsam_unstable/gtsam_unstable.i