Merge branch 'borglab:develop' into develop

.github/scripts/boost.sh

@@ -9,10 +9,10 @@ tar -zxf ${BOOST_FOLDER}.tar.gz
 
 # Bootstrap
 cd ${BOOST_FOLDER}/
-./bootstrap.sh
+./bootstrap.sh --with-libraries=serialization,filesystem,thread,system,atomic,date_time,timer,chrono,program_options,regex
 
 # Build and install
-sudo ./b2 install
+sudo ./b2 -j$(nproc) install
 
 # Rebuild ld cache
 sudo ldconfig

.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

.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 \
@@ -92,7 +94,11 @@ function build ()
 
   configure
 
-  make -j2
+  if [ "$(uname)" == "Linux" ]; then
+    make -j$(nproc)
+  elif [ "$(uname)" == "Darwin" ]; then
+    make -j$(sysctl -n hw.physicalcpu)
+  fi
 
   finish
 }
@@ -105,8 +111,12 @@ function test ()
 
   configure
 
-  # Actual build:
+  # Actual testing
-  make -j2 check
+  if [ "$(uname)" == "Linux" ]; then
+    make -j$(nproc) check
+  elif [ "$(uname)" == "Darwin" ]; then
+    make -j$(sysctl -n hw.physicalcpu) check
+  fi
 
   finish
 }

.github/workflows/build-linux.yml

@@ -47,8 +47,7 @@ jobs:
       - name: Checkout
         uses: actions/checkout@v2
 
-      - name: Install (Linux)
+      - name: Install Dependencies
-        if: runner.os == 'Linux'
         run: |
           # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository.
           if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
@@ -63,7 +62,7 @@ jobs:
           fi
           sudo apt-get -y update
 
-          sudo apt-get install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
+          sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
 
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
@@ -79,7 +78,5 @@ jobs:
         run: |
           bash .github/scripts/boost.sh
 
-      - name: Build and Test (Linux)
+      - name: Build and Test
-        if: runner.os == 'Linux'
+        run: bash .github/scripts/unix.sh -t
-        run: |
-          bash .github/scripts/unix.sh -t

.github/workflows/build-macos.yml

@@ -34,8 +34,7 @@ jobs:
       - name: Checkout
         uses: actions/checkout@v2
 
-      - name: Install (macOS)
+      - name: Install Dependencies
-        if: runner.os == 'macOS'
         run: |
           brew install cmake ninja
           brew install boost
@@ -48,7 +47,5 @@ jobs:
             echo "CC=clang" >> $GITHUB_ENV
             echo "CXX=clang++" >> $GITHUB_ENV
           fi
-      - name: Build and Test (macOS)
+      - name: Build and Test
-        if: runner.os == 'macOS'
+        run: bash .github/scripts/unix.sh -t
-        run: |
-          bash .github/scripts/unix.sh -t

.github/workflows/build-python.yml

@@ -19,22 +19,20 @@ jobs:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          # ubuntu-18.04-gcc-5,  #TODO Enable once the Python wrapper is optimized for memory
+          ubuntu-18.04-gcc-5,
           ubuntu-18.04-gcc-9,
           ubuntu-18.04-clang-9,
           macOS-10.15-xcode-11.3.1,
-          # ubuntu-18.04-gcc-5-tbb,
+          ubuntu-18.04-gcc-5-tbb,
         ]
 
-        #TODO update wrapper to prevent OOM
+        build_type: [Debug, Release]
-        # build_type: [Debug, Release]
-        build_type: [Release]
         python_version: [3]
         include:
-          # - name: ubuntu-18.04-gcc-5
+          - name: ubuntu-18.04-gcc-5
-          #   os: ubuntu-18.04
+            os: ubuntu-18.04
-          #   compiler: gcc
+            compiler: gcc
-          #   version: "5"
+            version: "5"
 
           - name: ubuntu-18.04-gcc-9
             os: ubuntu-18.04
@@ -46,7 +44,7 @@ jobs:
             compiler: clang
             version: "9"
 
-          #NOTE temporarily added this as it is a required check.
+          # NOTE temporarily added this as it is a required check.
           - name: ubuntu-18.04-clang-9
             os: ubuntu-18.04
             compiler: clang
@@ -59,11 +57,11 @@ jobs:
             compiler: xcode
             version: "11.3.1"
 
-          # - name: ubuntu-18.04-gcc-5-tbb
+          - name: ubuntu-18.04-gcc-5-tbb
-          #   os: ubuntu-18.04
+            os: ubuntu-18.04
-          #   compiler: gcc
+            compiler: gcc
-          #   version: "5"
+            version: "5"
-          #   flag: tbb
+            flag: tbb
 
     steps:
       - name: Checkout
@@ -83,7 +81,7 @@ jobs:
           fi
           sudo apt-get -y update
           
-          sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
+          sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
           
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib

.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
@@ -70,7 +76,7 @@ jobs:
           fi
           sudo apt-get -y update
 
-          sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
+          sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
 
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
@@ -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

.github/workflows/build-windows.yml

@@ -12,42 +12,46 @@ jobs:
       CTEST_PARALLEL_LEVEL: 2
       CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
       GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
+      BOOST_VERSION: 1.72.0
+      BOOST_EXE: boost_1_72_0-msvc-14.2
+
     strategy:
       fail-fast: false
       matrix:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          #TODO This build keeps timing out, need to understand why.
+            #TODO This build fails, need to understand why.
-          # windows-2016-cl,
+            # windows-2016-cl,
-          windows-2019-cl,
+            windows-2019-cl,
-        ]
+          ]
 
         build_type: [Debug, Release]
         build_unstable: [ON]
         include:
-
+          #TODO This build fails, need to understand why.
-          #TODO This build keeps timing out, need to understand why.
           # - name: windows-2016-cl
           #   os: windows-2016
           #   compiler: cl
+          #   platform: 32
 
           - name: windows-2019-cl
             os: windows-2019
             compiler: cl
+            platform: 64
 
     steps:
-      - name: Checkout
+      - name: Install Dependencies
-        uses: actions/checkout@v2
-      - name: Install (Windows)
-        if: runner.os == 'Windows'
         shell: powershell
         run: |
           Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh')
+          scoop install cmake --global  # So we don't get issues with CMP0074 policy
           scoop install ninja --global
+
           if ("${{ matrix.compiler }}".StartsWith("clang")) {
             scoop install llvm --global
           }
+
           if ("${{ matrix.compiler }}" -eq "gcc") {
             # Chocolatey GCC is broken on the windows-2019 image.
             # See: https://github.com/DaanDeMeyer/doctest/runs/231595515
@@ -55,27 +59,38 @@ jobs:
             scoop install gcc --global
             echo "CC=gcc" >> $GITHUB_ENV
             echo "CXX=g++" >> $GITHUB_ENV
+
           } elseif ("${{ matrix.compiler }}" -eq "clang") {
             echo "CC=clang" >> $GITHUB_ENV
             echo "CXX=clang++" >> $GITHUB_ENV
+
           } else {
-            echo "CC=${{ matrix.compiler }}" >> $GITHUB_ENV
+            echo "CC=${{ matrix.compiler }}" >> $env:GITHUB_ENV
-            echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV
+            echo "CXX=${{ matrix.compiler }}" >> $env:GITHUB_ENV
+
           }
+
           # Scoop modifies the PATH so we make the modified PATH global.
-          echo "$env:PATH" >> $GITHUB_PATH
+          echo "$env:PATH" >> $env:GITHUB_PATH
 
-      - name: Download and install Boost
+      - name: Install Boost
-        uses: MarkusJx/install-boost@v1.0.1
+        shell: powershell
-        id: install-boost
+        run: |
-        with:
+          # Snippet from: https://github.com/actions/virtual-environments/issues/2667
-          boost_version: 1.72.0
+          $BOOST_PATH = "C:\hostedtoolcache\windows\Boost\$env:BOOST_VERSION\x86_64"
-          toolset: msvc14.2
 
-      - name: Build (Windows)
+          # Use the prebuilt binary for Windows
-        if: runner.os == 'Windows'
+          $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE-${{matrix.platform}}.exe"
-        env:
+          (New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe")
-          BOOST_ROOT: ${{ steps.install-boost.outputs.BOOST_ROOT }}
+          Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=$BOOST_PATH"
+
+          # Set the BOOST_ROOT variable
+          echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV
+
+      - name: Checkout
+        uses: actions/checkout@v2
+
+      - name: Build
         run: |
           cmake -E remove_directory build
           cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib"

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

DEVELOP.md

@@ -17,3 +17,5 @@ class GTSAM_EXPORT MyClass { ... };
 
 GTSAM_EXPORT myFunction();
 ```
+
+More details [here](Using-GTSAM-EXPORT.md).

INSTALL.md

@@ -13,16 +13,18 @@ $ make install
 ## Important Installation Notes
 
 1. GTSAM requires the following libraries to be installed on your system:
-    - BOOST version 1.58 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
 
     Optional dependent libraries:
      - If TBB is installed and detectable by CMake GTSAM will use it automatically.
        Ensure that CMake prints "Use Intel TBB : Yes".  To disable the use of TBB,
-       disable the CMake flag GTSAM_WITH_TBB (enabled by default).  On Ubuntu, TBB
+       disable the CMake flag `GTSAM_WITH_TBB` (enabled by default) by providing
-       may be installed from the Ubuntu repositories, and for other platforms it
+       the argument `-DGTSAM_WITH_TBB=OFF` to `cmake`.  On Ubuntu, TBB may be
-       may be downloaded from https://www.threadingbuildingblocks.org/
+       installed from the Ubuntu repositories, and for other platforms it may be
+       downloaded from https://www.threadingbuildingblocks.org/
      - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and
        `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually
        achieved with MKL disabled. We therefore advise you to benchmark your problem 
@@ -65,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.67 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

README.md

@@ -40,7 +40,7 @@ $ make install
 
 Prerequisites:
 
-- [Boost](http://www.boost.org/users/download/) >= 1.58 (Ubuntu: `sudo apt-get install libboost-all-dev`)
+- [Boost](http://www.boost.org/users/download/) >= 1.65 (Ubuntu: `sudo apt-get install libboost-all-dev`)
 - [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`)
 - A modern compiler, i.e., at least gcc 4.7.3 on Linux.
 
@@ -55,9 +55,9 @@ Optional prerequisites - used automatically if findable by CMake:
 
 GTSAM 4 introduces several new features, most notably Expressions and a Python toolbox. It also introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 is deprecated, so please be aware that this might render functions using their default constructor incorrect.
 
-GTSAM 4 also deprecated some legacy functionality and wrongly named methods. If you are on a 4.0.X release, you can define the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 to use the deprecated methods.
+GTSAM 4 also deprecated some legacy functionality and wrongly named methods. If you are on a 4.0.X release, you can define the flag `GTSAM_ALLOW_DEPRECATED_SINCE_V4` to use the deprecated methods.
 
-GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag GTSAM_ALLOW_DEPRECATED_SINCE_V41 for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile.
+GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag `GTSAM_ALLOW_DEPRECATED_SINCE_V41` for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile.
 
 
 ## Wrappers
@@ -68,16 +68,16 @@ We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md)
 
 GTSAM includes a state of the art IMU handling scheme based on
 
-- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. [[link]](https://ieeexplore.ieee.org/document/6092505)
+- Todd Lupton and Salah Sukkarieh, _"Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions"_, TRO, 28(1):61-76, 2012. [[link]](https://ieeexplore.ieee.org/document/6092505)
 
 Our implementation improves on this using integration on the manifold, as detailed in
 
-- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483)
+- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, _"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors"_, Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483)
 - Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf)
 
 If you are using the factor in academic work, please cite the publications above.
 
-In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in [this document](doc/ImuFactor.pdf), is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
+In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in [this document](doc/ImuFactor.pdf), is enabled by default. To switch to the RSS 2015 version, set the flag `GTSAM_TANGENT_PREINTEGRATION` to OFF.
 
 
 ## Additional Information

Using-GTSAM-EXPORT.md

@@ -10,7 +10,7 @@ To create a DLL in windows, the `GTSAM_EXPORT` keyword has been created and need
 3.  If you have defined a class using `GTSAM_EXPORT`, do not use `GTSAM_EXPORT` in any of its individual function declarations.  (Note that you _can_ put `GTSAM_EXPORT` in the definition of individual functions within a class as long as you don't put `GTSAM_EXPORT` in the class definition.)
 
 ## When is GTSAM_EXPORT being used incorrectly
-Unfortunately, using `GTSAM_EXPORT` incorrectly often does not cause a compiler or linker error in the library that is being compiled, but only when you try to use that DLL in a different library.  For example, an error in gtsam/base will often show up when compiling the check_base_program or the MATLAB wrapper, but not when compiling/linking gtsam itself.  The most common errors will say something like:
+Unfortunately, using `GTSAM_EXPORT` incorrectly often does not cause a compiler or linker error in the library that is being compiled, but only when you try to use that DLL in a different library.  For example, an error in `gtsam/base` will often show up when compiling the `check_base_program` or the MATLAB wrapper, but not when compiling/linking gtsam itself.  The most common errors will say something like:
 
 ```
 Error	LNK2019	unresolved external symbol "public: void __cdecl gtsam::SO3::print(class std::basic_string<char,struct std::char_traits<char>,class std::allocator<char> > const &)const " (?print@SO3@gtsam@@QEBAXAEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@@Z) referenced in function "public: static void __cdecl gtsam::Testable<class gtsam::SO3>::Print(class gtsam::SO3 const &,class std::basic_string<char,struct std::char_traits<char>,class std::allocator<char> > const &)" (?Print@?$Testable@VSO3@gtsam@@@gtsam@@SAXAEBVSO3@2@AEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@@Z)	check_geometry_program	C:\AFIT\lib\gtsam\build\gtsam\geometry\tests\testSO3.obj

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"

cmake/HandleBoost.cmake

@@ -22,7 +22,7 @@ endif()
 
 
 # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
-set(BOOST_FIND_MINIMUM_VERSION 1.58)
+set(BOOST_FIND_MINIMUM_VERSION 1.65)
 set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
 
 find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS})
@@ -30,7 +30,7 @@ find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM
 # Required components
 if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR
     NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY)
-  message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.")
+  message(FATAL_ERROR "Missing required Boost components >= v1.65, please install/upgrade Boost or configure your search paths.")
 endif()
 
 option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)

cmake/HandleCPack.cmake

@@ -25,4 +25,4 @@ set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION
 
 # Deb-package specific cpack
 set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev")
-set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")
+set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.65)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")

cmake/HandleGeneralOptions.cmake

@@ -14,20 +14,21 @@ if(GTSAM_UNSTABLE_AVAILABLE)
     option(GTSAM_UNSTABLE_BUILD_PYTHON       "Enable/Disable Python wrapper for libgtsam_unstable" ON)
     option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF)
 endif()
-option(BUILD_SHARED_LIBS                 "Build shared gtsam library, instead of static" ON)
+option(BUILD_SHARED_LIBS                    "Build shared gtsam library, instead of static" ON)
-option(GTSAM_USE_QUATERNIONS             "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
+option(GTSAM_USE_QUATERNIONS                "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
-option(GTSAM_POSE3_EXPMAP                "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
+option(GTSAM_POSE3_EXPMAP                   "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
-option(GTSAM_ROT3_EXPMAP                 "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
+option(GTSAM_ROT3_EXPMAP                    "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
-option(GTSAM_ENABLE_CONSISTENCY_CHECKS   "Enable/Disable expensive consistency checks"       OFF)
+option(GTSAM_ENABLE_CONSISTENCY_CHECKS      "Enable/Disable expensive consistency checks"       OFF)
-option(GTSAM_WITH_TBB                    "Use Intel Threaded Building Blocks (TBB) if available" ON)
+option(GTSAM_WITH_TBB                       "Use Intel Threaded Building Blocks (TBB) if available" ON)
-option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available" OFF)
+option(GTSAM_WITH_EIGEN_MKL                 "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_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_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_BUILD_PYTHON                   "Enable/Disable building & installation of Python module with pybind11" OFF)
-option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab toolbox"  OFF)
+option(GTSAM_INSTALL_MATLAB_TOOLBOX         "Enable/Disable installation of matlab toolbox"  OFF)
-option(GTSAM_ALLOW_DEPRECATED_SINCE_V41  "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
+option(GTSAM_ALLOW_DEPRECATED_SINCE_V41     "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
-option(GTSAM_SUPPORT_NESTED_DISSECTION   "Support Metis-based nested dissection" ON)
+option(GTSAM_SUPPORT_NESTED_DISSECTION      "Support Metis-based nested dissection" ON)
-option(GTSAM_TANGENT_PREINTEGRATION      "Use new ImuFactor with integration on tangent space" ON)
+option(GTSAM_TANGENT_PREINTEGRATION         "Use new ImuFactor with integration on tangent space" ON)
+option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF)
 if(NOT MSVC AND NOT XCODE_VERSION)
     option(GTSAM_BUILD_WITH_CCACHE           "Use ccache compiler cache" ON)
 endif()

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})

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})")

cmake/HandleTBB.cmake

@@ -1,24 +1,32 @@
 ###############################################################################
-# Find TBB
+if (GTSAM_WITH_TBB)
-find_package(TBB 4.4 COMPONENTS tbb tbbmalloc)
+    # Find TBB
+    find_package(TBB 4.4 COMPONENTS tbb tbbmalloc)
 
-# Set up variables if we're using TBB
+    # Set up variables if we're using TBB
-if(TBB_FOUND AND GTSAM_WITH_TBB)
+    if(TBB_FOUND)
-    set(GTSAM_USE_TBB 1)  # This will go into config.h
+        set(GTSAM_USE_TBB 1)  # This will go into config.h
-    if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020))
+
-        set(TBB_GREATER_EQUAL_2020 1)
+        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()
+            set(TBB_GREATER_EQUAL_2020 0)
+        endif()
+        # all definitions and link requisites will go via imported targets:
+        # tbb & tbbmalloc
+        list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
     else()
-        set(TBB_GREATER_EQUAL_2020 0)
+        set(GTSAM_USE_TBB 0)  # This will go into config.h
+    endif()
+
+    ###############################################################################
+    # 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()
-    # all definitions and link requisites will go via imported targets:
-    # tbb & tbbmalloc
-    list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
-else()
-    set(GTSAM_USE_TBB 0)  # This will go into config.h
-endif()
 
-###############################################################################
-# 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()

doc/CMakeLists.txt

@@ -23,6 +23,7 @@ if (GTSAM_BUILD_DOCS)
     # GTSAM core subfolders
     set(gtsam_doc_subdirs 
         gtsam/base
+        gtsam/basis 
         gtsam/discrete
         gtsam/geometry
         gtsam/inference

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: 
+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.
-
-    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
-
-

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 .

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

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 .

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

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

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 .

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 && \

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 .

examples/IMUKittiExampleGPS.cpp

@@ -11,21 +11,23 @@
 
 /**
  * @file IMUKittiExampleGPS
- * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE
+ * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI
- * @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ Electronics
+ * VISION BENCHMARK SUITE
+ * @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ
+ * Electronics
  */
 
 // GTSAM related includes.
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/navigation/CombinedImuFactor.h>
 #include <gtsam/navigation/GPSFactor.h>
 #include <gtsam/navigation/ImuFactor.h>
-#include <gtsam/slam/dataset.h>
-#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/ISAM2Params.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/dataset.h>
 
 #include <cstring>
 #include <fstream>
@@ -34,35 +36,35 @@
 using namespace std;
 using namespace gtsam;
 
-using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)
-using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
 using symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
+using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
+using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)
 
 struct KittiCalibration {
-    double body_ptx;
+  double body_ptx;
-    double body_pty;
+  double body_pty;
-    double body_ptz;
+  double body_ptz;
-    double body_prx;
+  double body_prx;
-    double body_pry;
+  double body_pry;
-    double body_prz;
+  double body_prz;
-    double accelerometer_sigma;
+  double accelerometer_sigma;
-    double gyroscope_sigma;
+  double gyroscope_sigma;
-    double integration_sigma;
+  double integration_sigma;
-    double accelerometer_bias_sigma;
+  double accelerometer_bias_sigma;
-    double gyroscope_bias_sigma;
+  double gyroscope_bias_sigma;
-    double average_delta_t;
+  double average_delta_t;
 };
 
 struct ImuMeasurement {
-    double time;
+  double time;
-    double dt;
+  double dt;
-    Vector3 accelerometer;
+  Vector3 accelerometer;
-    Vector3 gyroscope;  // omega
+  Vector3 gyroscope;  // omega
 };
 
 struct GpsMeasurement {
-    double time;
+  double time;
-    Vector3 position;  // x,y,z
+  Vector3 position;  // x,y,z
 };
 
 const string output_filename = "IMUKittiExampleGPSResults.csv";
@@ -70,290 +72,313 @@ const string output_filename = "IMUKittiExampleGPSResults.csv";
 void loadKittiData(KittiCalibration& kitti_calibration,
                    vector<ImuMeasurement>& imu_measurements,
                    vector<GpsMeasurement>& gps_measurements) {
-    string line;
+  string line;
 
-    // Read IMU metadata and compute relative sensor pose transforms
+  // Read IMU metadata and compute relative sensor pose transforms
-    // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma
+  // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma
-    // AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT
+  // GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma
-    string imu_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt");
+  // AverageDeltaT
-    ifstream imu_metadata(imu_metadata_file.c_str());
+  string imu_metadata_file =
+      findExampleDataFile("KittiEquivBiasedImu_metadata.txt");
+  ifstream imu_metadata(imu_metadata_file.c_str());
 
-    printf("-- Reading sensor metadata\n");
+  printf("-- Reading sensor metadata\n");
 
-    getline(imu_metadata, line, '\n');  // ignore the first line
+  getline(imu_metadata, line, '\n');  // ignore the first line
 
-    // Load Kitti calibration
+  // Load Kitti calibration
-    getline(imu_metadata, line, '\n');
+  getline(imu_metadata, line, '\n');
-    sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
+  sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
-           &kitti_calibration.body_ptx,
+         &kitti_calibration.body_ptx, &kitti_calibration.body_pty,
-           &kitti_calibration.body_pty,
+         &kitti_calibration.body_ptz, &kitti_calibration.body_prx,
-           &kitti_calibration.body_ptz,
+         &kitti_calibration.body_pry, &kitti_calibration.body_prz,
-           &kitti_calibration.body_prx,
+         &kitti_calibration.accelerometer_sigma,
-           &kitti_calibration.body_pry,
+         &kitti_calibration.gyroscope_sigma,
-           &kitti_calibration.body_prz,
+         &kitti_calibration.integration_sigma,
-           &kitti_calibration.accelerometer_sigma,
+         &kitti_calibration.accelerometer_bias_sigma,
-           &kitti_calibration.gyroscope_sigma,
+         &kitti_calibration.gyroscope_bias_sigma,
-           &kitti_calibration.integration_sigma,
+         &kitti_calibration.average_delta_t);
-           &kitti_calibration.accelerometer_bias_sigma,
+  printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
-           &kitti_calibration.gyroscope_bias_sigma,
+         kitti_calibration.body_ptx, kitti_calibration.body_pty,
-           &kitti_calibration.average_delta_t);
+         kitti_calibration.body_ptz, kitti_calibration.body_prx,
-    printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
+         kitti_calibration.body_pry, kitti_calibration.body_prz,
-           kitti_calibration.body_ptx,
+         kitti_calibration.accelerometer_sigma,
-           kitti_calibration.body_pty,
+         kitti_calibration.gyroscope_sigma, kitti_calibration.integration_sigma,
-           kitti_calibration.body_ptz,
+         kitti_calibration.accelerometer_bias_sigma,
-           kitti_calibration.body_prx,
+         kitti_calibration.gyroscope_bias_sigma,
-           kitti_calibration.body_pry,
+         kitti_calibration.average_delta_t);
-           kitti_calibration.body_prz,
-           kitti_calibration.accelerometer_sigma,
-           kitti_calibration.gyroscope_sigma,
-           kitti_calibration.integration_sigma,
-           kitti_calibration.accelerometer_bias_sigma,
-           kitti_calibration.gyroscope_bias_sigma,
-           kitti_calibration.average_delta_t);
 
-    // Read IMU data
+  // Read IMU data
-    // Time dt accelX accelY accelZ omegaX omegaY omegaZ
+  // Time dt accelX accelY accelZ omegaX omegaY omegaZ
-    string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt");
+  string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt");
-    printf("-- Reading IMU measurements from file\n");
+  printf("-- Reading IMU measurements from file\n");
-    {
+  {
-        ifstream imu_data(imu_data_file.c_str());
+    ifstream imu_data(imu_data_file.c_str());
-        getline(imu_data, line, '\n');  // ignore the first line
+    getline(imu_data, line, '\n');  // ignore the first line
 
-        double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, gyro_y = 0, gyro_z = 0;
+    double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0,
-        while (!imu_data.eof()) {
+           gyro_y = 0, gyro_z = 0;
-            getline(imu_data, line, '\n');
+    while (!imu_data.eof()) {
-            sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf",
+      getline(imu_data, line, '\n');
-                   &time, &dt,
+      sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", &time, &dt,
-                   &acc_x, &acc_y, &acc_z,
+             &acc_x, &acc_y, &acc_z, &gyro_x, &gyro_y, &gyro_z);
-                   &gyro_x, &gyro_y, &gyro_z);
 
-            ImuMeasurement measurement;
+      ImuMeasurement measurement;
-            measurement.time = time;
+      measurement.time = time;
-            measurement.dt = dt;
+      measurement.dt = dt;
-            measurement.accelerometer = Vector3(acc_x, acc_y, acc_z);
+      measurement.accelerometer = Vector3(acc_x, acc_y, acc_z);
-            measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z);
+      measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z);
-            imu_measurements.push_back(measurement);
+      imu_measurements.push_back(measurement);
-        }
     }
+  }
 
-    // Read GPS data
+  // Read GPS data
-    // Time,X,Y,Z
+  // Time,X,Y,Z
-    string gps_data_file = findExampleDataFile("KittiGps_converted.txt");
+  string gps_data_file = findExampleDataFile("KittiGps_converted.txt");
-    printf("-- Reading GPS measurements from file\n");
+  printf("-- Reading GPS measurements from file\n");
-    {
+  {
-        ifstream gps_data(gps_data_file.c_str());
+    ifstream gps_data(gps_data_file.c_str());
-        getline(gps_data, line, '\n');  // ignore the first line
+    getline(gps_data, line, '\n');  // ignore the first line
 
-        double time = 0, gps_x = 0, gps_y = 0, gps_z = 0;
+    double time = 0, gps_x = 0, gps_y = 0, gps_z = 0;
-        while (!gps_data.eof()) {
+    while (!gps_data.eof()) {
-            getline(gps_data, line, '\n');
+      getline(gps_data, line, '\n');
-            sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z);
+      sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z);
 
-            GpsMeasurement measurement;
+      GpsMeasurement measurement;
-            measurement.time = time;
+      measurement.time = time;
-            measurement.position = Vector3(gps_x, gps_y, gps_z);
+      measurement.position = Vector3(gps_x, gps_y, gps_z);
-            gps_measurements.push_back(measurement);
+      gps_measurements.push_back(measurement);
-        }
     }
+  }
 }
 
 int main(int argc, char* argv[]) {
-    KittiCalibration kitti_calibration;
+  KittiCalibration kitti_calibration;
-    vector<ImuMeasurement> imu_measurements;
+  vector<ImuMeasurement> imu_measurements;
-    vector<GpsMeasurement> gps_measurements;
+  vector<GpsMeasurement> gps_measurements;
-    loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
+  loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
 
-    Vector6 BodyP = (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, kitti_calibration.body_ptz,
+  Vector6 BodyP =
-                                  kitti_calibration.body_prx, kitti_calibration.body_pry, kitti_calibration.body_prz)
+      (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty,
-                    .finished();
+       kitti_calibration.body_ptz, kitti_calibration.body_prx,
-    auto body_T_imu = Pose3::Expmap(BodyP);
+       kitti_calibration.body_pry, kitti_calibration.body_prz)
-    if (!body_T_imu.equals(Pose3(), 1e-5)) {
+          .finished();
-        printf("Currently only support IMUinBody is identity, i.e. IMU and body frame are the same");
+  auto body_T_imu = Pose3::Expmap(BodyP);
-        exit(-1);
+  if (!body_T_imu.equals(Pose3(), 1e-5)) {
-    }
+    printf(
+        "Currently only support IMUinBody is identity, i.e. IMU and body frame "
+        "are the same");
+    exit(-1);
+  }
 
-    // Configure different variables
+  // Configure different variables
-    // double t_offset = gps_measurements[0].time;
+  // double t_offset = gps_measurements[0].time;
-    size_t first_gps_pose = 1;
+  size_t first_gps_pose = 1;
-    size_t gps_skip = 10;  // Skip this many GPS measurements each time
+  size_t gps_skip = 10;  // Skip this many GPS measurements each time
-    double g = 9.8;
+  double g = 9.8;
-    auto w_coriolis = Vector3::Zero();  // zero vector
+  auto w_coriolis = Vector3::Zero();  // zero vector
 
-    // Configure noise models
+  // Configure noise models
-    auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0),
+  auto noise_model_gps = noiseModel::Diagonal::Precisions(
-                                                                          Vector3::Constant(1.0/0.07))
+      (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0 / 0.07))
-                                                            .finished());
+          .finished());
 
-    // Set initial conditions for the estimated trajectory
+  // Set initial conditions for the estimated trajectory
-    // initial pose is the reference frame (navigation frame)
+  // initial pose is the reference frame (navigation frame)
-    auto current_pose_global = Pose3(Rot3(), gps_measurements[first_gps_pose].position);
+  auto current_pose_global =
-    // the vehicle is stationary at the beginning at position 0,0,0
+      Pose3(Rot3(), gps_measurements[first_gps_pose].position);
-    Vector3 current_velocity_global = Vector3::Zero();
+  // the vehicle is stationary at the beginning at position 0,0,0
-    auto current_bias = imuBias::ConstantBias();  // init with zero bias
+  Vector3 current_velocity_global = Vector3::Zero();
+  auto current_bias = imuBias::ConstantBias();  // init with zero bias
 
-    auto sigma_init_x = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0),
+  auto sigma_init_x = noiseModel::Diagonal::Precisions(
-                                                                       Vector3::Constant(1.0))
+      (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0)).finished());
-                                                         .finished());
+  auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0));
-    auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0));
+  auto sigma_init_b = noiseModel::Diagonal::Sigmas(
-    auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100),
+      (Vector6() << Vector3::Constant(0.100), Vector3::Constant(5.00e-05))
-                                                                   Vector3::Constant(5.00e-05))
+          .finished());
-                                                     .finished());
 
-    // Set IMU preintegration parameters
+  // Set IMU preintegration parameters
-    Matrix33 measured_acc_cov = I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
+  Matrix33 measured_acc_cov =
-    Matrix33 measured_omega_cov = I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);
+      I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
-    // error committed in integrating position from velocities
+  Matrix33 measured_omega_cov =
-    Matrix33 integration_error_cov = I_3x3 * pow(kitti_calibration.integration_sigma, 2);
+      I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);
+  // error committed in integrating position from velocities
+  Matrix33 integration_error_cov =
+      I_3x3 * pow(kitti_calibration.integration_sigma, 2);
 
-    auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g);
+  auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g);
-    imu_params->accelerometerCovariance = measured_acc_cov;     // acc white noise in continuous
+  imu_params->accelerometerCovariance =
-    imu_params->integrationCovariance = integration_error_cov;  // integration uncertainty continuous
+      measured_acc_cov;  // acc white noise in continuous
-    imu_params->gyroscopeCovariance = measured_omega_cov;       // gyro white noise in continuous
+  imu_params->integrationCovariance =
-    imu_params->omegaCoriolis = w_coriolis;
+      integration_error_cov;  // integration uncertainty continuous
+  imu_params->gyroscopeCovariance =
+      measured_omega_cov;  // gyro white noise in continuous
+  imu_params->omegaCoriolis = w_coriolis;
 
-    std::shared_ptr<PreintegratedImuMeasurements> current_summarized_measurement = nullptr;
+  std::shared_ptr<PreintegratedImuMeasurements> current_summarized_measurement =
+      nullptr;
 
-    // Set ISAM2 parameters and create ISAM2 solver object
+  // Set ISAM2 parameters and create ISAM2 solver object
-    ISAM2Params isam_params;
+  ISAM2Params isam_params;
-    isam_params.factorization = ISAM2Params::CHOLESKY;
+  isam_params.factorization = ISAM2Params::CHOLESKY;
-    isam_params.relinearizeSkip = 10;
+  isam_params.relinearizeSkip = 10;
 
-    ISAM2 isam(isam_params);
+  ISAM2 isam(isam_params);
 
-    // Create the factor graph and values object that will store new factors and values to add to the incremental graph
+  // Create the factor graph and values object that will store new factors and
-    NonlinearFactorGraph new_factors;
+  // values to add to the incremental graph
-    Values new_values;  // values storing the initial estimates of new nodes in the factor graph
+  NonlinearFactorGraph new_factors;
+  Values new_values;  // values storing the initial estimates of new nodes in
+                      // the factor graph
 
-    /// Main loop:
+  /// Main loop:
-    /// (1) we read the measurements
+  /// (1) we read the measurements
-    /// (2) we create the corresponding factors in the graph
+  /// (2) we create the corresponding factors in the graph
-    /// (3) we solve the graph to obtain and optimal estimate of robot trajectory
+  /// (3) we solve the graph to obtain and optimal estimate of robot trajectory
-    printf("-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n");
+  printf(
-    size_t j = 0;
+      "-- Starting main loop: inference is performed at each time step, but we "
-    for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
+      "plot trajectory every 10 steps\n");
-        // At each non=IMU measurement we initialize a new node in the graph
+  size_t j = 0;
-        auto current_pose_key = X(i);
+  size_t included_imu_measurement_count = 0;
-        auto current_vel_key = V(i);
-        auto current_bias_key = B(i);
-        double t = gps_measurements[i].time;
 
-        if (i == first_gps_pose) {
+  for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
-            // Create initial estimate and prior on initial pose, velocity, and biases
+    // At each non=IMU measurement we initialize a new node in the graph
-            new_values.insert(current_pose_key, current_pose_global);
+    auto current_pose_key = X(i);
-            new_values.insert(current_vel_key, current_velocity_global);
+    auto current_vel_key = V(i);
-            new_values.insert(current_bias_key, current_bias);
+    auto current_bias_key = B(i);
-            new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, current_pose_global, sigma_init_x);
+    double t = gps_measurements[i].time;
-            new_factors.emplace_shared<PriorFactor<Vector3>>(current_vel_key, current_velocity_global, sigma_init_v);
-            new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(current_bias_key, current_bias, sigma_init_b);
-        } else {
-            double t_previous = gps_measurements[i-1].time;
 
-            // Summarize IMU data between the previous GPS measurement and now
+    if (i == first_gps_pose) {
-            current_summarized_measurement = std::make_shared<PreintegratedImuMeasurements>(imu_params, current_bias);
+      // Create initial estimate and prior on initial pose, velocity, and biases
-            static size_t included_imu_measurement_count = 0;
+      new_values.insert(current_pose_key, current_pose_global);
-            while (j < imu_measurements.size() && imu_measurements[j].time <= t) {
+      new_values.insert(current_vel_key, current_velocity_global);
-                if (imu_measurements[j].time >= t_previous) {
+      new_values.insert(current_bias_key, current_bias);
-                    current_summarized_measurement->integrateMeasurement(imu_measurements[j].accelerometer,
+      new_factors.emplace_shared<PriorFactor<Pose3>>(
-                                                                         imu_measurements[j].gyroscope,
+          current_pose_key, current_pose_global, sigma_init_x);
-                                                                         imu_measurements[j].dt);
+      new_factors.emplace_shared<PriorFactor<Vector3>>(
-                    included_imu_measurement_count++;
+          current_vel_key, current_velocity_global, sigma_init_v);
-                }
+      new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(
-                j++;
+          current_bias_key, current_bias, sigma_init_b);
-            }
+    } else {
+      double t_previous = gps_measurements[i - 1].time;
 
-            // Create IMU factor
+      // Summarize IMU data between the previous GPS measurement and now
-            auto previous_pose_key = X(i-1);
+      current_summarized_measurement =
-            auto previous_vel_key = V(i-1);
+          std::make_shared<PreintegratedImuMeasurements>(imu_params,
-            auto previous_bias_key = B(i-1);
+                                                         current_bias);
 
-            new_factors.emplace_shared<ImuFactor>(previous_pose_key, previous_vel_key,
+      while (j < imu_measurements.size() && imu_measurements[j].time <= t) {
-                                                  current_pose_key, current_vel_key,
+        if (imu_measurements[j].time >= t_previous) {
-                                                  previous_bias_key, *current_summarized_measurement);
+          current_summarized_measurement->integrateMeasurement(
-
+              imu_measurements[j].accelerometer, imu_measurements[j].gyroscope,
-            // Bias evolution as given in the IMU metadata
+              imu_measurements[j].dt);
-            auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector6() <<
+          included_imu_measurement_count++;
-                   Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma),
-                   Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.gyroscope_bias_sigma))
-                 .finished());
-            new_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(previous_bias_key,
-                                                                             current_bias_key,
-                                                                             imuBias::ConstantBias(),
-                                                                             sigma_between_b);
-
-            // Create GPS factor
-            auto gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position);
-            if ((i % gps_skip) == 0) {
-                new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, gps_pose, noise_model_gps);
-                new_values.insert(current_pose_key, gps_pose);
-
-                printf("################ POSE INCLUDED AT TIME %lf ################\n", t);
-                cout << gps_pose.translation();
-                printf("\n\n");
-            } else {
-                new_values.insert(current_pose_key, current_pose_global);
-            }
-
-            // Add initial values for velocity and bias based on the previous estimates
-            new_values.insert(current_vel_key, current_velocity_global);
-            new_values.insert(current_bias_key, current_bias);
-
-            // Update solver
-            // =======================================================================
-            // We accumulate 2*GPSskip GPS measurements before updating the solver at
-            // first so that the heading becomes observable.
-            if (i > (first_gps_pose + 2*gps_skip)) {
-                printf("################ NEW FACTORS AT TIME %lf ################\n", t);
-                new_factors.print();
-
-                isam.update(new_factors, new_values);
-
-                // Reset the newFactors and newValues list
-                new_factors.resize(0);
-                new_values.clear();
-
-                // Extract the result/current estimates
-                Values result = isam.calculateEstimate();
-
-                current_pose_global = result.at<Pose3>(current_pose_key);
-                current_velocity_global = result.at<Vector3>(current_vel_key);
-                current_bias = result.at<imuBias::ConstantBias>(current_bias_key);
-
-                printf("\n################ POSE AT TIME %lf ################\n", t);
-                current_pose_global.print();
-                printf("\n\n");
-            }
         }
+        j++;
+      }
+
+      // Create IMU factor
+      auto previous_pose_key = X(i - 1);
+      auto previous_vel_key = V(i - 1);
+      auto previous_bias_key = B(i - 1);
+
+      new_factors.emplace_shared<ImuFactor>(
+          previous_pose_key, previous_vel_key, current_pose_key,
+          current_vel_key, previous_bias_key, *current_summarized_measurement);
+
+      // Bias evolution as given in the IMU metadata
+      auto sigma_between_b = noiseModel::Diagonal::Sigmas(
+          (Vector6() << Vector3::Constant(
+               sqrt(included_imu_measurement_count) *
+               kitti_calibration.accelerometer_bias_sigma),
+           Vector3::Constant(sqrt(included_imu_measurement_count) *
+                             kitti_calibration.gyroscope_bias_sigma))
+              .finished());
+      new_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(
+          previous_bias_key, current_bias_key, imuBias::ConstantBias(),
+          sigma_between_b);
+
+      // Create GPS factor
+      auto gps_pose =
+          Pose3(current_pose_global.rotation(), gps_measurements[i].position);
+      if ((i % gps_skip) == 0) {
+        new_factors.emplace_shared<PriorFactor<Pose3>>(
+            current_pose_key, gps_pose, noise_model_gps);
+        new_values.insert(current_pose_key, gps_pose);
+
+        printf("############ POSE INCLUDED AT TIME %.6lf ############\n",
+               t);
+        cout << gps_pose.translation();
+        printf("\n\n");
+      } else {
+        new_values.insert(current_pose_key, current_pose_global);
+      }
+
+      // Add initial values for velocity and bias based on the previous
+      // estimates
+      new_values.insert(current_vel_key, current_velocity_global);
+      new_values.insert(current_bias_key, current_bias);
+
+      // Update solver
+      // =======================================================================
+      // We accumulate 2*GPSskip GPS measurements before updating the solver at
+      // first so that the heading becomes observable.
+      if (i > (first_gps_pose + 2 * gps_skip)) {
+        printf("############ NEW FACTORS AT TIME %.6lf ############\n",
+               t);
+        new_factors.print();
+
+        isam.update(new_factors, new_values);
+
+        // Reset the newFactors and newValues list
+        new_factors.resize(0);
+        new_values.clear();
+
+        // Extract the result/current estimates
+        Values result = isam.calculateEstimate();
+
+        current_pose_global = result.at<Pose3>(current_pose_key);
+        current_velocity_global = result.at<Vector3>(current_vel_key);
+        current_bias = result.at<imuBias::ConstantBias>(current_bias_key);
+
+        printf("\n############ POSE AT TIME %lf ############\n", t);
+        current_pose_global.print();
+        printf("\n\n");
+      }
     }
+  }
 
-    // Save results to file
+  // Save results to file
-    printf("\nWriting results to file...\n");
+  printf("\nWriting results to file...\n");
-    FILE* fp_out = fopen(output_filename.c_str(), "w+");
+  FILE* fp_out = fopen(output_filename.c_str(), "w+");
-    fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n");
+  fprintf(fp_out,
+          "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n");
 
-    Values result = isam.calculateEstimate();
+  Values result = isam.calculateEstimate();
-    for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
+  for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
-        auto pose_key = X(i);
+    auto pose_key = X(i);
-        auto vel_key = V(i);
+    auto vel_key = V(i);
-        auto bias_key = B(i);
+    auto bias_key = B(i);
 
-        auto pose = result.at<Pose3>(pose_key);
+    auto pose = result.at<Pose3>(pose_key);
-        auto velocity = result.at<Vector3>(vel_key);
+    auto velocity = result.at<Vector3>(vel_key);
-        auto bias = result.at<imuBias::ConstantBias>(bias_key);
+    auto bias = result.at<imuBias::ConstantBias>(bias_key);
 
-        auto pose_quat = pose.rotation().toQuaternion();
+    auto pose_quat = pose.rotation().toQuaternion();
-        auto gps = gps_measurements[i].position;
+    auto gps = gps_measurements[i].position;
 
-        cout << "State at #" << i << endl;
+    cout << "State at #" << i << endl;
-        cout << "Pose:" << endl << pose << endl;
+    cout << "Pose:" << endl << pose << endl;
-        cout << "Velocity:" << endl << velocity << endl;
+    cout << "Velocity:" << endl << velocity << endl;
-        cout << "Bias:" << endl << bias << endl;
+    cout << "Bias:" << endl << bias << endl;
 
-        fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
+    fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
-                gps_measurements[i].time,
+            gps_measurements[i].time, pose.x(), pose.y(), pose.z(),
-                pose.x(), pose.y(), pose.z(),
+            pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps(0),
-                pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(),
+            gps(1), gps(2));
-                gps(0), gps(1), gps(2));
+  }
-    }
 
-    fclose(fp_out);
+  fclose(fp_out);
 }

examples/README.md

@@ -51,13 +51,13 @@ The directory **vSLAMexample** includes 2 simple examples using GTSAM:
 
 See the separate README file there.
 
-##Undirected Graphical Models (UGM)
+## Undirected Graphical Models (UGM)
 The best representation for a Markov Random Field is a factor graph :-) This is illustrated with some discrete examples from the UGM MATLAB toolbox, which
 can be found at <http://www.di.ens.fr/~mschmidt/Software/UGM>
 
 
-##Building and Running
+## Building and Running
-To build, cd into the directory and do:
+To build, cd into the top-level gtsam directory and do:
 
 ```
 mkdir build

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)
+# metis: already handled in ROOT/cmake/HandleMetis.cmake
-if(GTSAM_SUPPORT_NESTED_DISSECTION)
-	add_subdirectory(metis)
-endif()
 
 add_subdirectory(ceres)
 

gtsam/CMakeLists.txt

@@ -5,6 +5,7 @@ project(gtsam LANGUAGES CXX)
 # The following variable is the master list of subdirs to add
 set (gtsam_subdirs
     base
+    basis
     geometry
     inference
     symbolic
@@ -88,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
@@ -154,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)

gtsam/base/Lie.h

@@ -17,6 +17,7 @@
  * @author Frank Dellaert
  * @author Mike Bosse
  * @author Duy Nguyen Ta
+ * @author Yotam Stern
  */
 
 
@@ -319,12 +320,28 @@ T expm(const Vector& x, int K=7) {
 }
 
 /**
- * Linear interpolation between X and Y by coefficient t in [0, 1].
+ * Linear interpolation between X and Y by coefficient t. Typically t \in [0,1],
+ * but can also be used to extrapolate before pose X or after pose Y.
  */
 template <typename T>
-T interpolate(const T& X, const T& Y, double t) {
+T interpolate(const T& X, const T& Y, double t,
-  assert(t >= 0 && t <= 1);
+              typename MakeOptionalJacobian<T, T>::type Hx = boost::none,
-  return traits<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
+              typename MakeOptionalJacobian<T, T>::type Hy = boost::none) {
+  if (Hx || Hy) {
+    typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
+    const T between =
+        traits<T>::Between(X, Y, between_H_x);  // between_H_y = identity
+    typename traits<T>::TangentVector delta = traits<T>::Logmap(between, log_H);
+    const T Delta = traits<T>::Expmap(t * delta, exp_H);
+    const T result = traits<T>::Compose(
+        X, Delta, compose_H_x);  // compose_H_xinv_y = identity
+
+    if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x;
+    if (Hy) *Hy = t * exp_H * log_H;
+    return result;
+  }
+  return traits<T>::Compose(
+      X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
 }
 
 /**

gtsam/base/Matrix.h

@@ -29,7 +29,7 @@
 #include <gtsam/config.h>
 
 #include <boost/format.hpp>
-#include <boost/function.hpp>
+#include <functional>
 #include <boost/tuple/tuple.hpp>
 #include <boost/math/special_functions/fpclassify.hpp>
 
@@ -489,7 +489,7 @@ struct MultiplyWithInverseFunction {
 
   // The function phi should calculate f(a)*b, with derivatives in a and b.
   // Naturally, the derivative in b is f(a).
-  typedef boost::function<VectorN(
+  typedef std::function<VectorN(
       const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
       Operator;
 

gtsam/base/OptionalJacobian.h

@@ -89,6 +89,13 @@ public:
     usurp(dynamic.data());
   }
 
+  /// Constructor that will resize a dynamic matrix (unless already correct)
+  OptionalJacobian(Eigen::MatrixXd* dynamic) :
+      map_(nullptr) {
+    dynamic->resize(Rows, Cols); // no malloc if correct size
+    usurp(dynamic->data());
+  }
+
 #ifndef OPTIONALJACOBIAN_NOBOOST
 
   /// Constructor with boost::none just makes empty

gtsam/base/Testable.h

@@ -34,8 +34,9 @@
 #pragma once
 
 #include <boost/concept_check.hpp>
-#include <boost/shared_ptr.hpp>
+#include <functional>
 #include <iostream>
+#include <memory>
 #include <string>
 
 #define GTSAM_PRINT(x)((x).print(#x))
@@ -119,10 +120,10 @@ namespace gtsam {
    * Binary predicate on shared pointers
    */
   template<class V>
-  struct equals_star : public std::function<bool(const boost::shared_ptr<V>&, const boost::shared_ptr<V>&)> {
+  struct equals_star : public std::function<bool(const std::shared_ptr<V>&, const std::shared_ptr<V>&)> {
     double tol_;
     equals_star(double tol = 1e-9) : tol_(tol) {}
-    bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
+    bool operator()(const std::shared_ptr<V>& expected, const std::shared_ptr<V>& actual) {
       if (!actual && !expected) return true;
       return actual && expected && traits<V>::Equals(*actual,*expected, tol_);
     }

gtsam/base/base.i

@@ -0,0 +1,108 @@
+//*************************************************************************
+// base
+//*************************************************************************
+
+namespace gtsam {
+
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3Fisheye.h>
+#include <gtsam/geometry/Cal3Unified.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/EssentialMatrix.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/StereoPoint2.h>
+#include <gtsam/navigation/ImuBias.h>
+
+// #####
+
+#include <gtsam/base/debug.h>
+bool isDebugVersion();
+
+#include <gtsam/base/DSFMap.h>
+class IndexPair {
+  IndexPair();
+  IndexPair(size_t i, size_t j);
+  size_t i() const;
+  size_t j() const;
+};
+
+template<KEY = {gtsam::IndexPair}>
+class DSFMap {
+  DSFMap();
+  KEY find(const KEY& key) const;
+  void merge(const KEY& x, const KEY& y);
+  std::map<KEY, Set> sets();
+};
+
+class IndexPairSet {
+  IndexPairSet();
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  void insert(gtsam::IndexPair key);
+  bool erase(gtsam::IndexPair key);        // returns true if value was removed
+  bool count(gtsam::IndexPair key) const;  // returns true if value exists
+};
+
+class IndexPairVector {
+  IndexPairVector();
+  IndexPairVector(const gtsam::IndexPairVector& other);
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  gtsam::IndexPair at(size_t i) const;
+  void push_back(gtsam::IndexPair key) const;
+};
+
+gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set);
+
+class IndexPairSetMap {
+  IndexPairSetMap();
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  gtsam::IndexPairSet at(gtsam::IndexPair& key);
+};
+
+#include <gtsam/base/Matrix.h>
+bool linear_independent(Matrix A, Matrix B, double tol);
+
+#include <gtsam/base/Value.h>
+virtual class Value {
+  // No constructors because this is an abstract class
+
+  // Testable
+  void print(string s = "") const;
+
+  // Manifold
+  size_t dim() const;
+};
+
+#include <gtsam/base/GenericValue.h>
+template <T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2,
+               gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2,
+               gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
+               gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix,
+               gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
+virtual class GenericValue : gtsam::Value {
+  void serializable() const;
+};
+
+}  // namespace gtsam

gtsam/base/lieProxies.h

@@ -24,7 +24,7 @@
  *
  * These should not be used outside of tests, as they are just remappings
  * of the original functions.  We use these to avoid needing to do
- * too much boost::bind magic or writing a bunch of separate proxy functions.
+ * too much std::bind magic or writing a bunch of separate proxy functions.
  *
  * Don't expect all classes to work for all of these functions.
  */

gtsam/base/numericalDerivative.h

@@ -18,8 +18,7 @@
 // \callgraph
 #pragma once
 
-#include <boost/function.hpp>
+#include <functional>
-#include <boost/bind/bind.hpp>
 
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/JacobianFactor.h>
@@ -38,13 +37,13 @@ namespace gtsam {
  *   for a function with one relevant param and an optional derivative:
  *     Foo bar(const Obj& a, boost::optional<Matrix&> H1)
  *   Use boost.bind to restructure:
- *     boost::bind(bar, boost::placeholders::_1, boost::none)
+ *     std::bind(bar, std::placeholders::_1, boost::none)
  *   This syntax will fix the optional argument to boost::none, while using the first argument provided
  *
  * For member functions, such as below, with an instantiated copy instanceOfSomeClass
  *     Foo SomeClass::bar(const Obj& a)
  * Use boost bind as follows to create a function pointer that uses the member function:
- *       boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), boost::placeholders::_1)
+ *       std::bind(&SomeClass::bar, ref(instanceOfSomeClass), std::placeholders::_1)
  *
  * For additional details, see the documentation:
  *     http://www.boost.org/doc/libs/release/libs/bind/bind.html
@@ -69,7 +68,7 @@ struct FixedSizeMatrix {
 
 template <class X, int N = traits<X>::dimension>
 typename Eigen::Matrix<double, N, 1> numericalGradient(
-    boost::function<double(const X&)> h, const X& x, double delta = 1e-5) {
+    std::function<double(const X&)> h, const X& x, double delta = 1e-5) {
   double factor = 1.0 / (2.0 * delta);
 
   BOOST_STATIC_ASSERT_MSG(
@@ -109,7 +108,7 @@ typename Eigen::Matrix<double, N, 1> numericalGradient(
 template <class Y, class X, int N = traits<X>::dimension>
 // TODO Should compute fixed-size matrix
 typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
-    boost::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
+    std::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
   typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
 
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
@@ -150,7 +149,7 @@ typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
 template<class Y, class X>
 typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const X&), const X& x,
     double delta = 1e-5) {
-  return numericalDerivative11<Y, X>(boost::bind(h, boost::placeholders::_1), x,
+  return numericalDerivative11<Y, X>(std::bind(h, std::placeholders::_1), x,
                                      delta);
 }
 
@@ -164,14 +163,14 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const
  * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time
  */
 template<class Y, class X1, class X2, int N = traits<X1>::dimension>
-typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
+typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const std::function<Y(const X1&, const X2&)>& h,
     const X1& x1, const X2& x2, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
   return numericalDerivative11<Y, X1, N>(
-      boost::bind(h, boost::placeholders::_1, boost::cref(x2)), x1, delta);
+      std::bind(h, std::placeholders::_1, std::cref(x2)), x1, delta);
 }
 
 /** use a raw C++ function pointer */
@@ -179,7 +178,7 @@ template<class Y, class X1, class X2>
 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
     const X2& x2, double delta = 1e-5) {
   return numericalDerivative21<Y, X1, X2>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
       delta);
 }
 
@@ -193,14 +192,14 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(cons
  * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time
  */
 template<class Y, class X1, class X2, int N = traits<X2>::dimension>
-typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
+typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(std::function<Y(const X1&, const X2&)> h,
     const X1& x1, const X2& x2, double delta = 1e-5) {
 //  BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
 //       "Template argument X1 must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
        "Template argument X2 must be a manifold type.");
   return numericalDerivative11<Y, X2, N>(
-      boost::bind(h, boost::cref(x1), boost::placeholders::_1), x2, delta);
+      std::bind(h, std::cref(x1), std::placeholders::_1), x2, delta);
 }
 
 /** use a raw C++ function pointer */
@@ -208,7 +207,7 @@ template<class Y, class X1, class X2>
 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
     const X2& x2, double delta = 1e-5) {
   return numericalDerivative22<Y, X1, X2>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
       delta);
 }
 
@@ -225,14 +224,14 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(cons
  */
 template<class Y, class X1, class X2, class X3, int N = traits<X1>::dimension>
 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
-    boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
   return numericalDerivative11<Y, X1, N>(
-      boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)),
+      std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3)),
       x1, delta);
 }
 
@@ -240,8 +239,8 @@ template<class Y, class X1, class X2, class X3>
 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalDerivative31<Y, X1, X2, X3>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3),
+                  std::placeholders::_3),
       x1, x2, x3, delta);
 }
 
@@ -258,14 +257,14 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(cons
  */
 template<class Y, class X1, class X2, class X3, int N = traits<X2>::dimension>
 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
-    boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
       "Template argument X2 must be a manifold type.");
   return numericalDerivative11<Y, X2, N>(
-      boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3)),
+      std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3)),
       x2, delta);
 }
 
@@ -273,8 +272,8 @@ template<class Y, class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalDerivative32<Y, X1, X2, X3>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3),
+                  std::placeholders::_3),
       x1, x2, x3, delta);
 }
 
@@ -291,14 +290,14 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*
  */
 template<class Y, class X1, class X2, class X3, int N = traits<X3>::dimension>
 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
-    boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
       "Template argument X3 must be a manifold type.");
   return numericalDerivative11<Y, X3, N>(
-      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1),
+      std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1),
       x3, delta);
 }
 
@@ -306,8 +305,8 @@ template<class Y, class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalDerivative33<Y, X1, X2, X3>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3),
+                  std::placeholders::_3),
       x1, x2, x3, delta);
 }
 
@@ -324,15 +323,15 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X1>::dimension>
 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
   return numericalDerivative11<Y, X1, N>(
-      boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
+      std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
-                  boost::cref(x4)),
+                  std::cref(x4)),
       x1, delta);
 }
 
@@ -340,8 +339,8 @@ template<class Y, class X1, class X2, class X3, class X4>
 inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
   return numericalDerivative41<Y, X1, X2, X3, X4>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4),
+                  std::placeholders::_3, std::placeholders::_4),
       x1, x2, x3, x4);
 }
 
@@ -358,15 +357,15 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X2>::dimension>
 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
       "Template argument X2 must be a manifold type.");
   return numericalDerivative11<Y, X2, N>(
-      boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3),
+      std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
-                  boost::cref(x4)),
+                  std::cref(x4)),
       x2, delta);
 }
 
@@ -374,8 +373,8 @@ template<class Y, class X1, class X2, class X3, class X4>
 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&),
   const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
   return numericalDerivative42<Y, X1, X2, X3, X4>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4),
+                  std::placeholders::_3, std::placeholders::_4),
       x1, x2, x3, x4);
 }
 
@@ -392,15 +391,15 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X3>::dimension>
 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
       "Template argument X3 must be a manifold type.");
   return numericalDerivative11<Y, X3, N>(
-      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1,
+      std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
-                  boost::cref(x4)),
+                  std::cref(x4)),
       x3, delta);
 }
 
@@ -408,8 +407,8 @@ template<class Y, class X1, class X2, class X3, class X4>
 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
   return numericalDerivative43<Y, X1, X2, X3, X4>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4),
+                  std::placeholders::_3, std::placeholders::_4),
       x1, x2, x3, x4);
 }
 
@@ -426,15 +425,15 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, int N = traits<X4>::dimension>
 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
       "Template argument X4 must be a manifold type.");
   return numericalDerivative11<Y, X4, N>(
-      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
-                  boost::placeholders::_1),
+                  std::placeholders::_1),
       x4, delta);
 }
 
@@ -442,8 +441,8 @@ template<class Y, class X1, class X2, class X3, class X4>
 inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&),
   const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
   return numericalDerivative44<Y, X1, X2, X3, X4>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4),
+                  std::placeholders::_3, std::placeholders::_4),
       x1, x2, x3, x4);
 }
 
@@ -461,15 +460,15 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X1>::dimension>
 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
   return numericalDerivative11<Y, X1, N>(
-      boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
+      std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
-                  boost::cref(x4), boost::cref(x5)),
+                  std::cref(x4), std::cref(x5)),
       x1, delta);
 }
 
@@ -477,9 +476,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5>
 inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   return numericalDerivative51<Y, X1, X2, X3, X4, X5>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4,
+                  std::placeholders::_3, std::placeholders::_4,
-                  boost::placeholders::_5),
+                  std::placeholders::_5),
       x1, x2, x3, x4, x5);
 }
 
@@ -497,15 +496,15 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X2>::dimension>
 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
   return numericalDerivative11<Y, X2, N>(
-      boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3),
+      std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
-                  boost::cref(x4), boost::cref(x5)),
+                  std::cref(x4), std::cref(x5)),
       x2, delta);
 }
 
@@ -513,9 +512,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5>
 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   return numericalDerivative52<Y, X1, X2, X3, X4, X5>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4,
+                  std::placeholders::_3, std::placeholders::_4,
-                  boost::placeholders::_5),
+                  std::placeholders::_5),
       x1, x2, x3, x4, x5);
 }
 
@@ -533,15 +532,15 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X3>::dimension>
 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
   return numericalDerivative11<Y, X3, N>(
-      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1,
+      std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
-                  boost::cref(x4), boost::cref(x5)),
+                  std::cref(x4), std::cref(x5)),
       x3, delta);
 }
 
@@ -549,9 +548,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5>
 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   return numericalDerivative53<Y, X1, X2, X3, X4, X5>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4,
+                  std::placeholders::_3, std::placeholders::_4,
-                  boost::placeholders::_5),
+                  std::placeholders::_5),
       x1, x2, x3, x4, x5);
 }
 
@@ -569,15 +568,15 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X4>::dimension>
 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
   return numericalDerivative11<Y, X4, N>(
-      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
-                  boost::placeholders::_1, boost::cref(x5)),
+                  std::placeholders::_1, std::cref(x5)),
       x4, delta);
 }
 
@@ -585,9 +584,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5>
 inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   return numericalDerivative54<Y, X1, X2, X3, X4, X5>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4,
+                  std::placeholders::_3, std::placeholders::_4,
-                  boost::placeholders::_5),
+                  std::placeholders::_5),
       x1, x2, x3, x4, x5);
 }
 
@@ -605,15 +604,15 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X5>::dimension>
 typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
   return numericalDerivative11<Y, X5, N>(
-      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
-                  boost::cref(x4), boost::placeholders::_1),
+                  std::cref(x4), std::placeholders::_1),
       x5, delta);
 }
 
@@ -621,9 +620,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5>
 inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
   return numericalDerivative55<Y, X1, X2, X3, X4, X5>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4,
+                  std::placeholders::_3, std::placeholders::_4,
-                  boost::placeholders::_5),
+                  std::placeholders::_5),
       x1, x2, x3, x4, x5);
 }
 
@@ -642,15 +641,15 @@ inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X1>::dimension>
 typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
   return numericalDerivative11<Y, X1, N>(
-      boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
+      std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
-                  boost::cref(x4), boost::cref(x5), boost::cref(x6)),
+                  std::cref(x4), std::cref(x5), std::cref(x6)),
       x1, delta);
 }
 
@@ -658,9 +657,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
 inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4,
+                  std::placeholders::_3, std::placeholders::_4,
-                  boost::placeholders::_5, boost::placeholders::_6),
+                  std::placeholders::_5, std::placeholders::_6),
       x1, x2, x3, x4, x5, x6);
 }
 
@@ -679,15 +678,15 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X2>::dimension>
 typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
   return numericalDerivative11<Y, X2, N>(
-      boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3),
+      std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
-                  boost::cref(x4), boost::cref(x5), boost::cref(x6)),
+                  std::cref(x4), std::cref(x5), std::cref(x6)),
       x2, delta);
 }
 
@@ -695,9 +694,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
 inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4,
+                  std::placeholders::_3, std::placeholders::_4,
-                  boost::placeholders::_5, boost::placeholders::_6),
+                  std::placeholders::_5, std::placeholders::_6),
       x1, x2, x3, x4, x5, x6);
 }
 
@@ -716,15 +715,15 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X3>::dimension>
 typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
   return numericalDerivative11<Y, X3, N>(
-      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1,
+      std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
-                  boost::cref(x4), boost::cref(x5), boost::cref(x6)),
+                  std::cref(x4), std::cref(x5), std::cref(x6)),
       x3, delta);
 }
 
@@ -732,9 +731,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
 inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4,
+                  std::placeholders::_3, std::placeholders::_4,
-                  boost::placeholders::_5, boost::placeholders::_6),
+                  std::placeholders::_5, std::placeholders::_6),
       x1, x2, x3, x4, x5, x6);
 }
 
@@ -753,15 +752,15 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X4>::dimension>
 typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
   return numericalDerivative11<Y, X4, N>(
-      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
-                  boost::placeholders::_1, boost::cref(x5), boost::cref(x6)),
+                  std::placeholders::_1, std::cref(x5), std::cref(x6)),
       x4, delta);
 }
 
@@ -769,9 +768,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
 inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   return numericalDerivative64<Y, X1, X2, X3, X4, X5>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4,
+                  std::placeholders::_3, std::placeholders::_4,
-                  boost::placeholders::_5, boost::placeholders::_6),
+                  std::placeholders::_5, std::placeholders::_6),
       x1, x2, x3, x4, x5, x6);
 }
 
@@ -790,15 +789,15 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X5>::dimension>
 typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
     const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
   return numericalDerivative11<Y, X5, N>(
-      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
-                  boost::cref(x4), boost::placeholders::_1, boost::cref(x6)),
+                  std::cref(x4), std::placeholders::_1, std::cref(x6)),
       x5, delta);
 }
 
@@ -806,9 +805,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
 inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4,
+                  std::placeholders::_3, std::placeholders::_4,
-                  boost::placeholders::_5, boost::placeholders::_6),
+                  std::placeholders::_5, std::placeholders::_6),
       x1, x2, x3, x4, x5, x6);
 }
 
@@ -827,7 +826,7 @@ inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*
  */
 template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X6>::dimension>
 typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
-    boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
+    std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
     double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
@@ -835,8 +834,8 @@ typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
   return numericalDerivative11<Y, X6, N>(
-      boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
-                  boost::cref(x4), boost::cref(x5), boost::placeholders::_1),
+                  std::cref(x4), std::cref(x5), std::placeholders::_1),
       x6, delta);
 }
 
@@ -844,9 +843,9 @@ template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
 inline typename internal::FixedSizeMatrix<Y,X6>::type numericalDerivative66(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
   return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(
-      boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
-                  boost::placeholders::_3, boost::placeholders::_4,
+                  std::placeholders::_3, std::placeholders::_4,
-                  boost::placeholders::_5, boost::placeholders::_6),
+                  std::placeholders::_5, std::placeholders::_6),
       x1, x2, x3, x4, x5, x6);
 }
 
@@ -859,22 +858,22 @@ inline typename internal::FixedSizeMatrix<Y,X6>::type numericalDerivative66(Y (*
  * @return n*n Hessian matrix computed via central differencing
  */
 template<class X>
-inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(boost::function<double(const X&)> f, const X& x,
+inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(std::function<double(const X&)> f, const X& x,
     double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
       "Template argument X must be a manifold type.");
   typedef Eigen::Matrix<double, traits<X>::dimension, 1> VectorD;
-  typedef boost::function<double(const X&)> F;
+  typedef std::function<double(const X&)> F;
-  typedef boost::function<VectorD(F, const X&, double)> G;
+  typedef std::function<VectorD(F, const X&, double)> G;
   G ng = static_cast<G>(numericalGradient<X> );
   return numericalDerivative11<VectorD, X>(
-      boost::bind(ng, f, boost::placeholders::_1, delta), x, delta);
+      std::bind(ng, f, std::placeholders::_1, delta), x, delta);
 }
 
 template<class X>
 inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f)(const X&), const X& x, double delta =
     1e-5) {
-  return numericalHessian(boost::function<double(const X&)>(f), x, delta);
+  return numericalHessian(std::function<double(const X&)>(f), x, delta);
 }
 
 /** Helper class that computes the derivative of f w.r.t. x1, centered about
@@ -882,86 +881,86 @@ inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f
  */
 template<class X1, class X2>
 class G_x1 {
-  const boost::function<double(const X1&, const X2&)>& f_;
+  const std::function<double(const X1&, const X2&)>& f_;
   X1 x1_;
   double delta_;
 public:
   typedef typename internal::FixedSizeMatrix<X1>::type Vector;
 
-  G_x1(const boost::function<double(const X1&, const X2&)>& f, const X1& x1,
+  G_x1(const std::function<double(const X1&, const X2&)>& f, const X1& x1,
       double delta) :
       f_(f), x1_(x1), delta_(delta) {
   }
   Vector operator()(const X2& x2) {
     return numericalGradient<X1>(
-        boost::bind(f_, boost::placeholders::_1, boost::cref(x2)), x1_, delta_);
+        std::bind(f_, std::placeholders::_1, std::cref(x2)), x1_, delta_);
   }
 };
 
 template<class X1, class X2>
 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(
-    boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
+    std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
     double delta = 1e-5) {
   typedef typename internal::FixedSizeMatrix<X1>::type Vector;
   G_x1<X1, X2> g_x1(f, x1, delta);
   return numericalDerivative11<Vector, X2>(
-      boost::function<Vector(const X2&)>(
+      std::function<Vector(const X2&)>(
-          boost::bind<Vector>(boost::ref(g_x1), boost::placeholders::_1)),
+          std::bind<Vector>(std::ref(g_x1), std::placeholders::_1)),
       x2, delta);
 }
 
 template<class X1, class X2>
 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(double (*f)(const X1&, const X2&),
     const X1& x1, const X2& x2, double delta = 1e-5) {
-  return numericalHessian212(boost::function<double(const X1&, const X2&)>(f),
+  return numericalHessian212(std::function<double(const X1&, const X2&)>(f),
       x1, x2, delta);
 }
 
 template<class X1, class X2>
 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
-    boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
+    std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
     double delta = 1e-5) {
 
   typedef typename internal::FixedSizeMatrix<X1>::type Vector;
 
-  Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
+  Vector (*numGrad)(std::function<double(const X1&)>, const X1&,
       double) = &numericalGradient<X1>;
-  boost::function<double(const X1&)> f2(
+  std::function<double(const X1&)> f2(
-      boost::bind(f, boost::placeholders::_1, boost::cref(x2)));
+      std::bind(f, std::placeholders::_1, std::cref(x2)));
 
   return numericalDerivative11<Vector, X1>(
-      boost::function<Vector(const X1&)>(
+      std::function<Vector(const X1&)>(
-          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+          std::bind(numGrad, f2, std::placeholders::_1, delta)),
       x1, delta);
 }
 
 template<class X1, class X2>
 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(double (*f)(const X1&, const X2&),
     const X1& x1, const X2& x2, double delta = 1e-5) {
-  return numericalHessian211(boost::function<double(const X1&, const X2&)>(f),
+  return numericalHessian211(std::function<double(const X1&, const X2&)>(f),
       x1, x2, delta);
 }
 
 template<class X1, class X2>
 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
-    boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
+    std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
     double delta = 1e-5) {
   typedef typename internal::FixedSizeMatrix<X2>::type Vector;
-  Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
+  Vector (*numGrad)(std::function<double(const X2&)>, const X2&,
       double) = &numericalGradient<X2>;
-  boost::function<double(const X2&)> f2(
+  std::function<double(const X2&)> f2(
-      boost::bind(f, boost::cref(x1), boost::placeholders::_1));
+      std::bind(f, std::cref(x1), std::placeholders::_1));
 
   return numericalDerivative11<Vector, X2>(
-      boost::function<Vector(const X2&)>(
+      std::function<Vector(const X2&)>(
-          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+          std::bind(numGrad, f2, std::placeholders::_1, delta)),
       x2, delta);
 }
 
 template<class X1, class X2>
 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(double (*f)(const X1&, const X2&),
     const X1& x1, const X2& x2, double delta = 1e-5) {
-  return numericalHessian222(boost::function<double(const X1&, const X2&)>(f),
+  return numericalHessian222(std::function<double(const X1&, const X2&)>(f),
       x1, x2, delta);
 }
 
@@ -971,17 +970,17 @@ inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(doubl
 /* **************************************************************** */
 template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
-    boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
+    std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   typedef typename internal::FixedSizeMatrix<X1>::type Vector;
-  Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
+  Vector (*numGrad)(std::function<double(const X1&)>, const X1&,
       double) = &numericalGradient<X1>;
-  boost::function<double(const X1&)> f2(boost::bind(
+  std::function<double(const X1&)> f2(std::bind(
-      f, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)));
+      f, std::placeholders::_1, std::cref(x2), std::cref(x3)));
 
   return numericalDerivative11<Vector, X1>(
-      boost::function<Vector(const X1&)>(
+      std::function<Vector(const X1&)>(
-          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+          std::bind(numGrad, f2, std::placeholders::_1, delta)),
       x1, delta);
 }
 
@@ -989,24 +988,24 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(double (*f)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian311(
-      boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
+      std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
       delta);
 }
 
 /* **************************************************************** */
 template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
-    boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
+    std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   typedef typename internal::FixedSizeMatrix<X2>::type Vector;
-  Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
+  Vector (*numGrad)(std::function<double(const X2&)>, const X2&,
       double) = &numericalGradient<X2>;
-  boost::function<double(const X2&)> f2(boost::bind(
+  std::function<double(const X2&)> f2(std::bind(
-      f, boost::cref(x1), boost::placeholders::_1, boost::cref(x3)));
+      f, std::cref(x1), std::placeholders::_1, std::cref(x3)));
 
   return numericalDerivative11<Vector, X2>(
-      boost::function<Vector(const X2&)>(
+      std::function<Vector(const X2&)>(
-          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+          std::bind(numGrad, f2, std::placeholders::_1, delta)),
       x2, delta);
 }
 
@@ -1014,24 +1013,24 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian322(
-      boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
+      std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
       delta);
 }
 
 /* **************************************************************** */
 template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
-    boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
+    std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   typedef typename internal::FixedSizeMatrix<X3>::type Vector;
-  Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
+  Vector (*numGrad)(std::function<double(const X3&)>, const X3&,
       double) = &numericalGradient<X3>;
-  boost::function<double(const X3&)> f2(boost::bind(
+  std::function<double(const X3&)> f2(std::bind(
-      f, boost::cref(x1), boost::cref(x2), boost::placeholders::_1));
+      f, std::cref(x1), std::cref(x2), std::placeholders::_1));
 
   return numericalDerivative11<Vector, X3>(
-      boost::function<Vector(const X3&)>(
+      std::function<Vector(const X3&)>(
-          boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
+          std::bind(numGrad, f2, std::placeholders::_1, delta)),
       x3, delta);
 }
 
@@ -1039,41 +1038,41 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(double (*f)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian333(
-      boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
+      std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
       delta);
 }
 
 /* **************************************************************** */
 template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
-    boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
+    std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X1, X2>(
-      boost::function<double(const X1&, const X2&)>(
+      std::function<double(const X1&, const X2&)>(
-          boost::bind(f, boost::placeholders::_1, boost::placeholders::_2,
+          std::bind(f, std::placeholders::_1, std::placeholders::_2,
-                      boost::cref(x3))),
+                      std::cref(x3))),
       x1, x2, delta);
 }
 
 template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
-    boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
+    std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X1, X3>(
-      boost::function<double(const X1&, const X3&)>(
+      std::function<double(const X1&, const X3&)>(
-          boost::bind(f, boost::placeholders::_1, boost::cref(x2),
+          std::bind(f, std::placeholders::_1, std::cref(x2),
-                      boost::placeholders::_2)),
+                      std::placeholders::_2)),
       x1, x3, delta);
 }
 
 template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
-    boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
+    std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X2, X3>(
-      boost::function<double(const X2&, const X3&)>(
+      std::function<double(const X2&, const X3&)>(
-          boost::bind(f, boost::cref(x1), boost::placeholders::_1,
+          std::bind(f, std::cref(x1), std::placeholders::_1,
-                      boost::placeholders::_2)),
+                      std::placeholders::_2)),
       x2, x3, delta);
 }
 
@@ -1082,7 +1081,7 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(double (*f)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian312(
-      boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
+      std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
       delta);
 }
 
@@ -1090,7 +1089,7 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(double (*f)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian313(
-      boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
+      std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
       delta);
 }
 
@@ -1098,7 +1097,7 @@ template<class X1, class X2, class X3>
 inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(double (*f)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian323(
-      boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
+      std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
       delta);
 }
 

gtsam/base/tests/testOptionalJacobian.cpp

@@ -24,40 +24,33 @@ using namespace std;
 using namespace gtsam;
 
 //******************************************************************************
+#define TEST_CONSTRUCTOR(DIM1, DIM2, X, TRUTHY) \
+  {                                             \
+    OptionalJacobian<DIM1, DIM2> H(X);          \
+    EXPECT(H == TRUTHY);                        \
+  }
 TEST( OptionalJacobian, Constructors ) {
   Matrix23 fixed;
-
-  OptionalJacobian<2, 3> H1;
-  EXPECT(!H1);
-
-  OptionalJacobian<2, 3> H2(fixed);
-  EXPECT(H2);
-
-  OptionalJacobian<2, 3> H3(&fixed);
-  EXPECT(H3);
-
   Matrix dynamic;
-  OptionalJacobian<2, 3> H4(dynamic);
-  EXPECT(H4);
-
-  OptionalJacobian<2, 3> H5(boost::none);
-  EXPECT(!H5);
-
   boost::optional<Matrix&> optional(dynamic);
-  OptionalJacobian<2, 3> H6(optional);
-  EXPECT(H6);
 
+  OptionalJacobian<2, 3> H;
+  EXPECT(!H);
+
+  TEST_CONSTRUCTOR(2, 3, fixed, true);
+  TEST_CONSTRUCTOR(2, 3, &fixed, true);
+  TEST_CONSTRUCTOR(2, 3, dynamic, true);
+  TEST_CONSTRUCTOR(2, 3, &dynamic, true);
+  TEST_CONSTRUCTOR(2, 3, boost::none, false);
+  TEST_CONSTRUCTOR(2, 3, optional, true);
+
+  // Test dynamic
   OptionalJacobian<-1, -1> H7;
   EXPECT(!H7);
 
-  OptionalJacobian<-1, -1> H8(dynamic);
+  TEST_CONSTRUCTOR(-1, -1, dynamic, true);
-  EXPECT(H8);
+  TEST_CONSTRUCTOR(-1, -1, boost::none, false);
-
+  TEST_CONSTRUCTOR(-1, -1, optional, true);
-  OptionalJacobian<-1, -1> H9(boost::none);
-  EXPECT(!H9);
-
-  OptionalJacobian<-1, -1> H10(optional);
-  EXPECT(H10);
 }
 
 //******************************************************************************
@@ -101,6 +94,25 @@ TEST( OptionalJacobian, Fixed) {
   dynamic2.setOnes();
   test(dynamic2);
   EXPECT(assert_equal(kTestMatrix, dynamic2));
+
+  {  // Dynamic pointer
+    // Passing in an empty matrix means we want it resized
+    Matrix dynamic0;
+    test(&dynamic0);
+    EXPECT(assert_equal(kTestMatrix, dynamic0));
+
+    // Dynamic wrong size
+    Matrix dynamic1(3, 5);
+    dynamic1.setOnes();
+    test(&dynamic1);
+    EXPECT(assert_equal(kTestMatrix, dynamic1));
+
+    // Dynamic right size
+    Matrix dynamic2(2, 5);
+    dynamic2.setOnes();
+    test(&dynamic2);
+    EXPECT(assert_equal(kTestMatrix, dynamic2));
+  }
 }
 
 //******************************************************************************

gtsam/base/treeTraversal-inst.h

@@ -158,8 +158,9 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData,
   // Typedefs
   typedef typename FOREST::Node Node;
 
-  internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
+  tbb::task::spawn_root_and_wait(
-      visitorPost, problemSizeThreshold);
+      internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
+          visitorPost, problemSizeThreshold));
 #else
   DepthFirstForest(forest, rootData, visitorPre, visitorPost);
 #endif

gtsam/base/treeTraversal/parallelTraversalTasks.h

@@ -22,7 +22,7 @@
 #include <boost/make_shared.hpp>
 
 #ifdef GTSAM_USE_TBB
-#include <tbb/task_group.h>         // tbb::task_group
+#include <tbb/task.h>               // tbb::task, tbb::task_list
 #include <tbb/scalable_allocator.h> // tbb::scalable_allocator
 
 namespace gtsam {
@@ -34,7 +34,7 @@ namespace gtsam {
 
       /* ************************************************************************* */
       template<typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      class PreOrderTask
+      class PreOrderTask : public tbb::task
       {
       public:
         const boost::shared_ptr<NODE>& treeNode;
@@ -42,30 +42,28 @@ namespace gtsam {
         VISITOR_PRE& visitorPre;
         VISITOR_POST& visitorPost;
         int problemSizeThreshold;
-        tbb::task_group& tg;
         bool makeNewTasks;
 
-        // Keep track of order phase across multiple calls to the same functor
+        bool isPostOrderPhase;
-        mutable bool isPostOrderPhase;
 
         PreOrderTask(const boost::shared_ptr<NODE>& treeNode, const boost::shared_ptr<DATA>& myData,
                      VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold,
-                     tbb::task_group& tg, bool makeNewTasks = true)
+                     bool makeNewTasks = true)
             : treeNode(treeNode),
               myData(myData),
               visitorPre(visitorPre),
               visitorPost(visitorPost),
               problemSizeThreshold(problemSizeThreshold),
-              tg(tg),
               makeNewTasks(makeNewTasks),
               isPostOrderPhase(false) {}
 
-        void operator()() const
+        tbb::task* execute() override
         {
           if(isPostOrderPhase)
           {
             // Run the post-order visitor since this task was recycled to run the post-order visitor
             (void) visitorPost(treeNode, *myData);
+            return nullptr;
           }
           else
           {
@@ -73,10 +71,14 @@ namespace gtsam {
             {
               if(!treeNode->children.empty())
               {
+                // Allocate post-order task as a continuation
+                isPostOrderPhase = true;
+                recycle_as_continuation();
+
                 bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold);
 
-                // If we have child tasks, start subtasks and wait for them to complete
+                tbb::task* firstChild = 0;
-                tbb::task_group ctg;
+                tbb::task_list childTasks;
                 for(const boost::shared_ptr<NODE>& child: treeNode->children)
                 {
                   // Process child in a subtask.  Important:  Run visitorPre before calling
@@ -84,30 +86,37 @@ namespace gtsam {
                   // allocated an extra child, this causes a TBB error.
                   boost::shared_ptr<DATA> childData = boost::allocate_shared<DATA>(
                       tbb::scalable_allocator<DATA>(), visitorPre(child, *myData));
-                  ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost,
+                  tbb::task* childTask =
-                      problemSizeThreshold, ctg, overThreshold));
+                      new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost,
+                                                          problemSizeThreshold, overThreshold);
+                  if (firstChild)
+                    childTasks.push_back(*childTask);
+                  else
+                    firstChild = childTask;
                 }
-                ctg.wait();
 
-                // Allocate post-order task as a continuation
+                // If we have child tasks, start subtasks and wait for them to complete
-                isPostOrderPhase = true;
+                set_ref_count((int)treeNode->children.size());
-                tg.run(*this);
+                spawn(childTasks);
+                return firstChild;
               }
               else
               {
                 // Run the post-order visitor in this task if we have no children
                 (void) visitorPost(treeNode, *myData);
+                return nullptr;
               }
             }
             else
             {
               // Process this node and its children in this task
               processNodeRecursively(treeNode, *myData);
+              return nullptr;
             }
           }
         }
 
-        void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData) const
+        void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData)
         {
           for(const boost::shared_ptr<NODE>& child: node->children)
           {
@@ -122,7 +131,7 @@ namespace gtsam {
 
       /* ************************************************************************* */
       template<typename ROOTS, typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      class RootTask
+      class RootTask : public tbb::task
       {
       public:
         const ROOTS& roots;
@@ -130,31 +139,38 @@ namespace gtsam {
         VISITOR_PRE& visitorPre;
         VISITOR_POST& visitorPost;
         int problemSizeThreshold;
-        tbb::task_group& tg;
         RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
-          int problemSizeThreshold, tbb::task_group& tg) :
+          int problemSizeThreshold) :
           roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
-          problemSizeThreshold(problemSizeThreshold), tg(tg) {}
+          problemSizeThreshold(problemSizeThreshold) {}
 
-        void operator()() const
+        tbb::task* execute() override
         {
           typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
           // Create data and tasks for our children
+          tbb::task_list tasks;
           for(const boost::shared_ptr<NODE>& root: roots)
           {
             boost::shared_ptr<DATA> rootData = boost::allocate_shared<DATA>(tbb::scalable_allocator<DATA>(), visitorPre(root, myData));
-            tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg));
+            tasks.push_back(*new(allocate_child())
+              PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold));
           }
+          // Set TBB ref count
+          set_ref_count(1 + (int) roots.size());
+          // Spawn tasks
+          spawn_and_wait_for_all(tasks);
+          // Return nullptr
+          return nullptr;
         }
       };
 
       template<typename NODE, typename ROOTS, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
+      RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST>&
+        CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
       {
           typedef RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST> RootTask;
-          tbb::task_group tg;
+          return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold);
-          tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg));
+        }
-      }
 
     }
 

gtsam/base/utilities.h

@@ -0,0 +1,29 @@
+#pragma once
+
+namespace gtsam {
+/**
+ * For Python __str__().
+ * Redirect std cout to a string stream so we can return a string representation
+ * of an object when it prints to cout.
+ * https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string
+ */
+struct RedirectCout {
+  /// constructor -- redirect stdout buffer to a stringstream buffer
+  RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {}
+
+  /// return the string
+  std::string str() const {
+    return ssBuffer_.str();
+  }
+
+  /// destructor -- redirect stdout buffer to its original buffer
+  ~RedirectCout() {
+    std::cout.rdbuf(coutBuffer_);
+  }
+
+private:
+  std::stringstream ssBuffer_;
+  std::streambuf* coutBuffer_;
+};
+
+}

gtsam/basis/Basis.h

@@ -0,0 +1,507 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Basis.h
+ * @brief Compute an interpolating basis
+ * @author Varun Agrawal, Jing Dong, Frank Dellaert
+ * @date July 4, 2020
+ */
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/basis/ParameterMatrix.h>
+
+#include <iostream>
+
+/**
+ * This file supports creating continuous functions `f(x;p)` as a linear
+ * combination of `basis functions` such as the Fourier basis on SO(2) or a set
+ * of Chebyshev polynomials on [-1,1].
+ *
+ * In the expression `f(x;p)` the variable `x` is
+ * the continuous argument at which the function is evaluated, and `p` are
+ * the parameters which are coefficients of the different basis functions,
+ * e.g. p = [4; 3; 2] => 4 + 3x + 2x^2 for a polynomial.
+ * However, different parameterizations are also possible.
+
+ * The `Basis` class below defines a number of functors that can be used to
+ * evaluate `f(x;p)` at a given `x`, and these functors also calculate
+ * the Jacobian of `f(x;p)` with respect to the parameters `p`.
+ * This is actually the most important calculation, as it will allow GTSAM
+ * to optimize over the parameters `p`.
+
+ * This functionality is implemented using the `CRTP` or "Curiously recurring
+ * template pattern" C++ idiom, which is a meta-programming technique in which
+ * the derived class is passed as a template argument to `Basis<DERIVED>`.
+ * The DERIVED class is assumed to satisfy a C++ concept,
+ * i.e., we expect it to define the following types and methods:
+
+  - type `Parameters`: the parameters `p` in f(x;p)
+  - `CalculateWeights(size_t N, double x, double a=default, double b=default)`
+  - `DerivativeWeights(size_t N, double x, double a=default, double b=default)`
+
+  where `Weights` is an N*1 row vector which defines the basis values for the
+ polynomial at the specified point `x`.
+
+ E.g. A Fourier series would give the following:
+  - `CalculateWeights` -> For N=5, the values for the bases:
+        [1, cos(x), sin(x), cos(2x), sin(2x)]
+  - `DerivativeWeights` -> For N=5, these are:
+        [0, -sin(x), cos(x), -2sin(2x), 2cos(x)]
+
+ Note that for a pseudo-spectral basis (as in Chebyshev2), the weights are
+ instead the values for the Barycentric interpolation formula, since the values
+ at the polynomial points (e.g. Chebyshev points) define the bases.
+ */
+
+namespace gtsam {
+
+using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
+
+/**
+ * @brief Function for computing the kronecker product of the 1*N Weight vector
+ * `w` with the MxM identity matrix `I` efficiently.
+ * The main reason for this is so we don't need to use Eigen's Unsupported
+ * library.
+ *
+ * @tparam M Size of the identity matrix.
+ * @param w The weights of the polynomial.
+ * @return Mx(M*N) kronecker product [w(0)*I, w(1)*I, ..., w(N-1)*I]
+ */
+template <size_t M>
+Matrix kroneckerProductIdentity(const Weights& w) {
+  Matrix result(M, w.cols() * M);
+  result.setZero();
+
+  for (int i = 0; i < w.cols(); i++) {
+    result.block(0, i * M, M, M).diagonal().array() = w(i);
+  }
+  return result;
+}
+
+/// CRTP Base class for function bases
+template <typename DERIVED>
+class GTSAM_EXPORT Basis {
+ public:
+  /**
+   * Calculate weights for all x in vector X.
+   * Returns M*N matrix where M is the size of the vector X,
+   * and N is the number of basis functions.
+   */
+  static Matrix WeightMatrix(size_t N, const Vector& X) {
+    Matrix W(X.size(), N);
+    for (int i = 0; i < X.size(); i++)
+      W.row(i) = DERIVED::CalculateWeights(N, X(i));
+    return W;
+  }
+
+  /**
+   * @brief Calculate weights for all x in vector X, with interval [a,b].
+   *
+   * @param N The number of basis functions.
+   * @param X The vector for which to compute the weights.
+   * @param a The lower bound for the interval range.
+   * @param b The upper bound for the interval range.
+   * @return Returns M*N matrix where M is the size of the vector X.
+   */
+  static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b) {
+    Matrix W(X.size(), N);
+    for (int i = 0; i < X.size(); i++)
+      W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b);
+    return W;
+  }
+
+  /**
+   * An instance of an EvaluationFunctor calculates f(x;p) at a given `x`,
+   * applied to Parameters `p`.
+   * This functor is used to evaluate a parameterized function at a given scalar
+   * value x. When given a specific N*1 vector of Parameters, returns the scalar
+   * value of the function at x, possibly with Jacobians wrpt the parameters.
+   */
+  class EvaluationFunctor {
+   protected:
+    Weights weights_;
+
+   public:
+    /// For serialization
+    EvaluationFunctor() {}
+
+    /// Constructor with interval [a,b]
+    EvaluationFunctor(size_t N, double x)
+        : weights_(DERIVED::CalculateWeights(N, x)) {}
+
+    /// Constructor with interval [a,b]
+    EvaluationFunctor(size_t N, double x, double a, double b)
+        : weights_(DERIVED::CalculateWeights(N, x, a, b)) {}
+
+    /// Regular 1D evaluation
+    double apply(const typename DERIVED::Parameters& p,
+                 OptionalJacobian<-1, -1> H = boost::none) const {
+      if (H) *H = weights_;
+      return (weights_ * p)(0);
+    }
+
+    /// c++ sugar
+    double operator()(const typename DERIVED::Parameters& p,
+                      OptionalJacobian<-1, -1> H = boost::none) const {
+      return apply(p, H);  // might call apply in derived
+    }
+
+    void print(const std::string& s = "") const {
+      std::cout << s << (s != "" ? " " : "") << weights_ << std::endl;
+    }
+  };
+
+  /**
+   * VectorEvaluationFunctor at a given x, applied to ParameterMatrix<M>.
+   * This functor is used to evaluate a parameterized function at a given scalar
+   * value x. When given a specific M*N parameters, returns an M-vector the M
+   * corresponding functions at x, possibly with Jacobians wrpt the parameters.
+   */
+  template <int M>
+  class VectorEvaluationFunctor : protected EvaluationFunctor {
+   protected:
+    using VectorM = Eigen::Matrix<double, M, 1>;
+    using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
+    Jacobian H_;
+
+    /**
+     * Calculate the `M*(M*N)` Jacobian of this functor with respect to
+     * the M*N parameter matrix `P`.
+     * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
+     *      H =[ w(0) 0 w(1)  0 w(2)  0
+     *           0  w(0)  0 w(1)  0 w(2) ]
+     * i.e., the Kronecker product of weights_ with the MxM identity matrix.
+     */
+    void calculateJacobian() {
+      H_ = kroneckerProductIdentity<M>(this->weights_);
+    }
+
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    /// For serialization
+    VectorEvaluationFunctor() {}
+
+    /// Default Constructor
+    VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) {
+      calculateJacobian();
+    }
+
+    /// Constructor, with interval [a,b]
+    VectorEvaluationFunctor(size_t N, double x, double a, double b)
+        : EvaluationFunctor(N, x, a, b) {
+      calculateJacobian();
+    }
+
+    /// M-dimensional evaluation
+    VectorM apply(const ParameterMatrix<M>& P,
+                  OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
+      if (H) *H = H_;
+      return P.matrix() * this->weights_.transpose();
+    }
+
+    /// c++ sugar
+    VectorM operator()(const ParameterMatrix<M>& P,
+                       OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
+      return apply(P, H);
+    }
+  };
+
+  /**
+   * Given a M*N Matrix of M-vectors at N polynomial points, an instance of
+   * VectorComponentFunctor computes the N-vector value for a specific row
+   * component of the M-vectors at all the polynomial points.
+   *
+   * This component is specified by the row index i, with 0<i<M.
+   */
+  template <int M>
+  class VectorComponentFunctor : public EvaluationFunctor {
+   protected:
+    using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
+    size_t rowIndex_;
+    Jacobian H_;
+
+    /*
+     * Calculate the `1*(M*N)` Jacobian of this functor with respect to
+     * the M*N parameter matrix `P`.
+     * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
+     *      H=[w(0) 0    w(1) 0    w(2) 0]    for rowIndex==0
+     *      H=[0    w(0) 0    w(1) 0    w(2)] for rowIndex==1
+     * i.e., one row of the Kronecker product of weights_ with the
+     * MxM identity matrix. See also VectorEvaluationFunctor.
+     */
+    void calculateJacobian(size_t N) {
+      H_.setZero(1, M * N);
+      for (int j = 0; j < EvaluationFunctor::weights_.size(); j++)
+        H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j);
+    }
+
+   public:
+    /// For serialization
+    VectorComponentFunctor() {}
+
+    /// Construct with row index
+    VectorComponentFunctor(size_t N, size_t i, double x)
+        : EvaluationFunctor(N, x), rowIndex_(i) {
+      calculateJacobian(N);
+    }
+
+    /// Construct with row index and interval
+    VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
+        : EvaluationFunctor(N, x, a, b), rowIndex_(i) {
+      calculateJacobian(N);
+    }
+
+    /// Calculate component of component rowIndex_ of P
+    double apply(const ParameterMatrix<M>& P,
+                 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
+      if (H) *H = H_;
+      return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
+    }
+
+    /// c++ sugar
+    double operator()(const ParameterMatrix<M>& P,
+                      OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
+      return apply(P, H);
+    }
+  };
+
+  /**
+   * Manifold EvaluationFunctor at a given x, applied to ParameterMatrix<M>.
+   * This functor is used to evaluate a parameterized function at a given scalar
+   * value x. When given a specific M*N parameters, returns an M-vector the M
+   * corresponding functions at x, possibly with Jacobians wrpt the parameters.
+   *
+   * The difference with the VectorEvaluationFunctor is that after computing the
+   * M*1 vector xi=F(x;P), with x a scalar and P the M*N parameter vector, we
+   * also retract xi back to the T manifold.
+   * For example, if T==Rot3, then we first compute a 3-vector xi using x and P,
+   * and then map that 3-vector xi back to the Rot3 manifold, yielding a valid
+   * 3D rotation.
+   */
+  template <class T>
+  class ManifoldEvaluationFunctor
+      : public VectorEvaluationFunctor<traits<T>::dimension> {
+    enum { M = traits<T>::dimension };
+    using Base = VectorEvaluationFunctor<M>;
+
+   public:
+    /// For serialization
+    ManifoldEvaluationFunctor() {}
+
+    /// Default Constructor
+    ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {}
+
+    /// Constructor, with interval [a,b]
+    ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
+        : Base(N, x, a, b) {}
+
+    /// Manifold evaluation
+    T apply(const ParameterMatrix<M>& P,
+            OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
+      // Interpolate the M-dimensional vector to yield a vector in tangent space
+      Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H);
+
+      // Now call retract with this M-vector, possibly with derivatives
+      Eigen::Matrix<double, M, M> D_result_xi;
+      T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0);
+
+      // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N)
+      // derivative of interpolation and D_result_xi is MxM derivative of
+      // retract.
+      if (H) *H = D_result_xi * (*H);
+
+      // and return a T
+      return result;
+    }
+
+    /// c++ sugar
+    T operator()(const ParameterMatrix<M>& P,
+                 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
+      return apply(P, H);  // might call apply in derived
+    }
+  };
+
+  /// Base class for functors below that calculate derivative weights
+  class DerivativeFunctorBase {
+   protected:
+    Weights weights_;
+
+   public:
+    /// For serialization
+    DerivativeFunctorBase() {}
+
+    DerivativeFunctorBase(size_t N, double x)
+        : weights_(DERIVED::DerivativeWeights(N, x)) {}
+
+    DerivativeFunctorBase(size_t N, double x, double a, double b)
+        : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {}
+
+    void print(const std::string& s = "") const {
+      std::cout << s << (s != "" ? " " : "") << weights_ << std::endl;
+    }
+  };
+
+  /**
+   * An instance of a DerivativeFunctor calculates f'(x;p) at a given `x`,
+   * applied to Parameters `p`.
+   * When given a scalar value x and a specific N*1 vector of Parameters,
+   * this functor returns the scalar derivative value of the function at x,
+   * possibly with Jacobians wrpt the parameters.
+   */
+  class DerivativeFunctor : protected DerivativeFunctorBase {
+   public:
+    /// For serialization
+    DerivativeFunctor() {}
+
+    DerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {}
+
+    DerivativeFunctor(size_t N, double x, double a, double b)
+        : DerivativeFunctorBase(N, x, a, b) {}
+
+    double apply(const typename DERIVED::Parameters& p,
+                 OptionalJacobian</*1xN*/ -1, -1> H = boost::none) const {
+      if (H) *H = this->weights_;
+      return (this->weights_ * p)(0);
+    }
+    /// c++ sugar
+    double operator()(const typename DERIVED::Parameters& p,
+                      OptionalJacobian</*1xN*/ -1, -1> H = boost::none) const {
+      return apply(p, H);  // might call apply in derived
+    }
+  };
+
+  /**
+   * VectorDerivativeFunctor at a given x, applied to ParameterMatrix<M>.
+   *
+   * This functor is used to evaluate the derivatives of a parameterized
+   * function at a given scalar value x. When given a specific M*N parameters,
+   * returns an M-vector the M corresponding function derivatives at x, possibly
+   * with Jacobians wrpt the parameters.
+   */
+  template <int M>
+  class VectorDerivativeFunctor : protected DerivativeFunctorBase {
+   protected:
+    using VectorM = Eigen::Matrix<double, M, 1>;
+    using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
+    Jacobian H_;
+
+    /**
+     * Calculate the `M*(M*N)` Jacobian of this functor with respect to
+     * the M*N parameter matrix `P`.
+     * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
+     *      H =[ w(0) 0 w(1)  0 w(2)  0
+     *           0  w(0)  0 w(1)  0 w(2) ]
+     * i.e., the Kronecker product of weights_ with the MxM identity matrix.
+     */
+    void calculateJacobian() {
+      H_ = kroneckerProductIdentity<M>(this->weights_);
+    }
+
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    /// For serialization
+    VectorDerivativeFunctor() {}
+
+    /// Default Constructor
+    VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {
+      calculateJacobian();
+    }
+
+    /// Constructor, with optional interval [a,b]
+    VectorDerivativeFunctor(size_t N, double x, double a, double b)
+        : DerivativeFunctorBase(N, x, a, b) {
+      calculateJacobian();
+    }
+
+    VectorM apply(const ParameterMatrix<M>& P,
+                  OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
+      if (H) *H = H_;
+      return P.matrix() * this->weights_.transpose();
+    }
+    /// c++ sugar
+    VectorM operator()(
+        const ParameterMatrix<M>& P,
+        OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
+      return apply(P, H);
+    }
+  };
+
+  /**
+   * Given a M*N Matrix of M-vectors at N polynomial points, an instance of
+   * ComponentDerivativeFunctor computes the N-vector derivative for a specific
+   * row component of the M-vectors at all the polynomial points.
+   *
+   * This component is specified by the row index i, with 0<i<M.
+   */
+  template <int M>
+  class ComponentDerivativeFunctor : protected DerivativeFunctorBase {
+   protected:
+    using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
+    size_t rowIndex_;
+    Jacobian H_;
+
+    /*
+     * Calculate the `1*(M*N)` Jacobian of this functor with respect to
+     * the M*N parameter matrix `P`.
+     * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
+     *      H=[w(0) 0    w(1) 0    w(2) 0]    for rowIndex==0
+     *      H=[0    w(0) 0    w(1) 0    w(2)] for rowIndex==1
+     * i.e., one row of the Kronecker product of weights_ with the
+     * MxM identity matrix. See also VectorDerivativeFunctor.
+     */
+    void calculateJacobian(size_t N) {
+      H_.setZero(1, M * N);
+      for (int j = 0; j < this->weights_.size(); j++)
+        H_(0, rowIndex_ + j * M) = this->weights_(j);
+    }
+
+   public:
+    /// For serialization
+    ComponentDerivativeFunctor() {}
+
+    /// Construct with row index
+    ComponentDerivativeFunctor(size_t N, size_t i, double x)
+        : DerivativeFunctorBase(N, x), rowIndex_(i) {
+      calculateJacobian(N);
+    }
+
+    /// Construct with row index and interval
+    ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
+        : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) {
+      calculateJacobian(N);
+    }
+    /// Calculate derivative of component rowIndex_ of F
+    double apply(const ParameterMatrix<M>& P,
+                 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
+      if (H) *H = H_;
+      return P.row(rowIndex_) * this->weights_.transpose();
+    }
+    /// c++ sugar
+    double operator()(const ParameterMatrix<M>& P,
+                      OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
+      return apply(P, H);
+    }
+  };
+
+  // Vector version for MATLAB :-(
+  static double Derivative(double x, const Vector& p,  //
+                           OptionalJacobian</*1xN*/ -1, -1> H = boost::none) {
+    return DerivativeFunctor(x)(p.transpose(), H);
+  }
+};
+
+}  // namespace gtsam

gtsam/basis/BasisFactors.h

@@ -0,0 +1,424 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  BasisFactors.h
+ *  @brief Factor definitions for various Basis functors.
+ *  @author Varun Agrawal
+ *  @date July 4, 2020
+ **/
+
+#pragma once
+
+#include <gtsam/basis/Basis.h>
+#include <gtsam/nonlinear/FunctorizedFactor.h>
+
+namespace gtsam {
+
+/**
+ * @brief Factor for enforcing the scalar value of the polynomial BASIS
+ * representation at `x` is the same as the measurement `z` when using a
+ * pseudo-spectral parameterization.
+ *
+ * @tparam BASIS The basis class to use e.g. Chebyshev2
+ */
+template <class BASIS>
+class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor<double, Vector> {
+ private:
+  using Base = FunctorizedFactor<double, Vector>;
+
+ public:
+  EvaluationFactor() {}
+
+  /**
+   * @brief Construct a new EvaluationFactor object
+   *
+   * @param key Symbol for value to optimize.
+   * @param z The measurement value.
+   * @param model Noise model
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the polynomial.
+   */
+  EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model,
+                   const size_t N, double x)
+      : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {}
+
+  /**
+   * @brief Construct a new EvaluationFactor object
+   *
+   * @param key Symbol for value to optimize.
+   * @param z The measurement value.
+   * @param model Noise model
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the polynomial.
+   * @param a Lower bound for the polynomial.
+   * @param b Upper bound for the polynomial.
+   */
+  EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model,
+                   const size_t N, double x, double a, double b)
+      : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {}
+
+  virtual ~EvaluationFactor() {}
+};
+
+/**
+ * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix
+ * of size (M, N) is equal to a vector-valued measurement at the same point,
+ when
+ * using a pseudo-spectral parameterization.
+ *
+ * This factors tries to enforce the basis function evaluation `f(x;p)` to take
+ * on the value `z` at location `x`, providing a gradient on the parameters p.
+ * In a probabilistic estimation context, `z` is known as a measurement, and the
+ * parameterized basis function can be seen as a
+ * measurement prediction function.
+ *
+ * @param BASIS: The basis class to use e.g. Chebyshev2
+ * @param M: Size of the evaluated state vector.
+ */
+template <class BASIS, int M>
+class GTSAM_EXPORT VectorEvaluationFactor
+    : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
+ private:
+  using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
+
+ public:
+  VectorEvaluationFactor() {}
+
+  /**
+   * @brief Construct a new VectorEvaluationFactor object.
+   *
+   * @param key The key to the ParameterMatrix object used to represent the
+   * polynomial.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the basis polynomial.
+   */
+  VectorEvaluationFactor(Key key, const Vector &z,
+                         const SharedNoiseModel &model, const size_t N,
+                         double x)
+      : Base(key, z, model,
+             typename BASIS::template VectorEvaluationFunctor<M>(N, x)) {}
+
+  /**
+   * @brief Construct a new VectorEvaluationFactor object.
+   *
+   * @param key The key to the ParameterMatrix object used to represent the
+   * polynomial.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the basis polynomial.
+   * @param a Lower bound for the polynomial.
+   * @param b Upper bound for the polynomial.
+   */
+  VectorEvaluationFactor(Key key, const Vector &z,
+                         const SharedNoiseModel &model, const size_t N,
+                         double x, double a, double b)
+      : Base(key, z, model,
+             typename BASIS::template VectorEvaluationFunctor<M>(N, x, a, b)) {}
+
+  virtual ~VectorEvaluationFactor() {}
+};
+
+/**
+ * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix
+ * of size (P, N) is equal to specified measurement at the same point, when
+ * using a pseudo-spectral parameterization.
+ *
+ * This factor is similar to `VectorEvaluationFactor` with the key difference
+ * being that it only enforces the constraint for a single scalar in the vector,
+ * indexed by `i`.
+ *
+ * @param BASIS: The basis class to use e.g. Chebyshev2
+ * @param P: Size of the fixed-size vector.
+ *
+ * Example:
+ *  VectorComponentFactor<BASIS, P> controlPrior(key, measured, model,
+ *                                               N, i, t, a, b);
+ *  where N is the degree and i is the component index.
+ */
+template <class BASIS, size_t P>
+class GTSAM_EXPORT VectorComponentFactor
+    : public FunctorizedFactor<double, ParameterMatrix<P>> {
+ private:
+  using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
+
+ public:
+  VectorComponentFactor() {}
+
+  /**
+   * @brief Construct a new VectorComponentFactor object.
+   *
+   * @param key The key to the ParameterMatrix object used to represent the
+   * polynomial.
+   * @param z The scalar value at a specified index `i` of the full measurement
+   * vector.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param i The index for the evaluated vector to give us the desired scalar
+   * value.
+   * @param x The point at which to evaluate the basis polynomial.
+   */
+  VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
+                        const size_t N, size_t i, double x)
+      : Base(key, z, model,
+             typename BASIS::template VectorComponentFunctor<P>(N, i, x)) {}
+
+  /**
+   * @brief Construct a new VectorComponentFactor object.
+   *
+   * @param key The key to the ParameterMatrix object used to represent the
+   * polynomial.
+   * @param z The scalar value at a specified index `i` of the full measurement
+   * vector.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param i The index for the evaluated vector to give us the desired scalar
+   * value.
+   * @param x The point at which to evaluate 0the basis polynomial.
+   * @param a Lower bound for the polynomial.
+   * @param b Upper bound for the polynomial.
+   */
+  VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
+                        const size_t N, size_t i, double x, double a, double b)
+      : Base(
+            key, z, model,
+            typename BASIS::template VectorComponentFunctor<P>(N, i, x, a, b)) {
+  }
+
+  virtual ~VectorComponentFactor() {}
+};
+
+/**
+ * For a measurement value of type T i.e. `T z = g(x)`, this unary
+ * factor enforces that the polynomial basis, when evaluated at `x`, gives us
+ * the same `z`, i.e. `T z = g(x) = f(x;p)`.
+ *
+ * This is done via computations on the tangent space of the
+ * manifold of T.
+ *
+ * @param BASIS: The basis class to use e.g. Chebyshev2
+ * @param T: Object type which is synthesized by the provided functor.
+ *
+ * Example:
+ *  ManifoldEvaluationFactor<Chebyshev2, Rot3> rotationFactor(key, measurement,
+ * model, N, x, a, b);
+ *
+ * where `x` is the value (e.g. timestep) at which the rotation was evaluated.
+ */
+template <class BASIS, typename T>
+class GTSAM_EXPORT ManifoldEvaluationFactor
+    : public FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>> {
+ private:
+  using Base = FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>>;
+
+ public:
+  ManifoldEvaluationFactor() {}
+
+  /**
+   * @brief Construct a new ManifoldEvaluationFactor object.
+   *
+   * @param key Key for the state matrix parameterizing the function to estimate
+   * via the BASIS.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which the estimated function is evaluated.
+   */
+  ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model,
+                           const size_t N, double x)
+      : Base(key, z, model,
+             typename BASIS::template ManifoldEvaluationFunctor<T>(N, x)) {}
+
+  /**
+   * @brief Construct a new ManifoldEvaluationFactor object.
+   *
+   * @param key Key for the state matrix parameterizing the function to estimate
+   * via the BASIS.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which the estimated function is evaluated.
+   * @param a Lower bound for the polynomial.
+   * @param b Upper bound for the polynomial.
+   */
+  ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model,
+                           const size_t N, double x, double a, double b)
+      : Base(
+            key, z, model,
+            typename BASIS::template ManifoldEvaluationFunctor<T>(N, x, a, b)) {
+  }
+
+  virtual ~ManifoldEvaluationFactor() {}
+};
+
+/**
+ * A unary factor which enforces the evaluation of the derivative of a BASIS
+ * polynomial at a specified point`x` is equal to the scalar measurement `z`.
+ *
+ * @param BASIS: The basis class to use e.g. Chebyshev2
+ */
+template <class BASIS>
+class GTSAM_EXPORT DerivativeFactor
+    : public FunctorizedFactor<double, typename BASIS::Parameters> {
+ private:
+  using Base = FunctorizedFactor<double, typename BASIS::Parameters>;
+
+ public:
+  DerivativeFactor() {}
+
+  /**
+   * @brief Construct a new DerivativeFactor object.
+   *
+   * @param key The key to the ParameterMatrix which represents the basis
+   * polynomial.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the basis polynomial.
+   */
+  DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model,
+                   const size_t N, double x)
+      : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x)) {}
+
+  /**
+   * @brief Construct a new DerivativeFactor object.
+   *
+   * @param key The key to the ParameterMatrix which represents the basis
+   * polynomial.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the basis polynomial.
+   * @param a Lower bound for the polynomial.
+   * @param b Upper bound for the polynomial.
+   */
+  DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model,
+                   const size_t N, double x, double a, double b)
+      : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x, a, b)) {}
+
+  virtual ~DerivativeFactor() {}
+};
+
+/**
+ * A unary factor which enforces the evaluation of the derivative of a BASIS
+ * polynomial at a specified point `x` is equal to the vector value `z`.
+ *
+ * @param BASIS: The basis class to use e.g. Chebyshev2
+ * @param M: Size of the evaluated state vector derivative.
+ */
+template <class BASIS, int M>
+class GTSAM_EXPORT VectorDerivativeFactor
+    : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
+ private:
+  using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
+  using Func = typename BASIS::template VectorDerivativeFunctor<M>;
+
+ public:
+  VectorDerivativeFactor() {}
+
+  /**
+   * @brief Construct a new VectorDerivativeFactor object.
+   *
+   * @param key The key to the ParameterMatrix which represents the basis
+   * polynomial.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the basis polynomial.
+   */
+  VectorDerivativeFactor(Key key, const Vector &z,
+                         const SharedNoiseModel &model, const size_t N,
+                         double x)
+      : Base(key, z, model, Func(N, x)) {}
+
+  /**
+   * @brief Construct a new VectorDerivativeFactor object.
+   *
+   * @param key The key to the ParameterMatrix which represents the basis
+   * polynomial.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the basis polynomial.
+   * @param a Lower bound for the polynomial.
+   * @param b Upper bound for the polynomial.
+   */
+  VectorDerivativeFactor(Key key, const Vector &z,
+                         const SharedNoiseModel &model, const size_t N,
+                         double x, double a, double b)
+      : Base(key, z, model, Func(N, x, a, b)) {}
+
+  virtual ~VectorDerivativeFactor() {}
+};
+
+/**
+ * A unary factor which enforces the evaluation of the derivative of a BASIS
+ * polynomial is equal to the scalar value at a specific index `i` of a
+ * vector-valued measurement `z`.
+ *
+ * @param BASIS: The basis class to use e.g. Chebyshev2
+ * @param P: Size of the control component derivative.
+ */
+template <class BASIS, int P>
+class GTSAM_EXPORT ComponentDerivativeFactor
+    : public FunctorizedFactor<double, ParameterMatrix<P>> {
+ private:
+  using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
+  using Func = typename BASIS::template ComponentDerivativeFunctor<P>;
+
+ public:
+  ComponentDerivativeFactor() {}
+
+  /**
+   * @brief Construct a new ComponentDerivativeFactor object.
+   *
+   * @param key The key to the ParameterMatrix which represents the basis
+   * polynomial.
+   * @param z The scalar measurement value at a specific index `i` of the full
+   * measurement vector.
+   * @param model The degree of the polynomial.
+   * @param N The degree of the polynomial.
+   * @param i The index for the evaluated vector to give us the desired scalar
+   * value.
+   * @param x The point at which to evaluate the basis polynomial.
+   */
+  ComponentDerivativeFactor(Key key, const double &z,
+                            const SharedNoiseModel &model, const size_t N,
+                            size_t i, double x)
+      : Base(key, z, model, Func(N, i, x)) {}
+
+  /**
+   * @brief Construct a new ComponentDerivativeFactor object.
+   *
+   * @param key The key to the ParameterMatrix which represents the basis
+   * polynomial.
+   * @param z The scalar measurement value at a specific index `i` of the full
+   * measurement vector.
+   * @param model The degree of the polynomial.
+   * @param N The degree of the polynomial.
+   * @param i The index for the evaluated vector to give us the desired scalar
+   * value.
+   * @param x The point at which to evaluate the basis polynomial.
+   * @param a Lower bound for the polynomial.
+   * @param b Upper bound for the polynomial.
+   */
+  ComponentDerivativeFactor(Key key, const double &z,
+                            const SharedNoiseModel &model, const size_t N,
+                            size_t i, double x, double a, double b)
+      : Base(key, z, model, Func(N, i, x, a, b)) {}
+
+  virtual ~ComponentDerivativeFactor() {}
+};
+
+}  // namespace gtsam

gtsam/basis/CMakeLists.txt

@@ -0,0 +1,6 @@
+# Install headers
+file(GLOB basis_headers "*.h")
+install(FILES ${basis_headers} DESTINATION include/gtsam/basis)
+
+# Build tests
+add_subdirectory(tests)

gtsam/basis/Chebyshev.cpp

@@ -0,0 +1,98 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Chebyshev.cpp
+ * @brief Chebyshev basis decompositions
+ * @author Varun Agrawal, Jing Dong, Frank Dellaert
+ * @date July 4, 2020
+ */
+
+#include <gtsam/basis/Chebyshev.h>
+
+namespace gtsam {
+
+/**
+ * @brief Scale x from [a, b] to [t1, t2]
+ *
+ * ((b'-a') * (x - a) / (b - a)) + a'
+ *
+ * @param x Value to scale to new range.
+ * @param a Original lower limit.
+ * @param b Original upper limit.
+ * @param t1 New lower limit.
+ * @param t2 New upper limit.
+ * @return double
+ */
+static double scale(double x, double a, double b, double t1, double t2) {
+  return ((t2 - t1) * (x - a) / (b - a)) + t1;
+}
+
+Weights Chebyshev1Basis::CalculateWeights(size_t N, double x, double a,
+                                          double b) {
+  Weights Tx(1, N);
+
+  x = scale(x, a, b, -1, 1);
+
+  Tx(0) = 1;
+  Tx(1) = x;
+  for (size_t i = 2; i < N; i++) {
+    // instead of cos(i*acos(x)), this recurrence is much faster
+    Tx(i) = 2 * x * Tx(i - 1) - Tx(i - 2);
+  }
+  return Tx;
+}
+
+Weights Chebyshev1Basis::DerivativeWeights(size_t N, double x, double a,
+                                           double b) {
+  Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b);
+  Weights weights = Weights::Zero(N);
+  for (size_t n = 1; n < N; n++) {
+    weights(n) = n * Ux(n - 1);
+  }
+  return weights;
+}
+
+Weights Chebyshev2Basis::CalculateWeights(size_t N, double x, double a,
+                                          double b) {
+  Weights Ux(N);
+
+  x = scale(x, a, b, -1, 1);
+
+  Ux(0) = 1;
+  Ux(1) = 2 * x;
+  for (size_t i = 2; i < N; i++) {
+    // instead of cos(i*acos(x)), this recurrence is much faster
+    Ux(i) = 2 * x * Ux(i - 1) - Ux(i - 2);
+  }
+  return Ux;
+}
+
+Weights Chebyshev2Basis::DerivativeWeights(size_t N, double x, double a,
+                                           double b) {
+  Weights Tx = Chebyshev1Basis::CalculateWeights(N + 1, x, a, b);
+  Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b);
+
+  Weights weights(N);
+
+  x = scale(x, a, b, -1, 1);
+  if (x == -1 || x == 1) {
+    throw std::runtime_error(
+        "Derivative of Chebyshev2 Basis does not exist at range limits.");
+  }
+
+  for (size_t n = 0; n < N; n++) {
+    weights(n) = ((n + 1) * Tx(n + 1) - x * Ux(n)) / (x * x - 1);
+  }
+  return weights;
+}
+
+}  // namespace gtsam

gtsam/basis/Chebyshev.h

@@ -0,0 +1,109 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Chebyshev.h
+ * @brief Chebyshev basis decompositions
+ * @author Varun Agrawal, Jing Dong, Frank Dellaert
+ * @date July 4, 2020
+ */
+
+#pragma once
+
+#include <gtsam/base/Manifold.h>
+#include <gtsam/basis/Basis.h>
+
+#include <unsupported/Eigen/KroneckerProduct>
+
+namespace gtsam {
+
+/**
+ * Basis of Chebyshev polynomials of the first kind
+ * https://en.wikipedia.org/wiki/Chebyshev_polynomials#First_kind
+ * These are typically denoted with the symbol T_n, where n is the degree.
+ * The parameter N is the number of coefficients, i.e., N = n+1.
+ */
+struct Chebyshev1Basis : Basis<Chebyshev1Basis> {
+  using Parameters = Eigen::Matrix<double, -1, 1 /*Nx1*/>;
+
+  Parameters parameters_;
+
+  /**
+   * @brief Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values)
+   *
+   * @param N Degree of the polynomial.
+   * @param x Point to evaluate polynomial at.
+   * @param a Lower limit of polynomial (default=-1).
+   * @param b Upper limit of polynomial (default=1).
+   */
+  static Weights CalculateWeights(size_t N, double x, double a = -1,
+                                  double b = 1);
+
+  /**
+   * @brief Evaluate Chebyshev derivative at x.
+   * The derivative weights are pre-multiplied to the polynomial Parameters.
+   *
+   * From Wikipedia we have D[T_n(x),x] = n*U_{n-1}(x)
+   * I.e. the derivative fo a first kind cheb is just a second kind cheb
+   * So, we define a second kind basis here of order N-1
+   * Note that it has one less weight.
+   *
+   * The Parameters pertain to 1st kind chebs up to order N-1
+   * But of course the first one (order 0) is constant, so omit that weight.
+   *
+   * @param N Degree of the polynomial.
+   * @param x Point to evaluate polynomial at.
+   * @param a Lower limit of polynomial (default=-1).
+   * @param b Upper limit of polynomial (default=1).
+   * @return Weights
+   */
+  static Weights DerivativeWeights(size_t N, double x, double a = -1,
+                                   double b = 1);
+};  // Chebyshev1Basis
+
+/**
+ * Basis of Chebyshev polynomials of the second kind.
+ * https://en.wikipedia.org/wiki/Chebyshev_polynomials#Second_kind
+ * These are typically denoted with the symbol U_n, where n is the degree.
+ * The parameter N is the number of coefficients, i.e., N = n+1.
+ * In contrast to the templates in Chebyshev2, the classes below specify
+ * basis functions, weighted combinations of which are used to approximate
+ * functions. In this sense, they are like the sines and cosines of the Fourier
+ * basis.
+ */
+struct Chebyshev2Basis : Basis<Chebyshev2Basis> {
+  using Parameters = Eigen::Matrix<double, -1, 1 /*Nx1*/>;
+
+  /**
+   *  Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values).
+   *
+   * @param N Degree of the polynomial.
+   * @param x Point to evaluate polynomial at.
+   * @param a Lower limit of polynomial (default=-1).
+   * @param b Upper limit of polynomial (default=1).
+   */
+  static Weights CalculateWeights(size_t N, double x, double a = -1,
+                                  double b = 1);
+
+  /**
+   * @brief Evaluate Chebyshev derivative at x.
+   *
+   * @param N Degree of the polynomial.
+   * @param x Point to evaluate polynomial at.
+   * @param a Lower limit of polynomial (default=-1).
+   * @param b Upper limit of polynomial (default=1).
+   * @return Weights
+   */
+  static Weights DerivativeWeights(size_t N, double x, double a = -1,
+                                   double b = 1);
+};  // Chebyshev2Basis
+
+}  // namespace gtsam

gtsam/basis/Chebyshev2.cpp

@@ -0,0 +1,205 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Chebyshev2.cpp
+ * @brief Chebyshev parameterizations on Chebyshev points of second kind
+ * @author Varun Agrawal, Jing Dong, Frank Dellaert
+ * @date July 4, 2020
+ */
+
+#include <gtsam/basis/Chebyshev2.h>
+
+namespace gtsam {
+
+Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
+  // Allocate space for weights
+  Weights weights(N);
+
+  // We start by getting distances from x to all Chebyshev points
+  // as well as getting smallest distance
+  Weights distances(N);
+
+  for (size_t j = 0; j < N; j++) {
+    const double dj =
+        x - Point(N, j, a, b);  // only thing that depends on [a,b]
+
+    if (std::abs(dj) < 1e-10) {
+      // exceptional case: x coincides with a Chebyshev point
+      weights.setZero();
+      weights(j) = 1;
+      return weights;
+    }
+    distances(j) = dj;
+  }
+
+  // Beginning of interval, j = 0, x(0) = a
+  weights(0) = 0.5 / distances(0);
+
+  // All intermediate points j=1:N-2
+  double d = weights(0), s = -1;  // changes sign s at every iteration
+  for (size_t j = 1; j < N - 1; j++, s = -s) {
+    weights(j) = s / distances(j);
+    d += weights(j);
+  }
+
+  // End of interval, j = N-1, x(N-1) = b
+  weights(N - 1) = 0.5 * s / distances(N - 1);
+  d += weights(N - 1);
+
+  // normalize
+  return weights / d;
+}
+
+Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
+  // Allocate space for weights
+  Weights weightDerivatives(N);
+
+  // toggle variable so we don't need to use `pow` for -1
+  double t = -1;
+
+  // We start by getting distances from x to all Chebyshev points
+  // as well as getting smallest distance
+  Weights distances(N);
+
+  for (size_t j = 0; j < N; j++) {
+    const double dj =
+        x - Point(N, j, a, b);  // only thing that depends on [a,b]
+    if (std::abs(dj) < 1e-10) {
+      // exceptional case: x coincides with a Chebyshev point
+      weightDerivatives.setZero();
+      // compute the jth row of the differentiation matrix for this point
+      double cj = (j == 0 || j == N - 1) ? 2. : 1.;
+      for (size_t k = 0; k < N; k++) {
+        if (j == 0 && k == 0) {
+          // we reverse the sign since we order the cheb points from -1 to 1
+          weightDerivatives(k) = -(cj * (N - 1) * (N - 1) + 1) / 6.0;
+        } else if (j == N - 1 && k == N - 1) {
+          // we reverse the sign since we order the cheb points from -1 to 1
+          weightDerivatives(k) = (cj * (N - 1) * (N - 1) + 1) / 6.0;
+        } else if (k == j) {
+          double xj = Point(N, j);
+          double xj2 = xj * xj;
+          weightDerivatives(k) = -0.5 * xj / (1 - xj2);
+        } else {
+          double xj = Point(N, j);
+          double xk = Point(N, k);
+          double ck = (k == 0 || k == N - 1) ? 2. : 1.;
+          t = ((j + k) % 2) == 0 ? 1 : -1;
+          weightDerivatives(k) = (cj / ck) * t / (xj - xk);
+        }
+      }
+      return 2 * weightDerivatives / (b - a);
+    }
+    distances(j) = dj;
+  }
+
+  // This section of code computes the derivative of
+  // the Barycentric Interpolation weights formula by applying
+  // the chain rule on the original formula.
+
+  // g and k are multiplier terms which represent the derivatives of
+  // the numerator and denominator
+  double g = 0, k = 0;
+  double w = 1;
+
+  for (size_t j = 0; j < N; j++) {
+    if (j == 0 || j == N - 1) {
+      w = 0.5;
+    } else {
+      w = 1.0;
+    }
+
+    t = (j % 2 == 0) ? 1 : -1;
+
+    double c = t / distances(j);
+    g += w * c;
+    k += (w * c / distances(j));
+  }
+
+  double s = 1;  // changes sign s at every iteration
+  double g2 = g * g;
+
+  for (size_t j = 0; j < N; j++, s = -s) {
+    // Beginning of interval, j = 0, x0 = -1.0 and end of interval, j = N-1,
+    // x0 = 1.0
+    if (j == 0 || j == N - 1) {
+      w = 0.5;
+    } else {
+      // All intermediate points j=1:N-2
+      w = 1.0;
+    }
+    weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) -
+                           (w * -s * k / (g2 * distances(j)));
+  }
+
+  return weightDerivatives;
+}
+
+Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a,
+                                                         double b) {
+  DiffMatrix D(N, N);
+  if (N == 1) {
+    D(0, 0) = 1;
+    return D;
+  }
+
+  // toggle variable so we don't need to use `pow` for -1
+  double t = -1;
+
+  for (size_t i = 0; i < N; i++) {
+    double xi = Point(N, i);
+    double ci = (i == 0 || i == N - 1) ? 2. : 1.;
+    for (size_t j = 0; j < N; j++) {
+      if (i == 0 && j == 0) {
+        // we reverse the sign since we order the cheb points from -1 to 1
+        D(i, j) = -(ci * (N - 1) * (N - 1) + 1) / 6.0;
+      } else if (i == N - 1 && j == N - 1) {
+        // we reverse the sign since we order the cheb points from -1 to 1
+        D(i, j) = (ci * (N - 1) * (N - 1) + 1) / 6.0;
+      } else if (i == j) {
+        double xi2 = xi * xi;
+        D(i, j) = -xi / (2 * (1 - xi2));
+      } else {
+        double xj = Point(N, j);
+        double cj = (j == 0 || j == N - 1) ? 2. : 1.;
+        t = ((i + j) % 2) == 0 ? 1 : -1;
+        D(i, j) = (ci / cj) * t / (xi - xj);
+      }
+    }
+  }
+  // scale the matrix to the range
+  return D / ((b - a) / 2.0);
+}
+
+Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) {
+  // Allocate space for weights
+  Weights weights(N);
+  size_t K = N - 1,  // number of intervals between N points
+      K2 = K * K;
+  weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1);
+  weights(N - 1) = weights(0);
+
+  size_t last_k = K / 2 + K % 2 - 1;
+
+  for (size_t i = 1; i <= N - 2; ++i) {
+    double theta = i * M_PI / K;
+    weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1;
+
+    for (size_t k = 1; k <= last_k; ++k)
+      weights(i) -= 2 * cos(2 * k * theta) / (4 * k * k - 1);
+    weights(i) *= (b - a) / K;
+  }
+
+  return weights;
+}
+
+}  // namespace gtsam

gtsam/basis/Chebyshev2.h

@@ -0,0 +1,148 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Chebyshev2.h
+ * @brief Pseudo-spectral parameterization for Chebyshev polynomials of the
+ * second kind.
+ *
+ * In a pseudo-spectral case, rather than the parameters acting as
+ * weights for the bases polynomials (as in Chebyshev2Basis), here the
+ * parameters are the *values* at a specific set of points in the interval, the
+ * "Chebyshev points". These values uniquely determine the polynomial that
+ * interpolates them at the Chebyshev points.
+ *
+ * This is different from Chebyshev.h since it leverage ideas from
+ * pseudo-spectral optimization, i.e. we don't decompose into basis functions,
+ * rather estimate function parameters that enforce function nodes at Chebyshev
+ * points.
+ *
+ * Please refer to Agrawal21icra for more details.
+ *
+ * @author Varun Agrawal, Jing Dong, Frank Dellaert
+ * @date July 4, 2020
+ */
+
+#pragma once
+
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/basis/Basis.h>
+
+#include <boost/function.hpp>
+
+namespace gtsam {
+
+/**
+ * Chebyshev Interpolation on Chebyshev points of the second kind
+ * Note that N here, the number of points, is one less than N from
+ * 'Approximation Theory and Approximation Practice by L. N. Trefethen (pg.42)'.
+ */
+class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  using Base = Basis<Chebyshev2>;
+  using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
+  using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;
+
+  /// Specific Chebyshev point
+  static double Point(size_t N, int j) {
+    assert(j >= 0 && size_t(j) < N);
+    const double dtheta = M_PI / (N > 1 ? (N - 1) : 1);
+    // We add -PI so that we get values ordered from -1 to +1
+    // sin(- M_PI_2 + dtheta*j); also works
+    return cos(-M_PI + dtheta * j);
+  }
+
+  /// Specific Chebyshev point, within [a,b] interval
+  static double Point(size_t N, int j, double a, double b) {
+    assert(j >= 0 && size_t(j) < N);
+    const double dtheta = M_PI / (N - 1);
+    // We add -PI so that we get values ordered from -1 to +1
+    return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
+  }
+
+  /// All Chebyshev points
+  static Vector Points(size_t N) {
+    Vector points(N);
+    for (size_t j = 0; j < N; j++) points(j) = Point(N, j);
+    return points;
+  }
+
+  /// All Chebyshev points, within [a,b] interval
+  static Vector Points(size_t N, double a, double b) {
+    Vector points = Points(N);
+    const double T1 = (a + b) / 2, T2 = (b - a) / 2;
+    points = T1 + (T2 * points).array();
+    return points;
+  }
+
+  /**
+   * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values)
+   * These weights implement barycentric interpolation at a specific x.
+   * More precisely, f(x) ~ [w0;...;wN] * [f0;...;fN], where the fj are the
+   * values of the function f at the Chebyshev points. As such, for a given x we
+   * obtain a linear map from parameter vectors f to interpolated values f(x).
+   * Optional [a,b] interval can be specified as well.
+   */
+  static Weights CalculateWeights(size_t N, double x, double a = -1,
+                                  double b = 1);
+
+  /**
+   *  Evaluate derivative of barycentric weights.
+   *  This is easy and efficient via the DifferentiationMatrix.
+   */
+  static Weights DerivativeWeights(size_t N, double x, double a = -1,
+                                   double b = 1);
+
+  /// compute D = differentiation matrix, Trefethen00book p.53
+  /// when given a parameter vector f of function values at the Chebyshev
+  /// points, D*f are the values of f'.
+  /// https://people.maths.ox.ac.uk/trefethen/8all.pdf Theorem 8.4
+  static DiffMatrix DifferentiationMatrix(size_t N, double a = -1,
+                                          double b = 1);
+
+  /**
+   *  Evaluate Clenshaw-Curtis integration weights.
+   *  Trefethen00book, pg 128, clencurt.m
+   *  Note that N in clencurt.m is 1 less than our N
+   *  K = N-1;
+      theta = pi*(0:K)'/K;
+      w = zeros(1,N); ii = 2:K; v = ones(K-1, 1);
+      if mod(K,2) == 0
+          w(1) = 1/(K^2-1); w(N) = w(1);
+          for k=1:K/2-1, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end
+          v = v - cos(K*theta(ii))/(K^2-1);
+      else
+          w(1) = 1/K^2; w(N) = w(1);
+          for k=1:K/2, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end
+      end
+      w(ii) = 2*v/K;
+
+   */
+  static Weights IntegrationWeights(size_t N, double a = -1, double b = 1);
+
+  /**
+   * Create matrix of values at Chebyshev points given vector-valued function.
+   */
+  template <size_t M>
+  static Matrix matrix(boost::function<Eigen::Matrix<double, M, 1>(double)> f,
+                       size_t N, double a = -1, double b = 1) {
+    Matrix Xmat(M, N);
+    for (size_t j = 0; j < N; j++) {
+      Xmat.col(j) = f(Point(N, j, a, b));
+    }
+    return Xmat;
+  }
+};  // \ Chebyshev2
+
+}  // namespace gtsam

gtsam/basis/FitBasis.h

@@ -0,0 +1,99 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file FitBasis.h
+ * @date July 4, 2020
+ * @author Varun Agrawal, Frank Dellaert
+ * @brief Fit a Basis using least-squares
+ */
+
+/*
+ *  Concept needed for LS. Parameters = Coefficients | Values
+ *    - Parameters, Jacobian
+ *    - PredictFactor(double x)(Parameters p, OptionalJacobian<1,N> H)
+ */
+
+#pragma once
+
+#include <gtsam/basis/Basis.h>
+#include <gtsam/basis/BasisFactors.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+
+namespace gtsam {
+
+/// Our sequence representation is a map of {x: y} values where y = f(x)
+using Sequence = std::map<double, double>;
+/// A sample is a key-value pair from a sequence.
+using Sample = std::pair<double, double>;
+
+/**
+ * Class that does regression via least squares
+ * Example usage:
+ *  size_t N = 3;
+ *  auto fit = FitBasis<Chebyshev2>(data_points, noise_model, N);
+ *  Vector coefficients = fit.parameters();
+ *
+ * where `data_points` are a map from `x` to `y` values indicating a function
+ * mapping at specific points, `noise_model` is the gaussian noise model, and
+ * `N` is the degree of the polynomial basis used to fit the function.
+ */
+template <class Basis>
+class FitBasis {
+ public:
+  using Parameters = typename Basis::Parameters;
+
+ private:
+  Parameters parameters_;
+
+ public:
+  /// Create nonlinear FG from Sequence
+  static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence,
+                                             const SharedNoiseModel& model,
+                                             size_t N) {
+    NonlinearFactorGraph graph;
+    for (const Sample sample : sequence) {
+      graph.emplace_shared<EvaluationFactor<Basis>>(0, sample.second, model, N,
+                                                    sample.first);
+    }
+    return graph;
+  }
+
+  /// Create linear FG from Sequence
+  static GaussianFactorGraph::shared_ptr LinearGraph(
+      const Sequence& sequence, const SharedNoiseModel& model, size_t N) {
+    NonlinearFactorGraph graph = NonlinearGraph(sequence, model, N);
+    Values values;
+    values.insert<Parameters>(0, Parameters::Zero(N));
+    GaussianFactorGraph::shared_ptr gfg = graph.linearize(values);
+    return gfg;
+  }
+
+  /**
+   * @brief Construct a new FitBasis object.
+   *
+   * @param sequence map of x->y values for a function, a.k.a. y = f(x).
+   * @param model The noise model to use.
+   * @param N The degree of the polynomial to fit.
+   */
+  FitBasis(const Sequence& sequence, const SharedNoiseModel& model, size_t N) {
+    GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model, N);
+    VectorValues solution = gfg->optimize();
+    parameters_ = solution.at(0);
+  }
+
+  /// Return Fourier coefficients
+  Parameters parameters() const { return parameters_; }
+};
+
+}  // namespace gtsam

gtsam/basis/Fourier.h

@@ -0,0 +1,112 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Fourier.h
+ * @brief Fourier decomposition, see e.g.
+ *        http://mathworld.wolfram.com/FourierSeries.html
+ * @author Varun Agrawal, Frank Dellaert
+ * @date July 4, 2020
+ */
+
+#pragma once
+
+#include <gtsam/basis/Basis.h>
+
+namespace gtsam {
+
+/// Fourier basis
+class GTSAM_EXPORT FourierBasis : public Basis<FourierBasis> {
+ public:
+  using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
+  using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;
+
+  /**
+   * @brief Evaluate Real Fourier Weights of size N in interval [a, b],
+   *        e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x)
+   *
+   * @param N The degree of the polynomial to use.
+   * @param x The point at which to compute the derivaive weights.
+   * @return Weights
+   */
+  static Weights CalculateWeights(size_t N, double x) {
+    Weights b(N);
+    b[0] = 1;
+    for (size_t i = 1, n = 1; i < N; i++) {
+      if (i % 2 == 1) {
+        b[i] = cos(n * x);
+      } else {
+        b[i] = sin(n * x);
+        n++;
+      }
+    }
+    return b;
+  }
+
+  /**
+   * @brief Evaluate Real Fourier Weights of size N in interval [a, b],
+   *        e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x)
+   *
+   * @param N The degree of the polynomial to use.
+   * @param x The point at which to compute the weights.
+   * @param a Lower bound of interval.
+   * @param b Upper bound of interval.
+   * @return Weights
+   */
+  static Weights CalculateWeights(size_t N, double x, double a, double b) {
+    // TODO(Varun) How do we enforce an interval for Fourier series?
+    return CalculateWeights(N, x);
+  }
+
+  /**
+   * Compute D = differentiation matrix.
+   * Given coefficients c of a Fourier series c, D*c are the values of c'.
+   */
+  static DiffMatrix DifferentiationMatrix(size_t N) {
+    DiffMatrix D = DiffMatrix::Zero(N, N);
+    double k = 1;
+    for (size_t i = 1; i < N; i += 2) {
+      D(i, i + 1) = k;   // sin'(k*x) = k*cos(k*x)
+      D(i + 1, i) = -k;  // cos'(k*x) = -k*sin(k*x)
+      k += 1;
+    }
+
+    return D;
+  }
+
+  /**
+   * @brief Get weights at a given x that calculate the derivative.
+   *
+   * @param N The degree of the polynomial to use.
+   * @param x The point at which to compute the derivaive weights.
+   * @return Weights
+   */
+  static Weights DerivativeWeights(size_t N, double x) {
+    return CalculateWeights(N, x) * DifferentiationMatrix(N);
+  }
+
+  /**
+   * @brief Get derivative weights at a given x that calculate the derivative,
+  in the interval [a, b].
+   *
+   * @param N The degree of the polynomial to use.
+   * @param x The point at which to compute the derivaive weights.
+   * @param a Lower bound of interval.
+   * @param b Upper bound of interval.
+   * @return Weights
+   */
+  static Weights DerivativeWeights(size_t N, double x, double a, double b) {
+    return CalculateWeights(N, x, a, b) * DifferentiationMatrix(N);
+  }
+
+};  // FourierBasis
+
+}  // namespace gtsam

gtsam/basis/ParameterMatrix.h

@@ -0,0 +1,215 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file ParamaterMatrix.h
+ * @brief Define ParameterMatrix class which is used to store values at
+ * interpolation points.
+ * @author Varun Agrawal, Frank Dellaert
+ * @date September 21, 2020
+ */
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/VectorSpace.h>
+
+#include <iostream>
+
+namespace gtsam {
+
+/**
+ * A matrix abstraction of MxN values at the Basis points.
+ * This class serves as a wrapper over an Eigen matrix.
+ * @tparam M: The dimension of the type you wish to evaluate.
+ * @param N: the number of Basis points (e.g. Chebyshev points of the second
+ * kind).
+ */
+template <int M>
+class ParameterMatrix {
+  using MatrixType = Eigen::Matrix<double, M, -1>;
+
+ private:
+  MatrixType matrix_;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  enum { dimension = Eigen::Dynamic };
+
+  /**
+   * Create ParameterMatrix using the number of basis points.
+   * @param N: The number of basis points (the columns).
+   */
+  ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); }
+
+  /**
+   * Create ParameterMatrix from an MxN Eigen Matrix.
+   * @param matrix: An Eigen matrix used to initialze the ParameterMatrix.
+   */
+  ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {}
+
+  /// Get the number of rows.
+  size_t rows() const { return matrix_.rows(); }
+
+  /// Get the number of columns.
+  size_t cols() const { return matrix_.cols(); }
+
+  /// Get the underlying matrix.
+  MatrixType matrix() const { return matrix_; }
+
+  /// Return the tranpose of the underlying matrix.
+  Eigen::Matrix<double, -1, M> transpose() const { return matrix_.transpose(); }
+
+  /**
+   * Get the matrix row specified by `index`.
+   * @param index: The row index to retrieve.
+   */
+  Eigen::Matrix<double, 1, -1> row(size_t index) const {
+    return matrix_.row(index);
+  }
+
+  /**
+   * Set the matrix row specified by `index`.
+   * @param index: The row index to set.
+   */
+  auto row(size_t index) -> Eigen::Block<MatrixType, 1, -1, false> {
+    return matrix_.row(index);
+  }
+
+  /**
+   * Get the matrix column specified by `index`.
+   * @param index: The column index to retrieve.
+   */
+  Eigen::Matrix<double, M, 1> col(size_t index) const {
+    return matrix_.col(index);
+  }
+
+  /**
+   * Set the matrix column specified by `index`.
+   * @param index: The column index to set.
+   */
+  auto col(size_t index) -> Eigen::Block<MatrixType, M, 1, true> {
+    return matrix_.col(index);
+  }
+
+  /**
+   * Set all matrix coefficients to zero.
+   */
+  void setZero() { matrix_.setZero(); }
+
+  /**
+   * Add a ParameterMatrix to another.
+   * @param other: ParameterMatrix to add.
+   */
+  ParameterMatrix<M> operator+(const ParameterMatrix<M>& other) const {
+    return ParameterMatrix<M>(matrix_ + other.matrix());
+  }
+
+  /**
+   * Add a MxN-sized vector to the ParameterMatrix.
+   * @param other: Vector which is reshaped and added.
+   */
+  ParameterMatrix<M> operator+(
+      const Eigen::Matrix<double, -1, 1>& other) const {
+    // This form avoids a deep copy and instead typecasts `other`.
+    Eigen::Map<const MatrixType> other_(other.data(), M, cols());
+    return ParameterMatrix<M>(matrix_ + other_);
+  }
+
+  /**
+   * Subtract a ParameterMatrix from another.
+   * @param other: ParameterMatrix to subtract.
+   */
+  ParameterMatrix<M> operator-(const ParameterMatrix<M>& other) const {
+    return ParameterMatrix<M>(matrix_ - other.matrix());
+  }
+
+  /**
+   * Subtract a MxN-sized vector from the ParameterMatrix.
+   * @param other: Vector which is reshaped and subracted.
+   */
+  ParameterMatrix<M> operator-(
+      const Eigen::Matrix<double, -1, 1>& other) const {
+    Eigen::Map<const MatrixType> other_(other.data(), M, cols());
+    return ParameterMatrix<M>(matrix_ - other_);
+  }
+
+  /**
+   * Multiply ParameterMatrix with an Eigen matrix.
+   * @param other: Eigen matrix which should be multiplication compatible with
+   * the ParameterMatrix.
+   */
+  MatrixType operator*(const Eigen::Matrix<double, -1, -1>& other) const {
+    return matrix_ * other;
+  }
+
+  /// @name Vector Space requirements, following LieMatrix
+  /// @{
+
+  /**
+   * Print the ParameterMatrix.
+   * @param s: The prepend string to add more contextual info.
+   */
+  void print(const std::string& s = "") const {
+    std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl;
+  }
+
+  /**
+   * Check for equality up to absolute tolerance.
+   * @param other: The ParameterMatrix to check equality with.
+   * @param tol: The absolute tolerance threshold.
+   */
+  bool equals(const ParameterMatrix<M>& other, double tol = 1e-8) const {
+    return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol);
+  }
+
+  /// Returns dimensionality of the tangent space
+  inline size_t dim() const { return matrix_.size(); }
+
+  /// Convert to vector form, is done row-wise
+  inline Vector vector() const {
+    using RowMajor = Eigen::Matrix<double, -1, -1, Eigen::RowMajor>;
+    Vector result(matrix_.size());
+    Eigen::Map<RowMajor>(&result(0), rows(), cols()) = matrix_;
+    return result;
+  }
+
+  /** Identity function to satisfy VectorSpace traits.
+   *
+   * NOTE: The size at compile time is unknown so this identity is zero
+   * length and thus not valid.
+   */
+  inline static ParameterMatrix identity() {
+    // throw std::runtime_error(
+    //     "ParameterMatrix::identity(): Don't use this function");
+    return ParameterMatrix(0);
+  }
+
+  /// @}
+};
+
+// traits for ParameterMatrix
+template <int M>
+struct traits<ParameterMatrix<M>>
+    : public internal::VectorSpace<ParameterMatrix<M>> {};
+
+/* ************************************************************************* */
+// Stream operator that takes a ParameterMatrix. Used for printing.
+template <int M>
+inline std::ostream& operator<<(std::ostream& os,
+                                const ParameterMatrix<M>& parameterMatrix) {
+  os << parameterMatrix.matrix();
+  return os;
+}
+
+}  // namespace gtsam

gtsam/basis/basis.i

@@ -0,0 +1,146 @@
+//*************************************************************************
+// basis
+//*************************************************************************
+
+namespace gtsam {
+
+// TODO(gerry): add all the Functors to the Basis interfaces, e.g.
+// `EvaluationFunctor`
+
+#include <gtsam/basis/Fourier.h>
+
+class FourierBasis {
+  static Vector CalculateWeights(size_t N, double x);
+  static Matrix WeightMatrix(size_t N, Vector x);
+
+  static Matrix DifferentiationMatrix(size_t N);
+  static Vector DerivativeWeights(size_t N, double x);
+};
+
+#include <gtsam/basis/Chebyshev.h>
+
+class Chebyshev1Basis {
+  static Matrix CalculateWeights(size_t N, double x);
+  static Matrix WeightMatrix(size_t N, Vector X);
+};
+
+class Chebyshev2Basis {
+  static Matrix CalculateWeights(size_t N, double x);
+  static Matrix WeightMatrix(size_t N, Vector x);
+};
+
+#include <gtsam/basis/Chebyshev2.h>
+class Chebyshev2 {
+  static double Point(size_t N, int j);
+  static double Point(size_t N, int j, double a, double b);
+
+  static Vector Points(size_t N);
+  static Vector Points(size_t N, double a, double b);
+
+  static Matrix WeightMatrix(size_t N, Vector X);
+  static Matrix WeightMatrix(size_t N, Vector X, double a, double b);
+
+  static Matrix CalculateWeights(size_t N, double x, double a, double b);
+  static Matrix DerivativeWeights(size_t N, double x, double a, double b);
+  static Matrix IntegrationWeights(size_t N, double a, double b);
+  static Matrix DifferentiationMatrix(size_t N, double a, double b);
+
+  // TODO Needs OptionalJacobian
+  // static double Derivative(double x, Vector f);
+};
+
+#include <gtsam/basis/ParameterMatrix.h>
+
+template <M = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}>
+class ParameterMatrix {
+  ParameterMatrix(const size_t N);
+  ParameterMatrix(const Matrix& matrix);
+
+  Matrix matrix() const;
+
+  void print(const string& s = "") const;
+};
+
+#include <gtsam/basis/BasisFactors.h>
+
+template <BASIS = {gtsam::Chebyshev2, gtsam::Chebyshev1Basis,
+                   gtsam::Chebyshev2Basis, gtsam::FourierBasis}>
+virtual class EvaluationFactor : gtsam::NoiseModelFactor {
+  EvaluationFactor();
+  EvaluationFactor(gtsam::Key key, const double z,
+                   const gtsam::noiseModel::Base* model, const size_t N,
+                   double x);
+  EvaluationFactor(gtsam::Key key, const double z,
+                   const gtsam::noiseModel::Base* model, const size_t N,
+                   double x, double a, double b);
+};
+
+template <BASIS, M>
+virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor {
+  VectorEvaluationFactor();
+  VectorEvaluationFactor(gtsam::Key key, const Vector& z,
+                         const gtsam::noiseModel::Base* model, const size_t N,
+                         double x);
+  VectorEvaluationFactor(gtsam::Key key, const Vector& z,
+                         const gtsam::noiseModel::Base* model, const size_t N,
+                         double x, double a, double b);
+};
+
+// TODO(Varun) Better way to support arbitrary dimensions?
+// Especially if users mainly do `pip install gtsam` for the Python wrapper.
+typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 3>
+    VectorEvaluationFactorChebyshev2D3;
+typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 4>
+    VectorEvaluationFactorChebyshev2D4;
+typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 12>
+    VectorEvaluationFactorChebyshev2D12;
+
+template <BASIS, P>
+virtual class VectorComponentFactor : gtsam::NoiseModelFactor {
+  VectorComponentFactor();
+  VectorComponentFactor(gtsam::Key key, const double z,
+                        const gtsam::noiseModel::Base* model, const size_t N,
+                        size_t i, double x);
+  VectorComponentFactor(gtsam::Key key, const double z,
+                        const gtsam::noiseModel::Base* model, const size_t N,
+                        size_t i, double x, double a, double b);
+};
+
+typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 3>
+    VectorComponentFactorChebyshev2D3;
+typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 4>
+    VectorComponentFactorChebyshev2D4;
+typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 12>
+    VectorComponentFactorChebyshev2D12;
+
+template <BASIS, T>
+virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor {
+  ManifoldEvaluationFactor();
+  ManifoldEvaluationFactor(gtsam::Key key, const T& z,
+                           const gtsam::noiseModel::Base* model, const size_t N,
+                           double x);
+  ManifoldEvaluationFactor(gtsam::Key key, const T& z,
+                           const gtsam::noiseModel::Base* model, const size_t N,
+                           double x, double a, double b);
+};
+
+// TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and
+// `ComponentDerivativeFactor`
+
+#include <gtsam/basis/FitBasis.h>
+template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
+                   gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
+class FitBasis {
+  FitBasis(const std::map<double, double>& sequence,
+           const gtsam::noiseModel::Base* model, size_t N);
+
+  static gtsam::NonlinearFactorGraph NonlinearGraph(
+      const std::map<double, double>& sequence,
+      const gtsam::noiseModel::Base* model, size_t N);
+  static gtsam::GaussianFactorGraph::shared_ptr LinearGraph(
+      const std::map<double, double>& sequence,
+      const gtsam::noiseModel::Base* model, size_t N);
+  Parameters parameters() const;
+};
+
+}  // namespace gtsam

gtsam/basis/tests/CMakeLists.txt

@@ -0,0 +1 @@
+gtsamAddTestsGlob(basis "test*.cpp" "" "gtsam")

gtsam/basis/tests/testChebyshev.cpp

@@ -0,0 +1,236 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testChebyshev.cpp
+ * @date July 4, 2020
+ * @author Varun Agrawal
+ * @brief Unit tests for Chebyshev Basis Decompositions
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/basis/Chebyshev.h>
+#include <gtsam/basis/FitBasis.h>
+#include <gtsam/nonlinear/factorTesting.h>
+
+using namespace std;
+using namespace gtsam;
+
+auto model = noiseModel::Unit::Create(1);
+
+const size_t N = 3;
+
+//******************************************************************************
+TEST(Chebyshev, Chebyshev1) {
+  using Synth = Chebyshev1Basis::EvaluationFunctor;
+  Vector c(N);
+  double x;
+  c << 12, 3, 1;
+  x = -1.0;
+  EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9);
+  x = -0.5;
+  EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9);
+  x = 0.3;
+  EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9);
+}
+
+//******************************************************************************
+TEST(Chebyshev, Chebyshev2) {
+  using Synth = Chebyshev2Basis::EvaluationFunctor;
+  Vector c(N);
+  double x;
+  c << 12, 3, 1;
+  x = -1.0;
+  EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9);
+  x = -0.5;
+  EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9);
+  x = 0.3;
+  EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9);
+}
+
+//******************************************************************************
+TEST(Chebyshev, Evaluation) {
+  Chebyshev1Basis::EvaluationFunctor fx(N, 0.5);
+  Vector c(N);
+  c << 3, 5, -12;
+  EXPECT_DOUBLES_EQUAL(11.5, fx(c), 1e-9);
+}
+
+//******************************************************************************
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+TEST(Chebyshev, Expression) {
+  // Create linear factor graph
+  NonlinearFactorGraph graph;
+  Key key(1);
+
+  // Let's pretend we have 6 GPS measurements (we just do x coordinate)
+  // at times
+  const size_t m = 6;
+  Vector t(m);
+  t << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9;
+  Vector x(m);
+  x << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9;
+
+  for (size_t i = 0; i < m; i++) {
+    graph.emplace_shared<EvaluationFactor<Chebyshev1Basis>>(key, x(i), model, N,
+                                                            t(i));
+  }
+
+  // Solve
+  Values initial;
+  initial.insert<Vector>(key, Vector::Zero(N));  // initial does not matter
+
+  // ... and optimize
+  GaussNewtonParams parameters;
+  GaussNewtonOptimizer optimizer(graph, initial, parameters);
+  Values result = optimizer.optimize();
+
+  // Check
+  Vector expected(N);
+  expected << 0, 1, 0;
+  Vector actual_c = result.at<Vector>(key);
+  EXPECT(assert_equal(expected, actual_c));
+
+  // Calculate and print covariances
+  Marginals marginals(graph, result);
+  Matrix3 cov = marginals.marginalCovariance(key);
+  EXPECT_DOUBLES_EQUAL(0.626, cov(1, 1), 1e-3);
+
+  // Predict x at time 1.0
+  Chebyshev1Basis::EvaluationFunctor f(N, 1.0);
+  Matrix H;
+  double actual = f(actual_c, H);
+  EXPECT_DOUBLES_EQUAL(1.0, actual, 1e-9);
+
+  // Calculate predictive variance on prediction
+  double actual_variance_on_prediction = (H * cov * H.transpose())(0);
+  EXPECT_DOUBLES_EQUAL(1.1494, actual_variance_on_prediction, 1e-4);
+}
+
+//******************************************************************************
+TEST(Chebyshev, Decomposition) {
+  const size_t M = 16;
+
+  // Create example sequence
+  Sequence sequence;
+  for (size_t i = 0; i < M; i++) {
+    double x = ((double)i / M);  // - 0.99;
+    double y = x;
+    sequence[x] = y;
+  }
+
+  // Do Chebyshev Decomposition
+  FitBasis<Chebyshev1Basis> actual(sequence, model, N);
+
+  // Check
+  Vector expected = Vector::Zero(N);
+  expected(1) = 1;
+  EXPECT(assert_equal(expected, (Vector)actual.parameters(), 1e-4));
+}
+
+//******************************************************************************
+TEST(Chebyshev1, Derivative) {
+  Vector c(N);
+  c << 12, 3, 2;
+
+  Weights D;
+
+  double x = -1.0;
+  D = Chebyshev1Basis::DerivativeWeights(N, x);
+  // regression
+  EXPECT_DOUBLES_EQUAL(-5, (D * c)(0), 1e-9);
+
+  x = -0.5;
+  D = Chebyshev1Basis::DerivativeWeights(N, x);
+  // regression
+  EXPECT_DOUBLES_EQUAL(-1, (D * c)(0), 1e-9);
+
+  x = 0.3;
+  D = Chebyshev1Basis::DerivativeWeights(N, x);
+  // regression
+  EXPECT_DOUBLES_EQUAL(5.4, (D * c)(0), 1e-9);
+}
+
+//******************************************************************************
+Vector3 f(-6, 1, 0.5);
+
+double proxy1(double x, size_t N) {
+  return Chebyshev1Basis::EvaluationFunctor(N, x)(Vector(f));
+}
+
+TEST(Chebyshev1, Derivative2) {
+  const double x = 0.5;
+  auto D = Chebyshev1Basis::DerivativeWeights(N, x);
+
+  Matrix numeric_dTdx =
+      numericalDerivative21<double, double, double>(proxy1, x, N);
+  // regression
+  EXPECT_DOUBLES_EQUAL(2, numeric_dTdx(0, 0), 1e-9);
+  EXPECT_DOUBLES_EQUAL(2, (D * f)(0), 1e-9);
+}
+
+//******************************************************************************
+TEST(Chebyshev2, Derivative) {
+  Vector c(N);
+  c << 12, 6, 2;
+
+  Weights D;
+
+  double x = -1.0;
+  CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error);
+  x = 1.0;
+  CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error);
+
+  x = -0.5;
+  D = Chebyshev2Basis::DerivativeWeights(N, x);
+  // regression
+  EXPECT_DOUBLES_EQUAL(4, (D * c)(0), 1e-9);
+
+  x = 0.3;
+  D = Chebyshev2Basis::DerivativeWeights(N, x);
+  // regression
+  EXPECT_DOUBLES_EQUAL(16.8, (D * c)(0), 1e-9);
+
+  x = 0.75;
+  D = Chebyshev2Basis::DerivativeWeights(N, x);
+  // regression
+  EXPECT_DOUBLES_EQUAL(24, (D * c)(0), 1e-9);
+
+  x = 10;
+  D = Chebyshev2Basis::DerivativeWeights(N, x, 0, 20);
+  // regression
+  EXPECT_DOUBLES_EQUAL(12, (D * c)(0), 1e-9);
+}
+
+//******************************************************************************
+double proxy2(double x, size_t N) {
+  return Chebyshev2Basis::EvaluationFunctor(N, x)(Vector(f));
+}
+
+TEST(Chebyshev2, Derivative2) {
+  const double x = 0.5;
+  auto D = Chebyshev2Basis::DerivativeWeights(N, x);
+
+  Matrix numeric_dTdx =
+      numericalDerivative21<double, double, double>(proxy2, x, N);
+  // regression
+  EXPECT_DOUBLES_EQUAL(4, numeric_dTdx(0, 0), 1e-9);
+  EXPECT_DOUBLES_EQUAL(4, (D * f)(0), 1e-9);
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************

gtsam/basis/tests/testChebyshev2.cpp

@@ -0,0 +1,435 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testChebyshev.cpp
+ * @date July 4, 2020
+ * @author Varun Agrawal
+ * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral
+ *        methods
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/basis/Chebyshev2.h>
+#include <gtsam/basis/FitBasis.h>
+#include <gtsam/nonlinear/factorTesting.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace boost::placeholders;
+
+noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
+
+const size_t N = 32;
+
+//******************************************************************************
+TEST(Chebyshev2, Point) {
+  static const int N = 5;
+  auto points = Chebyshev2::Points(N);
+  Vector expected(N);
+  expected << -1., -sqrt(2.) / 2., 0., sqrt(2.) / 2., 1.;
+  static const double tol = 1e-15;  // changing this reveals errors
+  EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol);
+  EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol);
+  EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol);
+  EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol);
+  EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol);
+
+  // Check symmetry
+  EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 0), -Chebyshev2::Point(N, 4), tol);
+  EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 1), -Chebyshev2::Point(N, 3), tol);
+}
+
+//******************************************************************************
+TEST(Chebyshev2, PointInInterval) {
+  static const int N = 5;
+  auto points = Chebyshev2::Points(N, 0, 20);
+  Vector expected(N);
+  expected << 0., 1. - sqrt(2.) / 2., 1., 1. + sqrt(2.) / 2., 2.;
+  expected *= 10.0;
+  static const double tol = 1e-15;  // changing this reveals errors
+  EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol);
+  EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol);
+  EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol);
+  EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol);
+  EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol);
+
+  // all at once
+  Vector actual = Chebyshev2::Points(N, 0, 20);
+  CHECK(assert_equal(expected, actual));
+}
+
+//******************************************************************************
+// InterpolatingPolynomial[{{-1, 4}, {0, 2}, {1, 6}}, 0.5]
+TEST(Chebyshev2, Interpolate2) {
+  size_t N = 3;
+  Chebyshev2::EvaluationFunctor fx(N, 0.5);
+  Vector f(N);
+  f << 4, 2, 6;
+  EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9);
+}
+
+//******************************************************************************
+// InterpolatingPolynomial[{{0, 4}, {1, 2}, {2, 6}}, 1.5]
+TEST(Chebyshev2, Interpolate2_Interval) {
+  Chebyshev2::EvaluationFunctor fx(3, 1.5, 0, 2);
+  Vector3 f(4, 2, 6);
+  EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9);
+}
+
+//******************************************************************************
+// InterpolatingPolynomial[{{-1, 4}, {-Sqrt[2]/2, 2}, {0, 6}, {Sqrt[2]/2,3}, {1,
+// 3}}, 0.5]
+TEST(Chebyshev2, Interpolate5) {
+  Chebyshev2::EvaluationFunctor fx(5, 0.5);
+  Vector f(5);
+  f << 4, 2, 6, 3, 3;
+  EXPECT_DOUBLES_EQUAL(4.34283, fx(f), 1e-5);
+}
+
+//******************************************************************************
+// Interpolating vectors
+TEST(Chebyshev2, InterpolateVector) {
+  double t = 30, a = 0, b = 100;
+  const size_t N = 3;
+  // Create 2x3 matrix with Vectors at Chebyshev points
+  ParameterMatrix<2> X(N);
+  X.row(0) = Chebyshev2::Points(N, a, b);  // slope 1 ramp
+
+  // Check value
+  Vector expected(2);
+  expected << t, 0;
+  Eigen::Matrix<double, /*2x2N*/ -1, -1> actualH(2, 2 * N);
+
+  Chebyshev2::VectorEvaluationFunctor<2> fx(N, t, a, b);
+  EXPECT(assert_equal(expected, fx(X, actualH), 1e-9));
+
+  // Check derivative
+  boost::function<Vector2(ParameterMatrix<2>)> f = boost::bind(
+      &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none);
+  Matrix numericalH =
+      numericalDerivative11<Vector2, ParameterMatrix<2>, 2 * N>(f, X);
+  EXPECT(assert_equal(numericalH, actualH, 1e-9));
+}
+
+//******************************************************************************
+TEST(Chebyshev2, Decomposition) {
+  // Create example sequence
+  Sequence sequence;
+  for (size_t i = 0; i < 16; i++) {
+    double x = (double)i / 16. - 0.99, y = x;
+    sequence[x] = y;
+  }
+
+  // Do Chebyshev Decomposition
+  FitBasis<Chebyshev2> actual(sequence, model, 3);
+
+  // Check
+  Vector expected(3);
+  expected << -1, 0, 1;
+  EXPECT(assert_equal(expected, actual.parameters(), 1e-4));
+}
+
+//******************************************************************************
+TEST(Chebyshev2, DifferentiationMatrix3) {
+  // Trefethen00book, p.55
+  const size_t N = 3;
+  Matrix expected(N, N);
+  // Differentiation matrix computed from Chebfun
+  expected << 1.5000, -2.0000, 0.5000,  //
+      0.5000, -0.0000, -0.5000,         //
+      -0.5000, 2.0000, -1.5000;
+  // multiply by -1 since the cheb points have a phase shift wrt Trefethen
+  // This was verified with chebfun
+  expected = -expected;
+
+  Matrix actual = Chebyshev2::DifferentiationMatrix(N);
+  EXPECT(assert_equal(expected, actual, 1e-4));
+}
+
+//******************************************************************************
+TEST(Chebyshev2, DerivativeMatrix6) {
+  // Trefethen00book, p.55
+  const size_t N = 6;
+  Matrix expected(N, N);
+  expected << 8.5000, -10.4721, 2.8944, -1.5279, 1.1056, -0.5000,  //
+      2.6180, -1.1708, -2.0000, 0.8944, -0.6180, 0.2764,           //
+      -0.7236, 2.0000, -0.1708, -1.6180, 0.8944, -0.3820,          //
+      0.3820, -0.8944, 1.6180, 0.1708, -2.0000, 0.7236,            //
+      -0.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180,           //
+      0.5000, -1.1056, 1.5279, -2.8944, 10.4721, -8.5000;
+  // multiply by -1 since the cheb points have a phase shift wrt Trefethen
+  // This was verified with chebfun
+  expected = -expected;
+
+  Matrix actual = Chebyshev2::DifferentiationMatrix(N);
+  EXPECT(assert_equal((Matrix)expected, actual, 1e-4));
+}
+
+// test function for CalculateWeights and DerivativeWeights
+double f(double x) {
+  // return 3*(x**3) - 2*(x**2) + 5*x - 11
+  return 3.0 * pow(x, 3) - 2.0 * pow(x, 2) + 5.0 * x - 11;
+}
+
+// its derivative
+double fprime(double x) {
+  // return 9*(x**2) - 4*(x) + 5
+  return 9.0 * pow(x, 2) - 4.0 * x + 5.0;
+}
+
+//******************************************************************************
+TEST(Chebyshev2, CalculateWeights) {
+  Eigen::Matrix<double, -1, 1> fvals(N);
+  for (size_t i = 0; i < N; i++) {
+    fvals(i) = f(Chebyshev2::Point(N, i));
+  }
+  double x1 = 0.7, x2 = -0.376;
+  Weights weights1 = Chebyshev2::CalculateWeights(N, x1);
+  Weights weights2 = Chebyshev2::CalculateWeights(N, x2);
+  EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8);
+  EXPECT_DOUBLES_EQUAL(f(x2), weights2 * fvals, 1e-8);
+}
+
+TEST(Chebyshev2, CalculateWeights2) {
+  double a = 0, b = 10, x1 = 7, x2 = 4.12;
+
+  Eigen::Matrix<double, -1, 1> fvals(N);
+  for (size_t i = 0; i < N; i++) {
+    fvals(i) = f(Chebyshev2::Point(N, i, a, b));
+  }
+
+  Weights weights1 = Chebyshev2::CalculateWeights(N, x1, a, b);
+  EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8);
+
+  Weights weights2 = Chebyshev2::CalculateWeights(N, x2, a, b);
+  double expected2 = f(x2);  // 185.454784
+  double actual2 = weights2 * fvals;
+  EXPECT_DOUBLES_EQUAL(expected2, actual2, 1e-8);
+}
+
+TEST(Chebyshev2, DerivativeWeights) {
+  Eigen::Matrix<double, -1, 1> fvals(N);
+  for (size_t i = 0; i < N; i++) {
+    fvals(i) = f(Chebyshev2::Point(N, i));
+  }
+  double x1 = 0.7, x2 = -0.376, x3 = 0.0;
+  Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1);
+  EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-9);
+
+  Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2);
+  EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-9);
+
+  Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3);
+  EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-9);
+
+  // test if derivative calculation and cheb point is correct
+  double x4 = Chebyshev2::Point(N, 3);
+  Weights dWeights4 = Chebyshev2::DerivativeWeights(N, x4);
+  EXPECT_DOUBLES_EQUAL(fprime(x4), dWeights4 * fvals, 1e-9);
+}
+
+TEST(Chebyshev2, DerivativeWeights2) {
+  double x1 = 5, x2 = 4.12, a = 0, b = 10;
+
+  Eigen::Matrix<double, -1, 1> fvals(N);
+  for (size_t i = 0; i < N; i++) {
+    fvals(i) = f(Chebyshev2::Point(N, i, a, b));
+  }
+
+  Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1, a, b);
+  EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-8);
+
+  Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b);
+  EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8);
+
+  // test if derivative calculation and cheb point is correct
+  double x3 = Chebyshev2::Point(N, 3, a, b);
+  Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b);
+  EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8);
+}
+
+//******************************************************************************
+// Check two different ways to calculate the derivative weights
+TEST(Chebyshev2, DerivativeWeightsDifferentiationMatrix) {
+  const size_t N6 = 6;
+  double x1 = 0.311;
+  Matrix D6 = Chebyshev2::DifferentiationMatrix(N6);
+  Weights expected = Chebyshev2::CalculateWeights(N6, x1) * D6;
+  Weights actual = Chebyshev2::DerivativeWeights(N6, x1);
+  EXPECT(assert_equal(expected, actual, 1e-12));
+
+  double a = -3, b = 8, x2 = 5.05;
+  Matrix D6_2 = Chebyshev2::DifferentiationMatrix(N6, a, b);
+  Weights expected1 = Chebyshev2::CalculateWeights(N6, x2, a, b) * D6_2;
+  Weights actual1 = Chebyshev2::DerivativeWeights(N6, x2, a, b);
+  EXPECT(assert_equal(expected1, actual1, 1e-12));
+}
+
+//******************************************************************************
+// Check two different ways to calculate the derivative weights
+TEST(Chebyshev2, DerivativeWeights6) {
+  const size_t N6 = 6;
+  Matrix D6 = Chebyshev2::DifferentiationMatrix(N6);
+  Chebyshev2::Parameters x = Chebyshev2::Points(N6);  // ramp with slope 1
+  EXPECT(assert_equal(Vector::Ones(N6), Vector(D6 * x)));
+}
+
+//******************************************************************************
+// Check two different ways to calculate the derivative weights
+TEST(Chebyshev2, DerivativeWeights7) {
+  const size_t N7 = 7;
+  Matrix D7 = Chebyshev2::DifferentiationMatrix(N7);
+  Chebyshev2::Parameters x = Chebyshev2::Points(N7);  // ramp with slope 1
+  EXPECT(assert_equal(Vector::Ones(N7), Vector(D7 * x)));
+}
+
+//******************************************************************************
+// Check derivative in two different ways: numerical and using D on f
+Vector6 f3_at_6points = (Vector6() << 4, 2, 6, 2, 4, 3).finished();
+double proxy3(double x) {
+  return Chebyshev2::EvaluationFunctor(6, x)(f3_at_6points);
+}
+
+TEST(Chebyshev2, Derivative6) {
+  // Check Derivative evaluation at point x=0.2
+
+  // calculate expected values by numerical derivative of synthesis
+  const double x = 0.2;
+  Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy3, x);
+
+  // Calculate derivatives at Chebyshev points using D3, interpolate
+  Matrix D6 = Chebyshev2::DifferentiationMatrix(6);
+  Vector derivative_at_points = D6 * f3_at_6points;
+  Chebyshev2::EvaluationFunctor fx(6, x);
+  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8);
+
+  // Do directly
+  Chebyshev2::DerivativeFunctor dfdx(6, x);
+  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8);
+}
+
+//******************************************************************************
+// Assert that derivative also works in non-standard interval [0,3]
+double proxy4(double x) {
+  return Chebyshev2::EvaluationFunctor(6, x, 0, 3)(f3_at_6points);
+}
+
+TEST(Chebyshev2, Derivative6_03) {
+  // Check Derivative evaluation at point x=0.2, in interval [0,3]
+
+  // calculate expected values by numerical derivative of synthesis
+  const double x = 0.2;
+  Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy4, x);
+
+  // Calculate derivatives at Chebyshev points using D3, interpolate
+  Matrix D6 = Chebyshev2::DifferentiationMatrix(6, 0, 3);
+  Vector derivative_at_points = D6 * f3_at_6points;
+  Chebyshev2::EvaluationFunctor fx(6, x, 0, 3);
+  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8);
+
+  // Do directly
+  Chebyshev2::DerivativeFunctor dfdx(6, x, 0, 3);
+  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8);
+}
+
+//******************************************************************************
+// Test VectorDerivativeFunctor
+TEST(Chebyshev2, VectorDerivativeFunctor) {
+  const size_t N = 3, M = 2;
+  const double x = 0.2;
+  using VecD = Chebyshev2::VectorDerivativeFunctor<M>;
+  VecD fx(N, x, 0, 3);
+  ParameterMatrix<M> X(N);
+  Matrix actualH(M, M * N);
+  EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8));
+
+  // Test Jacobian
+  Matrix expectedH = numericalDerivative11<Vector2, ParameterMatrix<M>, M * N>(
+      boost::bind(&VecD::operator(), fx, _1, boost::none), X);
+  EXPECT(assert_equal(expectedH, actualH, 1e-7));
+}
+
+//******************************************************************************
+// Test VectorDerivativeFunctor with polynomial function
+TEST(Chebyshev2, VectorDerivativeFunctor2) {
+  const size_t N = 64, M = 1, T = 15;
+  using VecD = Chebyshev2::VectorDerivativeFunctor<M>;
+
+  const Vector points = Chebyshev2::Points(N, 0, T);
+
+  // Assign the parameter matrix
+  Vector values(N);
+  for (size_t i = 0; i < N; ++i) {
+    values(i) = f(points(i));
+  }
+  ParameterMatrix<M> X(values);
+
+  // Evaluate the derivative at the chebyshev points using
+  // VectorDerivativeFunctor.
+  for (size_t i = 0; i < N; ++i) {
+    VecD d(N, points(i), 0, T);
+    Vector1 Dx = d(X);
+    EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6);
+  }
+
+  // Test Jacobian at the first chebyshev point.
+  Matrix actualH(M, M * N);
+  VecD vecd(N, points(0), 0, T);
+  vecd(X, actualH);
+  Matrix expectedH = numericalDerivative11<Vector1, ParameterMatrix<M>, M * N>(
+      boost::bind(&VecD::operator(), vecd, _1, boost::none), X);
+  EXPECT(assert_equal(expectedH, actualH, 1e-6));
+}
+
+//******************************************************************************
+// Test ComponentDerivativeFunctor
+TEST(Chebyshev2, ComponentDerivativeFunctor) {
+  const size_t N = 6, M = 2;
+  const double x = 0.2;
+  using CompFunc = Chebyshev2::ComponentDerivativeFunctor<M>;
+  size_t row = 1;
+  CompFunc fx(N, row, x, 0, 3);
+  ParameterMatrix<M> X(N);
+  Matrix actualH(1, M * N);
+  EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);
+
+  Matrix expectedH = numericalDerivative11<double, ParameterMatrix<M>, M * N>(
+      boost::bind(&CompFunc::operator(), fx, _1, boost::none), X);
+  EXPECT(assert_equal(expectedH, actualH, 1e-7));
+}
+
+//******************************************************************************
+TEST(Chebyshev2, IntegralWeights) {
+  const size_t N7 = 7;
+  Vector actual = Chebyshev2::IntegrationWeights(N7);
+  Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254,
+                     0.457142857142857, 0.520634920634921, 0.457142857142857,
+                     0.253968253968254, 0.0285714285714286)
+                        .finished();
+  EXPECT(assert_equal(expected, actual));
+
+  const size_t N8 = 8;
+  Vector actual2 = Chebyshev2::IntegrationWeights(N8);
+  Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208,
+                      0.352242423718159, 0.437208405798326, 0.437208405798326,
+                      0.352242423718159, 0.190141007218208, 0.0204081632653061)
+                         .finished();
+  EXPECT(assert_equal(expected2, actual2));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************

gtsam/basis/tests/testFourier.cpp

@@ -0,0 +1,254 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testFourier.cpp
+ * @date July 4, 2020
+ * @author Frank Dellaert, Varun Agrawal
+ * @brief Unit tests for Fourier Basis Decompositions with Expressions
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/basis/FitBasis.h>
+#include <gtsam/basis/Fourier.h>
+#include <gtsam/nonlinear/factorTesting.h>
+
+using namespace std;
+using namespace gtsam;
+
+auto model = noiseModel::Unit::Create(1);
+
+// Coefficients for testing, respectively 3 and 7 parameter Fourier basis.
+// They correspond to best approximation of TestFunction(x)
+const Vector k3Coefficients = (Vector3() << 1.5661, 1.2717, 1.2717).finished();
+const Vector7 k7Coefficients =
+    (Vector7() << 1.5661, 1.2717, 1.2717, -0.0000, 0.5887, -0.0943, 0.0943)
+        .finished();
+
+// The test-function used below
+static double TestFunction(double x) { return exp(sin(x) + cos(x)); }
+
+//******************************************************************************
+TEST(Basis, BasisEvaluationFunctor) {
+  // fx(0) takes coefficients c to calculate the value f(c;x==0)
+  FourierBasis::EvaluationFunctor fx(3, 0);
+  EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1],
+                       fx(k3Coefficients), 1e-9);
+}
+
+//******************************************************************************
+TEST(Basis, BasisEvaluationFunctorDerivative) {
+  // If we give the H argument, we get the derivative of fx(0) wrpt coefficients
+  // Needs to be Matrix so it can be used by OptionalJacobian.
+  Matrix H(1, 3);
+  FourierBasis::EvaluationFunctor fx(3, 0);
+  EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1],
+                       fx(k3Coefficients, H), 1e-9);
+
+  Matrix13 expectedH(1, 1, 0);
+  EXPECT(assert_equal(expectedH, H));
+}
+
+//******************************************************************************
+TEST(Basis, Manual) {
+  const size_t N = 3;
+
+  // We will create a linear factor graph
+  GaussianFactorGraph graph;
+
+  // We create an unknown Vector expression for the coefficients
+  Key key(1);
+
+  // We will need values below to test the Jacobians
+  Values values;
+  values.insert<Vector>(key, Vector::Zero(N));  // value does not really matter
+
+  // At 16 different samples points x, check Predict_ expression
+  for (size_t i = 0; i < 16; i++) {
+    const double x = i * M_PI / 8;
+    const double desiredValue = TestFunction(x);
+
+    // Manual JacobianFactor
+    Matrix A(1, N);
+    A << 1, cos(x), sin(x);
+    Vector b(1);
+    b << desiredValue;
+    JacobianFactor linearFactor(key, A, b);
+    graph.add(linearFactor);
+
+    // Create factor to predict value at x
+    EvaluationFactor<FourierBasis> predictFactor(key, desiredValue, model, N,
+                                                 x);
+
+    // Check expression Jacobians
+    EXPECT_CORRECT_FACTOR_JACOBIANS(predictFactor, values, 1e-5, 1e-9);
+
+    auto linearizedFactor = predictFactor.linearize(values);
+    auto linearizedJacobianFactor =
+        boost::dynamic_pointer_cast<JacobianFactor>(linearizedFactor);
+    CHECK(linearizedJacobianFactor);  // makes sure it's indeed a JacobianFactor
+    EXPECT(assert_equal(linearFactor, *linearizedJacobianFactor, 1e-9));
+  }
+
+  // Solve linear graph
+  VectorValues actual = graph.optimize();
+  EXPECT(assert_equal((Vector)k3Coefficients, actual.at(key), 1e-4));
+}
+
+//******************************************************************************
+TEST(Basis, EvaluationFactor) {
+  // Check fitting a function with a 7-parameter Fourier basis
+
+  // Create linear factor graph
+  NonlinearFactorGraph graph;
+  Key key(1);
+  for (size_t i = 0; i < 16; i++) {
+    double x = i * M_PI / 8, desiredValue = TestFunction(x);
+    graph.emplace_shared<EvaluationFactor<FourierBasis>>(key, desiredValue,
+                                                         model, 7, x);
+  }
+
+  // Solve FourierFactorGraph
+  Values values;
+  values.insert<Vector>(key, Vector::Zero(7));
+  GaussianFactorGraph::shared_ptr lfg = graph.linearize(values);
+  VectorValues actual = lfg->optimize();
+
+  EXPECT(assert_equal((Vector)k7Coefficients, actual.at(key), 1e-4));
+}
+
+//******************************************************************************
+TEST(Basis, WeightMatrix) {
+  // The WeightMatrix creates an m*n matrix, where m is the number of sample
+  // points, and n is the number of parameters.
+  Matrix expected(2, 3);
+  expected.row(0) << 1, cos(1), sin(1);
+  expected.row(1) << 1, cos(2), sin(2);
+  Vector2 X(1, 2);
+  Matrix actual = FourierBasis::WeightMatrix(3, X);
+  EXPECT(assert_equal(expected, actual, 1e-9));
+}
+
+//******************************************************************************
+TEST(Basis, Decomposition) {
+  // Create example sequence
+  Sequence sequence;
+  for (size_t i = 0; i < 16; i++) {
+    double x = i * M_PI / 8, y = TestFunction(x);
+    sequence[x] = y;
+  }
+
+  // Do Fourier Decomposition
+  FitBasis<FourierBasis> actual(sequence, model, 3);
+
+  // Check
+  EXPECT(assert_equal((Vector)k3Coefficients, actual.parameters(), 1e-4));
+}
+
+//******************************************************************************
+// Check derivative in two different ways: numerical and using D on f
+double proxy(double x) {
+  return FourierBasis::EvaluationFunctor(7, x)(k7Coefficients);
+}
+
+TEST(Basis, Derivative7) {
+  // Check Derivative evaluation at point x=0.2
+
+  // Calculate expected values by numerical derivative of proxy.
+  const double x = 0.2;
+  Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy, x);
+
+  // Calculate derivatives at Chebyshev points using D7, interpolate
+  Matrix D7 = FourierBasis::DifferentiationMatrix(7);
+  Vector derivativeCoefficients = D7 * k7Coefficients;
+  FourierBasis::EvaluationFunctor fx(7, x);
+  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivativeCoefficients), 1e-8);
+
+  // Do directly
+  FourierBasis::DerivativeFunctor dfdx(7, x);
+  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(k7Coefficients), 1e-8);
+}
+
+//******************************************************************************
+TEST(Basis, VecDerivativeFunctor) {
+  using DotShape = typename FourierBasis::VectorDerivativeFunctor<2>;
+  const size_t N = 3;
+
+  // MATLAB example, Dec 27 2019, commit 014eded5
+  double h = 2 * M_PI / 16;
+  Vector2 dotShape(0.5556, -0.8315);  // at h/2
+  DotShape dotShapeFunction(N, h / 2);
+  Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071)
+                           .finished()
+                           .transpose();
+  ParameterMatrix<2> theta(theta_mat);
+  EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4));
+}
+
+//******************************************************************************
+// Suppose we want to parameterize a periodic function with function values at
+// specific times, rather than coefficients. Can we do it? This would be a
+// generalization of the Fourier transform, and constitute a "pseudo-spectral"
+// parameterization. One way to do this is to establish hard constraints that
+// express the relationship between the new parameters and the coefficients.
+// For example, below I'd like the parameters to be the function values at
+// X = {0.1,0.2,0.3}, rather than a 3-vector of coefficients.
+// Because the values f(X) = at these points can be written as f(X) = W(X)*c,
+// we can simply express the coefficents c as c=inv(W(X))*f, which is a
+// generalized Fourier transform. That also means we can create factors with the
+// unknown f-values, as done manually below.
+TEST(Basis, PseudoSpectral) {
+  // We will create a linear factor graph
+  GaussianFactorGraph graph;
+
+  const size_t N = 3;
+  const Key key(1);
+
+  // The correct values at X = {0.1,0.2,0.3} are simply W*c
+  const Vector X = (Vector3() << 0.1, 0.2, 0.3).finished();
+  const Matrix W = FourierBasis::WeightMatrix(N, X);
+  const Vector expected = W * k3Coefficients;
+
+  // Check those values are indeed correct values of Fourier approximation
+  using Eval = FourierBasis::EvaluationFunctor;
+  EXPECT_DOUBLES_EQUAL(Eval(N, 0.1)(k3Coefficients), expected(0), 1e-9);
+  EXPECT_DOUBLES_EQUAL(Eval(N, 0.2)(k3Coefficients), expected(1), 1e-9);
+  EXPECT_DOUBLES_EQUAL(Eval(N, 0.3)(k3Coefficients), expected(2), 1e-9);
+
+  // Calculate "inverse Fourier transform" matrix
+  const Matrix invW = W.inverse();
+
+  // At 16 different samples points x, add a factor on fExpr
+  for (size_t i = 0; i < 16; i++) {
+    const double x = i * M_PI / 8;
+    const double desiredValue = TestFunction(x);
+
+    // Manual JacobianFactor
+    Matrix A(1, 3);
+    A << 1, cos(x), sin(x);
+    Vector b(1);
+    b << desiredValue;
+    JacobianFactor linearFactor(key, A * invW, b);
+    graph.add(linearFactor);
+  }
+
+  // Solve linear graph
+  VectorValues actual = graph.optimize();
+  EXPECT(assert_equal((Vector)expected, actual.at(key), 1e-4));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************

gtsam/basis/tests/testParameterMatrix.cpp

@@ -0,0 +1,145 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testParameterMatrix.cpp
+ * @date Sep 22, 2020
+ * @author Varun Agrawal, Frank Dellaert
+ * @brief Unit tests for ParameterMatrix.h
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/basis/BasisFactors.h>
+#include <gtsam/basis/Chebyshev2.h>
+#include <gtsam/basis/ParameterMatrix.h>
+#include <gtsam/inference/Symbol.h>
+
+using namespace std;
+using namespace gtsam;
+
+using Parameters = Chebyshev2::Parameters;
+
+const size_t M = 2, N = 5;
+
+//******************************************************************************
+TEST(ParameterMatrix, Constructor) {
+  ParameterMatrix<2> actual1(3);
+  ParameterMatrix<2> expected1(Matrix::Zero(2, 3));
+  EXPECT(assert_equal(expected1, actual1));
+
+  ParameterMatrix<2> actual2(Matrix::Ones(2, 3));
+  ParameterMatrix<2> expected2(Matrix::Ones(2, 3));
+  EXPECT(assert_equal(expected2, actual2));
+  EXPECT(assert_equal(expected2.matrix(), actual2.matrix()));
+}
+
+//******************************************************************************
+TEST(ParameterMatrix, Dimensions) {
+  ParameterMatrix<M> params(N);
+  EXPECT_LONGS_EQUAL(params.rows(), M);
+  EXPECT_LONGS_EQUAL(params.cols(), N);
+  EXPECT_LONGS_EQUAL(params.dim(), M * N);
+}
+
+//******************************************************************************
+TEST(ParameterMatrix, Getters) {
+  ParameterMatrix<M> params(N);
+
+  Matrix expectedMatrix = Matrix::Zero(2, 5);
+  EXPECT(assert_equal(expectedMatrix, params.matrix()));
+
+  Matrix expectedMatrixTranspose = Matrix::Zero(5, 2);
+  EXPECT(assert_equal(expectedMatrixTranspose, params.transpose()));
+
+  ParameterMatrix<M> p2(Matrix::Ones(M, N));
+  Vector expectedRowVector = Vector::Ones(N);
+  for (size_t r = 0; r < M; ++r) {
+    EXPECT(assert_equal(p2.row(r), expectedRowVector));
+  }
+
+  ParameterMatrix<M> p3(2 * Matrix::Ones(M, N));
+  Vector expectedColVector = 2 * Vector::Ones(M);
+  for (size_t c = 0; c < M; ++c) {
+    EXPECT(assert_equal(p3.col(c), expectedColVector));
+  }
+}
+
+//******************************************************************************
+TEST(ParameterMatrix, Setters) {
+  ParameterMatrix<M> params(Matrix::Zero(M, N));
+  Matrix expected = Matrix::Zero(M, N);
+
+  // row
+  params.row(0) = Vector::Ones(N);
+  expected.row(0) = Vector::Ones(N);
+  EXPECT(assert_equal(expected, params.matrix()));
+
+  // col
+  params.col(2) = Vector::Ones(M);
+  expected.col(2) = Vector::Ones(M);
+
+  EXPECT(assert_equal(expected, params.matrix()));
+
+  // setZero
+  params.setZero();
+  expected.setZero();
+  EXPECT(assert_equal(expected, params.matrix()));
+}
+
+//******************************************************************************
+TEST(ParameterMatrix, Addition) {
+  ParameterMatrix<M> params(Matrix::Ones(M, N));
+  ParameterMatrix<M> expected(2 * Matrix::Ones(M, N));
+
+  // Add vector
+  EXPECT(assert_equal(expected, params + Vector::Ones(M * N)));
+  // Add another ParameterMatrix
+  ParameterMatrix<M> actual = params + ParameterMatrix<M>(Matrix::Ones(M, N));
+  EXPECT(assert_equal(expected, actual));
+}
+
+//******************************************************************************
+TEST(ParameterMatrix, Subtraction) {
+  ParameterMatrix<M> params(2 * Matrix::Ones(M, N));
+  ParameterMatrix<M> expected(Matrix::Ones(M, N));
+
+  // Subtract vector
+  EXPECT(assert_equal(expected, params - Vector::Ones(M * N)));
+  // Subtract another ParameterMatrix
+  ParameterMatrix<M> actual = params - ParameterMatrix<M>(Matrix::Ones(M, N));
+  EXPECT(assert_equal(expected, actual));
+}
+
+//******************************************************************************
+TEST(ParameterMatrix, Multiplication) {
+  ParameterMatrix<M> params(Matrix::Ones(M, N));
+  Matrix multiplier = 2 * Matrix::Ones(N, 2);
+  Matrix expected = 2 * N * Matrix::Ones(M, 2);
+  EXPECT(assert_equal(expected, params * multiplier));
+}
+
+//******************************************************************************
+TEST(ParameterMatrix, VectorSpace) {
+  ParameterMatrix<M> params(Matrix::Ones(M, N));
+  // vector
+  EXPECT(assert_equal(Vector::Ones(M * N), params.vector()));
+  // identity
+  EXPECT(assert_equal(ParameterMatrix<M>::identity(),
+                      ParameterMatrix<M>(Matrix::Zero(M, 0))));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************

gtsam/config.h.in

@@ -77,3 +77,9 @@
 
 // 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
+
+// Toggle switch for BetweenFactor jacobian computation
+#cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR

gtsam/discrete/DecisionTree-inl.h

@@ -450,7 +450,7 @@ namespace gtsam {
   template<typename L, typename Y>
   template<typename M, typename X>
   DecisionTree<L, Y>::DecisionTree(const DecisionTree<M, X>& other,
-      const std::map<M, L>& map, boost::function<Y(const X&)> op)  {
+      const std::map<M, L>& map, std::function<Y(const X&)> op)  {
     root_ = convert(other.root_, map, op);
   }
 
@@ -568,7 +568,7 @@ namespace gtsam {
   template<typename M, typename X>
   typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::convert(
       const typename DecisionTree<M, X>::NodePtr& f, const std::map<M, L>& map,
-      boost::function<Y(const X&)> op) {
+      std::function<Y(const X&)> op) {
 
     typedef DecisionTree<M, X> MX;
     typedef typename MX::Leaf MXLeaf;

gtsam/discrete/DecisionTree.h

@@ -20,10 +20,12 @@
 #pragma once
 
 #include <gtsam/discrete/Assignment.h>
+
 #include <boost/function.hpp>
+#include <functional>
 #include <iostream>
-#include <vector>
 #include <map>
+#include <vector>
 
 namespace gtsam {
 
@@ -38,8 +40,8 @@ namespace gtsam {
   public:
 
     /** Handy typedefs for unary and binary function types */
-    typedef boost::function<Y(const Y&)> Unary;
+    typedef std::function<Y(const Y&)> Unary;
-    typedef boost::function<Y(const Y&, const Y&)> Binary;
+    typedef std::function<Y(const Y&, const Y&)> Binary;
 
     /** A label annotated with cardinality */
     typedef std::pair<L,size_t> LabelC;
@@ -107,7 +109,7 @@ namespace gtsam {
     /** Convert to a different type */
     template<typename M, typename X> NodePtr
     convert(const typename DecisionTree<M, X>::NodePtr& f, const std::map<M,
-        L>& map, boost::function<Y(const X&)> op);
+        L>& map, std::function<Y(const X&)> op);
 
     /** Default constructor */
     DecisionTree();
@@ -143,7 +145,7 @@ namespace gtsam {
     /** Convert from a different type */
     template<typename M, typename X>
     DecisionTree(const DecisionTree<M, X>& other,
-        const std::map<M, L>& map, boost::function<Y(const X&)> op);
+        const std::map<M, L>& map, std::function<Y(const X&)> op);
 
     /// @}
     /// @name Testable

gtsam/discrete/tests/testAlgebraicDecisionTree.cpp

@@ -54,7 +54,7 @@ void dot(const T&f, const string& filename) {
 }
 
 /** I can't get this to work !
- class Mul: boost::function<double(const double&, const double&)> {
+ class Mul: std::function<double(const double&, const double&)> {
  inline double operator()(const double& a, const double& b) {
  return a * b;
  }

gtsam/discrete/tests/testDecisionTree.cpp

@@ -196,7 +196,7 @@ TEST(DT, conversion)
   map<string, Label> ordering;
   ordering[A] = X;
   ordering[B] = Y;
-  boost::function<bool(const int&)> op = convert;
+  std::function<bool(const int&)> op = convert;
   BDT f2(f1, ordering, op);
   //  f1.print("f1");
   //  f2.print("f2");

gtsam/geometry/Cal3Fisheye.cpp

@@ -106,11 +106,21 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
 /* ************************************************************************* */
 Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
                               OptionalJacobian<2, 2> Dp) const {
-  // initial gues just inverts the pinhole model
+  // Apply inverse camera matrix to map the pixel coordinate (u, v) 
+  // of the equidistant fisheye image to angular coordinate space (xd, yd)
+  // with radius theta given in radians.
   const double u = uv.x(), v = uv.y();
   const double yd = (v - v0_) / fy_;
   const double xd = (u - s_ * yd - u0_) / fx_;
-  Point2 pi(xd, yd);
+  const double theta = sqrt(xd * xd + yd * yd);
+  
+  // Provide initial guess for the Gauss-Newton search.
+  // The angular coordinates given by (xd, yd) are mapped back to
+  // the focal plane of the perspective undistorted projection pi.
+  // See Cal3Unified.calibrate() using the same pattern for the 
+  // undistortion of omnidirectional fisheye projection.
+  const double scale = (theta > 0) ? tan(theta) / theta : 1.0;
+  Point2 pi(scale * xd, scale * yd);
 
   // Perform newtons method, break when solution converges past tol_,
   // throw exception if max iterations are reached

gtsam/geometry/CameraSet.h

@@ -147,51 +147,149 @@ public:
      * G = F' * F - F' * E * P * E' * F
      * g = F' * (b - E * P * E' * b)
      * Fixed size version
-     */
+    */
-    template<int N, int ND> // N = 2 or 3, ND is the camera dimension
+  template <int N,
-    static SymmetricBlockMatrix SchurComplement(
+            int ND>  // N = 2 or 3 (point dimension), ND is the camera dimension
-        const std::vector< Eigen::Matrix<double, ZDim, ND>, Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, ND> > >& Fs,
+  static SymmetricBlockMatrix SchurComplement(
-        const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
+      const std::vector<
+          Eigen::Matrix<double, ZDim, ND>,
+          Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND>>>& Fs,
+      const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
+    // a single point is observed in m cameras
+    size_t m = Fs.size();
 
-      // a single point is observed in m cameras
+    // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
-      size_t m = Fs.size();
+    size_t M1 = ND * m + 1;
+    std::vector<DenseIndex> dims(m + 1); // this also includes the b term
+    std::fill(dims.begin(), dims.end() - 1, ND);
+    dims.back() = 1;
+    SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
 
-      // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
+    // Blockwise Schur complement
-      size_t M1 = ND * m + 1;
+    for (size_t i = 0; i < m; i++) { // for each camera
-      std::vector<DenseIndex> dims(m + 1); // this also includes the b term
-      std::fill(dims.begin(), dims.end() - 1, ND);
-      dims.back() = 1;
-      SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
 
-      // Blockwise Schur complement
+      const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
-      for (size_t i = 0; i < m; i++) { // for each camera
+      const auto FiT = Fi.transpose();
+      const Eigen::Matrix<double, ZDim, N> Ei_P = //
+          E.block(ZDim * i, 0, ZDim, N) * P;
 
-        const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
+      // D = (Dx2) * ZDim
-        const auto FiT = Fi.transpose();
+      augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
-        const Eigen::Matrix<double, ZDim, N> Ei_P = //
+      - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
-            E.block(ZDim * i, 0, ZDim, N) * P;
 
-        // D = (Dx2) * ZDim
+      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
-        augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
+      augmentedHessian.setDiagonalBlock(i, FiT
-        - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
+          * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
 
-        // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
+      // upper triangular part of the hessian
-        augmentedHessian.setDiagonalBlock(i, FiT
+      for (size_t j = i + 1; j < m; j++) { // for each camera
-            * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
+        const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
 
-        // upper triangular part of the hessian
+        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
-        for (size_t j = i + 1; j < m; j++) { // for each camera
+        augmentedHessian.setOffDiagonalBlock(i, j, -FiT
-          const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
+            * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
+      }
+    } // end of for over cameras
 
-          // (DxD) = (Dx2) * ( (2x2) * (2xD) )
+    augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
-          augmentedHessian.setOffDiagonalBlock(i, j, -FiT
+    return augmentedHessian;
-              * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
+  }
+
+  /**
+   * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
+   * G = F' * F - F' * E * P * E' * F
+   * g = F' * (b - E * P * E' * b)
+   * In this version, we allow for the case where the keys in the Jacobian are
+   * organized differently from the keys in the output SymmetricBlockMatrix In
+   * particular: each diagonal block of the Jacobian F captures 2 poses (useful
+   * for rolling shutter and extrinsic calibration) such that F keeps the block
+   * structure that makes the Schur complement trick fast.
+   *
+   * N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is
+   * the Hessian block dimension
+   */
+  template <int N, int ND, int NDD>
+  static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(
+      const std::vector<
+          Eigen::Matrix<double, ZDim, ND>,
+          Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND>>>& Fs,
+      const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b,
+      const KeyVector& jacobianKeys, const KeyVector& hessianKeys) {
+    size_t nrNonuniqueKeys = jacobianKeys.size();
+    size_t nrUniqueKeys = hessianKeys.size();
+
+    // Marginalize point: note - we reuse the standard SchurComplement function.
+    SymmetricBlockMatrix augmentedHessian = SchurComplement<N, ND>(Fs, E, P, b);
+
+    // Pack into an Hessian factor, allow space for b term.
+    std::vector<DenseIndex> dims(nrUniqueKeys + 1);
+    std::fill(dims.begin(), dims.end() - 1, NDD);
+    dims.back() = 1;
+    SymmetricBlockMatrix augmentedHessianUniqueKeys;
+
+    // Deal with the fact that some blocks may share the same keys.
+    if (nrUniqueKeys == nrNonuniqueKeys) {
+      // Case when there is 1 calibration key per camera:
+      augmentedHessianUniqueKeys = SymmetricBlockMatrix(
+          dims, Matrix(augmentedHessian.selfadjointView()));
+    } else {
+      // When multiple cameras share a calibration we have to rearrange
+      // the results of the Schur complement matrix.
+      std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1);  // includes b
+      std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD);
+      nonuniqueDims.back() = 1;
+      augmentedHessian = SymmetricBlockMatrix(
+          nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
+
+      // Get map from key to location in the new augmented Hessian matrix (the
+      // one including only unique keys).
+      std::map<Key, size_t> keyToSlotMap;
+      for (size_t k = 0; k < nrUniqueKeys; k++) {
+        keyToSlotMap[hessianKeys[k]] = k;
+      }
+
+      // Initialize matrix to zero.
+      augmentedHessianUniqueKeys = SymmetricBlockMatrix(
+          dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1));
+
+      // Add contributions for each key: note this loops over the hessian with
+      // nonUnique keys (augmentedHessian) and populates an Hessian that only
+      // includes the unique keys (that is what we want to return).
+      for (size_t i = 0; i < nrNonuniqueKeys; i++) {  // rows
+        Key key_i = jacobianKeys.at(i);
+
+        // Update information vector.
+        augmentedHessianUniqueKeys.updateOffDiagonalBlock(
+            keyToSlotMap[key_i], nrUniqueKeys,
+            augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));
+
+        // Update blocks.
+        for (size_t j = i; j < nrNonuniqueKeys; j++) {  // cols
+          Key key_j = jacobianKeys.at(j);
+          if (i == j) {
+            augmentedHessianUniqueKeys.updateDiagonalBlock(
+                keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i));
+          } else {  // (i < j)
+            if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
+              augmentedHessianUniqueKeys.updateOffDiagonalBlock(
+                  keyToSlotMap[key_i], keyToSlotMap[key_j],
+                  augmentedHessian.aboveDiagonalBlock(i, j));
+            } else {
+              augmentedHessianUniqueKeys.updateDiagonalBlock(
+                  keyToSlotMap[key_i],
+                  augmentedHessian.aboveDiagonalBlock(i, j) +
+                      augmentedHessian.aboveDiagonalBlock(i, j).transpose());
+            }
+          }
         }
-      } // end of for over cameras
+      }
 
-      augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
+      // Update bottom right element of the matrix.
-      return augmentedHessian;
+      augmentedHessianUniqueKeys.updateDiagonalBlock(
+          nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
     }
+    return augmentedHessianUniqueKeys;
+  }
 
   /**
    * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
@@ -206,7 +304,7 @@ public:
   }
 
   /// Computes Point Covariance P, with lambda parameter
-  template<int N> // N = 2 or 3
+  template<int N> // N = 2 or 3 (point dimension)
   static void ComputePointCovariance(Eigen::Matrix<double, N, N>& P,
       const Matrix& E, double lambda, bool diagonalDamping = false) {
 
@@ -258,7 +356,7 @@ public:
    * Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
    * and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
    */
-  template<int N> // N = 2 or 3
+  template<int N> // N = 2 or 3 (point dimension)
   static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E,
       const Eigen::Matrix<double, N, N>& P, const Vector& b,
       const KeyVector& allKeys, const KeyVector& keys,

gtsam/geometry/Cyclic.h

@@ -17,7 +17,9 @@
 
 #include <gtsam/base/Group.h>
 #include <gtsam/base/Testable.h>
-#include <iostream> // for cout :-(
+
+#include <cassert>
+#include <iostream>  // for cout :-(
 
 namespace gtsam {
 

gtsam/geometry/PinholeCamera.h

@@ -30,7 +30,7 @@ namespace gtsam {
  * \nosubgrouping
  */
 template<typename Calibration>
-class PinholeCamera: public PinholeBaseK<Calibration> {
+class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
 
 public:
 

gtsam/geometry/PinholePose.h

@@ -31,7 +31,7 @@ namespace gtsam {
  * \nosubgrouping
  */
 template<typename CALIBRATION>
-class PinholeBaseK: public PinholeBase {
+class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
 
 private:
 

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;
 

gtsam/geometry/Pose3.h

@@ -142,7 +142,7 @@ public:
   static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none);
 
   /**
-   * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame
+   * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame
    * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi)
    */
   Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect

gtsam/geometry/SO3.cpp

@@ -261,25 +261,59 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
 
   // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
   // we do something special
-  if (tr + 1.0 < 1e-10) {
+  if (tr + 1.0 < 1e-3) {
-    if (std::abs(R33 + 1.0) > 1e-5)
+    if (R33 > R22 && R33 > R11) {
-      omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
+      // R33 is the largest diagonal, a=3, b=1, c=2
-    else if (std::abs(R22 + 1.0) > 1e-5)
+      const double W = R21 - R12;
-      omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
+      const double Q1 = 2.0 + 2.0 * R33;
-    else
+      const double Q2 = R31 + R13;
-      // if(std::abs(R.r1_.x()+1.0) > 1e-5)  This is implicit
+      const double Q3 = R23 + R32;
-      omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
+      const double r = sqrt(Q1);
+      const double one_over_r = 1 / r;
+      const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
+      const double sgn_w = W < 0 ? -1.0 : 1.0;
+      const double mag = M_PI - (2 * sgn_w * W) / norm;
+      const double scale = 0.5 * one_over_r * mag;
+      omega = sgn_w * scale * Vector3(Q2, Q3, Q1);
+    } else if (R22 > R11) {
+      // R22 is the largest diagonal, a=2, b=3, c=1
+      const double W = R13 - R31;
+      const double Q1 = 2.0 + 2.0 * R22;
+      const double Q2 = R23 + R32;
+      const double Q3 = R12 + R21;
+      const double r = sqrt(Q1);
+      const double one_over_r = 1 / r;
+      const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
+      const double sgn_w = W < 0 ? -1.0 : 1.0;
+      const double mag = M_PI - (2 * sgn_w * W) / norm;
+      const double scale = 0.5 * one_over_r * mag;
+      omega = sgn_w * scale * Vector3(Q3, Q1, Q2);
+    } else {
+      // R11 is the largest diagonal, a=1, b=2, c=3
+      const double W = R32 - R23;
+      const double Q1 = 2.0 + 2.0 * R11;
+      const double Q2 = R12 + R21;
+      const double Q3 = R31 + R13;
+      const double r = sqrt(Q1);
+      const double one_over_r = 1 / r;
+      const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
+      const double sgn_w = W < 0 ? -1.0 : 1.0;
+      const double mag = M_PI - (2 * sgn_w * W) / norm;
+      const double scale = 0.5 * one_over_r * mag;
+      omega = sgn_w * scale * Vector3(Q1, Q2, Q3);
+    }
   } else {
     double magnitude;
-    const double tr_3 = tr - 3.0;  // always negative
+    const double tr_3 = tr - 3.0; // could be non-negative if the matrix is off orthogonal
-    if (tr_3 < -1e-7) {
+    if (tr_3 < -1e-6) {
+      // this is the normal case -1 < trace < 3
       double theta = acos((tr - 1.0) / 2.0);
       magnitude = theta / (2.0 * sin(theta));
     } else {
       // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
       // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
       // see https://github.com/borglab/gtsam/issues/746 for details
-      magnitude = 0.5 - tr_3 / 12.0;
+      magnitude = 0.5 - tr_3 / 12.0 + tr_3*tr_3/60.0;
     }
     omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
   }

gtsam/geometry/SimpleCamera.h

@@ -21,6 +21,7 @@
 #include <gtsam/geometry/BearingRange.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3_S2.h>

gtsam/geometry/tests/testCalibratedCamera.cpp

@@ -20,12 +20,11 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
 #include <iostream>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -54,8 +53,8 @@ TEST(CalibratedCamera, Create) {
   EXPECT(assert_equal(camera, CalibratedCamera::Create(kDefaultPose, actualH)));
 
   // Check derivative
-  boost::function<CalibratedCamera(Pose3)> f =  //
+  std::function<CalibratedCamera(Pose3)> f =  //
-      boost::bind(CalibratedCamera::Create, _1, boost::none);
+      std::bind(CalibratedCamera::Create, std::placeholders::_1, boost::none);
   Matrix numericalH = numericalDerivative11<CalibratedCamera, Pose3>(f, kDefaultPose);
   EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }

gtsam/geometry/tests/testCameraSet.cpp

@@ -17,6 +17,7 @@
  */
 
 #include <gtsam/geometry/CameraSet.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
@@ -125,6 +126,89 @@ TEST(CameraSet, Pinhole) {
   EXPECT(assert_equal(actualE, E));
 }
 
+/* ************************************************************************* */
+TEST(CameraSet, SchurComplementAndRearrangeBlocks) {
+  typedef PinholePose<Cal3Bundler> Camera;
+  typedef CameraSet<Camera> Set;
+
+  // this is the (block) Jacobian with respect to the nonuniqueKeys
+  std::vector<Eigen::Matrix<double, 2, 12>,
+      Eigen::aligned_allocator<Eigen::Matrix<double, 2, 12> > > Fs;
+  Fs.push_back(1 * Matrix::Ones(2, 12));  // corresponding to key pair (0,1)
+  Fs.push_back(2 * Matrix::Ones(2, 12));  // corresponding to key pair (1,2)
+  Fs.push_back(3 * Matrix::Ones(2, 12));  // corresponding to key pair (2,0)
+  Matrix E = 4 * Matrix::Identity(6, 3) + Matrix::Ones(6, 3);
+  E(0, 0) = 3;
+  E(1, 1) = 2;
+  E(2, 2) = 5;
+  Matrix Et = E.transpose();
+  Matrix P = (Et * E).inverse();
+  Vector b = 5 * Vector::Ones(6);
+
+  {  // SchurComplement
+     // Actual
+    SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplement<3, 12>(Fs, E,
+                                                                          P, b);
+    Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView();
+
+    // Expected
+    Matrix F = Matrix::Zero(6, 3 * 12);
+    F.block<2, 12>(0, 0) = 1 * Matrix::Ones(2, 12);
+    F.block<2, 12>(2, 12) = 2 * Matrix::Ones(2, 12);
+    F.block<2, 12>(4, 24) = 3 * Matrix::Ones(2, 12);
+
+    Matrix Ft = F.transpose();
+    Matrix H = Ft * F - Ft * E * P * Et * F;
+    Vector v = Ft * (b - E * P * Et * b);
+    Matrix expectedAugmentedHessian = Matrix::Zero(3 * 12 + 1, 3 * 12 + 1);
+    expectedAugmentedHessian.block<36, 36>(0, 0) = H;
+    expectedAugmentedHessian.block<36, 1>(0, 36) = v;
+    expectedAugmentedHessian.block<1, 36>(36, 0) = v.transpose();
+    expectedAugmentedHessian(36, 36) = b.squaredNorm();
+
+    EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
+  }
+
+  {  // SchurComplementAndRearrangeBlocks
+    KeyVector nonuniqueKeys;
+    nonuniqueKeys.push_back(0);
+    nonuniqueKeys.push_back(1);
+    nonuniqueKeys.push_back(1);
+    nonuniqueKeys.push_back(2);
+    nonuniqueKeys.push_back(2);
+    nonuniqueKeys.push_back(0);
+
+    KeyVector uniqueKeys;
+    uniqueKeys.push_back(0);
+    uniqueKeys.push_back(1);
+    uniqueKeys.push_back(2);
+
+    // Actual
+    SymmetricBlockMatrix augmentedHessianBM =
+        Set::SchurComplementAndRearrangeBlocks<3, 12, 6>(
+            Fs, E, P, b, nonuniqueKeys, uniqueKeys);
+    Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView();
+
+    // Expected
+    // we first need to build the Jacobian F according to unique keys
+    Matrix F = Matrix::Zero(6, 18);
+    F.block<2, 6>(0, 0) = Fs[0].block<2, 6>(0, 0);
+    F.block<2, 6>(0, 6) = Fs[0].block<2, 6>(0, 6);
+    F.block<2, 6>(2, 6) = Fs[1].block<2, 6>(0, 0);
+    F.block<2, 6>(2, 12) = Fs[1].block<2, 6>(0, 6);
+    F.block<2, 6>(4, 12) = Fs[2].block<2, 6>(0, 0);
+    F.block<2, 6>(4, 0) = Fs[2].block<2, 6>(0, 6);
+
+    Matrix Ft = F.transpose();
+    Vector v = Ft * (b - E * P * Et * b);
+    Matrix H = Ft * F - Ft * E * P * Et * F;
+    Matrix expectedAugmentedHessian(19, 19);
+    expectedAugmentedHessian << H, v, v.transpose(), b.squaredNorm();
+
+    EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
+  }
+}
+
 /* ************************************************************************* */
 #include <gtsam/geometry/StereoCamera.h>
 TEST(CameraSet, Stereo) {

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -5,16 +5,15 @@
  * @date December 17, 2013
  */
 
-#include <gtsam/geometry/EssentialMatrix.h>
-#include <gtsam/geometry/CalibratedCamera.h>
-#include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/Testable.h>
-
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/EssentialMatrix.h>
+
 #include <sstream>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -42,15 +41,15 @@ TEST(EssentialMatrix, FromRotationAndDirection) {
       1e-8));
 
   Matrix expectedH1 = numericalDerivative11<EssentialMatrix, Rot3>(
-      boost::bind(EssentialMatrix::FromRotationAndDirection, _1, trueDirection, boost::none,
+      std::bind(EssentialMatrix::FromRotationAndDirection,
-                  boost::none),
+                std::placeholders::_1, trueDirection, boost::none, boost::none),
       trueRotation);
   EXPECT(assert_equal(expectedH1, actualH1, 1e-7));
 
   Matrix expectedH2 = numericalDerivative11<EssentialMatrix, Unit3>(
-      boost::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, _1, boost::none,
+      std::bind(EssentialMatrix::FromRotationAndDirection, trueRotation,
-                  boost::none),
+                std::placeholders::_1, boost::none, boost::none),
-                  trueDirection);
+      trueDirection);
   EXPECT(assert_equal(expectedH2, actualH2, 1e-7));
 }
 
@@ -176,7 +175,7 @@ TEST (EssentialMatrix, FromPose3_a) {
   Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras
   EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
   Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
-      boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
+      std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose);
   EXPECT(assert_equal(expectedH, actualH, 1e-7));
 }
 
@@ -189,7 +188,7 @@ TEST (EssentialMatrix, FromPose3_b) {
   Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
   EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
   Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
-      boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
+      std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose);
   EXPECT(assert_equal(expectedH, actualH, 1e-5));
 }
 

gtsam/geometry/tests/testOrientedPlane3.cpp

@@ -21,10 +21,9 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
 #include <boost/assign/std/vector.hpp>
-#include <boost/bind/bind.hpp>
 
 using namespace boost::assign;
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 using namespace std;
 using boost::none;
@@ -138,8 +137,9 @@ TEST(OrientedPlane3, errorVector) {
                       Vector2(actual[0], actual[1])));
   EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2]));
 
-  boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
+  std::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
-      boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
+      std::bind(&OrientedPlane3::errorVector, std::placeholders::_1,
+                std::placeholders::_2, boost::none, boost::none);
   expectedH1 = numericalDerivative21(f, plane1, plane2);
   expectedH2 = numericalDerivative22(f, plane1, plane2);
   EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
@@ -150,8 +150,8 @@ TEST(OrientedPlane3, errorVector) {
 TEST(OrientedPlane3, jacobian_retract) {
   OrientedPlane3 plane(-1, 0.1, 0.2, 5);
   Matrix33 H_actual;
-  boost::function<OrientedPlane3(const Vector3&)> f =
+  std::function<OrientedPlane3(const Vector3&)> f = std::bind(
-      boost::bind(&OrientedPlane3::retract, plane, _1, boost::none);
+      &OrientedPlane3::retract, plane, std::placeholders::_1, boost::none);
   {
       Vector3 v(-0.1, 0.2, 0.3);
       plane.retract(v, H_actual);

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -22,13 +22,12 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
-#include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
 
 #include <cmath>
 #include <iostream>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -66,8 +65,9 @@ TEST(PinholeCamera, Create) {
   EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2)));
 
   // Check derivative
-  boost::function<Camera(Pose3,Cal3_S2)> f = //
+  std::function<Camera(Pose3, Cal3_S2)> f =  //
-      boost::bind(Camera::Create,_1,_2,boost::none,boost::none);
+      std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2,
+                boost::none, boost::none);
   Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K);
   EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
   Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
@@ -81,8 +81,8 @@ TEST(PinholeCamera, Pose) {
   EXPECT(assert_equal(pose, camera.getPose(actualH)));
 
   // Check derivative
-  boost::function<Pose3(Camera)> f = //
+  std::function<Pose3(Camera)> f =  //
-      boost::bind(&Camera::getPose,_1,boost::none);
+      std::bind(&Camera::getPose, std::placeholders::_1, boost::none);
   Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
   EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }

gtsam/geometry/tests/testPinholePose.cpp

@@ -65,8 +65,8 @@ TEST(PinholeCamera, Pose) {
   EXPECT(assert_equal(pose, camera.getPose(actualH)));
 
   // Check derivative
-  boost::function<Pose3(Camera)> f = //
+  std::function<Pose3(Camera)> f = //
-      boost::bind(&Camera::getPose,_1,boost::none);
+      std::bind(&Camera::getPose,_1,boost::none);
   Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
   EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }

gtsam/geometry/tests/testPoint3.cpp

@@ -14,14 +14,14 @@
  * @brief  Unit tests for Point3 class
  */
 
-#include <gtsam/geometry/Point3.h>
+#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Point3.h>
 
-#include <boost/bind/bind.hpp>
+#include <boost/function.hpp>
-#include <CppUnitLite/TestHarness.h>
 
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 
 GTSAM_CONCEPT_TESTABLE_INST(Point3)
@@ -101,7 +101,7 @@ TEST( Point3, dot) {
 
   // Use numerical derivatives to calculate the expected Jacobians
   Matrix H1, H2;
-  boost::function<double(const Point3&, const Point3&)> f =
+  std::function<double(const Point3&, const Point3&)> f =
       [](const Point3& p, const Point3& q) { return gtsam::dot(p, q); };
   {
     gtsam::dot(p, q, H1, H2);
@@ -123,8 +123,9 @@ TEST( Point3, dot) {
 /* ************************************************************************* */
 TEST(Point3, cross) {
   Matrix aH1, aH2;
-  boost::function<Point3(const Point3&, const Point3&)> f =
+  std::function<Point3(const Point3&, const Point3&)> f =
-      boost::bind(&gtsam::cross, _1, _2, boost::none, boost::none);
+      std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2,
+                boost::none, boost::none);
   const Point3 omega(0, 1, 0), theta(4, 6, 8);
   cross(omega, theta, aH1, aH2);
   EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
@@ -142,8 +143,9 @@ TEST( Point3, cross2) {
 
   // Use numerical derivatives to calculate the expected Jacobians
   Matrix H1, H2;
-  boost::function<Point3(const Point3&, const Point3&)> f = boost::bind(&gtsam::cross, _1, _2,  //
+  std::function<Point3(const Point3&, const Point3&)> f =
-                                                                      boost::none, boost::none);
+      std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2,  //
+                boost::none, boost::none);
   {
     gtsam::cross(p, q, H1, H2);
     EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(f, p, q), H1, 1e-9));
@@ -163,7 +165,7 @@ TEST (Point3, normalize) {
   Point3 expected(point / sqrt(14.0));
   EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8));
   Matrix expectedH = numericalDerivative11<Point3, Point3>(
-      boost::bind(gtsam::normalize, _1, boost::none), point);
+      std::bind(gtsam::normalize, std::placeholders::_1, boost::none), point);
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 

gtsam/geometry/tests/testPose3.cpp

@@ -22,8 +22,7 @@
 
 #include <boost/assign/std/vector.hpp> // for operator +=
 using namespace boost::assign;
-#include <boost/bind/bind.hpp>
+using namespace std::placeholders;
-using namespace boost::placeholders;
 
 #include <CppUnitLite/TestHarness.h>
 #include <cmath>
@@ -215,7 +214,7 @@ TEST(Pose3, translation) {
   EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8));
 
   Matrix numericalH = numericalDerivative11<Point3, Pose3>(
-      boost::bind(&Pose3::translation, _1, boost::none), T);
+      std::bind(&Pose3::translation, std::placeholders::_1, boost::none), T);
   EXPECT(assert_equal(numericalH, actualH, 1e-6));
 }
 
@@ -226,7 +225,7 @@ TEST(Pose3, rotation) {
   EXPECT(assert_equal(R, T.rotation(actualH), 1e-8));
 
   Matrix numericalH = numericalDerivative11<Rot3, Pose3>(
-      boost::bind(&Pose3::rotation, _1, boost::none), T);
+      std::bind(&Pose3::rotation, std::placeholders::_1, boost::none), T);
   EXPECT(assert_equal(numericalH, actualH, 1e-6));
 }
 
@@ -1047,12 +1046,76 @@ TEST(Pose3, interpolate) {
   EXPECT(assert_equal(expected2, T2.interpolateRt(T3, t)));
 }
 
+/* ************************************************************************* */
+Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { return interpolate(t1,t2,gamma); }
+
+TEST(Pose3, interpolateJacobians) {
+  {
+    Pose3 X = Pose3::identity();
+    Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
+    double t = 0.5;
+    Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation
+    Matrix actualJacobianX, actualJacobianY;
+    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
+
+    Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
+
+    Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+  }
+  {
+    Pose3 X = Pose3::identity();
+    Pose3 Y(Rot3::identity(), Point3(1, 0, 0));
+    double t = 0.3;
+    Pose3 expectedPoseInterp(Rot3::identity(), Point3(0.3, 0, 0));
+    Matrix actualJacobianX, actualJacobianY;
+    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
+
+    Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
+
+    Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+  }
+  {
+    Pose3 X = Pose3::identity();
+    Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0));
+    double t = 0.5;
+    Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
+    Matrix actualJacobianX, actualJacobianY;
+    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
+
+    Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
+
+    Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+  }
+  {
+    Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2));
+    Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1));
+    double t = 0.3;
+    Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
+    Matrix actualJacobianX, actualJacobianY;
+    interpolate(X, Y, t, actualJacobianX, actualJacobianY);
+
+    Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
+
+    Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+  }
+}
+
 /* ************************************************************************* */
 TEST(Pose3, Create) {
   Matrix63 actualH1, actualH2;
   Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2);
   EXPECT(assert_equal(T, actual));
-  boost::function<Pose3(Rot3,Point3)> create = boost::bind(Pose3::Create,_1,_2,boost::none,boost::none);
+  std::function<Pose3(Rot3, Point3)> create =
+      std::bind(Pose3::Create, std::placeholders::_1, std::placeholders::_2,
+                boost::none, boost::none);
   EXPECT(assert_equal(numericalDerivative21<Pose3,Rot3,Point3>(create, R, P2), actualH1, 1e-9));
   EXPECT(assert_equal(numericalDerivative22<Pose3,Rot3,Point3>(create, R, P2), actualH2, 1e-9));
 }

gtsam/geometry/tests/testRot3.cpp

@@ -122,6 +122,21 @@ TEST( Rot3, AxisAngle)
   CHECK(assert_equal(expected,actual3,1e-5));
 }
 
+/* ************************************************************************* */
+TEST( Rot3, AxisAngle2)
+{
+  // constructor from a rotation matrix, as doubles in *row-major* order.
+  Rot3 R1(-0.999957, 0.00922903, 0.00203116, 0.00926964, 0.999739, 0.0208927, -0.0018374, 0.0209105, -0.999781);
+  
+  Unit3 actualAxis;
+  double actualAngle;
+  // convert Rot3 to quaternion using GTSAM
+  std::tie(actualAxis, actualAngle) = R1.axisAngle();
+  
+  double expectedAngle = 3.1396582;
+  CHECK(assert_equal(expectedAngle, actualAngle, 1e-5));
+}
+
 /* ************************************************************************* */
 TEST( Rot3, Rodrigues)
 {
@@ -181,13 +196,13 @@ TEST( Rot3, retract)
 }
 
 /* ************************************************************************* */
-TEST(Rot3, log) {
+TEST( Rot3, log) {
   static const double PI = boost::math::constants::pi<double>();
   Vector w;
   Rot3 R;
 
 #define CHECK_OMEGA(X, Y, Z)             \
-  w = (Vector(3) << X, Y, Z).finished(); \
+  w = (Vector(3) << (X), (Y), (Z)).finished(); \
   R = Rot3::Rodrigues(w);                \
   EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12));
 
@@ -219,17 +234,17 @@ TEST(Rot3, log) {
   CHECK_OMEGA(0, 0, PI)
 
   // Windows and Linux have flipped sign in quaternion mode
-#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
+//#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
   w = (Vector(3) << x * PI, y * PI, z * PI).finished();
   R = Rot3::Rodrigues(w);
   EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12));
-#else
+//#else
-  CHECK_OMEGA(x * PI, y * PI, z * PI)
+//  CHECK_OMEGA(x * PI, y * PI, z * PI)
-#endif
+//#endif
 
   // Check 360 degree rotations
 #define CHECK_OMEGA_ZERO(X, Y, Z)        \
-  w = (Vector(3) << X, Y, Z).finished(); \
+  w = (Vector(3) << (X), (Y), (Z)).finished(); \
   R = Rot3::Rodrigues(w);                \
   EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R)));
 
@@ -247,15 +262,15 @@ TEST(Rot3, log) {
   // Rot3's Logmap returns different, but equivalent compacted
   // axis-angle vectors depending on whether Rot3 is implemented
   // by Quaternions or SO3.
-  #if defined(GTSAM_USE_QUATERNIONS)
+#if defined(GTSAM_USE_QUATERNIONS)
-    // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees
+  // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees
-    EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211),
+  EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211),
+                      (Vector)Rot3::Logmap(Rlund), 1e-8));
+#else
+  // SO3 will be approximate because of the non-orthogonality
+  EXPECT(assert_equal(Vector3(0.264452, -0.742197708, -3.04098184),
                         (Vector)Rot3::Logmap(Rlund), 1e-8));
-  #else
+#endif
-    // SO3 does not bound angle resulting in ~180.1 degrees
-    EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314),
-                        (Vector)Rot3::Logmap(Rlund), 1e-8));
-  #endif
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testSO3.cpp

@@ -15,15 +15,12 @@
  * @author Frank Dellaert
  **/
 
-#include <gtsam/geometry/SO3.h>
+#include <CppUnitLite/TestHarness.h>
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/testLie.h>
+#include <gtsam/geometry/SO3.h>
 
-#include <boost/bind/bind.hpp>
+using namespace std::placeholders;
-#include <CppUnitLite/TestHarness.h>
-
-using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
@@ -211,7 +208,7 @@ TEST(SO3, ExpmapDerivative) {
 TEST(SO3, ExpmapDerivative2) {
   const Vector3 theta(0.1, 0, 0.1);
   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
-      boost::bind(&SO3::Expmap, _1, boost::none), theta);
+      std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta);
 
   CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
   CHECK(assert_equal(Matrix3(Jexpected.transpose()),
@@ -222,7 +219,7 @@ TEST(SO3, ExpmapDerivative2) {
 TEST(SO3, ExpmapDerivative3) {
   const Vector3 theta(10, 20, 30);
   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
-      boost::bind(&SO3::Expmap, _1, boost::none), theta);
+      std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta);
 
   CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
   CHECK(assert_equal(Matrix3(Jexpected.transpose()),
@@ -277,7 +274,7 @@ TEST(SO3, ExpmapDerivative5) {
 TEST(SO3, ExpmapDerivative6) {
   const Vector3 thetahat(0.1, 0, 0.1);
   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
-      boost::bind(&SO3::Expmap, _1, boost::none), thetahat);
+      std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), thetahat);
   Matrix3 Jactual;
   SO3::Expmap(thetahat, Jactual);
   EXPECT(assert_equal(Jexpected, Jactual));
@@ -288,7 +285,7 @@ TEST(SO3, LogmapDerivative) {
   const Vector3 thetahat(0.1, 0, 0.1);
   const SO3 R = SO3::Expmap(thetahat);  // some rotation
   const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
-      boost::bind(&SO3::Logmap, _1, boost::none), R);
+      std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R);
   const Matrix3 Jactual = SO3::LogmapDerivative(thetahat);
   EXPECT(assert_equal(Jexpected, Jactual));
 }
@@ -298,7 +295,7 @@ TEST(SO3, JacobianLogmap) {
   const Vector3 thetahat(0.1, 0, 0.1);
   const SO3 R = SO3::Expmap(thetahat);  // some rotation
   const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
-      boost::bind(&SO3::Logmap, _1, boost::none), R);
+      std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R);
   Matrix3 Jactual;
   SO3::Logmap(R, Jactual);
   EXPECT(assert_equal(Jexpected, Jactual));
@@ -308,7 +305,7 @@ TEST(SO3, JacobianLogmap) {
 TEST(SO3, ApplyDexp) {
   Matrix aH1, aH2;
   for (bool nearZeroApprox : {true, false}) {
-    boost::function<Vector3(const Vector3&, const Vector3&)> f =
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
           return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
         };
@@ -331,7 +328,7 @@ TEST(SO3, ApplyDexp) {
 TEST(SO3, ApplyInvDexp) {
   Matrix aH1, aH2;
   for (bool nearZeroApprox : {true, false}) {
-    boost::function<Vector3(const Vector3&, const Vector3&)> f =
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
           return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
         };
@@ -357,7 +354,7 @@ TEST(SO3, vec) {
   Matrix actualH;
   const Vector9 actual = R2.vec(actualH);
   CHECK(assert_equal(expected, actual));
-  boost::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
+  std::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
   const Matrix numericalH = numericalDerivative11(f, R2, 1e-5);
   CHECK(assert_equal(numericalH, actualH));
 }
@@ -371,7 +368,7 @@ TEST(Matrix, compose) {
   Matrix actualH;
   const Matrix3 actual = so3::compose(M, R, actualH);
   CHECK(assert_equal(expected, actual));
-  boost::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
+  std::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
     return so3::compose(M, R);
   };
   Matrix numericalH = numericalDerivative11(f, M, 1e-2);

gtsam/geometry/tests/testSO4.cpp

@@ -166,7 +166,7 @@ TEST(SO4, vec) {
   Matrix actualH;
   const Vector16 actual = Q2.vec(actualH);
   EXPECT(assert_equal(expected, actual));
-  boost::function<Vector16(const SO4&)> f = [](const SO4& Q) {
+  std::function<Vector16(const SO4&)> f = [](const SO4& Q) {
     return Q.vec();
   };
   const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5);
@@ -179,7 +179,7 @@ TEST(SO4, topLeft) {
   Matrix actualH;
   const Matrix3 actual = topLeft(Q3, actualH);
   EXPECT(assert_equal(expected, actual));
-  boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
+  std::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
     return topLeft(Q3);
   };
   const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
@@ -192,7 +192,7 @@ TEST(SO4, stiefel) {
   Matrix actualH;
   const Matrix43 actual = stiefel(Q3, actualH);
   EXPECT(assert_equal(expected, actual));
-  boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
+  std::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
     return stiefel(Q3);
   };
   const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);

gtsam/geometry/tests/testSOn.cpp

@@ -189,7 +189,7 @@ Matrix RetractJacobian(size_t n) { return SOn::VectorizedGenerators(n); }
 /// Test Jacobian of Retract at origin
 TEST(SOn, RetractJacobian) {
   Matrix actualH = RetractJacobian(3);
-  boost::function<Matrix(const Vector &)> h = [](const Vector &v) {
+  std::function<Matrix(const Vector &)> h = [](const Vector &v) {
     return SOn::ChartAtOrigin::Retract(v).matrix();
   };
   Vector3 v;
@@ -205,7 +205,7 @@ TEST(SOn, vec) {
   SOn Q = SOn::ChartAtOrigin::Retract(v);
   Matrix actualH;
   const Vector actual = Q.vec(actualH);
-  boost::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
+  std::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
   const Matrix H = numericalDerivative11<Vector, SOn, 10>(h, Q, 1e-5);
   CHECK(assert_equal(H, actualH));
 }

gtsam/geometry/tests/testSimilarity3.cpp

@@ -16,24 +16,22 @@
  * @author Zhaoyang Lv
  */
 
-#include <gtsam/geometry/Similarity3.h>
+#include <CppUnitLite/TestHarness.h>
-#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/base/Testable.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/ExpressionFactorGraph.h>
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/inference/Symbol.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/testLie.h>
-#include <gtsam/base/Testable.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Similarity3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/ExpressionFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/BetweenFactor.h>
 
-#include <CppUnitLite/TestHarness.h>
+#include <functional>
 
-#include <boost/bind/bind.hpp>
+using namespace std::placeholders;
-#include <boost/function.hpp>
-
-using namespace boost::placeholders;
 using namespace gtsam;
 using namespace std;
 using symbol_shorthand::X;
@@ -243,8 +241,9 @@ TEST(Similarity3, GroupAction) {
   EXPECT(assert_equal(Point3(2, 6, 6), Td.transformFrom(pa)));
 
   // Test derivative
-  boost::function<Point3(Similarity3, Point3)> f = boost::bind(
+  // Use lambda to resolve overloaded method
-      &Similarity3::transformFrom, _1, _2, boost::none, boost::none);
+  std::function<Point3(const Similarity3&, const Point3&)>
+      f = [](const Similarity3& S, const Point3& p){ return S.transformFrom(p); };
 
   Point3 q(1, 2, 3);
   for (const auto& T : { T1, T2, T3, T4, T5, T6 }) {

gtsam/geometry/tests/testUnit3.cpp

@@ -32,13 +32,12 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <boost/assign/std/vector.hpp>
-#include <boost/bind/bind.hpp>
 
 #include <cmath>
 #include <random>
 
 using namespace boost::assign;
-using namespace boost::placeholders;
+using namespace std::placeholders;
 using namespace gtsam;
 using namespace std;
 using gtsam::symbol_shorthand::U;
@@ -127,8 +126,9 @@ TEST(Unit3, dot) {
 
   // Use numerical derivatives to calculate the expected Jacobians
   Matrix H1, H2;
-  boost::function<double(const Unit3&, const Unit3&)> f = boost::bind(&Unit3::dot, _1, _2,  //
+  std::function<double(const Unit3&, const Unit3&)> f =
-                                                                      boost::none, boost::none);
+      std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2,  //
+                boost::none, boost::none);
   {
     p.dot(q, H1, H2);
     EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-5));
@@ -158,13 +158,13 @@ TEST(Unit3, error) {
   // Use numerical derivatives to calculate the expected Jacobian
   {
     expected = numericalDerivative11<Vector2,Unit3>(
-        boost::bind(&Unit3::error, &p, _1, boost::none), q);
+        std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), q);
     p.error(q, actual);
     EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
   }
   {
     expected = numericalDerivative11<Vector2,Unit3>(
-        boost::bind(&Unit3::error, &p, _1, boost::none), r);
+        std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), r);
     p.error(r, actual);
     EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
   }
@@ -185,25 +185,33 @@ TEST(Unit3, error2) {
   // Use numerical derivatives to calculate the expected Jacobian
   {
     expected = numericalDerivative21<Vector2, Unit3, Unit3>(
-        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
+        std::bind(&Unit3::errorVector, std::placeholders::_1,
+                  std::placeholders::_2, boost::none, boost::none),
+        p, q);
     p.errorVector(q, actual, boost::none);
     EXPECT(assert_equal(expected, actual, 1e-5));
   }
   {
     expected = numericalDerivative21<Vector2, Unit3, Unit3>(
-        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
+        std::bind(&Unit3::errorVector, std::placeholders::_1,
+                  std::placeholders::_2, boost::none, boost::none),
+        p, r);
     p.errorVector(r, actual, boost::none);
     EXPECT(assert_equal(expected, actual, 1e-5));
   }
   {
     expected = numericalDerivative22<Vector2, Unit3, Unit3>(
-        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
+        std::bind(&Unit3::errorVector, std::placeholders::_1,
+                  std::placeholders::_2, boost::none, boost::none),
+        p, q);
     p.errorVector(q, boost::none, actual);
     EXPECT(assert_equal(expected, actual, 1e-5));
   }
   {
     expected = numericalDerivative22<Vector2, Unit3, Unit3>(
-        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
+        std::bind(&Unit3::errorVector, std::placeholders::_1,
+                  std::placeholders::_2, boost::none, boost::none),
+        p, r);
     p.errorVector(r, boost::none, actual);
     EXPECT(assert_equal(expected, actual, 1e-5));
   }
@@ -221,13 +229,13 @@ TEST(Unit3, distance) {
   // Use numerical derivatives to calculate the expected Jacobian
   {
     expected = numericalGradient<Unit3>(
-        boost::bind(&Unit3::distance, &p, _1, boost::none), q);
+        std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), q);
     p.distance(q, actual);
     EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
   }
   {
     expected = numericalGradient<Unit3>(
-        boost::bind(&Unit3::distance, &p, _1, boost::none), r);
+        std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), r);
     p.distance(r, actual);
     EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
   }
@@ -319,7 +327,7 @@ TEST(Unit3, basis) {
 
   Matrix62 actualH;
   Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
-      boost::bind(BasisTest, _1, boost::none), p);
+      std::bind(BasisTest, std::placeholders::_1, boost::none), p);
 
   // without H, first time
   EXPECT(assert_equal(expected, p.basis(), 1e-6));
@@ -348,7 +356,7 @@ TEST(Unit3, basis_derivatives) {
     p.basis(actualH);
 
     Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
-                           boost::bind(BasisTest, _1, boost::none), p);
+        std::bind(BasisTest, std::placeholders::_1, boost::none), p);
     EXPECT(assert_equal(expectedH, actualH, 1e-5));
   }
 }
@@ -376,8 +384,8 @@ TEST(Unit3, retract) {
 TEST (Unit3, jacobian_retract) {
   Matrix22 H;
   Unit3 p;
-  boost::function<Unit3(const Vector2&)> f =
+  std::function<Unit3(const Vector2&)> f =
-      boost::bind(&Unit3::retract, p, _1, boost::none);
+      std::bind(&Unit3::retract, p, std::placeholders::_1, boost::none);
   {
       Vector2 v (-0.2, 0.1);
       p.retract(v, H);
@@ -440,7 +448,7 @@ TEST (Unit3, FromPoint3) {
   Unit3 expected(point);
   EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5));
   Matrix expectedH = numericalDerivative11<Unit3, Point3>(
-      boost::bind(Unit3::FromPoint3, _1, boost::none), point);
+      std::bind(Unit3::FromPoint3, std::placeholders::_1, boost::none), point);
   EXPECT(assert_equal(expectedH, actualH, 1e-5));
 }
 

gtsam/geometry/tests/testUtilities.cpp

@@ -0,0 +1,64 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * @file testUtilities.cpp
+ * @date Aug 19, 2021
+ * @author Varun Agrawal
+ * @brief Tests for the utilities.
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/utilities.h>
+
+using namespace gtsam;
+using gtsam::symbol_shorthand::L;
+using gtsam::symbol_shorthand::R;
+using gtsam::symbol_shorthand::X;
+
+/* ************************************************************************* */
+TEST(Utilities, ExtractPoint2) {
+  Point2 p0(0, 0), p1(1, 0);
+  Values values;
+  values.insert<Point2>(L(0), p0);
+  values.insert<Point2>(L(1), p1);
+  values.insert<Rot3>(R(0), Rot3());
+  values.insert<Pose3>(X(0), Pose3());
+
+  Matrix all_points = utilities::extractPoint2(values);
+  EXPECT_LONGS_EQUAL(2, all_points.rows());
+}
+
+/* ************************************************************************* */
+TEST(Utilities, ExtractPoint3) {
+  Point3 p0(0, 0, 0), p1(1, 0, 0);
+  Values values;
+  values.insert<Point3>(L(0), p0);
+  values.insert<Point3>(L(1), p1);
+  values.insert<Rot3>(R(0), Rot3());
+  values.insert<Pose3>(X(0), Pose3());
+
+  Matrix all_points = utilities::extractPoint3(values);
+  EXPECT_LONGS_EQUAL(2, all_points.rows());
+}
+
+/* ************************************************************************* */
+int main() {
+  srand(time(nullptr));
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/geometry/triangulation.h

@@ -18,14 +18,16 @@
 
 #pragma once
 
-#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3Fisheye.h>
+#include <gtsam/geometry/Cal3Unified.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/slam/TriangulationFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/TriangulationFactor.h>
 
 namespace gtsam {
 
@@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
 // Vector of Cameras - used by the Python/MATLAB wrapper
 using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
 using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
+using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
+using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
 
 } // \namespace gtsam
 

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 */
+    /// Construct from a conditional, leaving parent and child pointers
-    BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional), problemSize_(1) {}
+    /// uninitialized.
+    BayesTreeCliqueBase(const sharedConditional& conditional)
+        : conditional_(conditional), problemSize_(1) {}
 
-    /** Shallow copy constructor */
+    /// Shallow copy constructor.
-    BayesTreeCliqueBase(const BayesTreeCliqueBase& c) : conditional_(c.conditional_), parent_(c.parent_), children(c.children), problemSize_(c.problemSize_), is_root(c.is_root) {}
+    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

gtsam/inference/EliminateableFactorGraph.h

@@ -19,7 +19,7 @@
 #pragma once
 
 #include <boost/shared_ptr.hpp>
-#include <boost/function.hpp>
+#include <functional>
 #include <boost/variant.hpp>
 #include <boost/optional.hpp>
 
@@ -86,7 +86,7 @@ namespace gtsam {
     typedef std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<_FactorType> > EliminationResult;
 
     /// The function type that does a single dense elimination step on a subgraph.
-    typedef boost::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
+    typedef std::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
 
     /// Typedef for an optional variable index as an argument to elimination functions
     typedef boost::optional<const VariableIndex&> OptionalVariableIndex;

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;
+using KeyList = FastList<Key>;
-typedef FastSet<Key> KeySet;
+using KeySet = FastSet<Key>;
-typedef FastMap<Key, int> KeyGroupMap;
+using KeyGroupMap = FastMap<Key, int>;
 
 /// Utility function to print one key with optional prefix
-GTSAM_EXPORT void PrintKey(Key key, const std::string& s = "",
+GTSAM_EXPORT void PrintKey(
-    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+    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 = "",
+GTSAM_EXPORT void PrintKeyList(
-    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+    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 =
+GTSAM_EXPORT void PrintKeyVector(
-    "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+    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 = "",
+GTSAM_EXPORT void PrintKeySet(
-    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
+    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;

gtsam/inference/LabeledSymbol.cpp

@@ -15,15 +15,12 @@
  * @author: Alex Cunningham
  */
 
-#include <iostream>
-
-#include <boost/bind/bind.hpp>
-#include <boost/format.hpp>
-
-#include <boost/lexical_cast.hpp>
-
 #include <gtsam/inference/LabeledSymbol.h>
 
+#include <boost/format.hpp>
+#include <boost/lexical_cast.hpp>
+#include <iostream>
+
 namespace gtsam {
 
 using namespace std;
@@ -109,17 +106,37 @@ bool LabeledSymbol::operator!=(gtsam::Key comp) const {
 /* ************************************************************************* */
 static LabeledSymbol make(gtsam::Key key) { return LabeledSymbol(key);}
 
-boost::function<bool(gtsam::Key)> LabeledSymbol::TypeTest(unsigned char c) {
+std::function<bool(gtsam::Key)> LabeledSymbol::TypeTest(unsigned char c) {
-  return boost::bind(&LabeledSymbol::chr, boost::bind(make, boost::placeholders::_1)) == c;
+  // Use lambda function to check equality
+  auto equals = [](unsigned char s, unsigned char c) { return s == c; };
+  return std::bind(
+      equals,
+      std::bind(&LabeledSymbol::chr, std::bind(make, std::placeholders::_1)),
+      c);
 }
 
-boost::function<bool(gtsam::Key)> LabeledSymbol::LabelTest(unsigned char label) {
+std::function<bool(gtsam::Key)> LabeledSymbol::LabelTest(unsigned char label) {
-  return boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_1)) == label;
+  // Use lambda function to check equality
+  auto equals = [](unsigned char s, unsigned char c) { return s == c; };
+  return std::bind(
+      equals,
+      std::bind(&LabeledSymbol::label, std::bind(make, std::placeholders::_1)),
+      label);
 }
 
-boost::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) {
+std::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) {
-  return boost::bind(&LabeledSymbol::chr,   boost::bind(make, boost::placeholders::_1)) == c &&
+  // Use lambda functions for && and ==
-      boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_1)) == label;
+  auto logical_and = [](bool is_type, bool is_label) { return is_type == is_label; };
+  auto equals = [](unsigned char s, unsigned char c) { return s == c; };
+  return std::bind(logical_and,
+                   std::bind(equals,
+                             std::bind(&LabeledSymbol::chr,
+                                       std::bind(make, std::placeholders::_1)),
+                             c),
+                   std::bind(equals,
+                             std::bind(&LabeledSymbol::label,
+                                       std::bind(make, std::placeholders::_1)),
+                             label));
 }
 
 /* ************************************************************************* */

gtsam/inference/LabeledSymbol.h

@@ -19,8 +19,8 @@
 
 #pragma once
 
+#include <functional>
 #include <gtsam/inference/Symbol.h>
-#include <boost/function.hpp>
 
 namespace gtsam {
 
@@ -89,13 +89,13 @@ public:
    */
 
   // Checks only the type
-  static boost::function<bool(gtsam::Key)> TypeTest(unsigned char c);
+  static std::function<bool(gtsam::Key)> TypeTest(unsigned char c);
 
   // Checks only the robot ID (label_)
-  static boost::function<bool(gtsam::Key)> LabelTest(unsigned char label);
+  static std::function<bool(gtsam::Key)> LabelTest(unsigned char label);
 
   // Checks both type and the robot ID
-  static boost::function<bool(gtsam::Key)> TypeLabelTest(unsigned char c, unsigned char label);
+  static std::function<bool(gtsam::Key)> TypeLabelTest(unsigned char c, unsigned char label);
 
   // Converts to upper/lower versions of labels
   LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); }

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;
 

gtsam/inference/Symbol.cpp

@@ -62,8 +62,11 @@ Symbol::operator std::string() const {
 
 static Symbol make(gtsam::Key key) { return Symbol(key);}
 
-boost::function<bool(Key)> Symbol::ChrTest(unsigned char c) {
+std::function<bool(Key)> Symbol::ChrTest(unsigned char c) {
-  return boost::bind(&Symbol::chr, boost::bind(make, boost::placeholders::_1)) == c;
+  auto equals = [](unsigned char s, unsigned char c) { return s == c; };
+  return std::bind(
+      equals, std::bind(&Symbol::chr, std::bind(make, std::placeholders::_1)),
+      c);
 }
 
 GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const Symbol &symbol) {