Merge branch 'develop' into feature/cameraTemplateForAllSmartFactors

2021-09-23 09:42:03 -04:00 · 2021-09-23 09:42:03 -04:00 · c4b64877eb
parent f3c65d9de8 b4b2f41f25
commit c4b64877eb
51 changed files with 734 additions and 195 deletions
--- a/.github/scripts/python.sh
+++ b/.github/scripts/python.sh
@ -85,4 +85,4 @@ make -j2 install
 cd $GITHUB_WORKSPACE/build/python
 $PYTHON setup.py install --user --prefix=
 cd $GITHUB_WORKSPACE/python/gtsam/tests
-$PYTHON -m unittest discover
+$PYTHON -m unittest discover -v
--- a/.github/scripts/unix.sh
+++ b/.github/scripts/unix.sh
@ -68,6 +68,8 @@ function configure()
      -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
      -DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \
      -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \
+      -DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \
+      -DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \
      -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
      -DBOOST_ROOT=$BOOST_ROOT \
      -DBoost_NO_SYSTEM_PATHS=ON \
--- a/.github/workflows/build-special.yml
+++ b/.github/workflows/build-special.yml
@ -55,6 +55,12 @@ jobs:
            version: "9"
            flag: cayley

+          - name: ubuntu-system-libs
+            os: ubuntu-18.04
+            compiler: gcc
+            version: "9"
+            flag: system-libs
+
    steps:
      - name: Checkout
        uses: actions/checkout@v2
@ -126,6 +132,12 @@ jobs:
          echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV
          echo "GTSAM Uses Cayley map for Rot3"

+      - name: Use system versions of 3rd party libraries
+        if: matrix.flag == 'system'
+        run: |
+          echo "GTSAM_USE_SYSTEM_EIGEN=ON" >> $GITHUB_ENV
+          echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV
+
      - name: Build & Test
        run: |
          bash .github/scripts/unix.sh -t
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -38,11 +38,14 @@ if(${GTSAM_SOURCE_DIR} STREQUAL ${GTSAM_BINARY_DIR})
  message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ")
 endif()

+include(cmake/HandleGeneralOptions.cmake)   # CMake build options
+
+# Libraries:
 include(cmake/HandleBoost.cmake)            # Boost
 include(cmake/HandleCCache.cmake)           # ccache
 include(cmake/HandleCPack.cmake)            # CPack
 include(cmake/HandleEigen.cmake)            # Eigen3
-include(cmake/HandleGeneralOptions.cmake)  # CMake build options
+include(cmake/HandleMetis.cmake)            # metis
 include(cmake/HandleMKL.cmake)              # MKL
 include(cmake/HandleOpenMP.cmake)           # OpenMP
 include(cmake/HandlePerfTools.cmake)        # Google perftools
--- a/INSTALL.md
+++ b/INSTALL.md
@ -13,7 +13,8 @@ $ make install
 ## Important Installation Notes

 1. GTSAM requires the following libraries to be installed on your system:
-    - BOOST version 1.65 or greater (install through Linux repositories or MacPorts)
+    - BOOST version 1.65 or greater (install through Linux repositories or MacPorts). Please see [Boost Notes](#boost-notes).
+
    - Cmake version 3.0 or higher
    - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher

@ -66,11 +67,15 @@ execute commands as follows for an out-of-source build:
  This will build the library and unit tests, run all of the unit tests,
  and then install the library itself.

+## Boost Notes
+
+Versions of Boost prior to 1.65 have a known bug that prevents proper "deep" serialization of objects, which means that objects encapsulated inside other objects don't get serialized.
+This is particularly seen when using `clang` as the C++ compiler.
+
+For this reason we require Boost>=1.65, and recommend installing it through alternative channels when it is not available through your operating system's primary package manager.
+
 ## Known Issues

- When using `GTSAM_BUILD_WITH_MARCH_NATIVE=ON`, you may encounter issues in running tests which we are still investigating:
-  - Use of a version of GCC < 7.5 results in an "Indeterminant Linear System" error for `testSmartProjectionFactor`.
-  - Use of Boost version < 1.65 with clang will give a segfault for mulitple test cases.
 - MSVC 2013 is not yet supported because it cannot build the serialization module of Boost 1.55 (or earlier).

 # Windows Installation
--- a/cmake/FindTBB.cmake
+++ b/cmake/FindTBB.cmake
@ -144,7 +144,8 @@ if(NOT TBB_FOUND)

  elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
    # OS X
-    set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb")
+    set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb"
+                               "/usr/local/opt/tbb")

    # TODO: Check to see which C++ library is being used by the compiler.
    if(NOT ${CMAKE_SYSTEM_VERSION} VERSION_LESS 13.0)
@ -181,7 +182,18 @@ if(NOT TBB_FOUND)
  ##################################

  if(TBB_INCLUDE_DIRS)
-    file(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _tbb_version_file)
+    set(_tbb_version_file_prior_to_tbb_2021_1 "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h")
+    set(_tbb_version_file_after_tbb_2021_1 "${TBB_INCLUDE_DIRS}/oneapi/tbb/version.h")
+
+    if (EXISTS "${_tbb_version_file_prior_to_tbb_2021_1}")
+      file(READ "${_tbb_version_file_prior_to_tbb_2021_1}" _tbb_version_file )
+    elseif (EXISTS "${_tbb_version_file_after_tbb_2021_1}")
+      file(READ "${_tbb_version_file_after_tbb_2021_1}" _tbb_version_file )
+    else()
+        message(FATAL_ERROR "Found TBB installation: ${TBB_INCLUDE_DIRS} "
+      "missing version header.")
+    endif()
+
    string(REGEX REPLACE ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1"
        TBB_VERSION_MAJOR "${_tbb_version_file}")
    string(REGEX REPLACE ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1"
--- a/cmake/HandleMetis.cmake
+++ b/cmake/HandleMetis.cmake
@ -0,0 +1,44 @@
+###############################################################################
+# Metis library
+
+# For both system or bundle version, a cmake target "metis-gtsam-if" is defined (interface library)
+
+# Dont try to use metis if GTSAM_SUPPORT_NESTED_DISSECTION is disabled:
+if (NOT GTSAM_SUPPORT_NESTED_DISSECTION)
+  return()
+endif()
+
+option(GTSAM_USE_SYSTEM_METIS "Find and use system-installed libmetis. If 'off', use the one bundled with GTSAM" OFF)
+
+if(GTSAM_USE_SYSTEM_METIS)
+  # Debian package: libmetis-dev
+
+  find_path(METIS_INCLUDE_DIR metis.h REQUIRED)
+  find_library(METIS_LIBRARY metis REQUIRED)
+
+  if(METIS_INCLUDE_DIR AND METIS_LIBRARY)
+    mark_as_advanced(METIS_INCLUDE_DIR)
+    mark_as_advanced(METIS_LIBRARY)
+
+    add_library(metis-gtsam-if INTERFACE)
+    target_include_directories(metis-gtsam-if BEFORE INTERFACE ${METIS_INCLUDE_DIR})
+    target_link_libraries(metis-gtsam-if INTERFACE ${METIS_LIBRARY})
+  endif()
+else()
+  # Bundled version:
+  option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF)
+  add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis)
+
+  target_include_directories(metis-gtsam BEFORE PUBLIC
+    $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/include>
+    $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
+    $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
+    $<INSTALL_INTERFACE:include/gtsam/3rdparty/metis/>
+  )
+
+  add_library(metis-gtsam-if INTERFACE)
+  target_link_libraries(metis-gtsam-if INTERFACE metis-gtsam)
+endif()
+
+list(APPEND GTSAM_EXPORTED_TARGETS metis-gtsam-if)
+install(TARGETS metis-gtsam-if EXPORT GTSAM-exports ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR})
--- a/cmake/HandlePrintConfiguration.cmake
+++ b/cmake/HandlePrintConfiguration.cmake
@ -32,6 +32,7 @@ endif()
 print_build_options_for_target(gtsam)

 print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
+print_config("Use System Metis" "${GTSAM_USE_SYSTEM_METIS}")

 if(GTSAM_USE_TBB)
    print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})")
--- a/cmake/HandleTBB.cmake
+++ b/cmake/HandleTBB.cmake
@ -1,10 +1,16 @@
 ###############################################################################
-# Find TBB
-find_package(TBB 4.4 COMPONENTS tbb tbbmalloc)
+if (GTSAM_WITH_TBB)
+    # Find TBB
+    find_package(TBB 4.4 COMPONENTS tbb tbbmalloc)

-# Set up variables if we're using TBB
-if(TBB_FOUND AND GTSAM_WITH_TBB)
+    # Set up variables if we're using TBB
+    if(TBB_FOUND)
        set(GTSAM_USE_TBB 1)  # This will go into config.h
+
+        if ((${TBB_VERSION} VERSION_GREATER "2021.1") OR (${TBB_VERSION} VERSION_EQUAL "2021.1"))
+            message(FATAL_ERROR "TBB version greater than 2021.1 (oneTBB API) is not yet supported. Use an older version instead.")
+        endif()
+
        if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020))
            set(TBB_GREATER_EQUAL_2020 1)
        else()
@ -13,12 +19,14 @@ if(TBB_FOUND AND GTSAM_WITH_TBB)
        # all definitions and link requisites will go via imported targets:
        # tbb & tbbmalloc
        list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
-else()
+    else()
        set(GTSAM_USE_TBB 0)  # This will go into config.h
-endif()
+    endif()

-###############################################################################
-# Prohibit Timing build mode in combination with TBB
-if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE  STREQUAL "Timing"))
+    ###############################################################################
+    # Prohibit Timing build mode in combination with TBB
+    if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE  STREQUAL "Timing"))
        message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.")
+    endif()
+
 endif()
--- a/docker/README.md
+++ b/docker/README.md
@ -1,6 +1,57 @@
 # Instructions

-Build all docker images, in order:
+# Images on Docker Hub
+
+There are 4 images available on https://hub.docker.com/orgs/borglab/repositories:
+
+- `borglab/ubuntu-boost-tbb`: 18.06 Linux (nicknamed `bionic`) base image, with Boost and TBB installed.
+- `borglab/ubuntu-gtsam`: GTSAM Release version installed in `/usr/local`.
+- `borglab/ubuntu-gtsam-python`: installed GTSAM with python wrapper.
+- `borglab/ubuntu-gtsam-python-vnc`: image with GTSAM+python wrapper that will run a VNC server to connect to.
+
+# Using the images
+
+## Just GTSAM
+
+To start the Docker image, execute
+```bash
+docker run -it borglab/ubuntu-gtsam:bionic 
+```
+after you will find yourself in a bash shell, in the directory `/usr/src/gtsam/build`.
+## GTSAM with Python wrapper
+
+To use GTSAM via the python wrapper, similarly execute
+```bash
+docker run -it borglab/ubuntu-gtsam-python:bionic 
+```
+and then launch `python3`:
+```bash
+python3
+>>> import gtsam
+>>> gtsam.Pose2(1,2,3)
+(1, 2, 3)
+```
+
+## GTSAM with Python wrapper and VNC
+
+First, start the docker image, which will run a VNC server on port 5900:
+```bash
+docker run -p 5900:5900 borglab/ubuntu-gtsam-python-vnc:bionic
+```
+
+Then open a remote VNC X client, for example:
+
+### Linux
+```bash
+sudo apt-get install tigervnc-viewer
+xtigervncviewer :5900
+```
+### Mac
+The Finder's "Connect to Server..." with `vnc://127.0.0.1` does not work, for some reason. Using the free [VNC Viewer](https://www.realvnc.com/en/connect/download/viewer/), enter `0.0.0.0:5900` as the server.
+
+# Re-building the images locally
+
+To build all docker images, in order:

 ```bash
 (cd ubuntu-boost-tbb && ./build.sh)
@ -9,13 +60,4 @@ Build all docker images, in order:
 (cd ubuntu-gtsam-python-vnc && ./build.sh)
 ```

-Then launch with: 
-
-    docker run -p 5900:5900 dellaert/ubuntu-gtsam-python-vnc:bionic
-
-Then open a remote VNC X client, for example:
-
-    sudo apt-get install tigervnc-viewer
-    xtigervncviewer :5900
-
-
+Note: building GTSAM can take a lot of memory because of the heavy templating. It is advisable to give Docker enough resources, e.g., 8GB, to avoid OOM errors while compiling.
--- a/docker/ubuntu-boost-tbb/build.sh
+++ b/docker/ubuntu-boost-tbb/build.sh
@ -1,3 +1,3 @@
 # Build command for Docker image
 # TODO(dellaert): use docker compose and/or cmake
-docker build --no-cache -t dellaert/ubuntu-boost-tbb:bionic .
+docker build --no-cache -t borglab/ubuntu-boost-tbb:bionic .
--- a/docker/ubuntu-gtsam-python-vnc/Dockerfile
+++ b/docker/ubuntu-gtsam-python-vnc/Dockerfile
@ -1,7 +1,7 @@
 # This GTSAM image connects to the host X-server via VNC to provide a Graphical User Interface for interaction.

 # Get the base Ubuntu/GTSAM image from Docker Hub
-FROM dellaert/ubuntu-gtsam-python:bionic
+FROM borglab/ubuntu-gtsam-python:bionic

 # Things needed to get a python GUI
 ENV DEBIAN_FRONTEND noninteractive
--- a/docker/ubuntu-gtsam-python-vnc/build.sh
+++ b/docker/ubuntu-gtsam-python-vnc/build.sh
@ -1,4 +1,4 @@
 # Build command for Docker image
 # TODO(dellaert): use docker compose and/or cmake
 # Needs to be run in docker/ubuntu-gtsam-python-vnc directory
-docker build -t dellaert/ubuntu-gtsam-python-vnc:bionic .
+docker build -t borglab/ubuntu-gtsam-python-vnc:bionic .
--- a/docker/ubuntu-gtsam-python-vnc/vnc.sh
+++ b/docker/ubuntu-gtsam-python-vnc/vnc.sh
@ -2,4 +2,4 @@
 docker run -it \
    --workdir="/usr/src/gtsam" \
    -p 5900:5900 \
-    dellaert/ubuntu-gtsam-python-vnc:bionic
+    borglab/ubuntu-gtsam-python-vnc:bionic
--- a/docker/ubuntu-gtsam-python/Dockerfile
+++ b/docker/ubuntu-gtsam-python/Dockerfile
@ -1,7 +1,7 @@
 #  GTSAM Ubuntu image with Python wrapper support.

 # Get the base Ubuntu/GTSAM image from Docker Hub
-FROM dellaert/ubuntu-gtsam:bionic
+FROM borglab/ubuntu-gtsam:bionic

 # Install pip
 RUN apt-get install -y python3-pip python3-dev
@ -22,7 +22,9 @@ RUN cmake \
    ..

 # Build again, as ubuntu-gtsam image cleaned
-RUN make -j4 install && make clean
+RUN make -j4 install
+RUN make python-install
+RUN make clean

 # Needed to run python wrapper:
 RUN echo 'export PYTHONPATH=/usr/local/python/:$PYTHONPATH' >> /root/.bashrc
--- a/docker/ubuntu-gtsam-python/build.sh
+++ b/docker/ubuntu-gtsam-python/build.sh
@ -1,3 +1,3 @@
 # Build command for Docker image
 # TODO(dellaert): use docker compose and/or cmake
-docker build --no-cache -t dellaert/ubuntu-gtsam-python:bionic .
+docker build --no-cache -t borglab/ubuntu-gtsam-python:bionic .
--- a/docker/ubuntu-gtsam/Dockerfile
+++ b/docker/ubuntu-gtsam/Dockerfile
@ -1,7 +1,7 @@
 # Ubuntu image with GTSAM installed. Configured with  Boost and TBB support.

 # Get the base Ubuntu image from Docker Hub
-FROM dellaert/ubuntu-boost-tbb:bionic
+FROM borglab/ubuntu-boost-tbb:bionic

 # Install git
 RUN apt-get update && \
--- a/docker/ubuntu-gtsam/build.sh
+++ b/docker/ubuntu-gtsam/build.sh
@ -1,3 +1,3 @@
 # Build command for Docker image
 # TODO(dellaert): use docker compose and/or cmake
-docker build --no-cache -t dellaert/ubuntu-gtsam:bionic .
+docker build --no-cache -t borglab/ubuntu-gtsam:bionic .
--- a/gtsam/3rdparty/CMakeLists.txt
+++ b/gtsam/3rdparty/CMakeLists.txt
@ -49,10 +49,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)

 endif()

-option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF)
-if(GTSAM_SUPPORT_NESTED_DISSECTION)
-	add_subdirectory(metis)
-endif()
+# metis: already handled in ROOT/cmake/HandleMetis.cmake

 add_subdirectory(ceres)

--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@ -89,7 +89,8 @@ list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/d
 install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam)

 if(GTSAM_SUPPORT_NESTED_DISSECTION)
-    list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam)
+    # target metis-gtsam-if is defined in both cases: embedded metis or system version:
+    list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam-if)
 endif()

 # Versions
@ -155,16 +156,6 @@ target_include_directories(gtsam SYSTEM BEFORE PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Include>
  $<INSTALL_INTERFACE:include/gtsam/3rdparty/CCOLAMD>
 )
-if(GTSAM_SUPPORT_NESTED_DISSECTION)
-  target_include_directories(gtsam BEFORE PUBLIC
-    $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/include>
-    $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
-    $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
-    $<INSTALL_INTERFACE:include/gtsam/3rdparty/metis/>
-  )
-endif()
-
-

 if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
 	if (NOT BUILD_SHARED_LIBS)
--- a/gtsam/basis/Fourier.h
+++ b/gtsam/basis/Fourier.h
@ -40,9 +40,13 @@ class GTSAM_EXPORT FourierBasis : public Basis<FourierBasis> {
  static Weights CalculateWeights(size_t N, double x) {
    Weights b(N);
    b[0] = 1;
-    for (size_t i = 1, n = 1; i < N; i += 2, n++) {
+    for (size_t i = 1, n = 1; i < N; i++) {
+      if (i % 2 == 1) {
        b[i] = cos(n * x);
-      b[i + 1] = sin(n * x);
+      } else {
+        b[i] = sin(n * x);
+        n++;
+      }
    }
    return b;
  }
--- a/gtsam/config.h.in
+++ b/gtsam/config.h.in
@ -77,3 +77,6 @@

 // Support Metis-based nested dissection
 #cmakedefine GTSAM_TANGENT_PREINTEGRATION
+
+// Whether to use the system installed Metis instead of the provided one
+#cmakedefine GTSAM_USE_SYSTEM_METIS
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@ -26,6 +26,12 @@ namespace gtsam {
 /// it is now possible to just typedef Point2 to Vector2
 typedef Vector2 Point2;
  
+// Convenience typedef
+using Point2Pair = std::pair<Point2, Point2>;
+GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
+
+using Point2Pairs = std::vector<Point2Pair>;
+
 /// Distance of the point from the origin, with Jacobian
 GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);

@ -34,10 +40,6 @@ GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q,
                 OptionalJacobian<1, 2> H1 = boost::none,
                 OptionalJacobian<1, 2> H2 = boost::none);

-// Convenience typedef
-typedef std::pair<Point2, Point2> Point2Pair;
-GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
-
 // For MATLAB wrapper
 typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;

--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@ -32,6 +32,14 @@ class Point2 {
  void pickle() const;
 };
  
+class Point2Pairs {
+  Point2Pairs();
+  size_t size() const;
+  bool empty() const;
+  gtsam::Point2Pair at(size_t n) const;
+  void push_back(const gtsam::Point2Pair& point_pair);
+};
+
 // std::vector<gtsam::Point2>
 class Point2Vector {
  // Constructors
@ -430,6 +438,8 @@ class Pose2 {
  void pickle() const;
 };
  
+boost::optional<gtsam::Pose2> align(const gtsam::Point2Pairs& pairs);
+
 #include <gtsam/geometry/Pose3.h>
 class Pose3 {
  // Standard Constructors
--- a/gtsam/inference/BayesTreeCliqueBase.h
+++ b/gtsam/inference/BayesTreeCliqueBase.h
@ -70,16 +70,23 @@ namespace gtsam {
    /// @name Standard Constructors
    /// @{

-    /** Default constructor */
+    /// Default constructor
    BayesTreeCliqueBase() : problemSize_(1) {}

-    /** Construct from a conditional, leaving parent and child pointers uninitialized */
-    BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional), problemSize_(1) {}
+    /// Construct from a conditional, leaving parent and child pointers
+    /// uninitialized.
+    BayesTreeCliqueBase(const sharedConditional& conditional)
+        : conditional_(conditional), problemSize_(1) {}

-    /** Shallow copy constructor */
-    BayesTreeCliqueBase(const BayesTreeCliqueBase& c) : conditional_(c.conditional_), parent_(c.parent_), children(c.children), problemSize_(c.problemSize_), is_root(c.is_root) {}
+    /// Shallow copy constructor.
+    BayesTreeCliqueBase(const BayesTreeCliqueBase& c)
+        : conditional_(c.conditional_),
+          parent_(c.parent_),
+          children(c.children),
+          problemSize_(c.problemSize_),
+          is_root(c.is_root) {}

-    /** Shallow copy assignment constructor */
+    /// Shallow copy assignment constructor
    BayesTreeCliqueBase& operator=(const BayesTreeCliqueBase& c) {
      conditional_ = c.conditional_;
      parent_ = c.parent_;
@ -89,6 +96,9 @@ namespace gtsam {
      return *this;
    }

+    // Virtual destructor.
+    virtual ~BayesTreeCliqueBase() {}
+
    /// @}

    /// This stores the Cached separator marginal P(S)
@ -119,7 +129,9 @@ namespace gtsam {
    bool equals(const DERIVED& other, double tol = 1e-9) const;

    /** print this node */
-    virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+    virtual void print(
+        const std::string& s = "",
+        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;

    /// @}
    /// @name Standard Interface
--- a/gtsam/inference/Key.h
+++ b/gtsam/inference/Key.h
@ -32,7 +32,7 @@
 namespace gtsam {

 /// Typedef for a function to format a key, i.e. to convert it to a string
-typedef std::function<std::string(Key)> KeyFormatter;
+using KeyFormatter = std::function<std::string(Key)>;

 // Helper function for DefaultKeyFormatter
 GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
@ -83,28 +83,32 @@ class key_formatter {
 };

 /// Define collection type once and for all - also used in wrappers
-typedef FastVector<Key> KeyVector;
+using KeyVector = FastVector<Key>;

 // TODO(frank): Nothing fast about these :-(
-typedef FastList<Key> KeyList;
-typedef FastSet<Key> KeySet;
-typedef FastMap<Key, int> KeyGroupMap;
+using KeyList = FastList<Key>;
+using KeySet = FastSet<Key>;
+using KeyGroupMap = FastMap<Key, int>;

 /// Utility function to print one key with optional prefix
-GTSAM_EXPORT void PrintKey(Key key, const std::string& s = "",
-    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+GTSAM_EXPORT void PrintKey(
+    Key key, const std::string &s = "",
+    const KeyFormatter &keyFormatter = DefaultKeyFormatter);

 /// Utility function to print sets of keys with optional prefix
-GTSAM_EXPORT void PrintKeyList(const KeyList& keys, const std::string& s = "",
-    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+GTSAM_EXPORT void PrintKeyList(
+    const KeyList &keys, const std::string &s = "",
+    const KeyFormatter &keyFormatter = DefaultKeyFormatter);

 /// Utility function to print sets of keys with optional prefix
-GTSAM_EXPORT void PrintKeyVector(const KeyVector& keys, const std::string& s =
-    "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+GTSAM_EXPORT void PrintKeyVector(
+    const KeyVector &keys, const std::string &s = "",
+    const KeyFormatter &keyFormatter = DefaultKeyFormatter);

 /// Utility function to print sets of keys with optional prefix
-GTSAM_EXPORT void PrintKeySet(const KeySet& keys, const std::string& s = "",
-    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+GTSAM_EXPORT void PrintKeySet(
+    const KeySet &keys, const std::string &s = "",
+    const KeyFormatter &keyFormatter = DefaultKeyFormatter);

 // Define Key to be Testable by specializing gtsam::traits
 template<typename T> struct traits;
--- a/gtsam/inference/Ordering.cpp
+++ b/gtsam/inference/Ordering.cpp
@ -25,8 +25,12 @@
 #include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>

 #ifdef GTSAM_SUPPORT_NESTED_DISSECTION
+#ifdef GTSAM_USE_SYSTEM_METIS
+#include <metis.h>
+#else
 #include <gtsam/3rdparty/metis/include/metis.h>
 #endif
+#endif

 using namespace std;

--- a/gtsam/linear/linear.i
+++ b/gtsam/linear/linear.i
@ -16,9 +16,9 @@ virtual class Base {
 };

 virtual class Gaussian : gtsam::noiseModel::Base {
-  static gtsam::noiseModel::Gaussian* Information(Matrix R);
-  static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
-  static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
+  static gtsam::noiseModel::Gaussian* Information(Matrix R, bool smart = true);
+  static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R, bool smart = true);
+  static gtsam::noiseModel::Gaussian* Covariance(Matrix R, bool smart = true);

  bool equals(gtsam::noiseModel::Base& expected, double tol);

@ -37,9 +37,9 @@ virtual class Gaussian : gtsam::noiseModel::Base {
 };

 virtual class Diagonal : gtsam::noiseModel::Gaussian {
-  static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
-  static gtsam::noiseModel::Diagonal* Variances(Vector variances);
-  static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
+  static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas, bool smart = true);
+  static gtsam::noiseModel::Diagonal* Variances(Vector variances, bool smart = true);
+  static gtsam::noiseModel::Diagonal* Precisions(Vector precisions, bool smart = true);
  Matrix R() const;

  // access to noise model
@ -69,9 +69,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal {
 };

 virtual class Isotropic : gtsam::noiseModel::Diagonal {
-  static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
-  static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
-  static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
+  static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma, bool smart = true);
+  static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace, bool smart = true);
+  static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision, bool smart = true);

  // access to noise model
  double sigma() const;
@ -289,6 +289,13 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
  JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3,
      Vector b, const gtsam::noiseModel::Diagonal* model);
  JacobianFactor(const gtsam::GaussianFactorGraph& graph);
+  JacobianFactor(const gtsam::GaussianFactorGraph& graph,
+                 const gtsam::VariableSlots& p_variableSlots);
+  JacobianFactor(const gtsam::GaussianFactorGraph& graph,
+                 const gtsam::Ordering& ordering);
+  JacobianFactor(const gtsam::GaussianFactorGraph& graph,
+                 const gtsam::Ordering& ordering,
+                 const gtsam::VariableSlots& p_variableSlots);

  //Testable
  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
--- a/gtsam/navigation/tests/testImuFactor.cpp
+++ b/gtsam/navigation/tests/testImuFactor.cpp
@ -819,7 +819,6 @@ struct ImuFactorMergeTest {
        loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) {
    // arbitrary noise values
    p_->gyroscopeCovariance = I_3x3 * 0.01;
-    p_->accelerometerCovariance = I_3x3 * 0.02;
    p_->accelerometerCovariance = I_3x3 * 0.03;
  }

--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@ -75,12 +75,15 @@ size_t Z(size_t j);
 }  // namespace symbol_shorthand

 // Default keyformatter
-void PrintKeyList(const gtsam::KeyList& keys);
-void PrintKeyList(const gtsam::KeyList& keys, string s);
-void PrintKeyVector(const gtsam::KeyVector& keys);
-void PrintKeyVector(const gtsam::KeyVector& keys, string s);
-void PrintKeySet(const gtsam::KeySet& keys);
-void PrintKeySet(const gtsam::KeySet& keys, string s);
+void PrintKeyList(
+    const gtsam::KeyList& keys, const string& s = "",
+    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
+void PrintKeyVector(
+    const gtsam::KeyVector& keys, const string& s = "",
+    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
+void PrintKeySet(
+    const gtsam::KeySet& keys, const string& s = "",
+    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);

 #include <gtsam/inference/LabeledSymbol.h>
 class LabeledSymbol {
@ -527,7 +530,14 @@ template<PARAMS>
 virtual class GncParams {
  GncParams(const PARAMS& baseOptimizerParams);
  GncParams();
+  void setVerbosityGNC(const This::Verbosity value);
  void print(const string& str) const;
+  
+  enum Verbosity {
+    SILENT,
+    SUMMARY,
+    VALUES
+  };
 };

 typedef gtsam::GncParams<gtsam::GaussNewtonParams> GncGaussNewtonParams;
--- a/gtsam_unstable/partition/FindSeparator-inl.h
+++ b/gtsam_unstable/partition/FindSeparator-inl.h
@ -20,6 +20,8 @@

 #include "FindSeparator.h"

+#ifndef GTSAM_USE_SYSTEM_METIS
+
 extern "C" {
 #include <metis.h>
 #include "metislib.h"
@ -564,3 +566,5 @@ namespace gtsam { namespace partition {
  }

 }} //namespace
+
+#endif
--- a/gtsam_unstable/partition/tests/testFindSeparator.cpp
+++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp
@ -20,6 +20,8 @@ using namespace std;
 using namespace gtsam;
 using namespace gtsam::partition;

+#ifndef GTSAM_USE_SYSTEM_METIS
+
 /* ************************************************************************* */
 // x0 - x1 - x2
 // l3        l4
@ -227,6 +229,8 @@ TEST ( Partition, findSeparator3_with_reduced_camera )
  LONGS_EQUAL(2, partitionTable[28]);
 }

+#endif
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@ -43,6 +43,7 @@ set(ignore
    gtsam::BetweenFactorPose2s
    gtsam::BetweenFactorPose3s
    gtsam::Point2Vector
+    gtsam::Point2Pairs
    gtsam::Point3Pairs
    gtsam::Pose3Pairs
    gtsam::Pose3Vector
@ -166,5 +167,6 @@ add_custom_target(
        COMMAND
          ${CMAKE_COMMAND} -E env # add package to python path so no need to install
          "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}"
-          ${PYTHON_EXECUTABLE} -m unittest discover -s "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests"
-          DEPENDS ${GTSAM_PYTHON_DEPENDENCIES})
+          ${PYTHON_EXECUTABLE} -m unittest discover -v -s .
+          DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}
+          WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests")
--- a/python/gtsam/examples/ImuFactorExample.py
+++ b/python/gtsam/examples/ImuFactorExample.py
@ -10,28 +10,30 @@ A script validating and demonstrating the ImuFactor inference.
 Author: Frank Dellaert, Varun Agrawal
 """

+# pylint: disable=no-name-in-module,unused-import,arguments-differ
+
 from __future__ import print_function

 import argparse
 import math

-import gtsam
 import matplotlib.pyplot as plt
 import numpy as np
+from mpl_toolkits.mplot3d import Axes3D
+
+import gtsam
 from gtsam.symbol_shorthand import B, V, X
 from gtsam.utils.plot import plot_pose3
-from mpl_toolkits.mplot3d import Axes3D

 from PreintegrationExample import POSES_FIG, PreintegrationExample

 BIAS_KEY = B(0)

-
 np.set_printoptions(precision=3, suppress=True)


 class ImuFactorExample(PreintegrationExample):
-
+    """Class to run example of the Imu Factor."""
    def __init__(self, twist_scenario="sick_twist"):
        self.velocity = np.array([2, 0, 0])
        self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
@ -42,9 +44,8 @@ class ImuFactorExample(PreintegrationExample):
            zero_twist=(np.zeros(3), np.zeros(3)),
            forward_twist=(np.zeros(3), self.velocity),
            loop_twist=(np.array([0, -math.radians(30), 0]), self.velocity),
-            sick_twist=(np.array([math.radians(30), -math.radians(30), 0]),
-                        self.velocity)
-        )
+            sick_twist=(np.array([math.radians(30), -math.radians(30),
+                                  0]), self.velocity))

        accBias = np.array([-0.3, 0.1, 0.2])
        gyroBias = np.array([0.1, 0.3, -0.1])
@ -55,19 +56,44 @@ class ImuFactorExample(PreintegrationExample):
                                               bias, dt)

    def addPrior(self, i, graph):
+        """Add priors at time step `i`."""
        state = self.scenario.navState(i)
-        graph.push_back(gtsam.PriorFactorPose3(
-            X(i), state.pose(), self.priorNoise))
-        graph.push_back(gtsam.PriorFactorVector(
-            V(i), state.velocity(), self.velNoise))
+        graph.push_back(
+            gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
+        graph.push_back(
+            gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise))
+
+    def optimize(self, graph, initial):
+        """Optimize using Levenberg-Marquardt optimization."""
+        params = gtsam.LevenbergMarquardtParams()
+        params.setVerbosityLM("SUMMARY")
+        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
+        result = optimizer.optimize()
+        return result
+
+    def plot(self, result):
+        """Plot resulting poses."""
+        i = 0
+        while result.exists(X(i)):
+            pose_i = result.atPose3(X(i))
+            plot_pose3(POSES_FIG + 1, pose_i, 1)
+            i += 1
+        plt.title("Estimated Trajectory")
+
+        gtsam.utils.plot.set_axes_equal(POSES_FIG + 1)
+
+        print("Bias Values", result.atConstantBias(BIAS_KEY))
+
+        plt.ioff()
+        plt.show()

    def run(self, T=12, compute_covariances=False, verbose=True):
+        """Main runner."""
        graph = gtsam.NonlinearFactorGraph()

        # initialize data structure for pre-integrated IMU measurements
        pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)

-        T = 12
        num_poses = T  # assumes 1 factor per second
        initial = gtsam.Values()
        initial.insert(BIAS_KEY, self.actualBias)
@ -91,14 +117,13 @@ class ImuFactorExample(PreintegrationExample):
            if k % 10 == 0:
                self.plotImu(t, measuredOmega, measuredAcc)

-            if (k+1) % int(1 / self.dt) == 0:
+            if (k + 1) % int(1 / self.dt) == 0:
                # Plot every second
                self.plotGroundTruthPose(t, scale=1)
                plt.title("Ground Truth Trajectory")

                # create IMU factor every second
-                factor = gtsam.ImuFactor(X(i), V(i),
-                                         X(i + 1), V(i + 1),
+                factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1),
                                         BIAS_KEY, pim)
                graph.push_back(factor)

@ -108,34 +133,34 @@ class ImuFactorExample(PreintegrationExample):

                pim.resetIntegration()

-                rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1)
-                translationNoise = gtsam.Point3(*np.random.randn(3)*1)
+                rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1)
+                translationNoise = gtsam.Point3(*np.random.randn(3) * 1)
                poseNoise = gtsam.Pose3(rotationNoise, translationNoise)

                actual_state_i = self.scenario.navState(t + self.dt)
                print("Actual state at {0}:\n{1}".format(
-                    t+self.dt, actual_state_i))
+                    t + self.dt, actual_state_i))

                noisy_state_i = gtsam.NavState(
                    actual_state_i.pose().compose(poseNoise),
-                    actual_state_i.velocity() + np.random.randn(3)*0.1)
+                    actual_state_i.velocity() + np.random.randn(3) * 0.1)

-                initial.insert(X(i+1), noisy_state_i.pose())
-                initial.insert(V(i+1), noisy_state_i.velocity())
+                initial.insert(X(i + 1), noisy_state_i.pose())
+                initial.insert(V(i + 1), noisy_state_i.velocity())
                i += 1

        # add priors on end
        self.addPrior(num_poses - 1, graph)

-        initial.print_("Initial values:")
+        initial.print("Initial values:")

-        # optimize using Levenberg-Marquardt optimization
-        params = gtsam.LevenbergMarquardtParams()
-        params.setVerbosityLM("SUMMARY")
-        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
-        result = optimizer.optimize()
+        result = self.optimize(graph, initial)

-        result.print_("Optimized values:")
+        result.print("Optimized values:")
+        print("------------------")
+        print(graph.error(initial))
+        print(graph.error(result))
+        print("------------------")

        if compute_covariances:
            # Calculate and print marginal covariances
@ -148,33 +173,26 @@ class ImuFactorExample(PreintegrationExample):
                print("Covariance on vel {}:\n{}\n".format(
                    i, marginals.marginalCovariance(V(i))))

-        # Plot resulting poses
-        i = 0
-        while result.exists(X(i)):
-            pose_i = result.atPose3(X(i))
-            plot_pose3(POSES_FIG+1, pose_i, 1)
-            i += 1
-        plt.title("Estimated Trajectory")
-
-        gtsam.utils.plot.set_axes_equal(POSES_FIG+1)
-
-        print("Bias Values", result.atConstantBias(BIAS_KEY))
-
-        plt.ioff()
-        plt.show()
+        self.plot(result)


 if __name__ == '__main__':
    parser = argparse.ArgumentParser("ImuFactorExample.py")
    parser.add_argument("--twist_scenario",
                        default="sick_twist",
-                        choices=("zero_twist", "forward_twist", "loop_twist", "sick_twist"))
-    parser.add_argument("--time", "-T", default=12,
-                        type=int, help="Total time in seconds")
+                        choices=("zero_twist", "forward_twist", "loop_twist",
+                                 "sick_twist"))
+    parser.add_argument("--time",
+                        "-T",
+                        default=12,
+                        type=int,
+                        help="Total time in seconds")
    parser.add_argument("--compute_covariances",
-                        default=False, action='store_true')
+                        default=False,
+                        action='store_true')
    parser.add_argument("--verbose", default=False, action='store_true')
    args = parser.parse_args()

-    ImuFactorExample(args.twist_scenario).run(
-        args.time, args.compute_covariances, args.verbose)
+    ImuFactorExample(args.twist_scenario).run(args.time,
+                                              args.compute_covariances,
+                                              args.verbose)
--- a/python/gtsam/gtsam.tpl
+++ b/python/gtsam/gtsam.tpl
@ -1,7 +1,7 @@
 /**
- * @file  gtsam.cpp
+ * @file    {module_name}.cpp
 * @brief   The auto-generated wrapper C++ source code.
- * @author  Duy-Nguyen Ta, Fan Jiang, Matthew Sklar
+ * @author  Duy-Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal
 * @date    Aug. 18, 2020
 *
 * ** THIS FILE IS AUTO-GENERATED, DO NOT MODIFY! **
--- a/python/gtsam/preamble/basis.h
+++ b/python/gtsam/preamble/basis.h
@ -10,3 +10,5 @@
 * Without this they will be automatically converted to a Python object, and all
 * mutations on Python side will not be reflected on C++.
 */
+
+#include <pybind11/stl.h>
--- a/python/gtsam/preamble/geometry.h
+++ b/python/gtsam/preamble/geometry.h
@ -10,9 +10,18 @@
 * Without this they will be automatically converted to a Python object, and all
 * mutations on Python side will not be reflected on C++.
 */
+#include <pybind11/stl.h>
+
+// Support for binding boost::optional types in C++11.
+// https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+namespace pybind11 { namespace detail {
+    template <typename T>
+    struct type_caster<boost::optional<T>> : optional_caster<boost::optional<T>> {};
+}}

 PYBIND11_MAKE_OPAQUE(
    std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>);
+PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs);
 PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
 PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
--- a/python/gtsam/specializations/geometry.h
+++ b/python/gtsam/specializations/geometry.h
@ -14,6 +14,7 @@
 py::bind_vector<
    std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>>(
    m_, "Point2Vector");
+py::bind_vector<std::vector<gtsam::Point2Pair>>(m_, "Point2Pairs");
 py::bind_vector<std::vector<gtsam::Point3Pair>>(m_, "Point3Pairs");
 py::bind_vector<std::vector<gtsam::Pose3Pair>>(m_, "Pose3Pairs");
 py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector");
--- a/python/gtsam/tests/test_Pose2.py
+++ b/python/gtsam/tests/test_Pose2.py
@ -6,27 +6,64 @@ All Rights Reserved
 See LICENSE for the license information

 Pose2 unit tests.
-Author: Frank Dellaert & Duy Nguyen Ta (Python)
+Author: Frank Dellaert & Duy Nguyen Ta & John Lambert
 """
 import unittest

 import numpy as np

 import gtsam
-from gtsam import Pose2
+from gtsam import Point2, Point2Pairs, Pose2
 from gtsam.utils.test_case import GtsamTestCase


 class TestPose2(GtsamTestCase):
    """Test selected Pose2 methods."""
-
-    def test_adjoint(self):
+    def test_adjoint(self) -> None:
        """Test adjoint method."""
        xi = np.array([1, 2, 3])
        expected = np.dot(Pose2.adjointMap_(xi), xi)
        actual = Pose2.adjoint_(xi, xi)
        np.testing.assert_array_equal(actual, expected)

+    def test_align(self) -> None:
+        """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds.
+
+        Two point clouds represent horseshoe-shapes of the same size, just rotated and translated:
+
+                |  X---X
+                |  |
+                |  X---X
+        ------------------
+                |
+                |
+              O | O
+              | | |
+              O---O
+        """
+        pts_a = [
+            Point2(3, 1),
+            Point2(1, 1),
+            Point2(1, 3),
+            Point2(3, 3),
+        ]
+        pts_b = [
+            Point2(1, -3),
+            Point2(1, -5),
+            Point2(-1, -5),
+            Point2(-1, -3),
+        ]
+
+        # fmt: on
+        ab_pairs = Point2Pairs(list(zip(pts_a, pts_b)))
+        bTa = gtsam.align(ab_pairs)
+        aTb = bTa.inverse()
+        assert aTb is not None
+
+        for pt_a, pt_b in zip(pts_a, pts_b):
+            pt_a_ = aTb.transformFrom(pt_b)
+            assert np.allclose(pt_a, pt_a_)
+

 if __name__ == "__main__":
    unittest.main()
--- a/python/gtsam/tests/test_basis.py
+++ b/python/gtsam/tests/test_basis.py
@ -0,0 +1,96 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Basis unit tests.
+Author: Frank Dellaert & Gerry Chen (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+from gtsam.symbol_shorthand import B
+
+
+class TestBasis(GtsamTestCase):
+    """
+    Tests FitBasis python binding for FourierBasis, Chebyshev1Basis, Chebyshev2Basis, and Chebyshev2.
+    
+    It tests FitBasis by fitting to a ground-truth function that can be represented exactly in
+    the basis, then checking that the regression (fit result) matches the function.  For the
+    Chebyshev bases, the line y=x is used to generate the data while for Fourier, 0.7*cos(x) is
+    used.
+    """
+    def setUp(self):
+        self.N = 2
+        self.x = [0., 0.5, 0.75]
+        self.interpx = np.linspace(0., 1., 10)
+        self.noise = gtsam.noiseModel.Unit.Create(1)
+
+    def evaluate(self, basis, fitparams, x):
+        """
+        Until wrapper for Basis functors are ready,
+        this is how to evaluate a basis function.
+        """
+        return basis.WeightMatrix(self.N, x) @ fitparams
+
+    def fit_basis_helper(self, fitter, basis, f=lambda x: x):
+        """Helper method to fit data to a specified fitter using a specified basis."""
+        data = {x: f(x) for x in self.x}
+        fit = fitter(data, self.noise, self.N)
+        coeff = fit.parameters()
+        interpy = self.evaluate(basis, coeff, self.interpx)
+        return interpy
+
+    def test_fit_basis_fourier(self):
+        """Fit a Fourier basis."""
+
+        f = lambda x: 0.7 * np.cos(x)
+        interpy = self.fit_basis_helper(gtsam.FitBasisFourierBasis,
+                                        gtsam.FourierBasis, f)
+        # test a basis by checking that the fit result matches the function at x-values interpx.
+        np.testing.assert_almost_equal(interpy,
+                                       np.array([f(x) for x in self.interpx]),
+                                       decimal=7)
+
+    def test_fit_basis_chebyshev1basis(self):
+        """Fit a Chebyshev1 basis."""
+
+        f = lambda x: x
+        interpy = self.fit_basis_helper(gtsam.FitBasisChebyshev1Basis,
+                                        gtsam.Chebyshev1Basis, f)
+        # test a basis by checking that the fit result matches the function at x-values interpx.
+        np.testing.assert_almost_equal(interpy,
+                                       np.array([f(x) for x in self.interpx]),
+                                       decimal=7)
+
+    def test_fit_basis_chebyshev2basis(self):
+        """Fit a Chebyshev2 basis."""
+
+        f = lambda x: x
+        interpy = self.fit_basis_helper(gtsam.FitBasisChebyshev2Basis,
+                                        gtsam.Chebyshev2Basis)
+        # test a basis by checking that the fit result matches the function at x-values interpx.
+        np.testing.assert_almost_equal(interpy,
+                                       np.array([f(x) for x in self.interpx]),
+                                       decimal=7)
+
+    def test_fit_basis_chebyshev2(self):
+        """Fit a Chebyshev2 pseudospectral basis."""
+
+        f = lambda x: x
+        interpy = self.fit_basis_helper(gtsam.FitBasisChebyshev2,
+                                        gtsam.Chebyshev2)
+        # test a basis by checking that the fit result matches the function at x-values interpx.
+        np.testing.assert_almost_equal(interpy,
+                                       np.array([f(x) for x in self.interpx]),
+                                       decimal=7)
+
+
+if __name__ == "__main__":
+    unittest.main()
--- a/python/gtsam/utils/plot.py
+++ b/python/gtsam/utils/plot.py
@ -23,7 +23,10 @@ def set_axes_equal(fignum: int) -> None:
      fignum: An integer representing the figure number for Matplotlib.
    """
    fig = plt.figure(fignum)
-    ax = fig.gca(projection='3d')
+    if not fig.axes:
+        ax = fig.add_subplot(projection='3d')
+    else:
+        ax = fig.axes[0]

    limits = np.array([
        ax.get_xlim3d(),
@ -339,7 +342,11 @@ def plot_pose3(
    """
    # get figure object
    fig = plt.figure(fignum)
-    axes = fig.gca(projection='3d')
+    if not fig.axes:
+        axes = fig.add_subplot(projection='3d')
+    else:
+        axes = fig.axes[0]
+
    plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length)

    axes.set_xlabel(axis_labels[0])
--- a/wrap/gtwrap/interface_parser/classes.py
+++ b/wrap/gtwrap/interface_parser/classes.py
@ -119,24 +119,27 @@ class Constructor:
    Can have 0 or more arguments.
    """
    rule = (
-        IDENT("name")  #
+        Optional(Template.rule("template"))  #
+        + IDENT("name")  #
        + LPAREN  #
        + ArgumentList.rule("args_list")  #
        + RPAREN  #
        + SEMI_COLON  # BR
-    ).setParseAction(lambda t: Constructor(t.name, t.args_list))
+    ).setParseAction(lambda t: Constructor(t.name, t.args_list, t.template))

    def __init__(self,
                 name: str,
                 args: ArgumentList,
+                 template: Union[Template, Any],
                 parent: Union["Class", Any] = ''):
        self.name = name
        self.args = args
+        self.template = template

        self.parent = parent

    def __repr__(self) -> str:
-        return "Constructor: {}".format(self.name)
+        return "Constructor: {}{}".format(self.name, self.args)


 class Operator:
@ -260,17 +263,9 @@ class Class:
        + RBRACE  #
        + SEMI_COLON  # BR
    ).setParseAction(lambda t: Class(
-        t.template,
-        t.is_virtual,
-        t.name,
-        t.parent_class,
-        t.members.ctors,
-        t.members.methods,
-        t.members.static_methods,
-        t.members.properties,
-        t.members.operators,
-        t.members.enums
-    ))
+        t.template, t.is_virtual, t.name, t.parent_class, t.members.ctors, t.
+        members.methods, t.members.static_methods, t.members.properties, t.
+        members.operators, t.members.enums))

    def __init__(
        self,
--- a/wrap/gtwrap/interface_parser/function.py
+++ b/wrap/gtwrap/interface_parser/function.py
@ -81,7 +81,7 @@ class ArgumentList:
            return ArgumentList([])

    def __repr__(self) -> str:
-        return self.args_list.__repr__()
+        return repr(tuple(self.args_list))

    def __len__(self) -> int:
        return len(self.args_list)
--- a/wrap/gtwrap/template_instantiator.py
+++ b/wrap/gtwrap/template_instantiator.py
@ -41,6 +41,8 @@ def instantiate_type(ctype: parser.Type,

    str_arg_typename = str(ctype.typename)

+    # Instantiate templates which have enumerated instantiations in the template.
+    # E.g. `template<T={double}>`.
    if str_arg_typename in template_typenames:
        idx = template_typenames.index(str_arg_typename)
        return parser.Type(
@ -51,14 +53,15 @@ def instantiate_type(ctype: parser.Type,
            is_ref=ctype.is_ref,
            is_basic=ctype.is_basic,
        )
+
+    # If a method has the keyword `This`, we replace it with the (instantiated) class.
    elif str_arg_typename == 'This':
+        # Check if the class is template instantiated
+        # so we can replace it with the instantiated version.
        if instantiated_class:
            name = instantiated_class.original.name
            namespaces_name = instantiated_class.namespaces()
            namespaces_name.append(name)
-            # print("INST: {}, {}, CPP: {}, CLS: {}".format(
-            #     ctype, instantiations, cpp_typename, instantiated_class.instantiations
-            # ), file=sys.stderr)
            cpp_typename = parser.Typename(
                namespaces_name,
                instantiations=instantiated_class.instantiations)
@ -71,6 +74,14 @@ def instantiate_type(ctype: parser.Type,
            is_ref=ctype.is_ref,
            is_basic=ctype.is_basic,
        )
+
+    # Case when 'This' is present in the type namespace, e.g `This::Subclass`.
+    elif 'This' in str_arg_typename:
+        # Simply get the index of `This` in the namespace and replace it with the instantiated name.
+        namespace_idx = ctype.typename.namespaces.index('This')
+        ctype.typename.namespaces[namespace_idx] = cpp_typename.name
+        return ctype
+
    else:
        return ctype

@ -368,21 +379,47 @@ class InstantiatedClass(parser.Class):
        """
        instantiated_ctors = []

-        for ctor in self.original.ctors:
+        def instantiate(instantiated_ctors, ctor, typenames, instantiations):
            instantiated_args = instantiate_args_list(
                ctor.args.list(),
                typenames,
-                self.instantiations,
+                instantiations,
                self.cpp_typename(),
            )
            instantiated_ctors.append(
                parser.Constructor(
                    name=self.name,
                    args=parser.ArgumentList(instantiated_args),
+                    template=self.original.template,
                    parent=self,
                ))
            return instantiated_ctors

+        for ctor in self.original.ctors:
+            # Add constructor templates to the typenames and instantiations
+            if isinstance(ctor.template, parser.template.Template):
+                typenames.extend(ctor.template.typenames)
+
+                # Get all combinations of template args
+                for instantiations in itertools.product(
+                        *ctor.template.instantiations):
+                    instantiations = self.instantiations + list(instantiations)
+
+                    instantiated_ctors = instantiate(
+                        instantiated_ctors,
+                        ctor,
+                        typenames=typenames,
+                        instantiations=instantiations)
+
+            else:
+                # If no constructor level templates, just use the class templates
+                instantiated_ctors = instantiate(
+                    instantiated_ctors,
+                    ctor,
+                    typenames=typenames,
+                    instantiations=self.instantiations)
+        return instantiated_ctors
+
    def instantiate_static_methods(self, typenames):
        """
        Instantiate static methods in the class.
--- a/wrap/tests/expected/matlab/MyFactorPosePoint2.m
+++ b/wrap/tests/expected/matlab/MyFactorPosePoint2.m
@ -15,9 +15,9 @@ classdef MyFactorPosePoint2 < handle
    function obj = MyFactorPosePoint2(varargin)
      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
        my_ptr = varargin{2};
-        class_wrapper(56, my_ptr);
+        class_wrapper(62, 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 = class_wrapper(57, varargin{1}, varargin{2}, varargin{3}, varargin{4});
+        my_ptr = class_wrapper(63, varargin{1}, varargin{2}, varargin{3}, varargin{4});
      else
        error('Arguments do not match any overload of MyFactorPosePoint2 constructor');
      end
@ -25,7 +25,7 @@ classdef MyFactorPosePoint2 < handle
    end

    function delete(obj)
-      class_wrapper(58, obj.ptr_MyFactorPosePoint2);
+      class_wrapper(64, obj.ptr_MyFactorPosePoint2);
    end

    function display(obj), obj.print(''); end
@ -36,7 +36,7 @@ classdef MyFactorPosePoint2 < handle
      % PRINT usage: print(string s, KeyFormatter keyFormatter) : returns void
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.KeyFormatter')
-        class_wrapper(59, this, varargin{:});
+        class_wrapper(65, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function MyFactorPosePoint2.print');
--- a/wrap/tests/expected/matlab/class_wrapper.cpp
+++ b/wrap/tests/expected/matlab/class_wrapper.cpp
@ -33,6 +33,8 @@ typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_Multip
 static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
 typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
 static Collector_ForwardKinematics collector_ForwardKinematics;
+typedef std::set<boost::shared_ptr<TemplatedConstructor>*> Collector_TemplatedConstructor;
+static Collector_TemplatedConstructor collector_TemplatedConstructor;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;

@ -97,6 +99,12 @@ void _deleteAllObjects()
    collector_ForwardKinematics.erase(iter++);
    anyDeleted = true;
  } }
+  { for(Collector_TemplatedConstructor::iterator iter = collector_TemplatedConstructor.begin();
+      iter != collector_TemplatedConstructor.end(); ) {
+    delete *iter;
+    collector_TemplatedConstructor.erase(iter++);
+    anyDeleted = true;
+  } }
  { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
      iter != collector_MyFactorPosePoint2.end(); ) {
    delete *iter;
@ -682,7 +690,76 @@ void ForwardKinematics_deconstructor_55(int nargout, mxArray *out[], int nargin,
  }
 }

-void MyFactorPosePoint2_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void TemplatedConstructor_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<TemplatedConstructor> Shared;
+
+  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
+  collector_TemplatedConstructor.insert(self);
+}
+
+void TemplatedConstructor_constructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<TemplatedConstructor> Shared;
+
+  Shared *self = new Shared(new TemplatedConstructor());
+  collector_TemplatedConstructor.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+}
+
+void TemplatedConstructor_constructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<TemplatedConstructor> Shared;
+
+  string& arg = *unwrap_shared_ptr< string >(in[0], "ptr_string");
+  Shared *self = new Shared(new TemplatedConstructor(arg));
+  collector_TemplatedConstructor.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+}
+
+void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<TemplatedConstructor> Shared;
+
+  int arg = unwrap< int >(in[0]);
+  Shared *self = new Shared(new TemplatedConstructor(arg));
+  collector_TemplatedConstructor.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+}
+
+void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<TemplatedConstructor> Shared;
+
+  double arg = unwrap< double >(in[0]);
+  Shared *self = new Shared(new TemplatedConstructor(arg));
+  collector_TemplatedConstructor.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+}
+
+void TemplatedConstructor_deconstructor_61(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<TemplatedConstructor> Shared;
+  checkArguments("delete_TemplatedConstructor",nargout,nargin,1);
+  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
+  Collector_TemplatedConstructor::iterator item;
+  item = collector_TemplatedConstructor.find(self);
+  if(item != collector_TemplatedConstructor.end()) {
+    delete self;
+    collector_TemplatedConstructor.erase(item);
+  }
+}
+
+void MyFactorPosePoint2_collectorInsertAndMakeBase_62(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@ -691,7 +768,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_56(int nargout, mxArray *out[
  collector_MyFactorPosePoint2.insert(self);
 }

-void MyFactorPosePoint2_constructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_constructor_63(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@ -706,7 +783,7 @@ void MyFactorPosePoint2_constructor_57(int nargout, mxArray *out[], int nargin,
  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }

-void MyFactorPosePoint2_deconstructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_deconstructor_64(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);
@ -719,7 +796,7 @@ void MyFactorPosePoint2_deconstructor_58(int nargout, mxArray *out[], int nargin
  }
 }

-void MyFactorPosePoint2_print_59(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_print_65(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("print",nargout,nargin-1,2);
  auto obj = unwrap_shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>>(in[0], "ptr_MyFactorPosePoint2");
@ -909,16 +986,34 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
      ForwardKinematics_deconstructor_55(nargout, out, nargin-1, in+1);
      break;
    case 56:
-      MyFactorPosePoint2_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1);
+      TemplatedConstructor_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1);
      break;
    case 57:
-      MyFactorPosePoint2_constructor_57(nargout, out, nargin-1, in+1);
+      TemplatedConstructor_constructor_57(nargout, out, nargin-1, in+1);
      break;
    case 58:
-      MyFactorPosePoint2_deconstructor_58(nargout, out, nargin-1, in+1);
+      TemplatedConstructor_constructor_58(nargout, out, nargin-1, in+1);
      break;
    case 59:
-      MyFactorPosePoint2_print_59(nargout, out, nargin-1, in+1);
+      TemplatedConstructor_constructor_59(nargout, out, nargin-1, in+1);
+      break;
+    case 60:
+      TemplatedConstructor_constructor_60(nargout, out, nargin-1, in+1);
+      break;
+    case 61:
+      TemplatedConstructor_deconstructor_61(nargout, out, nargin-1, in+1);
+      break;
+    case 62:
+      MyFactorPosePoint2_collectorInsertAndMakeBase_62(nargout, out, nargin-1, in+1);
+      break;
+    case 63:
+      MyFactorPosePoint2_constructor_63(nargout, out, nargin-1, in+1);
+      break;
+    case 64:
+      MyFactorPosePoint2_deconstructor_64(nargout, out, nargin-1, in+1);
+      break;
+    case 65:
+      MyFactorPosePoint2_print_65(nargout, out, nargin-1, in+1);
      break;
    }
  } catch(const std::exception& e) {
--- a/wrap/tests/expected/python/class_pybind.cpp
+++ b/wrap/tests/expected/python/class_pybind.cpp
@ -86,6 +86,12 @@ PYBIND11_MODULE(class_py, m_) {
    py::class_<ForwardKinematics, std::shared_ptr<ForwardKinematics>>(m_, "ForwardKinematics")
        .def(py::init<const gtdynamics::Robot&, const string&, const string&, const gtsam::Values&, const gtsam::Pose3&>(), py::arg("robot"), py::arg("start_link_name"), py::arg("end_link_name"), py::arg("joint_angles"), py::arg("l2Tp") = gtsam::Pose3());

+    py::class_<TemplatedConstructor, std::shared_ptr<TemplatedConstructor>>(m_, "TemplatedConstructor")
+        .def(py::init<>())
+        .def(py::init<const string&>(), py::arg("arg"))
+        .def(py::init<const int&>(), py::arg("arg"))
+        .def(py::init<const double&>(), py::arg("arg"));
+
    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"))
        .def("print",[](MyFactor<gtsam::Pose2, gtsam::Matrix>* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter)
--- a/wrap/tests/expected/python/enum_pybind.cpp
+++ b/wrap/tests/expected/python/enum_pybind.cpp
@ -69,6 +69,16 @@ PYBIND11_MODULE(enum_py, m_) {
        .value("Groot", gtsam::MCU::GotG::Groot);


+    py::class_<gtsam::Optimizer<gtsam::GaussNewtonParams>, std::shared_ptr<gtsam::Optimizer<gtsam::GaussNewtonParams>>> optimizergaussnewtonparams(m_gtsam, "OptimizerGaussNewtonParams");
+    optimizergaussnewtonparams
+        .def("setVerbosity",[](gtsam::Optimizer<gtsam::GaussNewtonParams>* self, const Optimizer<gtsam::GaussNewtonParams>::Verbosity value){ self->setVerbosity(value);}, py::arg("value"));
+
+    py::enum_<gtsam::Optimizer<gtsam::GaussNewtonParams>::Verbosity>(optimizergaussnewtonparams, "Verbosity", py::arithmetic())
+        .value("SILENT", gtsam::Optimizer<gtsam::GaussNewtonParams>::Verbosity::SILENT)
+        .value("SUMMARY", gtsam::Optimizer<gtsam::GaussNewtonParams>::Verbosity::SUMMARY)
+        .value("VERBOSE", gtsam::Optimizer<gtsam::GaussNewtonParams>::Verbosity::VERBOSE);
+
+

 #include "python/specializations.h"

--- a/wrap/tests/fixtures/class.i
+++ b/wrap/tests/fixtures/class.i
@ -7,6 +7,7 @@ class FunRange {

 template<M={double}>
 class Fun {
+
  static This staticMethodWithThis();

  template<T={string}>
@ -118,5 +119,14 @@ class ForwardKinematics {
                    const gtsam::Pose3& l2Tp = gtsam::Pose3());
 };

+// Test for templated constructor
+class TemplatedConstructor {
+  TemplatedConstructor();
+
+  template<T={string, int, double}>
+  TemplatedConstructor(const T& arg);
+};
+
+
 class SuperCoolFactor;
 typedef SuperCoolFactor<gtsam::Pose3> SuperCoolFactorPose3;
--- a/wrap/tests/fixtures/enum.i
+++ b/wrap/tests/fixtures/enum.i
@ -42,4 +42,17 @@ class MCU {

 };

+template<PARAMS>
+class Optimizer {
+  enum Verbosity {
+    SILENT,
+    SUMMARY,
+    VERBOSE
+  };
+
+  void setVerbosity(const This::Verbosity value);
+};
+
+typedef gtsam::Optimizer<gtsam::GaussNewtonParams> OptimizerGaussNewtonParams;
+
 }  // namespace gtsam
--- a/wrap/tests/test_interface_parser.py
+++ b/wrap/tests/test_interface_parser.py
@ -314,6 +314,25 @@ class TestInterfaceParser(unittest.TestCase):
        self.assertEqual(5, len(ret.args))
        self.assertEqual("gtsam::Pose3()", ret.args.list()[4].default)

+    def test_constructor_templated(self):
+        """Test for templated class constructor."""
+        f = """
+        template<T = {double, int}>
+        Class();
+        """
+        ret = Constructor.rule.parseString(f)[0]
+        self.assertEqual("Class", ret.name)
+        self.assertEqual(0, len(ret.args))
+
+        f = """
+        template<T = {double, int}>
+        Class(const T& name);
+        """
+        ret = Constructor.rule.parseString(f)[0]
+        self.assertEqual("Class", ret.name)
+        self.assertEqual(1, len(ret.args))
+        self.assertEqual("const T & name", ret.args.args_list[0].to_cpp())
+
    def test_operator_overload(self):
        """Test for operator overloading."""
        # Unary operator