Merge branch 'develop' into feature/system-metis-lib

.github/scripts/boost.sh

@@ -0,0 +1,18 @@
+### Script to install Boost
+BOOST_FOLDER=boost_${BOOST_VERSION//./_}
+
+# Download Boost
+wget https://boostorg.jfrog.io/artifactory/main/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz
+
+# Unzip
+tar -zxf ${BOOST_FOLDER}.tar.gz
+
+# Bootstrap
+cd ${BOOST_FOLDER}/
+./bootstrap.sh --with-libraries=serialization,filesystem,thread,system,atomic,date_time,timer,chrono,program_options,regex
+
+# Build and install
+sudo ./b2 -j$(nproc) install
+
+# Rebuild ld cache
+sudo ldconfig

.github/scripts/python.sh

@@ -58,28 +58,29 @@ PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
 
 
 BUILD_PYBIND="ON"
-TYPEDEF_POINTS_TO_VECTORS="ON"
 
 sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
 
 mkdir $GITHUB_WORKSPACE/build
 cd $GITHUB_WORKSPACE/build
 
-cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=Release \
+cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
-    -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_UNSTABLE=ON \
+    -DGTSAM_BUILD_TESTS=OFF \
+    -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
     -DGTSAM_USE_QUATERNIONS=OFF \
     -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
     -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
     -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
     -DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \
-    -DGTSAM_TYPEDEF_POINTS_TO_VECTORS=${TYPEDEF_POINTS_TO_VECTORS} \
+    -DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \
     -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
     -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \
     -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=OFF \
     -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install
 
-make -j$(nproc) install
 
+# Set to 2 cores so that Actions does not error out during resource provisioning.
+make -j2 install
 
 cd $GITHUB_WORKSPACE/build/python
 $PYTHON setup.py install --user --prefix=

.github/scripts/unix.sh

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

.github/workflows/build-linux.yml

@@ -12,6 +12,7 @@ jobs:
       CTEST_PARALLEL_LEVEL: 2
       CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
       GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
+      BOOST_VERSION: 1.67.0
 
     strategy:
       fail-fast: false
@@ -44,9 +45,9 @@ jobs:
 
     steps:
       - name: Checkout
-        uses: actions/checkout@master
+        uses: actions/checkout@v2
-      - name: Install (Linux)
+
-        if: runner.os == 'Linux'
+      - name: Install Dependencies
         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
@@ -55,17 +56,14 @@ jobs:
             # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository
             # This key is not in the keystore by default for Ubuntu so we need to add it.
             LLVM_KEY=15CF4D18AF4F7421
-            gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY
+            gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
             gpg -a --export $LLVM_KEY | sudo apt-key add -
             sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
           fi
           sudo apt-get -y update
 
-          sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
+          sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
-          
+
-          echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV
-          echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV
-          
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
             echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
@@ -75,11 +73,10 @@ jobs:
             echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
             echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
           fi
-      - name: Check Boost version
+
-        if: runner.os == 'Linux'
+      - name: Install Boost
         run: |
-          echo "BOOST_ROOT = $BOOST_ROOT"
+          bash .github/scripts/boost.sh
-      - name: Build and Test (Linux)
+
-        if: runner.os == 'Linux'
+      - name: Build and Test
-        run: |
+        run: bash .github/scripts/unix.sh -t
-          bash .github/scripts/unix.sh -t

.github/workflows/build-macos.yml

@@ -12,6 +12,7 @@ jobs:
       CTEST_PARALLEL_LEVEL: 2
       CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
       GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
+
     strategy:
       fail-fast: false
       matrix:
@@ -31,13 +32,12 @@ jobs:
 
     steps:
       - name: Checkout
-        uses: actions/checkout@master
+        uses: actions/checkout@v2
-      - name: Install (macOS)
+
-        if: runner.os == 'macOS'
+      - name: Install Dependencies
         run: |
-          brew tap ProfFan/robotics
           brew install cmake ninja
-          brew install ProfFan/robotics/boost
+          brew install boost
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             brew install gcc@${{ matrix.version }}
             echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
@@ -47,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

@@ -12,6 +12,7 @@ jobs:
       CTEST_PARALLEL_LEVEL: 2
       CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
       PYTHON_VERSION: ${{ matrix.python_version }}
+
     strategy:
       fail-fast: false
       matrix:
@@ -43,6 +44,14 @@ jobs:
             compiler: clang
             version: "9"
 
+          # NOTE temporarily added this as it is a required check.
+          - name: ubuntu-18.04-clang-9
+            os: ubuntu-18.04
+            compiler: clang
+            version: "9"
+            build_type: Debug
+            python_version: "3"
+
           - name: macOS-10.15-xcode-11.3.1
             os: macOS-10.15
             compiler: xcode
@@ -56,7 +65,7 @@ jobs:
 
     steps:
       - name: Checkout
-        uses: actions/checkout@master
+        uses: actions/checkout@v2
       - name: Install (Linux)
         if: runner.os == 'Linux'
         run: |
@@ -66,13 +75,13 @@ jobs:
             # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository
             # This key is not in the keystore by default for Ubuntu so we need to add it.
             LLVM_KEY=15CF4D18AF4F7421
-            gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY
+            gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
             gpg -a --export $LLVM_KEY | sudo apt-key add -
             sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
           fi
           sudo apt-get -y update
           
-          sudo apt install cmake build-essential pkg-config libpython-dev python-numpy 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
@@ -88,7 +97,7 @@ jobs:
         run: |
           brew tap ProfFan/robotics
           brew install cmake ninja
-          brew install ProfFan/robotics/boost
+          brew install boost
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             brew install gcc@${{ matrix.version }}
             echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV

.github/workflows/build-special.yml

@@ -12,6 +12,7 @@ jobs:
       CTEST_PARALLEL_LEVEL: 2
       CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
       GTSAM_BUILD_UNSTABLE: ON
+      BOOST_VERSION: 1.67.0
 
     strategy:
       fail-fast: false
@@ -56,23 +57,20 @@ jobs:
 
     steps:
       - name: Checkout
-        uses: actions/checkout@master
+        uses: actions/checkout@v2
 
       - name: Install (Linux)
         if: runner.os == 'Linux'
         run: |
           # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository.
           if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
-            gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421
+            gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
             gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add -
             sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
           fi
           sudo apt-get -y update
 
-          sudo apt install cmake build-essential pkg-config libpython-dev python-numpy
+          sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
-
-          echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV
-          echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV
 
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
@@ -84,6 +82,11 @@ jobs:
             echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
           fi
 
+      - name: Install Boost
+        if: runner.os == 'Linux'
+        run: |
+          bash .github/scripts/boost.sh
+
       - name: Install (macOS)
         if: runner.os == 'macOS'
         run: |

.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@master
-      - 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,24 +59,44 @@ 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: Build (Windows)
+
-        if: runner.os == 'Windows'
+      - name: Install Boost
+        shell: powershell
+        run: |
+          # Snippet from: https://github.com/actions/virtual-environments/issues/2667
+          $BOOST_PATH = "C:\hostedtoolcache\windows\Boost\$env:BOOST_VERSION\x86_64"
+
+          # Use the prebuilt binary for Windows
+          $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE-${{matrix.platform}}.exe"
+          (New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe")
+          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
-          echo "BOOST_ROOT_1_72_0: ${env:BOOST_ROOT_1_72_0}"
+          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"
-          cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT_1_72_0}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT_1_72_0}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT_1_72_0}\lib"
           cmake --build build --config ${{ matrix.build_type }} --target gtsam
           cmake --build build --config ${{ matrix.build_type }} --target gtsam_unstable
           cmake --build build --config ${{ matrix.build_type }} --target wrap
           cmake --build build --config ${{ matrix.build_type }} --target check.base
           cmake --build build --config ${{ matrix.build_type }} --target check.base_unstable
-          cmake --build build --config ${{ matrix.build_type }} --target check.linear
+          cmake --build build --config ${{ matrix.build_type }} --target check.linear

CMakeLists.txt

@@ -34,7 +34,7 @@ include(GtsamTesting)
 include(GtsamPrinting)
 
 # guard against in-source builds
-if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
+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()
 
@@ -63,12 +63,6 @@ include(cmake/HandleGlobalBuildFlags.cmake) # Build flags
 # Build CppUnitLite
 add_subdirectory(CppUnitLite)
 
-# This is the new wrapper
-if(GTSAM_BUILD_PYTHON)
-    list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")
-    add_subdirectory(python)
-endif()
-
 # Build GTSAM library
 add_subdirectory(gtsam)
 
@@ -86,8 +80,24 @@ if (GTSAM_BUILD_UNSTABLE)
     add_subdirectory(gtsam_unstable)
 endif()
 
+# This is the new wrapper
+if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
+    # Need to set this for the wrap package so we don't use the default value.
+    set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION}
+        CACHE STRING "The Python version to use for wrapping")
+    # Set the include directory for matlab.h
+    set(GTWRAP_INCLUDE_NAME "wrap")
+    add_subdirectory(wrap)
+    list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")
+endif()
+
+# Python toolbox
+if(GTSAM_BUILD_PYTHON)
+    add_subdirectory(python)
+endif()
+
 # Matlab toolbox
-if (GTSAM_INSTALL_MATLAB_TOOLBOX)
+if(GTSAM_INSTALL_MATLAB_TOOLBOX)
     add_subdirectory(matlab)
 endif()
 

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,17 @@ $ 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)
     - 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 
@@ -41,11 +42,6 @@ $ make install
     - MacOS 10.6 - 10.14
     - Windows 7, 8, 8.1, 10
 
-    Known issues:
-
-    - MSVC 2013 is not yet supported because it cannot build the serialization module
-      of Boost 1.55 (or earlier).
-
 2. GTSAM makes extensive use of debug assertions, and we highly recommend you work
 in Debug mode while developing (enabled by default). Likewise, it is imperative
 that you switch to release mode when running finished code and for timing. GTSAM
@@ -70,7 +66,41 @@ 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.
 
-## CMake Configuration Options and Details
+## Known Issues
+
+- When using `GTSAM_BUILD_WITH_MARCH_NATIVE=ON`, you may encounter issues in running tests which we are still investigating:
+  - Use of a version of GCC < 7.5 results in an "Indeterminant Linear System" error for `testSmartProjectionFactor`.
+  - Use of Boost version < 1.65 with clang will give a segfault for mulitple test cases.
+- MSVC 2013 is not yet supported because it cannot build the serialization module of Boost 1.55 (or earlier).
+
+# Windows Installation
+
+This section details how to build a GTSAM `.sln` file using Visual Studio.
+
+### Prerequisites
+
+- Visual Studio with C++ CMake tools for Windows
+- All the other pre-requisites listed above.
+
+### Steps
+
+1. Open Visual Studio.
+2. Select `Open a local folder` and select the GTSAM source directory.
+3. Go to `Project -> CMake Settings`.
+  - (Optional) Set `Configuration name`.
+  - (Optional) Set `Configuration type`.
+  - Set the `Toolset` to `msvc_x64_x64`. If you know what toolset you require, then skip this step.
+  - Update the `Build root` to `${projectDir}\build\${name}`.
+  - You can optionally create a new configuration for a `Release` build.
+  - Set the necessary CMake variables for your use case.
+  - Click on `Show advanced settings`.
+  - For `CMake generator`, select a version which matches `Visual Studio <Version> <Year> Win64`, e.g. `Visual Studio 16 2019 Win64`.
+  - Save the settings (Ctrl + S).
+4. Click on `Project -> Generate Cache`. This will generate the CMake build files (as seen in the Output window).
+5. The last step will generate a `GTSAM.sln` file in the `build` directory. At this point, GTSAM can be used as a regular Visual Studio project.
+
+
+# CMake Configuration Options and Details
 
 GTSAM has a number of options that can be configured, which is best done with
 one of the following:
@@ -78,7 +108,7 @@ one of the following:
   - ccmake      the curses GUI for cmake
   - cmake-gui   a real GUI for cmake
 
-### Important Options:
+## Important Options:
 
 #### CMAKE_BUILD_TYPE
 We support several build configurations for GTSAM (case insensitive)

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/CMakeLists.txt

@@ -16,11 +16,8 @@ install(FILES
   dllexport.h.in
   GtsamBuildTypes.cmake
   GtsamMakeConfigFile.cmake
-  GtsamMatlabWrap.cmake
   GtsamTesting.cmake
   GtsamPrinting.cmake
   FindNumPy.cmake
   README.html
   DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools")
-
-

cmake/Config.cmake.in

@@ -6,7 +6,7 @@
 get_filename_component(OUR_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
 if(EXISTS "${OUR_CMAKE_DIR}/CMakeCache.txt")
   # In build tree
-  set(@PACKAGE_NAME@_INCLUDE_DIR @CMAKE_SOURCE_DIR@ CACHE PATH "@PACKAGE_NAME@ include directory")
+  set(@PACKAGE_NAME@_INCLUDE_DIR @GTSAM_SOURCE_DIR@ CACHE PATH "@PACKAGE_NAME@ include directory")
 else()
   # Find installed library
   set(@PACKAGE_NAME@_INCLUDE_DIR "${OUR_CMAKE_DIR}/@CONF_REL_INCLUDE_DIR@" CACHE PATH "@PACKAGE_NAME@ include directory")
@@ -15,7 +15,7 @@ endif()
 # Find dependencies, required by cmake exported targets:
 include(CMakeFindDependencyMacro)
 # Allow using cmake < 3.8
-if(${CMAKE_VERSION} VERSION_LESS "3.8.0") 
+if(${CMAKE_VERSION} VERSION_LESS "3.8.0")
 find_package(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
 else()
 find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)

cmake/FindTBB.cmake

@@ -99,11 +99,8 @@ if(NOT TBB_FOUND)
   ##################################
 
   if(NOT DEFINED TBB_USE_DEBUG_BUILD)
-    if(CMAKE_BUILD_TYPE MATCHES "(Debug|DEBUG|debug|RelWithDebInfo|RELWITHDEBINFO|relwithdebinfo)")
+    # Set build type to RELEASE by default for optimization.
-      set(TBB_BUILD_TYPE DEBUG)
+    set(TBB_BUILD_TYPE RELEASE)
-    else()
-      set(TBB_BUILD_TYPE RELEASE)
-    endif()
   elseif(TBB_USE_DEBUG_BUILD)
     set(TBB_BUILD_TYPE DEBUG)
   else()

cmake/GtsamBuildTypes.cmake

@@ -59,10 +59,10 @@ endif()
 option(GTSAM_BUILD_TYPE_POSTFIXES        "Enable/Disable appending the build type to the name of compiled libraries" ON)
 
 # Define all cache variables, to be populated below depending on the OS/compiler:
-set(GTSAM_COMPILE_OPTIONS_PRIVATE        "" CACHE STRING "(Do not edit) Private compiler flags for all build configurations." FORCE)
+set(GTSAM_COMPILE_OPTIONS_PRIVATE        "" CACHE INTERNAL "(Do not edit) Private compiler flags for all build configurations." FORCE)
-set(GTSAM_COMPILE_OPTIONS_PUBLIC         "" CACHE STRING "(Do not edit) Public compiler flags (exported to user projects) for all build configurations."  FORCE)
+set(GTSAM_COMPILE_OPTIONS_PUBLIC         "" CACHE INTERNAL "(Do not edit) Public compiler flags (exported to user projects) for all build configurations."  FORCE)
-set(GTSAM_COMPILE_DEFINITIONS_PRIVATE    "" CACHE STRING "(Do not edit) Private preprocessor macros for all build configurations." FORCE)
+set(GTSAM_COMPILE_DEFINITIONS_PRIVATE    "" CACHE INTERNAL "(Do not edit) Private preprocessor macros for all build configurations." FORCE)
-set(GTSAM_COMPILE_DEFINITIONS_PUBLIC     "" CACHE STRING "(Do not edit) Public preprocessor macros for all build configurations." FORCE)
+set(GTSAM_COMPILE_DEFINITIONS_PUBLIC     "" CACHE INTERNAL "(Do not edit) Public preprocessor macros for all build configurations." FORCE)
 mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE)
 mark_as_advanced(GTSAM_COMPILE_OPTIONS_PUBLIC)
 mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE)
@@ -71,7 +71,7 @@ mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PUBLIC)
 foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES})
   string(TOUPPER "${build_type}" build_type_toupper)
 
-  # Define empty cache variables for "public". "private" are creaed below.
+  # Define empty cache variables for "public". "private" are created below.
   set(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper}      "" CACHE STRING "(User editable) Public compiler flags (exported to user projects) for `${build_type_toupper}` configuration.")
   set(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper}  "" CACHE STRING "(User editable) Public preprocessor macros for `${build_type_toupper}` configuration.")
 endforeach()
@@ -204,9 +204,9 @@ endif()
 
 # Make common binary output directory when on Windows
 if(WIN32)
-  set(RUNTIME_OUTPUT_PATH "${CMAKE_BINARY_DIR}/bin")
+  set(RUNTIME_OUTPUT_PATH "${GTSAM_BINARY_DIR}/bin")
-  set(EXECUTABLE_OUTPUT_PATH "${CMAKE_BINARY_DIR}/bin")
+  set(EXECUTABLE_OUTPUT_PATH "${GTSAM_BINARY_DIR}/bin")
-  set(LIBRARY_OUTPUT_PATH "${CMAKE_BINARY_DIR}/lib")
+  set(LIBRARY_OUTPUT_PATH "${GTSAM_BINARY_DIR}/lib")
 endif()
 
 # Set up build type list for cmake-gui

cmake/GtsamMatlabWrap.cmake

@@ -1,421 +0,0 @@
-# Set up cache options
-option(GTSAM_MEX_BUILD_STATIC_MODULE "Build MATLAB wrapper statically (increases build time)" OFF)
-set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation")
-set(GTSAM_TOOLBOX_INSTALL_PATH "" CACHE PATH "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox")
-if(NOT GTSAM_TOOLBOX_INSTALL_PATH)
-	set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox")
-endif()
-
-# GTSAM_MEX_BUILD_STATIC_MODULE is not for Windows - on Windows any static
-# are already compiled into the library by the linker
-if(GTSAM_MEX_BUILD_STATIC_MODULE AND WIN32)
-	message(FATAL_ERROR "GTSAM_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries.  To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).")
-endif()
-
-# Try to automatically configure mex path
-if(APPLE)
-	file(GLOB matlab_bin_directories "/Applications/MATLAB*/bin")
-	set(mex_program_name "mex")
-elseif(WIN32)
-	file(GLOB matlab_bin_directories "C:/Program Files*/MATLAB/*/bin")
-	set(mex_program_name "mex.bat")
-else()
-	file(GLOB matlab_bin_directories "/usr/local/MATLAB/*/bin")
-	set(mex_program_name "mex")
-endif()
-
-if(GTSAM_CUSTOM_MATLAB_PATH)
-	set(matlab_bin_directories ${GTSAM_CUSTOM_MATLAB_PATH})
-endif()
-
-# Run find_program explicitly putting $PATH after our predefined program
-# directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents
-# finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is
-# on the system path.
-list(REVERSE matlab_bin_directories) # Reverse list so the highest version (sorted alphabetically) is preferred
-find_program(MEX_COMMAND ${mex_program_name}
-	PATHS ${matlab_bin_directories} ENV PATH
-	NO_DEFAULT_PATH)
-mark_as_advanced(FORCE MEX_COMMAND)
-# Now that we have mex, trace back to find the Matlab installation root
-get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
-get_filename_component(mex_path "${MEX_COMMAND}" PATH)
-if(mex_path MATCHES ".*/win64$")
-	get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE)
-else()
-	get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
-endif()
-set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
-
-
-# User-friendly wrapping function.  Builds a mex module from the provided
-# interfaceHeader.  For example, for the interface header gtsam.h,
-# this will build the wrap module 'gtsam'.
-#
-# Arguments:
-#
-# interfaceHeader:  The relative path to the wrapper interface definition file.
-# linkLibraries:    Any *additional* libraries to link.  Your project library
-#                   (e.g. `lba`), libraries it depends on, and any necessary
-#                   MATLAB libraries will be linked automatically.  So normally,
-#                   leave this empty.
-# extraIncludeDirs: Any *additional* include paths required by dependent
-#                   libraries that have not already been added by
-#                   include_directories.  Again, normally, leave this empty.
-# extraMexFlags:    Any *additional* flags to pass to the compiler when building
-#                   the wrap code.  Normally, leave this empty.
-function(wrap_and_install_library interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)
-	wrap_library_internal("${interfaceHeader}" "${linkLibraries}" "${extraIncludeDirs}" "${mexFlags}")
-	install_wrapped_library_internal("${interfaceHeader}")
-endfunction()
-
-
-# Internal function that wraps a library and compiles the wrapper
-function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)
-	if(UNIX AND NOT APPLE)
-		if(CMAKE_SIZEOF_VOID_P EQUAL 8)
-			set(mexModuleExt mexa64)
-		else()
-			set(mexModuleExt mexglx)
-		endif()
-	elseif(APPLE)
-		set(mexModuleExt mexmaci64)
-	elseif(MSVC)
-		if(CMAKE_CL_64)
-			set(mexModuleExt mexw64)
-		else()
-			set(mexModuleExt mexw32)
-		endif()
-	endif()
-
-	# Wrap codegen interface
-    #usage: wrap interfacePath moduleName toolboxPath headerPath
-    #  interfacePath : *absolute* path to directory of module interface file
-    #  moduleName    : the name of the module, interface file must be called moduleName.h
-    #  toolboxPath   : the directory in which to generate the wrappers
-    #  headerPath    : path to matlab.h
-
-	# Extract module name from interface header file name
-	get_filename_component(interfaceHeader "${interfaceHeader}" ABSOLUTE)
-	get_filename_component(modulePath "${interfaceHeader}" PATH)
-	get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
-
-	# Paths for generated files
-	set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
-	set(generated_cpp_file "${generated_files_path}/${moduleName}_wrapper.cpp")
-	set(compiled_mex_modules_root "${PROJECT_BINARY_DIR}/wrap/${moduleName}_mex")
-
-	message(STATUS "Building wrap module ${moduleName}")
-
-	# Find matlab.h in GTSAM
-	if(("${PROJECT_NAME}" STREQUAL "gtsam") OR
-		("${PROJECT_NAME}" STREQUAL "gtsam_unstable"))
-		set(matlab_h_path "${PROJECT_SOURCE_DIR}")
-	else()
-		if(NOT GTSAM_INCLUDE_DIR)
-			message(FATAL_ERROR "You must call find_package(GTSAM) before using wrap")
-		endif()
-		list(GET GTSAM_INCLUDE_DIR 0 installed_includes_path)
-		set(matlab_h_path "${installed_includes_path}/wrap")
-	endif()
-
-	# If building a static mex module, add all cmake-linked libraries to the
-	# explicit link libraries list so that the next block of code can unpack
-	# any static libraries
-	set(automaticDependencies "")
-	foreach(lib ${moduleName} ${linkLibraries})
-	  #message("MODULE NAME: ${moduleName}")
-		if(TARGET "${lib}")
-            get_target_property(dependentLibraries ${lib} INTERFACE_LINK_LIBRARIES)
-           # message("DEPENDENT LIBRARIES:  ${dependentLibraries}")
-            if(dependentLibraries)
-               list(APPEND automaticDependencies ${dependentLibraries})
-            endif()
-        endif()
-    endforeach()
-    
-    ## CHRIS: Temporary fix. On my system the get_target_property above returned Not-found for gtsam module
-    ## This needs to be fixed!!
-    if(UNIX AND NOT APPLE)
-      list(APPEND automaticDependencies ${Boost_SERIALIZATION_LIBRARY_RELEASE} ${Boost_FILESYSTEM_LIBRARY_RELEASE}
-        ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE})
-      if(Boost_TIMER_LIBRARY_RELEASE AND NOT GTSAM_DISABLE_NEW_TIMERS) # Only present in Boost >= 1.48.0
-        list(APPEND automaticDependencies ${Boost_TIMER_LIBRARY_RELEASE} ${Boost_CHRONO_LIBRARY_RELEASE})
-        if(GTSAM_MEX_BUILD_STATIC_MODULE)
-          #list(APPEND automaticDependencies -Wl,--no-as-needed -lrt)
-        endif()
-      endif()
-    endif()
-    
-    #message("AUTOMATIC DEPENDENCIES:  ${automaticDependencies}")
-    ## CHRIS: End temporary fix
-
-	# Separate dependencies
-	set(correctedOtherLibraries "")
-	set(otherLibraryTargets "")
-    set(otherLibraryNontargets "")
-    set(otherSourcesAndObjects "")
-	foreach(lib ${moduleName} ${linkLibraries} ${automaticDependencies})
-		if(TARGET "${lib}")
-		    if(GTSAM_MEX_BUILD_STATIC_MODULE)
-		    	get_target_property(target_sources ${lib} SOURCES)
-		    	list(APPEND otherSourcesAndObjects ${target_sources})
-		    else()
-				list(APPEND correctedOtherLibraries ${lib})
-				list(APPEND otherLibraryTargets ${lib})
-		    endif()
-		else()
-			get_filename_component(file_extension "${lib}" EXT)
-			get_filename_component(lib_name "${lib}" NAME_WE)
-			if(file_extension STREQUAL ".a" AND GTSAM_MEX_BUILD_STATIC_MODULE)
-				# For building a static MEX module, unpack the static library
-				# and compile its object files into our module
-				file(MAKE_DIRECTORY "${generated_files_path}/${lib_name}_objects")
-				execute_process(COMMAND ar -x "${lib}"
-				                WORKING_DIRECTORY "${generated_files_path}/${lib_name}_objects"
-				                RESULT_VARIABLE ar_result)
-				if(NOT ar_result EQUAL 0)
-					message(FATAL_ERROR "Failed extracting ${lib}")
-				endif()
-
-				# Get list of object files
-				execute_process(COMMAND ar -t "${lib}"
-								OUTPUT_VARIABLE object_files
-				                RESULT_VARIABLE ar_result)
-				if(NOT ar_result EQUAL 0)
-					message(FATAL_ERROR "Failed listing ${lib}")
-				endif()
-
-				# Add directory to object files
-				string(REPLACE "\n" ";" object_files_list "${object_files}")
-				foreach(object_file ${object_files_list})
-					get_filename_component(file_extension "${object_file}" EXT)
-					if(file_extension STREQUAL ".o")
-						list(APPEND otherSourcesAndObjects "${generated_files_path}/${lib_name}_objects/${object_file}")
-					endif()
-				endforeach()
-			else()
-				list(APPEND correctedOtherLibraries ${lib})
-	            list(APPEND otherLibraryNontargets ${lib})
-            endif()
-		endif()
-	endforeach()
-    
-    # Check libraries for conflicting versions built-in to MATLAB
-    set(dependentLibraries "")
-    if(NOT "${otherLibraryTargets}" STREQUAL "")
-        foreach(target ${otherLibraryTargets})
-            get_target_property(dependentLibrariesOne ${target} INTERFACE_LINK_LIBRARIES)
-            list(APPEND dependentLibraries ${dependentLibrariesOne})
-        endforeach()
-    endif()
-    list(APPEND dependentLibraries ${otherLibraryNontargets})
-    check_conflicting_libraries_internal("${dependentLibraries}")
-
-	# Set up generation of module source file
-	file(MAKE_DIRECTORY "${generated_files_path}")
-
-    find_package(PythonInterp
-            ${GTSAM_PYTHON_VERSION}
-            EXACT
-            REQUIRED)
-    find_package(PythonLibs
-            ${GTSAM_PYTHON_VERSION}
-            EXACT
-            REQUIRED)
-
-
-	set(_ignore gtsam::Point2
-			gtsam::Point3)
-	add_custom_command(
-		OUTPUT ${generated_cpp_file}
-		DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects}
-        COMMAND
-			${PYTHON_EXECUTABLE}
-			${CMAKE_SOURCE_DIR}/wrap/matlab_wrapper.py
-            --src ${interfaceHeader}
-			--module_name ${moduleName}
-            --out ${generated_files_path}
-			--top_module_namespaces ${moduleName}
-			--ignore ${_ignore}
-		VERBATIM
-		WORKING_DIRECTORY ${generated_files_path})
-		
-	# Set up building of mex module
-	string(REPLACE ";" " " extraMexFlagsSpaced "${extraMexFlags}")
-	string(REPLACE ";" " " mexFlagsSpaced "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
-	add_library(${moduleName}_matlab_wrapper MODULE ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects})
-	target_link_libraries(${moduleName}_matlab_wrapper ${correctedOtherLibraries})
-	target_link_libraries(${moduleName}_matlab_wrapper ${moduleName})
-	set_target_properties(${moduleName}_matlab_wrapper PROPERTIES
-		OUTPUT_NAME              "${moduleName}_wrapper"
-		PREFIX                   ""
-		SUFFIX                   ".${mexModuleExt}"
-		LIBRARY_OUTPUT_DIRECTORY "${compiled_mex_modules_root}"
-		ARCHIVE_OUTPUT_DIRECTORY "${compiled_mex_modules_root}"
-		RUNTIME_OUTPUT_DIRECTORY "${compiled_mex_modules_root}"
-		CLEAN_DIRECT_OUTPUT 1)
-	set_property(TARGET ${moduleName}_matlab_wrapper APPEND_STRING PROPERTY COMPILE_FLAGS " ${extraMexFlagsSpaced} ${mexFlagsSpaced} \"-I${MATLAB_ROOT}/extern/include\" -DMATLAB_MEX_FILE -DMX_COMPAT_32")
-	set_property(TARGET ${moduleName}_matlab_wrapper APPEND PROPERTY INCLUDE_DIRECTORIES ${extraIncludeDirs})
-	# Disable build type postfixes for the mex module - we install in different directories for each build type instead
-	foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
-		string(TOUPPER "${build_type}" build_type_upper)
-		set_target_properties(${moduleName}_matlab_wrapper PROPERTIES ${build_type_upper}_POSTFIX "")
-	endforeach()
-	# Set up platform-specific flags
-	if(MSVC)
-		if(CMAKE_CL_64)
-			set(mxLibPath "${MATLAB_ROOT}/extern/lib/win64/microsoft")
-		else()
-			set(mxLibPath "${MATLAB_ROOT}/extern/lib/win32/microsoft")
-		endif()
-		target_link_libraries(${moduleName}_matlab_wrapper "${mxLibPath}/libmex.lib" "${mxLibPath}/libmx.lib" "${mxLibPath}/libmat.lib")
-		set_target_properties(${moduleName}_matlab_wrapper PROPERTIES LINK_FLAGS "/export:mexFunction")
-		set_property(SOURCE "${generated_cpp_file}" APPEND PROPERTY COMPILE_FLAGS "/bigobj")
-	elseif(APPLE)
-		set(mxLibPath "${MATLAB_ROOT}/bin/maci64")
-		target_link_libraries(${moduleName}_matlab_wrapper "${mxLibPath}/libmex.dylib" "${mxLibPath}/libmx.dylib" "${mxLibPath}/libmat.dylib")
-	endif()
-
-    # Hacking around output issue with custom command
-    # Deletes generated build folder 
-    add_custom_target(wrap_${moduleName}_matlab_distclean 
-	    COMMAND cmake -E remove_directory ${generated_files_path}
-		COMMAND cmake -E remove_directory ${compiled_mex_modules_root})
-endfunction()
-
-# Internal function that installs a wrap toolbox
-function(install_wrapped_library_internal interfaceHeader)
-	get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
-	set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
-
-    # NOTE: only installs .m and mex binary files (not .cpp) - the trailing slash on the directory name
-	# here prevents creating the top-level module name directory in the destination.
-	message(STATUS "Installing Matlab Toolbox to ${GTSAM_TOOLBOX_INSTALL_PATH}")
-	if(GTSAM_BUILD_TYPE_POSTFIXES)
-		foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
-			string(TOUPPER "${build_type}" build_type_upper)
-			if(${build_type_upper} STREQUAL "RELEASE")
-				set(build_type_tag "") # Don't create release mode tag on installed directory
-			else()
-				set(build_type_tag "${build_type}")
-			endif()
-			# Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one
-			get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH)
-			get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME)
-			install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING PATTERN "*.m")
-			install(TARGETS ${moduleName}_matlab_wrapper
-				LIBRARY DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}"
-				RUNTIME DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}")
-		endforeach()
-	else()
-		install(DIRECTORY "${generated_files_path}/" DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH} FILES_MATCHING PATTERN "*.m")
-		install(TARGETS ${moduleName}_matlab_wrapper
-			LIBRARY DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}
-			RUNTIME DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH})
-	endif()
-endfunction()
-
-# Internal function to check for libraries installed with MATLAB that may conflict
-# and prints a warning to move them if problems occur.
-function(check_conflicting_libraries_internal libraries)
-    if(UNIX)
-        # Set path for matlab's built-in libraries
-        if(APPLE)
-            set(mxLibPath "${MATLAB_ROOT}/bin/maci64")
-        else()
-            if(CMAKE_CL_64)
-                set(mxLibPath "${MATLAB_ROOT}/bin/glnxa64")
-            else()
-                set(mxLibPath "${MATLAB_ROOT}/bin/glnx86")
-            endif()
-        endif()
-        
-        # List matlab's built-in libraries
-        file(GLOB matlabLibs RELATIVE "${mxLibPath}" "${mxLibPath}/lib*")
-        
-        # Convert to base names
-        set(matlabLibNames "")
-        foreach(lib ${matlabLibs})
-            get_filename_component(libName "${lib}" NAME_WE)
-            list(APPEND matlabLibNames "${libName}")
-        endforeach()
-        
-        # Get names of link libraries
-        set(linkLibNames "")
-        foreach(lib ${libraries})
-            string(FIND "${lib}" "/" slashPos)
-            if(NOT slashPos EQUAL -1)
-                # If the name is a path, just get the library name
-                get_filename_component(libName "${lib}" NAME_WE)
-                list(APPEND linkLibNames "${libName}")
-            else()
-                # It's not a path, so see if it looks like a filename
-                get_filename_component(ext "${lib}" EXT)
-                if(NOT "${ext}" STREQUAL "")
-                    # It's a filename, so get the base name
-                    get_filename_component(libName "${lib}" NAME_WE)
-                    list(APPEND linkLibNames "${libName}")
-                else()
-                    # It's not a filename so it must be a short name, add the "lib" prefix
-                    list(APPEND linkLibNames "lib${lib}")
-                endif()
-            endif()
-        endforeach()
-        
-        # Remove duplicates
-        list(REMOVE_DUPLICATES linkLibNames)
-        
-        set(conflictingLibs "")
-        foreach(lib ${linkLibNames})
-            list(FIND matlabLibNames "${lib}" libPos)
-            if(NOT libPos EQUAL -1)
-                if(NOT conflictingLibs STREQUAL "")
-                    set(conflictingLibs "${conflictingLibs}, ")
-                endif()
-                set(conflictingLibs "${conflictingLibs}${lib}")
-            endif()
-        endforeach()
-        
-        if(NOT "${conflictingLibs}" STREQUAL "")
-            message(WARNING "GTSAM links to the libraries [ ${conflictingLibs} ] on your system, but "
-                "MATLAB is distributed with its own versions of these libraries which may conflict. "
-                "If you get strange errors or crashes with the GTSAM MATLAB wrapper, move these "
-                "libraries out of MATLAB's built-in library directory, which is ${mxLibPath} on "
-                "your system.  MATLAB will usually still work with these libraries moved away, but "
-                "if not, you'll have to compile the static GTSAM MATLAB wrapper module.")
-        endif()
-    endif()
-endfunction()
-
-# Helper function to install MATLAB scripts and handle multiple build types where the scripts
-# should be installed to all build type toolboxes
-function(install_matlab_scripts source_directory patterns)
-	set(patterns_args "")
-	set(exclude_patterns "")
-	if(NOT GTSAM_WRAP_SERIALIZATION)
-	    set(exclude_patterns "testSerialization.m")
-	endif()
-
-	foreach(pattern ${patterns})
-		list(APPEND patterns_args PATTERN "${pattern}")
-	endforeach()
-	if(GTSAM_BUILD_TYPE_POSTFIXES)
-		foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
-			string(TOUPPER "${build_type}" build_type_upper)
-			if(${build_type_upper} STREQUAL "RELEASE")
-				set(build_type_tag "") # Don't create release mode tag on installed directory
-			else()
-				set(build_type_tag "${build_type}")
-			endif()
-			# Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one
-			get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH)
-			get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME)
-			install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
-		endforeach()
-	else()
-		install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
-	endif()
-
-endfunction()
-

cmake/GtsamTesting.cmake

@@ -88,36 +88,36 @@ enable_testing()
 
 option(GTSAM_BUILD_TESTS                 "Enable/Disable building of tests"          ON)
 option(GTSAM_BUILD_EXAMPLES_ALWAYS       "Build examples with 'make all' (build with 'make examples' if not)"       ON)
-	option(GTSAM_BUILD_TIMING_ALWAYS         "Build timing scripts with 'make all' (build with 'make timing' if not"    OFF)
+option(GTSAM_BUILD_TIMING_ALWAYS         "Build timing scripts with 'make all' (build with 'make timing' if not"    OFF)
 
-		# Add option for combining unit tests
+# Add option for combining unit tests
-		if(MSVC OR XCODE_VERSION)
+if(MSVC OR XCODE_VERSION)
-			option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" ON)
+	option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" ON)
-		else()
+else()
-			option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" OFF)
+	option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" OFF)
-		endif()
+endif()
-		mark_as_advanced(GTSAM_SINGLE_TEST_EXE)
+mark_as_advanced(GTSAM_SINGLE_TEST_EXE)
 
-		# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
+# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
-		if(GTSAM_BUILD_TESTS)
+if(GTSAM_BUILD_TESTS)
-			add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
+	add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
-			# Also add alternative checks using valgrind.
+	# Also add alternative checks using valgrind.
-			# We don't look for valgrind being installed in the system, since these
+	# We don't look for valgrind being installed in the system, since these
-			# targets are not invoked unless directly instructed by the user.
+	# targets are not invoked unless directly instructed by the user.
-			if (UNIX)
+	if (UNIX)
-				# Run all tests using valgrind:
+		# Run all tests using valgrind:
-				add_custom_target(check_valgrind)
+		add_custom_target(check_valgrind)
-			endif()
+	endif()
 
-			# Add target to build tests without running
+	# Add target to build tests without running
-			add_custom_target(all.tests)
+	add_custom_target(all.tests)
-		endif()
+endif()
 
-		# Add examples target
+# Add examples target
-		add_custom_target(examples)
+add_custom_target(examples)
 
-		# Add timing target
+# Add timing target
-		add_custom_target(timing)
+add_custom_target(timing)
 
 
 # Implementations of this file's macros:

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/HandleEigen.cmake

@@ -42,7 +42,7 @@ else()
     set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/")
 
     # The actual include directory (for BUILD cmake target interface):
-    set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${CMAKE_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
+    set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${GTSAM_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
 endif()
 
 # Detect Eigen version:

cmake/HandleGeneralOptions.cmake

@@ -24,6 +24,7 @@ option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available"
 option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
 option(GTSAM_THROW_CHEIRALITY_EXCEPTION  "Throw exception when a triangulated point is behind a camera" ON)
 option(GTSAM_BUILD_PYTHON                "Enable/Disable building & installation of Python module with pybind11" OFF)
+option(GTSAM_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_SUPPORT_NESTED_DISSECTION   "Support Metis-based nested dissection" ON)
 option(GTSAM_TANGENT_PREINTEGRATION      "Use new ImuFactor with integration on tangent space" ON)
@@ -40,16 +41,5 @@ elseif(GTSAM_ROT3_EXPMAP)
     set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE)
 endif()
 
-# Options relating to MATLAB wrapper
+# Set the default Python version. This is later updated in HandlePython.cmake.
-# TODO: Check for matlab mex binary before handling building of binaries
-option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab toolbox"  OFF)
 set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.")
-
-# Check / set dependent variables for MATLAB wrapper
-if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES)
-    set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
-endif()
-
-if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
-    message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries.  If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
-endif()

cmake/HandlePython.cmake

@@ -1,22 +1,48 @@
 # Set Python version if either Python or MATLAB wrapper is requested.
 if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
-    if(${GTSAM_PYTHON_VERSION} STREQUAL "Default")
+  if(${GTSAM_PYTHON_VERSION} STREQUAL "Default")
-        # Get info about the Python3 interpreter
-        # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3
-        find_package(Python3 COMPONENTS Interpreter Development)
 
-        if(NOT ${Python3_FOUND})
+    if(${CMAKE_VERSION} VERSION_LESS "3.12.0")
-            message(FATAL_ERROR "Cannot find Python3 interpreter. Please install Python >= 3.6.")
+      # Use older version of cmake's find_python
-        endif()
+      find_package(PythonInterp)
+
+      if(NOT ${PYTHONINTERP_FOUND})
+        message(
+          FATAL_ERROR
+            "Cannot find Python interpreter. Please install Python >= 3.6.")
+      endif()
+
+      find_package(PythonLibs ${PYTHON_VERSION_STRING})
+
+      set(Python_VERSION_MAJOR ${PYTHON_VERSION_MAJOR})
+      set(Python_VERSION_MINOR ${PYTHON_VERSION_MINOR})
+      set(Python_EXECUTABLE ${PYTHON_EXECUTABLE})
+
+    else()
+      # Get info about the Python3 interpreter
+      # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3
+      find_package(Python3 COMPONENTS Interpreter Development)
+
+      if(NOT ${Python3_FOUND})
+        message(
+          FATAL_ERROR
+            "Cannot find Python3 interpreter. Please install Python >= 3.6.")
+      endif()
+
+      set(Python_VERSION_MAJOR ${Python3_VERSION_MAJOR})
+      set(Python_VERSION_MINOR ${Python3_VERSION_MINOR})
 
-        set(GTSAM_PYTHON_VERSION "${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}"
-                CACHE
-                STRING
-                "The version of Python to build the wrappers against."
-                FORCE)
     endif()
+
+    set(GTSAM_PYTHON_VERSION
+        "${Python_VERSION_MAJOR}.${Python_VERSION_MINOR}"
+        CACHE STRING "The version of Python to build the wrappers against."
+              FORCE)
+
+  endif()
 endif()
 
+# Check for build of Unstable modules
 if(GTSAM_BUILD_PYTHON)
     if(GTSAM_UNSTABLE_BUILD_PYTHON)
         if (NOT GTSAM_BUILD_UNSTABLE)

cmake/HandleUninstall.cmake

@@ -6,5 +6,11 @@ configure_file(
   "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake"
   IMMEDIATE @ONLY)
 
-add_custom_target(uninstall
+if (NOT TARGET uninstall) # avoid duplicating this target
-  "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
+  add_custom_target(uninstall
+    "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
+else()
+    add_custom_target(uninstall_gtsam
+      "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
+    add_dependencies(uninstall uninstall_gtsam)
+endif()

cmake/README.md

@@ -67,32 +67,6 @@ Defines two useful functions for creating CTest unit tests.  Also immediately cr
                        an empty string "" if nothing needs to be excluded.
         linkLibraries: The list of libraries to link to.
 
-## GtsamMatlabWrap
-
-    include(GtsamMatlabWrap)
-    
-Defines functions for generating MATLAB wrappers.  Also immediately creates several CMake options for configuring the wrapper.
-
-*   `wrap_and_install_library(interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)` Generates wrap code and compiles the wrapper.
-
-    Usage example:
-    
-        `wrap_and_install_library("lba.h" "" "" "")`
-    
-    Arguments:
-    
-        interfaceHeader:  The relative or absolute path to the wrapper interface
-                          definition file.
-        linkLibraries:    Any *additional* libraries to link.  Your project library
-                          (e.g. `lba`), libraries it depends on, and any necessary
-                          MATLAB libraries will be linked automatically.  So normally,
-                          leave this empty.
-        extraIncludeDirs: Any *additional* include paths required by dependent
-                          libraries that have not already been added by
-                          include_directories.  Again, normally, leave this empty.
-        extraMexFlags:    Any *additional* flags to pass to the compiler when building
-                          the wrap code.  Normally, leave this empty.
-
 ## GtsamMakeConfigFile
 
     include(GtsamMakeConfigFile)

cmake/example_project/CMakeLists.txt

@@ -1,49 +0,0 @@
-# This file should be used as a template for creating new projects using the CMake tools
-# This project has the following features
-#    - GTSAM linking
-#    - Unit tests via CppUnitLite
-#    - Scripts
-#    - Automatic MATLAB wrapper generation
-
-###################################################################################
-# To create your own project, replace "example" with the actual name of your project
-cmake_minimum_required(VERSION 3.0)
-project(example CXX C)
-
-# Include GTSAM CMake tools
-find_package(GTSAMCMakeTools)
-include(GtsamBuildTypes) # Load build type flags and default to Debug mode
-include(GtsamTesting)    # Easy functions for creating unit tests and scripts
-include(GtsamMatlabWrap) # Automatic MATLAB wrapper generation
-
-# Ensure that local folder is searched before library folders
-include_directories(BEFORE "${PROJECT_SOURCE_DIR}")
-
-###################################################################################
-# Find GTSAM components
-find_package(GTSAM REQUIRED) # Uses installed package
-# Note: Since Jan-2019, GTSAMConfig.cmake defines exported CMake targets
-# that automatically do include the include_directories() without the need
-# to call include_directories(), just target_link_libraries(NAME gtsam)
-#include_directories(${GTSAM_INCLUDE_DIR})
-
-###################################################################################
-# Build static library from common sources
-set(CONVENIENCE_LIB_NAME ${PROJECT_NAME})
-add_library(${CONVENIENCE_LIB_NAME} SHARED example/PrintExamples.h example/PrintExamples.cpp)
-target_link_libraries(${CONVENIENCE_LIB_NAME} gtsam)
-
-# Install library
-install(TARGETS ${CONVENIENCE_LIB_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
-
-###################################################################################
-# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library)
-gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}")
-
-###################################################################################
-# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library)
-gtsamAddExamplesGlob("*.cpp" "" "${CONVENIENCE_LIB_NAME}")
-
-###################################################################################
-# Build MATLAB wrapper (CMake tracks the dependecy to link with GTSAM through our project's static library)
-wrap_and_install_library("example.h" "${CONVENIENCE_LIB_NAME}" "" "")

cmake/example_project/README.md

@@ -1,32 +0,0 @@
-# MATLAB Wrapper Example Project
-
-This project serves as a lightweight example for demonstrating how to wrap C++ code in MATLAB using GTSAM.
-
-## Compiling
-
-We follow the regular build procedure inside the `example_project` directory:
-
-```sh
-mkdir build && cd build
-cmake ..
-make -j8
-sudo make install
-
-sudo ldconfig  # ensures the shared object file generated is correctly loaded
-```
-
-## Usage
-
-Now you can open MATLAB and add the `gtsam_toolbox` to the MATLAB path
-
-```matlab
-addpath('/usr/local/gtsam_toolbox')
-```
-
-At this point you are ready to run the example project. Starting from the `example_project` directory inside MATLAB, simply run code like regular MATLAB, e.g.
-
-```matlab
-pe = example.PrintExamples();
-pe.sayHello();
-pe.sayGoodbye();
-```

cmake/example_project/SayGoodbye.cpp

@@ -1,23 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file     SayGoodbye.cpp
- * @brief    Example script for example project
- * @author   Richard Roberts
- */
-
-#include <example/PrintExamples.h>
-
-int main(int argc, char *argv[]) {
-  example::PrintExamples().sayGoodbye();
-  return 0;
-}

cmake/example_project/SayHello.cpp

@@ -1,23 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file     SayHello.cpp
- * @brief    Example script for example project
- * @author   Richard Roberts
- */
-
-#include <example/PrintExamples.h>
-
-int main(int argc, char *argv[]) {
-  example::PrintExamples().sayHello();
-  return 0;
-}

cmake/example_project/example.h

@@ -1,31 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file     example.h
- * @brief    Example wrapper interface file
- * @author   Richard Roberts
- */
-
-// This is an interface file for automatic MATLAB wrapper generation.  See
-// gtsam.h for full documentation and more examples.
-
-#include <example/PrintExamples.h>
-
-namespace example {
-
-class PrintExamples {
-    PrintExamples();
-    void sayHello() const;
-    void sayGoodbye() const;
-};
-
-}

cmake/example_project/example/PrintExamples.cpp

@@ -1,44 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file     print_examples.cpp
- * @brief    Example library file
- * @author   Richard Roberts
- */
-
-#include <iostream>
-
-#include <example/PrintExamples.h>
-
-namespace example {
-
-void PrintExamples::sayHello() const {
-  std::cout << internal::getHelloString() << std::endl;
-}
-
-void PrintExamples::sayGoodbye() const {
-  std::cout << internal::getGoodbyeString() << std::endl;
-}
-
-namespace internal {
-
-std::string getHelloString() {
-  return "Hello!";
-}
-
-std::string getGoodbyeString() {
-  return "See you soon!";
-}
-
-}  // namespace internal
-
-}  // namespace example

cmake/example_project/example/PrintExamples.h

@@ -1,42 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file     print_examples.h
- * @brief    Example library file
- * @author   Richard Roberts
- */
-
-#pragma once
-
-#include <iostream>
-#include <string>
-
-namespace example {
-
-class PrintExamples {
-public:
-  /// Print a greeting
-  void sayHello() const;
-
-  /// Print a farewell
-  void sayGoodbye() const;
-};
-
-namespace internal {
-
-std::string getHelloString();
-
-std::string getGoodbyeString();
-
-}  // namespace internal
-
-}  // namespace example

cmake/example_project/tests/testExample.cpp

@@ -1,43 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file     testExample.cpp
- * @brief    Unit tests for example
- * @author   Richard Roberts
- */
-
-#include <CppUnitLite/TestHarness.h>
-
-#include <gtsam/base/TestableAssertions.h>
-
-#include <example/PrintExamples.h>
-
-using namespace gtsam;
-
-TEST(Example, HelloString) {
-  const std::string expectedString = "Hello!";
-  EXPECT(assert_equal(expectedString, example::internal::getHelloString()));
-}
-
-TEST(Example, GoodbyeString) {
-  const std::string expectedString = "See you soon!";
-  EXPECT(assert_equal(expectedString, example::internal::getGoodbyeString()));
-}
-
-/* ************************************************************************* */
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-/* ************************************************************************* */
-
-

doc/Code/LocalizationFactor.cpp

@@ -8,7 +8,10 @@ public:
   Vector evaluateError(const Pose2& q,
                        boost::optional<Matrix&> H = boost::none) const
   {
-    if (H) (*H) = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
+    const Rot2& R = q.rotation();
+    if (H) (*H) = (gtsam::Matrix(2, 3) <<
+            R.c(), -R.s(), 0.0,
+            R.s(), R.c(), 0.0).finished();
     return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
   }
 };

doc/LieGroups.lyx

@@ -2452,7 +2452,7 @@ First transform
 , and then rotate back:
 \begin_inset Formula 
 \[
-q=Re^{\Skew{\omega}}R^{T}
+q=Re^{\Skew{\omega}}R^{T}p
 \]
 
 \end_inset

doc/gtsam.lyx

@@ -1,7 +1,9 @@
-#LyX 2.1 created this file. For more info see http://www.lyx.org/
+#LyX 2.2 created this file. For more info see http://www.lyx.org/
-\lyxformat 474
+\lyxformat 508
 \begin_document
 \begin_header
+\save_transient_properties true
+\origin unavailable
 \textclass article
 \begin_preamble
 \usepackage{times}
@@ -50,16 +52,16 @@
 \language_package default
 \inputencoding auto
 \fontencoding T1
-\font_roman ae
+\font_roman "ae" "default"
-\font_sans default
+\font_sans "default" "default"
-\font_typewriter default
+\font_typewriter "default" "default"
-\font_math auto
+\font_math "auto" "auto"
 \font_default_family rmdefault
 \use_non_tex_fonts false
 \font_sc false
 \font_osf false
-\font_sf_scale 100
+\font_sf_scale 100 100
-\font_tt_scale 100
+\font_tt_scale 100 100
 \graphics default
 \default_output_format default
 \output_sync 0
@@ -1061,7 +1063,7 @@ noindent
 \begin_layout Subsection
 \begin_inset CommandInset label
 LatexCommand label
-name "sub:Full-Posterior-Inference"
+name "subsec:Full-Posterior-Inference"
 
 \end_inset
 
@@ -1272,7 +1274,7 @@ ing to odometry measurements.
  (see Section 
 \begin_inset CommandInset ref
 LatexCommand ref
-reference "sub:Full-Posterior-Inference"
+reference "subsec:Full-Posterior-Inference"
 
 \end_inset
 
@@ -1469,7 +1471,7 @@ GPS-like
 \begin_inset CommandInset include
 LatexCommand lstinputlisting
 filename "Code/LocalizationFactor.cpp"
-lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},caption={Excerpt from examples/LocalizationExample.cpp},captionpos=b,frame=single,identifierstyle={\\bfseries},label={listing:LocalizationFactor},language={C++},numbers=left"
+lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={C++},numbers=left,caption={Excerpt from examples/LocalizationExample.cpp},label={listing:LocalizationFactor}"
 
 \end_inset
 
@@ -1590,15 +1592,15 @@ q_{y}
 \begin_inset Formula $2\times3$
 \end_inset
 
- matrix:
+ matrix in tangent space which is the same the as the rotation matrix:
 \end_layout
 
 \begin_layout Standard
 \begin_inset Formula 
 \[
 H=\left[\begin{array}{ccc}
-1 & 0 & 0\\
+\cos(q_{\theta}) & -\sin(q_{\theta}) & 0\\
-0 & 1 & 0
+\sin(q_{\theta}) & \cos(q_{\theta}) & 0
 \end{array}\right]
 \]
 
@@ -1750,7 +1752,7 @@ global
  The marginals can be recovered exactly as in Section 
 \begin_inset CommandInset ref
 LatexCommand ref
-reference "sub:Full-Posterior-Inference"
+reference "subsec:Full-Posterior-Inference"
 
 \end_inset
 
@@ -1770,7 +1772,7 @@ filename "Code/LocalizationOutput5.txt"
 Comparing this with the covariance matrices in Section 
 \begin_inset CommandInset ref
 LatexCommand ref
-reference "sub:Full-Posterior-Inference"
+reference "subsec:Full-Posterior-Inference"
 
 \end_inset
 

examples/CombinedImuFactorsExample.cpp

@@ -23,7 +23,8 @@
  *  A row starting with "i" is the first initial position formatted with
  *  N, E, D, qx, qY, qZ, qW, velN, velE, velD
  *  A row starting with "0" is an imu measurement
- *  linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
+ *  (body frame - Forward, Right, Down)
+ *  linAccX, linAccY, linAccZ, angVelX, angVelY, angVelX
  *  A row starting with "1" is a gps correction formatted with
  *  N, E, D, qX, qY, qZ, qW
  * Note that for GPS correction, we're only using the position not the

examples/CreateSFMExampleData.cpp

@@ -15,8 +15,8 @@
  * @author  Frank Dellaert
  */
 
-#include <gtsam/slam/dataset.h>
 #include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/slam/dataset.h>
 
 #include <boost/assign/std/vector.hpp>
 
@@ -26,22 +26,16 @@ using namespace gtsam;
 
 /* ************************************************************************* */
 
-void createExampleBALFile(const string& filename, const vector<Point3>& P,
+void createExampleBALFile(const string& filename, const vector<Point3>& points,
-    const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K =
+                          const Pose3& pose1, const Pose3& pose2,
-        Cal3Bundler()) {
+                          const Cal3Bundler& K = Cal3Bundler()) {
-
   // Class that will gather all data
   SfmData data;
-
+  // Create two cameras and add them to data
-  // Create two cameras
-  Rot3 aRb = Rot3::Yaw(M_PI_2);
-  Point3 aTb(0.1, 0, 0);
-  Pose3 identity, aPb(aRb, aTb);
   data.cameras.push_back(SfmCamera(pose1, K));
   data.cameras.push_back(SfmCamera(pose2, K));
 
-  for(const Point3& p: P) {
+  for (const Point3& p : points) {
-
     // Create the track
     SfmTrack track;
     track.p = p;
@@ -51,7 +45,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
 
     // Project points in both cameras
     for (size_t i = 0; i < 2; i++)
-    track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));
+      track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));
 
     // Add track to data
     data.tracks.push_back(track);
@@ -63,49 +57,66 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
 /* ************************************************************************* */
 
 void create5PointExample1() {
-
   // Create two cameras poses
   Rot3 aRb = Rot3::Yaw(M_PI_2);
   Point3 aTb(0.1, 0, 0);
   Pose3 pose1, pose2(aRb, aTb);
 
   // Create test data, we need at least 5 points
-  vector<Point3> P;
+  vector<Point3> points = {
-  P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
+      {0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, {0, 0.5, 0.5}, {0, -0.5, 0.5}};
-  Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);
 
   // Assumes example is run in ${GTSAM_TOP}/build/examples
-  const string filename = "../../examples/data/5pointExample1.txt";
+  const string filename = "../../examples/Data/5pointExample1.txt";
-  createExampleBALFile(filename, P, pose1, pose2);
+  createExampleBALFile(filename, points, pose1, pose2);
 }
 
 /* ************************************************************************* */
 
 void create5PointExample2() {
-
   // Create two cameras poses
   Rot3 aRb = Rot3::Yaw(M_PI_2);
   Point3 aTb(10, 0, 0);
   Pose3 pose1, pose2(aRb, aTb);
 
   // Create test data, we need at least 5 points
-  vector<Point3> P;
+  vector<Point3> points = {{0, 0, 100},  {-10, 0, 100}, {10, 0, 100},  //
-  P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), //
+                           {0, 50, 50},  {0, -50, 50},  {-20, 0, 80},  //
-  Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80);
+                           {20, -50, 80}};
 
   // Assumes example is run in ${GTSAM_TOP}/build/examples
-  const string filename = "../../examples/data/5pointExample2.txt";
+  const string filename = "../../examples/Data/5pointExample2.txt";
   Cal3Bundler K(500, 0, 0);
-  createExampleBALFile(filename, P, pose1, pose2,K);
+  createExampleBALFile(filename, points, pose1, pose2, K);
 }
 
 /* ************************************************************************* */
 
+void create18PointExample1() {
+  // Create two cameras poses
+  Rot3 aRb = Rot3::Yaw(M_PI_2);
+  Point3 aTb(0.1, 0, 0);
+  Pose3 pose1, pose2(aRb, aTb);
+
+  // Create test data, we need 15 points
+  vector<Point3> points = {
+      {0, 0, 1},         {-0.1, 0, 1},      {0.1, 0, 1},       //
+      {0, 0.5, 0.5},     {0, -0.5, 0.5},    {-1, -0.5, 2},     //
+      {-1, 0.5, 2},      {0.25, -0.5, 1.5}, {0.25, 0.5, 1.5},  //
+      {-0.1, -0.5, 0.5}, {0.1, -0.5, 1},    {0.1, 0.5, 1},     //
+      {-0.1, 0, 0.5},    {-0.1, 0.5, 0.5},  {0, 0, 0.5},       //
+      {0.1, -0.5, 0.5},  {0.1, 0, 0.5},     {0.1, 0.5, 0.5}};
+
+  // Assumes example is run in ${GTSAM_TOP}/build/examples
+  const string filename = "../../examples/Data/18pointExample1.txt";
+  createExampleBALFile(filename, points, pose1, pose2);
+}
+
 int main(int argc, char* argv[]) {
   create5PointExample1();
   create5PointExample2();
+  create18PointExample1();
   return 0;
 }
 
 /* ************************************************************************* */
-

examples/Data/18pointExample1.txt

@@ -0,0 +1,131 @@
+2 18 36
+
+0 0 0 -0
+1 0 -6.123233995736766344e-18 -0.10000000000000000555
+0 1 -0.10000000000000000555 -0
+1 1 -1.2246467991473532688e-17 -0.2000000000000000111
+0 2 0.10000000000000000555 -0
+1 2 0 -0
+0 3 0 -1
+1 3 1 -0.20000000000000006661
+0 4 0 1
+1 4 -1 -0.19999999999999995559
+0 5 -0.5 0.25
+1 5 -0.25000000000000005551 -0.55000000000000004441
+0 6 -0.5 -0.25
+1 6 0.24999999999999997224 -0.55000000000000004441
+0 7 0.16666666666666665741 0.33333333333333331483
+1 7 -0.33333333333333331483 0.10000000000000000555
+0 8 0.16666666666666665741 -0.33333333333333331483
+1 8 0.33333333333333331483 0.099999999999999977796
+0 9 -0.2000000000000000111 1
+1 9 -1 -0.39999999999999996669
+0 10 0.10000000000000000555 0.5
+1 10 -0.5 3.0616169978683830179e-17
+0 11 0.10000000000000000555 -0.5
+1 11 0.5 -3.0616169978683830179e-17
+0 12 -0.2000000000000000111 -0
+1 12 -2.4492935982947065376e-17 -0.4000000000000000222
+0 13 -0.2000000000000000111 -1
+1 13 1 -0.40000000000000007772
+0 14 0 -0
+1 14 -1.2246467991473532688e-17 -0.2000000000000000111
+0 15 0.2000000000000000111 1
+1 15 -1 6.1232339957367660359e-17
+0 16 0.2000000000000000111 -0
+1 16 0 -0
+0 17 0.2000000000000000111 -1
+1 17 1 -6.1232339957367660359e-17
+
+3.141592653589793116
+                   0
+                   0
+-0
+0
+0
+1
+0
+0
+
+2.2214414690791830509
+2.2214414690791826068
+                    0
+-6.123233995736766344e-18
+-0.10000000000000000555
+0
+1
+0
+0
+
+0
+0
+1
+
+-0.10000000000000000555
+0
+1
+
+0.10000000000000000555
+0
+1
+
+0
+0.5
+0.5
+
+0
+-0.5
+0.5
+
+-1
+-0.5
+2
+
+-1
+0.5
+2
+
+0.25
+-0.5
+1.5
+
+0.25
+0.5
+1.5
+
+-0.10000000000000000555
+-0.5
+0.5
+
+0.10000000000000000555
+-0.5
+1
+
+0.10000000000000000555
+0.5
+1
+
+-0.10000000000000000555
+0
+0.5
+
+-0.10000000000000000555
+0.5
+0.5
+
+0
+0
+0.5
+
+0.10000000000000000555
+-0.5
+0.5
+
+0.10000000000000000555
+0
+0.5
+
+0.10000000000000000555
+0.5
+0.5
+

examples/ImuFactorsExample.cpp

@@ -25,7 +25,8 @@
  *  A row starting with "i" is the first initial position formatted with
  *  N, E, D, qx, qY, qZ, qW, velN, velE, velD
  *  A row starting with "0" is an imu measurement
- *  linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
+ *  (body frame - Forward, Right, Down)
+ *  linAccX, linAccY, linAccZ, angVelX, angVelY, angVelX
  *  A row starting with "1" is a gps correction formatted with
  *  N, E, D, qX, qY, qZ, qW
  * Note that for GPS correction, we're only using the position not the
@@ -125,7 +126,7 @@ int main(int argc, char* argv[]) {
   output_filename = var_map["output_filename"].as<string>();
   use_isam = var_map["use_isam"].as<bool>();
 
-  ISAM2* isam2;
+  ISAM2* isam2 = 0;
   if (use_isam) {
     printf("Using ISAM2\n");
     ISAM2Params parameters;

examples/LocalizationExample.cpp

@@ -78,21 +78,22 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
   UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
     NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
 
-  virtual ~UnaryFactor() {}
+  ~UnaryFactor() override {}
 
   // Using the NoiseModelFactor1 base class there are two functions that must be overridden.
   // The first is the 'evaluateError' function. This function implements the desired measurement
   // function, returning a vector of errors when evaluated at the provided variable value. It
   // must also calculate the Jacobians for this measurement function, if requested.
-  Vector evaluateError(const Pose2& q,
+  Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const override {
-                       boost::optional<Matrix&> H = boost::none) const override {
+    // The measurement function for a GPS-like measurement h(q) which predicts the measurement (m) is h(q) = q, q = [qx qy qtheta]
-    // The measurement function for a GPS-like measurement is simple:
+    // The error is then simply calculated as E(q) = h(q) - m:
-    // error_x = pose.x - measurement.x
+    // error_x = q.x - mx
-    // error_y = pose.y - measurement.y
+    // error_y = q.y - my
-    // Consequently, the Jacobians are:
+    // Node's orientation reflects in the Jacobian, in tangent space this is equal to the right-hand rule rotation matrix
-    // [ derror_x/dx  derror_x/dy  derror_x/dtheta ] = [1 0 0]
+    // H =  [ cos(q.theta)  -sin(q.theta) 0 ]
-    // [ derror_y/dx  derror_y/dy  derror_y/dtheta ] = [0 1 0]
+    //      [ sin(q.theta)   cos(q.theta) 0 ]
-    if (H) (*H) = (Matrix(2, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0).finished();
+    const Rot2& R = q.rotation();
+    if (H) (*H) = (gtsam::Matrix(2, 3) << R.c(), -R.s(), 0.0, R.s(), R.c(), 0.0).finished();
     return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
   }
 

examples/Pose3Localization.cpp

@@ -43,7 +43,7 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const Values::ConstKeyValuePair& key_value : *initial) {
+  for (const auto key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graph->addPrior(firstKey, Pose3(), priorModel);
@@ -74,7 +74,7 @@ int main(const int argc, const char* argv[]) {
 
   // Calculate and print marginal covariances for all variables
   Marginals marginals(*graph, result);
-  for (const auto& key_value : result) {
+  for (const auto key_value : result) {
     auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
     if (!p) continue;
     std::cout << marginals.marginalCovariance(key_value.key) << endl;

examples/Pose3SLAMExample_changeKeys.cpp

@@ -55,7 +55,7 @@ int main(const int argc, const char *argv[]) {
     std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
     // Additional: rewrite input with simplified keys 0,1,...
     Values simpleInitial;
-    for(const Values::ConstKeyValuePair& key_value: *initial) {
+    for(const auto key_value: *initial) {
       Key key;
       if(add)
         key = key_value.key + firstKey;

examples/Pose3SLAMExample_g2o.cpp

@@ -42,7 +42,7 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const Values::ConstKeyValuePair& key_value : *initial) {
+  for (const auto key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graph->addPrior(firstKey, Pose3(), priorModel);

examples/Pose3SLAMExample_initializePose3Chordal.cpp

@@ -42,7 +42,7 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const Values::ConstKeyValuePair& key_value : *initial) {
+  for (const auto key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graph->addPrior(firstKey, Pose3(), priorModel);

examples/Pose3SLAMExample_initializePose3Gradient.cpp

@@ -42,7 +42,7 @@ int main(const int argc, const char* argv[]) {
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const Values::ConstKeyValuePair& key_value : *initial) {
+  for (const auto key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graph->addPrior(firstKey, Pose3(), priorModel);

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

examples/ShonanAveragingCLI.cpp

@@ -25,6 +25,9 @@
  * Read 3D dataset sphere25000.txt and output to shonan.g2o (default)
  * ./ShonanAveragingCLI -i spere2500.txt
  *
+ * If you prefer using a robust Huber loss, you can add the option "-h true",
+ * for instance
+ * ./ShonanAveragingCLI -i spere2500.txt -h true
  */
 
 #include <gtsam/base/timing.h>
@@ -43,7 +46,8 @@ int main(int argc, char* argv[]) {
   string datasetName;
   string inputFile;
   string outputFile;
-  int d, seed;
+  int d, seed, pMin;
+  bool useHuberLoss;
   po::options_description desc(
       "Shonan Rotation Averaging CLI reads a *pose* graph, extracts the "
       "rotation constraints, and runs the Shonan algorithm.");
@@ -58,6 +62,10 @@ int main(int argc, char* argv[]) {
       "Write solution to the specified file")(
       "dimension,d", po::value<int>(&d)->default_value(3),
       "Optimize over 2D or 3D rotations")(
+      "useHuberLoss,h", po::value<bool>(&useHuberLoss)->default_value(false),
+      "set True to use Huber loss")("pMin,p",
+                                    po::value<int>(&pMin)->default_value(3),
+                                    "set to use desired rank pMin")(
       "seed,s", po::value<int>(&seed)->default_value(42),
       "Random seed for initial estimate");
   po::variables_map vm;
@@ -85,11 +93,14 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph::shared_ptr inputGraph;
   Values::shared_ptr posesInFile;
   Values poses;
+  auto lmParams = LevenbergMarquardtParams::CeresDefaults();
   if (d == 2) {
     cout << "Running Shonan averaging for SO(2) on " << inputFile << endl;
-    ShonanAveraging2 shonan(inputFile);
+    ShonanAveraging2::Parameters parameters(lmParams);
+    parameters.setUseHuber(useHuberLoss);
+    ShonanAveraging2 shonan(inputFile, parameters);
     auto initial = shonan.initializeRandomly(rng);
-    auto result = shonan.run(initial);
+    auto result = shonan.run(initial, pMin);
 
     // Parse file again to set up translation problem, adding a prior
     boost::tie(inputGraph, posesInFile) = load2D(inputFile);
@@ -101,9 +112,11 @@ int main(int argc, char* argv[]) {
     poses = initialize::computePoses<Pose2>(result.first, &poseGraph);
   } else if (d == 3) {
     cout << "Running Shonan averaging for SO(3) on " << inputFile << endl;
-    ShonanAveraging3 shonan(inputFile);
+    ShonanAveraging3::Parameters parameters(lmParams);
+    parameters.setUseHuber(useHuberLoss);
+    ShonanAveraging3 shonan(inputFile, parameters);
     auto initial = shonan.initializeRandomly(rng);
-    auto result = shonan.run(initial);
+    auto result = shonan.run(initial, pMin);
 
     // Parse file again to set up translation problem, adding a prior
     boost::tie(inputGraph, posesInFile) = load3D(inputFile);
@@ -118,7 +131,7 @@ int main(int argc, char* argv[]) {
     return 1;
   }
   cout << "Writing result to " << outputFile << endl;
-  writeG2o(NonlinearFactorGraph(), poses, outputFile);
+  writeG2o(*inputGraph, poses, outputFile);
   return 0;
 }
 

examples/SolverComparer.cpp

@@ -559,7 +559,7 @@ void runPerturb()
 
   // Perturb values
   VectorValues noise;
-  for(const Values::KeyValuePair& key_val: initial)
+  for(const Values::KeyValuePair key_val: initial)
   {
     Vector noisev(key_val.value.dim());
     for(Vector::Index i = 0; i < noisev.size(); ++i)

gtsam/3rdparty/metis/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8)
+cmake_minimum_required(VERSION 3.0)
 project(METIS)
 
 # Add flags for currect directory and below

gtsam/3rdparty/metis/libmetis/CMakeLists.txt

@@ -12,6 +12,7 @@ endif()
 if(WIN32)
 		set_target_properties(metis-gtsam PROPERTIES
 			PREFIX ""
+			COMPILE_FLAGS /w
 			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin")
 endif()
 

gtsam/CMakeLists.txt

@@ -135,12 +135,12 @@ endif()
 # of any previously installed GTSAM headers.
 target_include_directories(gtsam BEFORE PUBLIC
   # main gtsam includes:
-  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
+  $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}>
   $<INSTALL_INTERFACE:include/>
   # config.h
-  $<BUILD_INTERFACE:${CMAKE_BINARY_DIR}>
+  $<BUILD_INTERFACE:${GTSAM_BINARY_DIR}>
   # unit tests:
-  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/CppUnitLite>
+  $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/CppUnitLite>
 )
 # 3rdparty libraries: use the "system" flag so they are included via "-isystem"
 # and warnings (and warnings-considered-errors) in those headers are not
@@ -190,23 +190,8 @@ if(WIN32)
 else()
   if("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
     # Suppress all warnings from 3rd party sources.
-    set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w")
+    set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w -Wno-everything")
   else()
     set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-Wno-error")
   endif()
 endif()
-
-# Create the matlab toolbox for the gtsam library
-if (GTSAM_INSTALL_MATLAB_TOOLBOX)
-    # Set up codegen
-    include(GtsamMatlabWrap)
-
-    # Generate, build and install toolbox
-    set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
-        if(NOT BUILD_SHARED_LIBS)
-                list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
-        endif()
-
-    # Wrap
-    wrap_and_install_library(gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
-endif ()

gtsam/base/FastList.h

@@ -22,6 +22,8 @@
 #include <list>
 #include <boost/utility/enable_if.hpp>
 #include <boost/serialization/nvp.hpp>
+#include <boost/serialization/version.hpp>
+#include <boost/serialization/optional.hpp>
 #include <boost/serialization/list.hpp>
 
 namespace gtsam {

gtsam/base/GenericValue.h

@@ -74,7 +74,7 @@ public:
   }
 
   /// Destructor
-  virtual ~GenericValue() {
+  ~GenericValue() override {
   }
 
   /// equals implementing generic Value interface

gtsam/base/Matrix.cpp

@@ -153,7 +153,7 @@ const Eigen::IOFormat& matlabFormat() {
 /* ************************************************************************* */
 //3 argument call
 void print(const Matrix& A, const string &s, ostream& stream) {
-  cout << s << A.format(matlabFormat()) << endl;
+  stream << s << A.format(matlabFormat()) << endl;
 }
 
 /* ************************************************************************* */

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>
 
@@ -90,7 +90,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBas
 
   for(size_t i=0; i<m1; i++)
     for(size_t j=0; j<n1; j++) {
-      if(!fpEqual(A(i,j), B(i,j), tol)) {
+      if(!fpEqual(A(i,j), B(i,j), tol, false)) {
         return false;
       }
     }
@@ -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/ProductLieGroup.h

@@ -161,6 +161,9 @@ public:
     }
     return v;
   }
+  static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
+    return Logmap(p, Hp);
+  }
   ProductLieGroup expmap(const TangentVector& v) const {
     return compose(ProductLieGroup::Expmap(v));
   }

gtsam/base/Testable.h

@@ -33,9 +33,10 @@
 
 #pragma once
 
-#include <boost/shared_ptr.hpp>
 #include <boost/concept_check.hpp>
-#include <stdio.h>
+#include <functional>
+#include <iostream>
+#include <memory>
 #include <string>
 
 #define GTSAM_PRINT(x)((x).print(#x))
@@ -72,10 +73,10 @@ namespace gtsam {
   }; // \ Testable
 
   inline void print(float v, const std::string& s = "") {
-    printf("%s%f\n",s.c_str(),v);
+    std::cout << (s.empty() ? s : s + " ") << v << std::endl;
   }
   inline void print(double v, const std::string& s = "") {
-    printf("%s%lf\n",s.c_str(),v);
+    std::cout << (s.empty() ? s : s + " ") << v << std::endl;
   }
 
   /** Call equal on the object */
@@ -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/TestableAssertions.h

@@ -372,16 +372,19 @@ bool assert_stdout_equal(const std::string& expected, const V& actual) {
 
 /**
  * Capture print function output and compare against string.
+ *
+ * @param s: Optional string to pass to the print() method.
  */
-template<class V>
+template <class V>
-bool assert_print_equal(const std::string& expected, const V& actual) {
+bool assert_print_equal(const std::string& expected, const V& actual,
+                        const std::string& s = "") {
   // Redirect output to buffer so we can compare
   std::stringstream buffer;
   // Save the original output stream so we can reset later
   std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
 
   // We test against actual std::cout for faithful reproduction
-  actual.print();
+  actual.print(s);
 
   // Get output string and reset stdout
   std::string actual_ = buffer.str();

gtsam/base/ThreadsafeException.h

@@ -72,7 +72,7 @@ protected:
   }
 
   /// Default destructor doesn't have the noexcept
-  virtual ~ThreadsafeException() noexcept {
+  ~ThreadsafeException() noexcept override {
   }
 
 public:
@@ -114,7 +114,7 @@ class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
 {
 public:
   CholeskyFailed() noexcept {}
-  virtual ~CholeskyFailed() noexcept {}
+  ~CholeskyFailed() noexcept override {}
 };
 
 } // namespace gtsam

gtsam/base/Vector.cpp

@@ -39,7 +39,7 @@ namespace gtsam {
  * 1. https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
  * 2. https://floating-point-gui.de/errors/comparison/
  * ************************************************************************* */
-bool fpEqual(double a, double b, double tol) {
+bool fpEqual(double a, double b, double tol, bool check_relative_also) {
   using std::abs;
   using std::isnan;
   using std::isinf;
@@ -48,7 +48,7 @@ bool fpEqual(double a, double b, double tol) {
   double larger = (abs(b) > abs(a)) ? abs(b) : abs(a);
 
   // handle NaNs
-  if(std::isnan(a) || isnan(b)) {
+  if(isnan(a) || isnan(b)) {
     return isnan(a) && isnan(b);
   }
   // handle inf
@@ -60,13 +60,15 @@ bool fpEqual(double a, double b, double tol) {
   else if(a == 0 || b == 0 || (abs(a) + abs(b)) < DOUBLE_MIN_NORMAL) {
     return abs(a-b) <= tol * DOUBLE_MIN_NORMAL;
   }
-  // Check if the numbers are really close
+  // Check if the numbers are really close.
-  // Needed when comparing numbers near zero or tol is in vicinity
+  // Needed when comparing numbers near zero or tol is in vicinity.
-  else if(abs(a-b) <= tol) {
+  else if (abs(a - b) <= tol) {
     return true;
   }
-  // Use relative error
+  // Check for relative error
-  else if(abs(a-b) <= tol * min(larger, std::numeric_limits<double>::max())) {
+  else if (abs(a - b) <=
+               tol * min(larger, std::numeric_limits<double>::max()) &&
+           check_relative_also) {
     return true;
   }
 

gtsam/base/Vector.h

@@ -85,9 +85,15 @@ static_assert(
  * respectively for the comparison to be true.
  * If one is NaN/Inf and the other is not, returns false.
  *
+ * @param check_relative_also is a flag which toggles additional checking for
+ * relative error. This means that if either the absolute error or the relative
+ * error is within the tolerance, the result will be true.
+ * By default, the flag is true.
+ *
  * Return true if two numbers are close wrt tol.
  */
-GTSAM_EXPORT bool fpEqual(double a, double b, double tol);
+GTSAM_EXPORT bool fpEqual(double a, double b, double tol,
+                          bool check_relative_also = true);
 
 /**
  * print without optional string, must specify cout yourself

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,15 +18,7 @@
 // \callgraph
 #pragma once
 
-#include <boost/function.hpp>
+#include <functional>
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-variable"
-#endif
-#include <boost/bind.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
 
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/JacobianFactor.h>
@@ -45,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, _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), _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
@@ -76,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(
@@ -116,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),
@@ -157,7 +149,8 @@ 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, _1), x, delta);
+  return numericalDerivative11<Y, X>(std::bind(h, std::placeholders::_1), x,
+                                     delta);
 }
 
 /**
@@ -170,20 +163,23 @@ 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, _1, boost::cref(x2)), x1, delta);
+  return numericalDerivative11<Y, X1, N>(
+      std::bind(h, std::placeholders::_1, std::cref(x2)), x1, delta);
 }
 
 /** use a raw C++ function pointer */
 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, _1, _2), x1, x2, delta);
+  return numericalDerivative21<Y, X1, X2>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
+      delta);
 }
 
 /**
@@ -196,20 +192,23 @@ 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), _1), x2, delta);
+  return numericalDerivative11<Y, X2, N>(
+      std::bind(h, std::cref(x1), std::placeholders::_1), x2, delta);
 }
 
 /** use a raw C++ function pointer */
 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, _1, _2), x1, x2, delta);
+  return numericalDerivative22<Y, X1, X2>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
+      delta);
 }
 
 /**
@@ -225,20 +224,24 @@ 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, _1, boost::cref(x2), boost::cref(x3)), x1, delta);
+  return numericalDerivative11<Y, X1, N>(
+      std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3)),
+      x1, delta);
 }
 
 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, _1, _2, _3), x1,
+  return numericalDerivative31<Y, X1, X2, X3>(
-      x2, x3, delta);
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3),
+      x1, x2, x3, delta);
 }
 
 /**
@@ -254,20 +257,24 @@ 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), _1, boost::cref(x3)), x2, delta);
+  return numericalDerivative11<Y, X2, N>(
+      std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3)),
+      x2, delta);
 }
 
 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, _1, _2, _3), x1,
+  return numericalDerivative32<Y, X1, X2, X3>(
-      x2, x3, delta);
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3),
+      x1, x2, x3, delta);
 }
 
 /**
@@ -283,20 +290,24 @@ 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), _1), x3, delta);
+  return numericalDerivative11<Y, X3, N>(
+      std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1),
+      x3, delta);
 }
 
 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, _1, _2, _3), x1,
+  return numericalDerivative33<Y, X1, X2, X3>(
-      x2, x3, delta);
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3),
+      x1, x2, x3, delta);
 }
 
 /**
@@ -312,19 +323,25 @@ 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, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta);
+  return numericalDerivative11<Y, X1, N>(
+      std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
+                  std::cref(x4)),
+      x1, delta);
 }
 
 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, _1, _2, _3, _4), x1, x2, x3, x4);
+  return numericalDerivative41<Y, X1, X2, X3, X4>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4),
+      x1, x2, x3, x4);
 }
 
 /**
@@ -340,19 +357,25 @@ 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), _1, boost::cref(x3), boost::cref(x4)), x2, delta);
+  return numericalDerivative11<Y, X2, N>(
+      std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
+                  std::cref(x4)),
+      x2, delta);
 }
 
 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, _1, _2, _3, _4), x1, x2, x3, x4);
+  return numericalDerivative42<Y, X1, X2, X3, X4>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4),
+      x1, x2, x3, x4);
 }
 
 /**
@@ -368,19 +391,25 @@ 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), _1, boost::cref(x4)), x3, delta);
+  return numericalDerivative11<Y, X3, N>(
+      std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
+                  std::cref(x4)),
+      x3, delta);
 }
 
 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, _1, _2, _3, _4), x1, x2, x3, x4);
+  return numericalDerivative43<Y, X1, X2, X3, X4>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4),
+      x1, x2, x3, x4);
 }
 
 /**
@@ -396,19 +425,25 @@ 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), _1), x4, delta);
+  return numericalDerivative11<Y, X4, N>(
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
+                  std::placeholders::_1),
+      x4, delta);
 }
 
 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, _1, _2, _3, _4), x1, x2, x3, x4);
+  return numericalDerivative44<Y, X1, X2, X3, X4>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4),
+      x1, x2, x3, x4);
 }
 
 /**
@@ -425,19 +460,26 @@ 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, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta);
+  return numericalDerivative11<Y, X1, N>(
+      std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
+                  std::cref(x4), std::cref(x5)),
+      x1, delta);
 }
 
 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, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
+  return numericalDerivative51<Y, X1, X2, X3, X4, X5>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5),
+      x1, x2, x3, x4, x5);
 }
 
 /**
@@ -454,19 +496,26 @@ 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), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta);
+  return numericalDerivative11<Y, X2, N>(
+      std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
+                  std::cref(x4), std::cref(x5)),
+      x2, delta);
 }
 
 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, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
+  return numericalDerivative52<Y, X1, X2, X3, X4, X5>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5),
+      x1, x2, x3, x4, x5);
 }
 
 /**
@@ -483,19 +532,26 @@ 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), _1, boost::cref(x4), boost::cref(x5)), x3, delta);
+  return numericalDerivative11<Y, X3, N>(
+      std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
+                  std::cref(x4), std::cref(x5)),
+      x3, delta);
 }
 
 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, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
+  return numericalDerivative53<Y, X1, X2, X3, X4, X5>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5),
+      x1, x2, x3, x4, x5);
 }
 
 /**
@@ -512,19 +568,26 @@ 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), _1, boost::cref(x5)), x4, delta);
+  return numericalDerivative11<Y, X4, N>(
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
+                  std::placeholders::_1, std::cref(x5)),
+      x4, delta);
 }
 
 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, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
+  return numericalDerivative54<Y, X1, X2, X3, X4, X5>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5),
+      x1, x2, x3, x4, x5);
 }
 
 /**
@@ -541,19 +604,26 @@ 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), boost::cref(x4), _1), x5, delta);
+  return numericalDerivative11<Y, X5, N>(
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
+                  std::cref(x4), std::placeholders::_1),
+      x5, delta);
 }
 
 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, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
+  return numericalDerivative55<Y, X1, X2, X3, X4, X5>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5),
+      x1, x2, x3, x4, x5);
 }
 
 /**
@@ -571,19 +641,26 @@ 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, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta);
+  return numericalDerivative11<Y, X1, N>(
+      std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
+                  std::cref(x4), std::cref(x5), std::cref(x6)),
+      x1, delta);
 }
 
 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, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
+  return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5, std::placeholders::_6),
+      x1, x2, x3, x4, x5, x6);
 }
 
 /**
@@ -601,19 +678,26 @@ 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), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta);
+  return numericalDerivative11<Y, X2, N>(
+      std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
+                  std::cref(x4), std::cref(x5), std::cref(x6)),
+      x2, delta);
 }
 
 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, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
+  return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5, std::placeholders::_6),
+      x1, x2, x3, x4, x5, x6);
 }
 
 /**
@@ -631,19 +715,26 @@ 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), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta);
+  return numericalDerivative11<Y, X3, N>(
+      std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
+                  std::cref(x4), std::cref(x5), std::cref(x6)),
+      x3, delta);
 }
 
 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, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
+  return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5, std::placeholders::_6),
+      x1, x2, x3, x4, x5, x6);
 }
 
 /**
@@ -661,19 +752,26 @@ 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), _1, boost::cref(x5), boost::cref(x6)), x4, delta);
+  return numericalDerivative11<Y, X4, N>(
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
+                  std::placeholders::_1, std::cref(x5), std::cref(x6)),
+      x4, delta);
 }
 
 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, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
+  return numericalDerivative64<Y, X1, X2, X3, X4, X5>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5, std::placeholders::_6),
+      x1, x2, x3, x4, x5, x6);
 }
 
 /**
@@ -691,19 +789,26 @@ 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), boost::cref(x4), _1, boost::cref(x6)), x5, delta);
+  return numericalDerivative11<Y, X5, N>(
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
+                  std::cref(x4), std::placeholders::_1, std::cref(x6)),
+      x5, delta);
 }
 
 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, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
+  return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5, std::placeholders::_6),
+      x1, x2, x3, x4, x5, x6);
 }
 
 /**
@@ -721,20 +826,27 @@ 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),
       "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, X6, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta);
+  return numericalDerivative11<Y, X6, N>(
+      std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
+                  std::cref(x4), std::cref(x5), std::placeholders::_1),
+      x6, delta);
 }
 
 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, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
+  return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(
+      std::bind(h, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, std::placeholders::_4,
+                  std::placeholders::_5, std::placeholders::_6),
+      x1, x2, x3, x4, x5, x6);
 }
 
 /**
@@ -746,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, _1, delta), x,
+  return numericalDerivative11<VectorD, 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
@@ -769,80 +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_, _1, boost::cref(x2)), x1_, delta_);
+    return numericalGradient<X1>(
+        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), _1)), x2, delta);
+          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(boost::bind(f, _1, boost::cref(x2)));
+  std::function<double(const X1&)> f2(
+      std::bind(f, std::placeholders::_1, std::cref(x2)));
 
   return numericalDerivative11<Vector, X1>(
-      boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
+      std::function<Vector(const X1&)>(
+          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(boost::bind(f, boost::cref(x1), _1));
+  std::function<double(const X2&)> f2(
+      std::bind(f, std::cref(x1), std::placeholders::_1));
 
   return numericalDerivative11<Vector, X2>(
-      boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
+      std::function<Vector(const X2&)>(
+          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);
 }
 
@@ -852,15 +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(f, _1, boost::cref(x2), boost::cref(x3)));
+  std::function<double(const X1&)> f2(std::bind(
+      f, std::placeholders::_1, std::cref(x2), std::cref(x3)));
 
   return numericalDerivative11<Vector, X1>(
-      boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
+      std::function<Vector(const X1&)>(
+          std::bind(numGrad, f2, std::placeholders::_1, delta)),
       x1, delta);
 }
 
@@ -868,22 +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(f, boost::cref(x1), _1, boost::cref(x3)));
+  std::function<double(const X2&)> f2(std::bind(
+      f, std::cref(x1), std::placeholders::_1, std::cref(x3)));
 
   return numericalDerivative11<Vector, X2>(
-      boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
+      std::function<Vector(const X2&)>(
+          std::bind(numGrad, f2, std::placeholders::_1, delta)),
       x2, delta);
 }
 
@@ -891,22 +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(f, boost::cref(x1), boost::cref(x2), _1));
+  std::function<double(const X3&)> f2(std::bind(
+      f, std::cref(x1), std::cref(x2), std::placeholders::_1));
 
   return numericalDerivative11<Vector, X3>(
-      boost::function<Vector(const X3&)>(boost::bind(numGrad, f2, _1, delta)),
+      std::function<Vector(const X3&)>(
+          std::bind(numGrad, f2, std::placeholders::_1, delta)),
       x3, delta);
 }
 
@@ -914,35 +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&)>(boost::bind(f, _1, _2, boost::cref(x3))),
+      std::function<double(const X1&, const X2&)>(
+          std::bind(f, std::placeholders::_1, std::placeholders::_2,
+                      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&)>(boost::bind(f, _1, boost::cref(x2), _2)),
+      std::function<double(const X1&, const X3&)>(
+          std::bind(f, std::placeholders::_1, std::cref(x2),
+                      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&)>(boost::bind(f, boost::cref(x1), _1, _2)),
+      std::function<double(const X2&, const X3&)>(
+          std::bind(f, std::cref(x1), std::placeholders::_1,
+                      std::placeholders::_2)),
       x2, x3, delta);
 }
 
@@ -951,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);
 }
 
@@ -959,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);
 }
 
@@ -967,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/testMatrix.cpp

@@ -1163,6 +1163,19 @@ TEST(Matrix , IsVectorSpace) {
   BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>));
 }
 
+TEST(Matrix, AbsoluteError) {
+  double a = 2000, b = 1997, tol = 1e-1;
+  bool isEqual;
+
+  // Test only absolute error
+  isEqual = fpEqual(a, b, tol, false);
+  EXPECT(!isEqual);
+
+  // Test relative error as well
+  isEqual = fpEqual(a, b, tol);
+  EXPECT(isEqual);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/base/tests/testNumericalDerivative.cpp

@@ -30,10 +30,10 @@ double f(const Vector2& x) {
 /* ************************************************************************* */
 //
 TEST(testNumericalDerivative, numericalGradient) {
-  Vector2 x(1, 1);
+  Vector2 x(1, 1.1);
 
   Vector expected(2);
-  expected << cos(x(1)), -sin(x(0));
+  expected << cos(x(0)), -sin(x(1));
 
   Vector actual = numericalGradient<Vector2>(f, x);
 
@@ -42,7 +42,7 @@ TEST(testNumericalDerivative, numericalGradient) {
 
 /* ************************************************************************* */
 TEST(testNumericalDerivative, numericalHessian) {
-  Vector2 x(1, 1);
+  Vector2 x(1, 1.1);
 
   Matrix expected(2, 2);
   expected << -sin(x(0)), 0.0, 0.0, -cos(x(1));

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/discrete/DecisionTree-inl.h

@@ -155,7 +155,7 @@ namespace gtsam {
 
   public:
 
-    virtual ~Choice() {
+    ~Choice() override {
 #ifdef DT_DEBUG_MEMORY
         std::std::cout << Node::nrNodes << " destructing (Choice) " << this->id() << std::std::endl;
 #endif
@@ -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/DiscreteBayesNet.h

@@ -55,6 +55,9 @@ namespace gtsam {
     template<class DERIVEDCONDITIONAL>
     DiscreteBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {}
 
+    /// Destructor
+    virtual ~DiscreteBayesNet() {}
+
     /// @}
 
     /// @name Testable

gtsam/discrete/DiscreteFactor.h

@@ -74,13 +74,14 @@ public:
   /// @name Testable
   /// @{
 
-  // equals
+  /// equals
   virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0;
 
-  // print
+  /// print
-  virtual void print(const std::string& s = "DiscreteFactor\n",
+  void print(
-      const KeyFormatter& formatter = DefaultKeyFormatter) const {
+      const std::string& s = "DiscreteFactor\n",
-    Factor::print(s, formatter);
+      const KeyFormatter& formatter = DefaultKeyFormatter) const override {
+    Base::print(s, formatter);
   }
 
   /** Test whether the factor is empty */

gtsam/discrete/DiscreteFactorGraph.h

@@ -91,6 +91,9 @@ public:
   template<class DERIVEDFACTOR>
   DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
 
+  /// Destructor
+  virtual ~DiscreteFactorGraph() {}
+
   /// @name Testable
   /// @{
 
@@ -129,8 +132,9 @@ public:
   double operator()(const DiscreteFactor::Values & values) const;
 
   /// print
-  void print(const std::string& s = "DiscreteFactorGraph",
+  void print(
-      const KeyFormatter& formatter =DefaultKeyFormatter) const;
+      const std::string& s = "DiscreteFactorGraph",
+      const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
   /** Solve the factor graph by performing variable elimination in COLAMD order using
    *  the dense elimination function specified in \c function,

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/Cal3.cpp

@@ -0,0 +1,75 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   Cal3.cpp
+ * @brief  Common code for all calibration models.
+ * @author Frank Dellaert
+ */
+
+#include <gtsam/geometry/Cal3.h>
+
+#include <cmath>
+#include <fstream>
+#include <iostream>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+Cal3::Cal3(double fov, int w, int h)
+    : s_(0), u0_((double)w / 2.0), v0_((double)h / 2.0) {
+  double a = fov * M_PI / 360.0;  // fov/2 in radians
+  fx_ = double(w) / (2.0 * tan(a));
+  fy_ = fx_;
+}
+
+/* ************************************************************************* */
+Cal3::Cal3(const std::string& path) {
+  const auto buffer = path + std::string("/calibration_info.txt");
+  std::ifstream infile(buffer, std::ios::in);
+
+  if (infile && !infile.eof()) {
+    infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
+  } else {
+    throw std::runtime_error("Cal3: Unable to load the calibration");
+  }
+
+  infile.close();
+}
+
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3& cal) {
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py();
+  return os;
+}
+
+/* ************************************************************************* */
+void Cal3::print(const std::string& s) const { gtsam::print((Matrix)K(), s); }
+
+/* ************************************************************************* */
+bool Cal3::equals(const Cal3& K, double tol) const {
+  return (std::fabs(fx_ - K.fx_) < tol && std::fabs(fy_ - K.fy_) < tol &&
+          std::fabs(s_ - K.s_) < tol && std::fabs(u0_ - K.u0_) < tol &&
+          std::fabs(v0_ - K.v0_) < tol);
+}
+
+Matrix3 Cal3::inverse() const {
+  const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
+  Matrix3 K_inverse;
+  K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, 1.0 / fy_,
+      -v0_ / fy_, 0.0, 0.0, 1.0;
+  return K_inverse;
+}
+
+/* ************************************************************************* */
+
+}  // \ namespace gtsam

gtsam/geometry/Cal3.h

@@ -0,0 +1,206 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   Cal3.h
+ * @brief  Common code for all Calibration models.
+ * @author Varun Agrawal
+ */
+
+/**
+ * @addtogroup geometry
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Point2.h>
+
+namespace gtsam {
+
+/**
+ * Function which makes use of the Implicit Function Theorem to compute the
+ * Jacobians of `calibrate` using `uncalibrate`.
+ * This is useful when there are iterative operations in the `calibrate`
+ * function which make computing jacobians difficult.
+ *
+ * Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can
+ * easily compute the Jacobians:
+ * df/pi = -I (pn and pi are independent args)
+ * Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
+ * Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
+ *
+ * @tparam Cal Calibration model.
+ * @tparam Dim The number of parameters in the calibration model.
+ * @param p Calibrated point.
+ * @param Dcal optional 2*p Jacobian wrpt `p` Cal3DS2 parameters.
+ * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates.
+ */
+template <typename Cal, size_t Dim>
+void calibrateJacobians(const Cal& calibration, const Point2& pn,
+                        OptionalJacobian<2, Dim> Dcal = boost::none,
+                        OptionalJacobian<2, 2> Dp = boost::none) {
+  if (Dcal || Dp) {
+    Eigen::Matrix<double, 2, Dim> H_uncal_K;
+    Matrix22 H_uncal_pn, H_uncal_pn_inv;
+
+    // Compute uncalibrate Jacobians
+    calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
+
+    H_uncal_pn_inv = H_uncal_pn.inverse();
+
+    if (Dp) *Dp = H_uncal_pn_inv;
+    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
+  }
+}
+
+/**
+ * @brief Common base class for all calibration models.
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT Cal3 {
+ protected:
+  double fx_ = 1.0f, fy_ = 1.0f;  ///< focal length
+  double s_ = 0.0f;               ///< skew
+  double u0_ = 0.0f, v0_ = 0.0f;  ///< principal point
+
+ public:
+  enum { dimension = 5 };
+  ///< shared pointer to calibration object
+  using shared_ptr = boost::shared_ptr<Cal3>;
+
+  /// @name Standard Constructors
+  /// @{
+
+  /// Create a default calibration that leaves coordinates unchanged
+  Cal3() = default;
+
+  /// constructor from doubles
+  Cal3(double fx, double fy, double s, double u0, double v0)
+      : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {}
+
+  /// constructor from vector
+  Cal3(const Vector5& d)
+      : fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {}
+
+  /**
+   * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
+   * @param fov field of view in degrees
+   * @param w image width
+   * @param h image height
+   */
+  Cal3(double fov, int w, int h);
+
+  /// Virtual destructor
+  virtual ~Cal3() {}
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+
+  /**
+   * Load calibration parameters from `calibration_info.txt` file located in
+   * `path` directory.
+   *
+   * The contents of calibration file should be the 5 parameters in order:
+   * `fx, fy, s, u0, v0`
+   *
+   * @param path path to directory containing `calibration_info.txt`.
+   */
+  Cal3(const std::string& path);
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3& cal);
+
+  /// print with optional string
+  virtual void print(const std::string& s = "") const;
+
+  /// Check if equal up to specified tolerance
+  bool equals(const Cal3& K, double tol = 10e-9) const;
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /// focal length x
+  inline double fx() const { return fx_; }
+
+  /// focal length y
+  inline double fy() const { return fy_; }
+
+  /// aspect ratio
+  inline double aspectRatio() const { return fx_ / fy_; }
+
+  /// skew
+  inline double skew() const { return s_; }
+
+  /// image center in x
+  inline double px() const { return u0_; }
+
+  /// image center in y
+  inline double py() const { return v0_; }
+
+  /// return the principal point
+  Point2 principalPoint() const { return Point2(u0_, v0_); }
+
+  /// vectorized form (column-wise)
+  Vector5 vector() const {
+    Vector5 v;
+    v << fx_, fy_, s_, u0_, v0_;
+    return v;
+  }
+
+  /// return calibration matrix K
+  virtual Matrix3 K() const {
+    Matrix3 K;
+    K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
+    return K;
+  }
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
+  /** @deprecated The following function has been deprecated, use K above */
+  Matrix3 matrix() const { return K(); }
+#endif
+
+  /// Return inverted calibration matrix inv(K)
+  Matrix3 inverse() const;
+
+  /// return DOF, dimensionality of tangent space
+  inline virtual size_t dim() const { return Dim(); }
+
+  /// return DOF, dimensionality of tangent space
+  inline static size_t Dim() { return dimension; }
+
+  /// @}
+  /// @name Advanced Interface
+  /// @{
+
+ private:
+  /// Serialization function
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_NVP(fx_);
+    ar& BOOST_SERIALIZATION_NVP(fy_);
+    ar& BOOST_SERIALIZATION_NVP(s_);
+    ar& BOOST_SERIALIZATION_NVP(u0_);
+    ar& BOOST_SERIALIZATION_NVP(v0_);
+  }
+
+  /// @}
+};
+
+}  // \ namespace gtsam

gtsam/geometry/Cal3Bundler.cpp

@@ -15,28 +15,19 @@
  * @author ydjian
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3Bundler.h>
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3Bundler::Cal3Bundler() :
-    f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) {
-}
-
-/* ************************************************************************* */
-Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0,
-                         double tol)
-    : f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {}
-
 /* ************************************************************************* */
 Matrix3 Cal3Bundler::K() const {
+  // This function is needed to ensure skew = 0;
   Matrix3 K;
-  K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1;
+  K << fx_, 0, u0_, 0, fy_, v0_, 0, 0, 1.0;
   return K;
 }
 
@@ -48,35 +39,42 @@ Vector4 Cal3Bundler::k() const {
 }
 
 /* ************************************************************************* */
-Vector3 Cal3Bundler::vector() const {
+Vector3 Cal3Bundler::vector() const { return Vector3(fx_, k1_, k2_); }
-  return Vector3(f_, k1_, k2_);
+
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
+  os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()
+     << ", px: " << cal.px() << ", py: " << cal.py();
+  return os;
 }
 
 /* ************************************************************************* */
 void Cal3Bundler::print(const std::string& s) const {
-  gtsam::print((Vector)(Vector(5) << f_, k1_, k2_, u0_, v0_).finished(), s + ".K");
+  gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(),
+               s + ".K");
 }
 
 /* ************************************************************************* */
 bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
-  if (std::abs(f_ - K.f_) > tol || std::abs(k1_ - K.k1_) > tol
+  const Cal3* base = dynamic_cast<const Cal3*>(&K);
-      || std::abs(k2_ - K.k2_) > tol || std::abs(u0_ - K.u0_) > tol
+  return (Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
-      || std::abs(v0_ - K.v0_) > tol)
+          std::fabs(k2_ - K.k2_) < tol && std::fabs(u0_ - K.u0_) < tol &&
-    return false;
+          std::fabs(v0_ - K.v0_) < tol);
-  return true;
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::uncalibrate(const Point2& p, //
+Point2 Cal3Bundler::uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal,
-    OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const {
+                                OptionalJacobian<2, 2> Dp) const {
-  //  r = x^2 + y^2;
+  //  r = x² + y²;
-  //  g = (1 + k(1)*r + k(2)*r^2);
+  //  g = (1 + k(1)*r + k(2)*r²);
   //  pi(:,i) = g * pn(:,i)
   const double x = p.x(), y = p.y();
   const double r = x * x + y * y;
   const double g = 1. + (k1_ + k2_ * r) * r;
   const double u = g * x, v = g * y;
 
+  const double f_ = fx_;
+
   // Derivatives make use of intermediate variables above
   if (Dcal) {
     double rx = r * x, ry = r * y;
@@ -94,51 +92,38 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::calibrate(const Point2& pi,
+Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
-                              OptionalJacobian<2, 3> Dcal,
                               OptionalJacobian<2, 2> Dp) const {
-  // Copied from Cal3DS2 :-(
+  // Copied from Cal3DS2
-  // but specialized with k1,k2 non-zero only and fx=fy and s=0
+  // but specialized with k1, k2 non-zero only and fx=fy and s=0
-  double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_;
+  double px = (pi.x() - u0_) / fx_, py = (pi.y() - v0_) / fx_;
-  const Point2 invKPi(x, y);
+  const Point2 invKPi(px, py);
-
+  Point2 pn;
-  // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
-  Point2 pn(x, y);
 
   // iterate until the uncalibrate is close to the actual pixel coordinate
   const int maxIterations = 10;
-  int iteration;
+  int iteration = 0;
-  for (iteration = 0; iteration < maxIterations; ++iteration) {
+  do {
-    if (distance2(uncalibrate(pn), pi) <= tol_)
+    // initialize pn with distortion included
-      break;
+    const double rr = (px * px) + (py * py);
-    const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
-    const double rr = xx + yy;
     const double g = (1 + k1_ * rr + k2_ * rr * rr);
     pn = invKPi / g;
-  }
+
+    if (distance2(uncalibrate(pn), pi) <= tol_) break;
+
+    // Set px and py using intrinsic coordinates since that is where radial
+    // distortion correction is done.
+    px = pn.x(), py = pn.y();
+    iteration++;
+
+  } while (iteration < maxIterations);
 
   if (iteration >= maxIterations)
     throw std::runtime_error(
-        "Cal3Bundler::calibrate fails to converge. need a better initialization");
+        "Cal3Bundler::calibrate fails to converge. need a better "
+        "initialization");
 
-  // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
+  calibrateJacobians<Cal3Bundler, dimension>(*this, pn, Dcal, Dp);
-  // Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians
-  // df/pi = -I (pn and pi are independent args)
-  // Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
-  // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
-  Matrix23 H_uncal_K;
-  Matrix22 H_uncal_pn, H_uncal_pn_inv;
-
-  if (Dcal || Dp) {
-    // Compute uncalibrate Jacobians
-    uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
-
-    H_uncal_pn_inv = H_uncal_pn.inverse();
-
-    if (Dp) *Dp = H_uncal_pn_inv;
-    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
-
-  }
 
   return pn;
 }
@@ -167,14 +152,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
   return H;
 }
 
-/* ************************************************************************* */
+}  // namespace gtsam
-Cal3Bundler Cal3Bundler::retract(const Vector& d) const {
-  return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
-}
-
-/* ************************************************************************* */
-Vector3 Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const {
-  return T2.vector() - vector();
-}
-
-}

gtsam/geometry/Cal3Bundler.h

@@ -14,10 +14,12 @@
  * @brief Calibration used by Bundler
  * @date Sep 25, 2010
  * @author Yong Dian Jian
+ * @author Varun Agrawal
  */
 
 #pragma once
 
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -27,23 +29,23 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Cal3Bundler {
+class GTSAM_EXPORT Cal3Bundler : public Cal3 {
+ private:
+  double k1_ = 0.0f, k2_ = 0.0f;  ///< radial distortion
+  double tol_ = 1e-5;             ///< tolerance value when calibrating
 
-private:
+  // NOTE: We use the base class fx to represent the common focal length.
-  double f_; ///< focal length
+  // Also, image center parameters (u0, v0) are not optimized
-  double k1_, k2_; ///< radial distortion
+  // but are treated as constants.
-  double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
-  double tol_; ///< tolerance value when calibrating
-
-public:
 
+ public:
   enum { dimension = 3 };
 
   /// @name Standard Constructors
   /// @{
 
   /// Default constructor
-  Cal3Bundler();
+  Cal3Bundler() = default;
 
   /**
    *  Constructor
@@ -55,16 +57,21 @@ public:
    *  @param tol optional calibration tolerance value
    */
   Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
-              double tol = 1e-5);
+              double tol = 1e-5)
+      : Cal3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
 
-  virtual ~Cal3Bundler() {}
+  ~Cal3Bundler() override {}
 
   /// @}
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3Bundler& cal);
+
   /// print with optional string
-  void print(const std::string& s = "") const;
+  void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
@@ -73,64 +80,41 @@ public:
   /// @name Standard Interface
   /// @{
 
-  Matrix3 K() const; ///< Standard 3*3 calibration matrix
+  /// distorsion parameter k1
-  Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
+  inline double k1() const { return k1_; }
+
+  /// distorsion parameter k2
+  inline double k2() const { return k2_; }
+
+  /// image center in x
+  inline double px() const { return u0_; }
+
+  /// image center in y
+  inline double py() const { return v0_; }
+
+  Matrix3 K() const override;  ///< Standard 3*3 calibration matrix
+  Vector4 k() const;  ///< Radial distortion parameters (4 of them, 2 0)
 
   Vector3 vector() const;
 
-  /// focal length x
-  inline double fx() const {
-    return f_;
-  }
-
-  /// focal length y
-  inline double fy() const {
-    return f_;
-  }
-
-  /// distorsion parameter k1
-  inline double k1() const {
-    return k1_;
-  }
-
-  /// distorsion parameter k2
-  inline double k2() const {
-    return k2_;
-  }
-
-  /// image center in x
-  inline double px() const {
-    return u0_;
-  }
-
-  /// image center in y
-  inline double py() const {
-    return v0_;
-  }
-
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
   /// get parameter u0
-  inline double u0() const {
+  inline double u0() const { return u0_; }
-    return u0_;
-  }
 
   /// get parameter v0
-  inline double v0() const {
+  inline double v0() const { return v0_; }
-    return v0_;
-  }
 #endif
 
-
   /**
    * @brief: convert intrinsic coordinates xy to image coordinates uv
    * Version of uncalibrate with derivatives
    * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
+   * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in image coordinates
    */
   Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
-      OptionalJacobian<2, 2> Dp = boost::none) const;
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /**
    * Convert a pixel coordinate to ideal coordinate xy
@@ -140,8 +124,7 @@ public:
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in intrinsic coordinates
    */
-  Point2 calibrate(const Point2& pi,
+  Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none,
-                   OptionalJacobian<2, 3> Dcal = boost::none,
                    OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
@@ -157,48 +140,45 @@ public:
   /// @name Manifold
   /// @{
 
+  /// return DOF, dimensionality of tangent space
+  size_t dim() const override { return Dim(); }
+
+  /// return DOF, dimensionality of tangent space
+  inline static size_t Dim() { return dimension; }
+
   /// Update calibration with tangent space delta
-  Cal3Bundler retract(const Vector& d) const;
+  inline Cal3Bundler retract(const Vector& d) const {
+    return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
+  }
 
   /// Calculate local coordinates to another calibration
-  Vector3 localCoordinates(const Cal3Bundler& T2) const;
+  Vector3 localCoordinates(const Cal3Bundler& T2) const {
-
+    return T2.vector() - vector();
-  /// dimensionality
-  virtual size_t dim() const {
-    return 3;
   }
 
-  /// dimensionality
+ private:
-  static size_t Dim() {
-    return 3;
-  }
-
-private:
-
   /// @}
   /// @name Advanced Interface
   /// @{
 
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class Archive>
+  template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/) {
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_NVP(f_);
+    ar& boost::serialization::make_nvp(
-    ar & BOOST_SERIALIZATION_NVP(k1_);
+        "Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
-    ar & BOOST_SERIALIZATION_NVP(k2_);
+    ar& BOOST_SERIALIZATION_NVP(k1_);
-    ar & BOOST_SERIALIZATION_NVP(u0_);
+    ar& BOOST_SERIALIZATION_NVP(k2_);
-    ar & BOOST_SERIALIZATION_NVP(v0_);
+    ar& BOOST_SERIALIZATION_NVP(tol_);
-    ar & BOOST_SERIALIZATION_NVP(tol_);
   }
 
   /// @}
-
 };
 
-template<>
+template <>
 struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
 
-template<>
+template <>
 struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
 
-} // namespace gtsam
+}  // namespace gtsam

gtsam/geometry/Cal3DS2.cpp

@@ -13,28 +13,30 @@
  * @file Cal3DS2.cpp
  * @date Feb 28, 2010
  * @author ydjian
+ * @author Varun Agrawal
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3DS2.h>
 
 namespace gtsam {
 
 /* ************************************************************************* */
-void Cal3DS2::print(const std::string& s_) const {
+std::ostream& operator<<(std::ostream& os, const Cal3DS2& cal) {
-  Base::print(s_);
+  os << (Cal3DS2_Base&)cal;
+  return os;
 }
 
+/* ************************************************************************* */
+void Cal3DS2::print(const std::string& s_) const { Base::print(s_); }
+
 /* ************************************************************************* */
 bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
+  const Cal3DS2_Base* base = dynamic_cast<const Cal3DS2_Base*>(&K);
-      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
+  return Cal3DS2_Base::equals(*base, tol);
-      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol)
-    return false;
-  return true;
 }
 
 /* ************************************************************************* */
@@ -46,8 +48,5 @@ Cal3DS2 Cal3DS2::retract(const Vector& d) const {
 Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const {
   return T2.vector() - vector();
 }
-
 }
 /* ************************************************************************* */
-
-

gtsam/geometry/Cal3DS2.h

@@ -11,9 +11,11 @@
 
 /**
  * @file Cal3DS2.h
- * @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base
+ * @brief Calibration of a camera with radial distortion, calculations in base
+ * class Cal3DS2_Base
  * @date Feb 28, 2010
  * @author ydjian
+ * @autho Varun Agrawal
  */
 
 #pragma once
@@ -30,35 +32,37 @@ namespace gtsam {
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
+  using Base = Cal3DS2_Base;
 
-  typedef Cal3DS2_Base Base;
+ public:
-
-public:
-
   enum { dimension = 9 };
 
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3DS2() : Base() {}
+  Cal3DS2() = default;
 
-  Cal3DS2(double fx, double fy, double s, double u0, double v0,
+  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
+          double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
-        Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {}
+      : Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
 
-  virtual ~Cal3DS2() {}
+  ~Cal3DS2() override {}
 
   /// @}
   /// @name Advanced Constructors
   /// @{
 
-  Cal3DS2(const Vector &v) : Base(v) {}
+  Cal3DS2(const Vector9& v) : Base(v) {}
 
   /// @}
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3DS2& cal);
+
   /// print with optional string
   void print(const std::string& s = "") const override;
 
@@ -70,16 +74,16 @@ public:
   /// @{
 
   /// Given delta vector, update calibration
-  Cal3DS2 retract(const Vector& d) const ;
+  Cal3DS2 retract(const Vector& d) const;
 
   /// Given a different calibration, calculate update to obtain it
-  Vector localCoordinates(const Cal3DS2& T2) const ;
+  Vector localCoordinates(const Cal3DS2& T2) const;
 
   /// Return dimensions of calibration manifold object
-  virtual size_t dim() const { return dimension ; }
+  size_t dim() const override { return Dim(); }
 
   /// Return dimensions of calibration manifold object
-  static size_t Dim() { return dimension; }
+  inline static size_t Dim() { return dimension; }
 
   /// @}
   /// @name Clone
@@ -92,30 +96,24 @@ public:
 
   /// @}
 
-
+ private:
-private:
-
   /// @name Advanced Interface
   /// @{
 
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class Archive>
+  template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/)
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-  {
+    ar& boost::serialization::make_nvp(
-    ar & boost::serialization::make_nvp("Cal3DS2",
+        "Cal3DS2", boost::serialization::base_object<Cal3DS2_Base>(*this));
-        boost::serialization::base_object<Cal3DS2_Base>(*this));
   }
 
   /// @}
-
 };
 
-template<>
+template <>
 struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
 
-template<>
+template <>
 struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
-
 }
-

gtsam/geometry/Cal3DS2_Base.cpp

@@ -13,27 +13,17 @@
  * @file Cal3DS2_Base.cpp
  * @date Feb 28, 2010
  * @author ydjian
+ * @author Varun Agrawal
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3DS2_Base.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3DS2_Base.h>
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
-    fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
-
-/* ************************************************************************* */
-Matrix3 Cal3DS2_Base::K() const {
-    Matrix3 K;
-    K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
-    return K;
-}
-
 /* ************************************************************************* */
 Vector9 Cal3DS2_Base::vector() const {
   Vector9 v;
@@ -41,6 +31,14 @@ Vector9 Cal3DS2_Base::vector() const {
   return v;
 }
 
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3DS2_Base& cal) {
+  os << (Cal3&)cal;
+  os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", p1: " << cal.p1()
+     << ", p2: " << cal.p2();
+  return os;
+}
+
 /* ************************************************************************* */
 void Cal3DS2_Base::print(const std::string& s_) const {
   gtsam::print((Matrix)K(), s_ + ".K");
@@ -49,31 +47,30 @@ void Cal3DS2_Base::print(const std::string& s_) const {
 
 /* ************************************************************************* */
 bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
+  const Cal3* base = dynamic_cast<const Cal3*>(&K);
-      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
+  return Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
-      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol)
+         std::fabs(k2_ - K.k2_) < tol && std::fabs(p1_ - K.p1_) < tol &&
-    return false;
+         std::fabs(p2_ - K.p2_) < tol;
-  return true;
 }
 
 /* ************************************************************************* */
-static Matrix29 D2dcalibration(double x, double y, double xx,
+static Matrix29 D2dcalibration(double x, double y, double xx, double yy,
-    double yy, double xy, double rr, double r4, double pnx, double pny,
+                               double xy, double rr, double r4, double pnx,
-    const Matrix2& DK) {
+                               double pny, const Matrix2& DK) {
   Matrix25 DR1;
   DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
   Matrix24 DR2;
-  DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
+  DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx,  //
-         y * rr, y * r4, rr + 2 * yy, 2 * xy;
+      y * rr, y * r4, rr + 2 * yy, 2 * xy;
   Matrix29 D;
   D << DR1, DK * DR2;
   return D;
 }
 
 /* ************************************************************************* */
-static Matrix2 D2dintrinsic(double x, double y, double rr,
+static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1,
-    double g, double k1, double k2, double p1, double p2,
+                            double k2, double p1, double p2,
-    const Matrix2& DK) {
+                            const Matrix2& DK) {
   const double drdx = 2. * x;
   const double drdy = 2. * y;
   const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
@@ -87,24 +84,23 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
   const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
 
   Matrix2 DR;
-  DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
+  DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy,  //
-        y * dgdx + dDydx, g + y * dgdy + dDydy;
+      y * dgdx + dDydx, g + y * dgdy + dDydy;
 
   return DK * DR;
 }
 
 /* ************************************************************************* */
-Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
+Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal,
-    OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const {
+                                 OptionalJacobian<2, 2> Dp) const {
-
+  //  r² = x² + y²;
-  //  rr = x^2 + y^2;
+  //  g = (1 + k(1)*r² + k(2)*r⁴);
-  //  g = (1 + k(1)*rr + k(2)*rr^2);
+  //  dp = [2*k(3)*x*y + k(4)*(r² + 2*x²); 2*k(4)*x*y + k(3)*(r² + 2*y²)];
-  //  dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
   //  pi(:,i) = g * pn(:,i) + dp;
   const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
   const double rr = xx + yy;
   const double r4 = rr * rr;
-  const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
+  const double g = 1. + k1_ * rr + k2_ * r4;  // scaling factor
 
   // tangential component
   const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
@@ -115,37 +111,44 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
   const double pny = g * y + dy;
 
   Matrix2 DK;
-  if (H1 || H2) DK << fx_, s_, 0.0, fy_;
+  if (Dcal || Dp) {
+    DK << fx_, s_, 0.0, fy_;
+  }
 
   // Derivative for calibration
-  if (H1)
+  if (Dcal) {
-    *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
+    *Dcal = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
+  }
 
   // Derivative for points
-  if (H2)
+  if (Dp) {
-    *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
+    *Dp = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
+  }
 
   // Regular uncalibrate after distortion
   return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
 }
 
 /* ************************************************************************* */
-Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
+Point2 Cal3DS2_Base::calibrate(const Point2& pi, OptionalJacobian<2, 9> Dcal,
+                               OptionalJacobian<2, 2> Dp) const {
   // Use the following fixed point iteration to invert the radial distortion.
   // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
 
-  const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
+  const Point2 invKPi((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
-                       (1 / fy_) * (pi.y() - v0_));
+                      (1 / fy_) * (pi.y() - v0_));
 
-  // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
+  // initialize by ignoring the distortion at all, might be problematic for
+  // pixels around boundary
   Point2 pn = invKPi;
 
   // iterate until the uncalibrate is close to the actual pixel coordinate
   const int maxIterations = 10;
   int iteration;
   for (iteration = 0; iteration < maxIterations; ++iteration) {
-    if (distance2(uncalibrate(pn), pi) <= tol) break;
+    if (distance2(uncalibrate(pn), pi) <= tol_) break;
-    const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
+    const double px = pn.x(), py = pn.y(), xy = px * py, xx = px * px,
+                 yy = py * py;
     const double rr = xx + yy;
     const double g = (1 + k1_ * rr + k2_ * rr * rr);
     const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
@@ -153,8 +156,11 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
     pn = (invKPi - Point2(dx, dy)) / g;
   }
 
-  if ( iteration >= maxIterations )
+  if (iteration >= maxIterations)
-    throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
+    throw std::runtime_error(
+        "Cal3DS2::calibrate fails to converge. need a better initialization");
+
+  calibrateJacobians<Cal3DS2_Base, dimension>(*this, pn, Dcal, Dp);
 
   return pn;
 }
@@ -184,8 +190,5 @@ Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const {
   DK << fx_, s_, 0.0, fy_;
   return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
 }
-
 }
 /* ************************************************************************* */
-
-

gtsam/geometry/Cal3DS2_Base.h

@@ -14,10 +14,12 @@
  * @brief Calibration of a camera with radial distortion
  * @date Feb 28, 2010
  * @author ydjian
+ * @author Varun Agrawal
  */
 
 #pragma once
 
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -31,82 +33,77 @@ namespace gtsam {
  * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
  * but using only k1,k2,p1, and p2 coefficients.
  * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
- * rr = Pn.x^2 + Pn.y^2
+ * r² = P.x² + P.y²
- * \hat{Pn} = (1 + k1*rr + k2*rr^2 ) Pn + [ 2*p1 Pn.x Pn.y + p2 (rr + 2 Pn.x^2) ;
+ * P̂ = (1 + k1*r² + k2*r⁴) P + [ (2*p1 P.x P.y) + p2 (r² + 2 Pn.x²)
- *                      p1 (rr + 2 Pn.y^2) + 2*p2 Pn.x Pn.y  ]
+ *                            p1 (r² + 2 Pn.y²) + (2*p2 Pn.x Pn.y) ]
- * pi = K*Pn
+ * pi = K*P̂
  */
-class GTSAM_EXPORT Cal3DS2_Base {
+class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
+ protected:
+  double k1_ = 0.0f, k2_ = 0.0f;  ///< radial 2nd-order and 4th-order
+  double p1_ = 0.0f, p2_ = 0.0f;  ///< tangential distortion
+  double tol_ = 1e-5;             ///< tolerance value when calibrating
 
-protected:
+ public:
-
+  enum { dimension = 9 };
-  double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
-  double k1_, k2_ ; // radial 2nd-order and 4th-order
-  double p1_, p2_ ; // tangential distortion
-
-public:
 
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3DS2_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
+  Cal3DS2_Base() = default;
 
-  Cal3DS2_Base(double fx, double fy, double s, double u0, double v0,
+  Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
+               double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
-  fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
+      : Cal3(fx, fy, s, u0, v0),
+        k1_(k1),
+        k2_(k2),
+        p1_(p1),
+        p2_(p2),
+        tol_(tol) {}
 
-  virtual ~Cal3DS2_Base() {}
+  ~Cal3DS2_Base() override {}
 
   /// @}
   /// @name Advanced Constructors
   /// @{
 
-  Cal3DS2_Base(const Vector &v) ;
+  Cal3DS2_Base(const Vector9& v)
+      : Cal3(v(0), v(1), v(2), v(3), v(4)),
+        k1_(v(5)),
+        k2_(v(6)),
+        p1_(v(7)),
+        p2_(v(8)) {}
 
   /// @}
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3DS2_Base& cal);
+
   /// print with optional string
-  virtual void print(const std::string& s = "") const;
+  void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
-  bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
+  bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const;
 
   /// @}
   /// @name Standard Interface
   /// @{
 
-  /// focal length x
-  inline double fx() const { return fx_;}
-
-  /// focal length x
-  inline double fy() const { return fy_;}
-
-  /// skew
-  inline double skew() const { return s_;}
-
-  /// image center in x
-  inline double px() const { return u0_;}
-
-  /// image center in y
-  inline double py() const { return v0_;}
-
   /// First distortion coefficient
-  inline double k1() const { return k1_;}
+  inline double k1() const { return k1_; }
 
   /// Second distortion coefficient
-  inline double k2() const { return k2_;}
+  inline double k2() const { return k2_; }
 
   /// First tangential distortion coefficient
-  inline double p1() const { return p1_;}
+  inline double p1() const { return p1_; }
 
   /// Second tangential distortion coefficient
-  inline double p2() const { return p2_;}
+  inline double p2() const { return p2_; }
-
-  /// return calibration matrix -- not really applicable
-  Matrix3 K() const;
 
   /// return distortion parameter vector
   Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
@@ -121,18 +118,24 @@ public:
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in (distorted) image coordinates
    */
-  Point2 uncalibrate(const Point2& p,
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
-       OptionalJacobian<2,9> Dcal = boost::none,
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
-       OptionalJacobian<2,2> Dp = boost::none) const ;
 
   /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
-  Point2 calibrate(const Point2& p, const double tol=1e-5) const;
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Derivative of uncalibrate wrpt intrinsic coordinates
-  Matrix2 D2d_intrinsic(const Point2& p) const ;
+  Matrix2 D2d_intrinsic(const Point2& p) const;
 
   /// Derivative of uncalibrate wrpt the calibration parameters
-  Matrix29 D2d_calibration(const Point2& p) const ;
+  Matrix29 D2d_calibration(const Point2& p) const;
+
+  /// return DOF, dimensionality of tangent space
+  size_t dim() const override { return Dim(); }
+
+  /// return DOF, dimensionality of tangent space
+  inline static size_t Dim() { return dimension; }
 
   /// @}
   /// @name Clone
@@ -145,30 +148,23 @@ public:
 
   /// @}
 
-private:
+ private:
-
   /// @name Advanced Interface
   /// @{
 
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class Archive>
+  template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/)
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-  {
+    ar& boost::serialization::make_nvp(
-    ar & BOOST_SERIALIZATION_NVP(fx_);
+        "Cal3DS2_Base", boost::serialization::base_object<Cal3>(*this));
-    ar & BOOST_SERIALIZATION_NVP(fy_);
+    ar& BOOST_SERIALIZATION_NVP(k1_);
-    ar & BOOST_SERIALIZATION_NVP(s_);
+    ar& BOOST_SERIALIZATION_NVP(k2_);
-    ar & BOOST_SERIALIZATION_NVP(u0_);
+    ar& BOOST_SERIALIZATION_NVP(p1_);
-    ar & BOOST_SERIALIZATION_NVP(v0_);
+    ar& BOOST_SERIALIZATION_NVP(p2_);
-    ar & BOOST_SERIALIZATION_NVP(k1_);
+    ar& BOOST_SERIALIZATION_NVP(tol_);
-    ar & BOOST_SERIALIZATION_NVP(k2_);
-    ar & BOOST_SERIALIZATION_NVP(p1_);
-    ar & BOOST_SERIALIZATION_NVP(p2_);
   }
 
   /// @}
-
 };
-
 }
-

gtsam/geometry/Cal3Fisheye.cpp

@@ -13,6 +13,7 @@
  * @file Cal3Fisheye.cpp
  * @date Apr 8, 2020
  * @author ghaggin
+ * @author Varun Agrawal
  */
 
 #include <gtsam/base/Matrix.h>
@@ -23,18 +24,6 @@
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3Fisheye::Cal3Fisheye(const Vector9& v)
-    : fx_(v[0]),
-      fy_(v[1]),
-      s_(v[2]),
-      u0_(v[3]),
-      v0_(v[4]),
-      k1_(v[5]),
-      k2_(v[6]),
-      k3_(v[7]),
-      k4_(v[8]) {}
-
 /* ************************************************************************* */
 Vector9 Cal3Fisheye::vector() const {
   Vector9 v;
@@ -42,13 +31,6 @@ Vector9 Cal3Fisheye::vector() const {
   return v;
 }
 
-/* ************************************************************************* */
-Matrix3 Cal3Fisheye::K() const {
-  Matrix3 K;
-  K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
-  return K;
-}
-
 /* ************************************************************************* */
 double Cal3Fisheye::Scaling(double r) {
   static constexpr double threshold = 1e-8;
@@ -122,14 +104,25 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
 }
 
 /* ************************************************************************* */
-Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
+Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
-  // initial gues just inverts the pinhole model
+                              OptionalJacobian<2, 2> Dp) const {
+  // 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,
+  // Perform newtons method, break when solution converges past tol_,
   // throw exception if max iterations are reached
   const int maxIterations = 10;
   int iteration;
@@ -140,7 +133,7 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
     const Point2 uv_hat = uncalibrate(pi, boost::none, jac);
 
     // Test convergence
-    if ((uv_hat - uv).norm() < tol) break;
+    if ((uv_hat - uv).norm() < tol_) break;
 
     // Newton's method update step
     pi = pi - jac.inverse() * (uv_hat - uv);
@@ -151,9 +144,19 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
         "Cal3Fisheye::calibrate fails to converge. need a better "
         "initialization");
 
+  calibrateJacobians<Cal3Fisheye, dimension>(*this, pi, Dcal, Dp);
+
   return pi;
 }
 
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3Fisheye& cal) {
+  os << (Cal3&)cal;
+  os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", k3: " << cal.k3()
+     << ", k4: " << cal.k4();
+  return os;
+}
+
 /* ************************************************************************* */
 void Cal3Fisheye::print(const std::string& s_) const {
   gtsam::print((Matrix)K(), s_ + ".K");
@@ -162,24 +165,12 @@ void Cal3Fisheye::print(const std::string& s_) const {
 
 /* ************************************************************************* */
 bool Cal3Fisheye::equals(const Cal3Fisheye& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol ||
+  const Cal3* base = dynamic_cast<const Cal3*>(&K);
-      std::abs(s_ - K.s_) > tol || std::abs(u0_ - K.u0_) > tol ||
+  return Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
-      std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
+         std::fabs(k2_ - K.k2_) < tol && std::fabs(k3_ - K.k3_) < tol &&
-      std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol ||
+         std::fabs(k4_ - K.k4_) < tol;
-      std::abs(k4_ - K.k4_) > tol)
-    return false;
-  return true;
 }
 
 /* ************************************************************************* */
-Cal3Fisheye Cal3Fisheye::retract(const Vector& d) const {
-  return Cal3Fisheye(vector() + d);
-}
 
-/* ************************************************************************* */
+}  // \ namespace gtsam
-Vector Cal3Fisheye::localCoordinates(const Cal3Fisheye& T2) const {
-  return T2.vector() - vector();
-}
-
-}  // namespace gtsam
-/* ************************************************************************* */

gtsam/geometry/Cal3Fisheye.h

@@ -14,10 +14,12 @@
  * @brief Calibration of a fisheye camera
  * @date Apr 8, 2020
  * @author ghaggin
+ * @author Varun Agrawal
  */
 
 #pragma once
 
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 #include <string>
@@ -36,71 +38,58 @@ namespace gtsam {
  * Intrinsic coordinates:
  *   [x_i;y_i] = [x/z; y/z]
  * Distorted coordinates:
- *   r^2 = (x_i)^2 + (y_i)^2
+ *   r² = (x_i)² + (y_i)²
  *   th = atan(r)
- *   th_d = th(1 + k1*th^2 + k2*th^4 + k3*th^6 + k4*th^8)
+ *   th_d = th(1 + k1*th² + k2*th⁴ + k3*th⁶ + k4*th⁸)
  *   [x_d; y_d] = (th_d / r)*[x_i; y_i]
  * Pixel coordinates:
  *   K = [fx s u0; 0 fy v0 ;0 0 1]
  *   [u; v; 1] = K*[x_d; y_d; 1]
  */
-class GTSAM_EXPORT Cal3Fisheye {
+class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
  private:
-  double fx_, fy_, s_, u0_, v0_;  // focal length, skew and principal point
+  double k1_ = 0.0f, k2_ = 0.0f;  ///< fisheye distortion coefficients
-  double k1_, k2_, k3_, k4_;      // fisheye distortion coefficients
+  double k3_ = 0.0f, k4_ = 0.0f;  ///< fisheye distortion coefficients
+  double tol_ = 1e-5;             ///< tolerance value when calibrating
 
  public:
   enum { dimension = 9 };
-  typedef boost::shared_ptr<Cal3Fisheye>
+  ///< shared pointer to fisheye calibration object
-      shared_ptr;  ///< shared pointer to fisheye calibration object
+  using shared_ptr = boost::shared_ptr<Cal3Fisheye>;
 
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3Fisheye()
+  Cal3Fisheye() = default;
-      : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {}
 
   Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
               const double v0, const double k1, const double k2,
-              const double k3, const double k4)
+              const double k3, const double k4, double tol = 1e-5)
-      : fx_(fx),
+      : Cal3(fx, fy, s, u0, v0),
-        fy_(fy),
-        s_(s),
-        u0_(u0),
-        v0_(v0),
         k1_(k1),
         k2_(k2),
         k3_(k3),
-        k4_(k4) {}
+        k4_(k4),
+        tol_(tol) {}
 
-  virtual ~Cal3Fisheye() {}
+  ~Cal3Fisheye() override {}
 
   /// @}
   /// @name Advanced Constructors
   /// @{
 
-  explicit Cal3Fisheye(const Vector9& v);
+  explicit Cal3Fisheye(const Vector9& v)
+      : Cal3(v(0), v(1), v(2), v(3), v(4)),
+        k1_(v(5)),
+        k2_(v(6)),
+        k3_(v(7)),
+        k4_(v(8)) {}
 
   /// @}
   /// @name Standard Interface
   /// @{
 
-  /// focal length x
-  inline double fx() const { return fx_; }
-
-  /// focal length x
-  inline double fy() const { return fy_; }
-
-  /// skew
-  inline double skew() const { return s_; }
-
-  /// image center in x
-  inline double px() const { return u0_; }
-
-  /// image center in y
-  inline double py() const { return v0_; }
-
   /// First distortion coefficient
   inline double k1() const { return k1_; }
 
@@ -113,9 +102,6 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// Second tangential distortion coefficient
   inline double k4() const { return k4_; }
 
-  /// return calibration matrix
-  Matrix3 K() const;
-
   /// return distortion parameter vector
   Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
 
@@ -129,24 +115,34 @@ class GTSAM_EXPORT Cal3Fisheye {
    * @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image
    * coordinates [u; v]
    * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters (fx, fy, ...,
+   * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters
-   * k4)
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
    * @return point in (distorted) image coordinates
    */
   Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
                      OptionalJacobian<2, 2> Dp = boost::none) const;
 
-  /// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
+  /**
-  /// y_i]
+   * Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
-  Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
+   * y_i]
+   * @param p point in image coordinates
+   * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
+   * @return point in intrinsic coordinates
+   */
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// @}
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3Fisheye& cal);
+
   /// print with optional string
-  virtual void print(const std::string& s = "") const;
+  void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3Fisheye& K, double tol = 10e-9) const;
@@ -155,17 +151,21 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// @name Manifold
   /// @{
 
+  /// Return dimensions of calibration manifold object
+  size_t dim() const override { return Dim(); }
+
+  /// Return dimensions of calibration manifold object
+  inline static size_t Dim() { return dimension; }
+
   /// Given delta vector, update calibration
-  Cal3Fisheye retract(const Vector& d) const;
+  inline Cal3Fisheye retract(const Vector& d) const {
+    return Cal3Fisheye(vector() + d);
+  }
 
   /// Given a different calibration, calculate update to obtain it
-  Vector localCoordinates(const Cal3Fisheye& T2) const;
+  Vector localCoordinates(const Cal3Fisheye& T2) const {
-
+    return T2.vector() - vector();
-  /// Return dimensions of calibration manifold object
+  }
-  virtual size_t dim() const { return 9; }
-
-  /// Return dimensions of calibration manifold object
-  static size_t Dim() { return 9; }
 
   /// @}
   /// @name Clone
@@ -186,11 +186,8 @@ class GTSAM_EXPORT Cal3Fisheye {
   friend class boost::serialization::access;
   template <class Archive>
   void serialize(Archive& ar, const unsigned int /*version*/) {
-    ar& BOOST_SERIALIZATION_NVP(fx_);
+    ar& boost::serialization::make_nvp(
-    ar& BOOST_SERIALIZATION_NVP(fy_);
+        "Cal3Fisheye", boost::serialization::base_object<Cal3>(*this));
-    ar& BOOST_SERIALIZATION_NVP(s_);
-    ar& BOOST_SERIALIZATION_NVP(u0_);
-    ar& BOOST_SERIALIZATION_NVP(v0_);
     ar& BOOST_SERIALIZATION_NVP(k1_);
     ar& BOOST_SERIALIZATION_NVP(k2_);
     ar& BOOST_SERIALIZATION_NVP(k3_);

gtsam/geometry/Cal3Unified.cpp

@@ -13,21 +13,18 @@
  * @file Cal3Unified.cpp
  * @date Mar 8, 2014
  * @author Jing Dong
+ * @author Varun Agrawal
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
-#include <gtsam/geometry/Point2.h>
+#include <gtsam/base/Vector.h>
 #include <gtsam/geometry/Cal3Unified.h>
+#include <gtsam/geometry/Point2.h>
 
 #include <cmath>
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3Unified::Cal3Unified(const Vector &v):
-    Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {}
-
 /* ************************************************************************* */
 Vector10 Cal3Unified::vector() const {
   Vector10 v;
@@ -35,6 +32,13 @@ Vector10 Cal3Unified::vector() const {
   return v;
 }
 
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3Unified& cal) {
+  os << (Cal3DS2_Base&)cal;
+  os << ", xi: " << cal.xi();
+  return os;
+}
+
 /* ************************************************************************* */
 void Cal3Unified::print(const std::string& s) const {
   Base::print(s);
@@ -43,20 +47,14 @@ void Cal3Unified::print(const std::string& s) const {
 
 /* ************************************************************************* */
 bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
+  const Cal3DS2_Base* base = dynamic_cast<const Cal3DS2_Base*>(&K);
-      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
+  return Cal3DS2_Base::equals(*base, tol) && std::fabs(xi_ - K.xi_) < tol;
-      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol ||
-      std::abs(xi_ - K.xi_) > tol)
-    return false;
-  return true;
 }
 
 /* ************************************************************************* */
 // todo: make a fixed sized jacobian version of this
-Point2 Cal3Unified::uncalibrate(const Point2& p,
+Point2 Cal3Unified::uncalibrate(const Point2& p, OptionalJacobian<2, 10> Dcal,
-       OptionalJacobian<2,10> H1,
+                                OptionalJacobian<2, 2> Dp) const {
-       OptionalJacobian<2,2> H2) const {
-
   // this part of code is modified from Cal3DS2,
   // since the second part of this model (after project to normalized plane)
   // is same as Cal3DS2
@@ -69,50 +67,53 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
   const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0);
   const double xi_sqrt_nx = 1.0 / (1 + xi * sqrt_nx);
   const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx;
-  const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane
+  const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx;  // points on NPlane
 
   // Part2: project NPlane point to pixel plane: use Cal3DS2
-  Point2 m(x,y);
+  Point2 m(x, y);
   Matrix29 H1base;
-  Matrix2 H2base;    // jacobians from Base class
+  Matrix2 H2base;  // jacobians from Base class
   Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
 
   // Inlined derivative for calibration
-  if (H1) {
+  if (Dcal) {
     // part1
     Vector2 DU;
-    DU << -xs * sqrt_nx * xi_sqrt_nx2, //
+    DU << -xs * sqrt_nx * xi_sqrt_nx2,  //
         -ys * sqrt_nx * xi_sqrt_nx2;
-    *H1 << H1base, H2base * DU;
+    *Dcal << H1base, H2base * DU;
   }
 
   // Inlined derivative for points
-  if (H2) {
+  if (Dp) {
     // part1
     const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
-    const double mid = -(xi * xs*ys) * denom;
+    const double mid = -(xi * xs * ys) * denom;
     Matrix2 DU;
-    DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
+    DU << (sqrt_nx + xi * (ys * ys + 1)) * denom, mid,  //
-        mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
+        mid, (sqrt_nx + xi * (xs * xs + 1)) * denom;
 
-    *H2 << H2base * DU;
+    *Dp << H2base * DU;
   }
 
   return puncalib;
 }
 
 /* ************************************************************************* */
-Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const {
+Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal,
-
+                              OptionalJacobian<2, 2> Dp) const {
   // calibrate point to Nplane use base class::calibrate()
-  Point2 pnplane = Base::calibrate(pi, tol);
+  Point2 pnplane = Base::calibrate(pi);
 
   // call nplane to space
-  return this->nPlaneToSpace(pnplane);
+  Point2 pn = this->nPlaneToSpace(pnplane);
+
+  calibrateJacobians<Cal3Unified, dimension>(*this, pn, Dcal, Dp);
+
+  return pn;
 }
 /* ************************************************************************* */
 Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
-
   const double x = p.x(), y = p.y();
   const double xy2 = x * x + y * y;
   const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1);
@@ -122,7 +123,6 @@ Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
 
 /* ************************************************************************* */
 Point2 Cal3Unified::spaceToNPlane(const Point2& p) const {
-
   const double x = p.x(), y = p.y();
   const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1);
 
@@ -135,11 +135,10 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const {
 }
 
 /* ************************************************************************* */
-Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
+Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
   return T2.vector() - vector();
 }
 
-}
 /* ************************************************************************* */
 
-
+}  // \ namespace gtsam

gtsam/geometry/Cal3Unified.h

@@ -14,6 +14,7 @@
  * @brief Unified Calibration Model, see Mei07icra for details
  * @date Mar 8, 2014
  * @author Jing Dong
+ * @author Varun Agrawal
  */
 
 /**
@@ -27,53 +28,57 @@
 namespace gtsam {
 
 /**
- * @brief Calibration of a omni-directional camera with mirror + lens radial distortion
+ * @brief Calibration of a omni-directional camera with mirror + lens radial
+ * distortion
  * @addtogroup geometry
  * \nosubgrouping
  *
  * Similar to Cal3DS2, does distortion but has additional mirror parameter xi
  * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
- * Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
+ * Pn = [ P.x / (1 + xi * \sqrt{P.x² + P.y² + 1}), P.y / (1 + xi * \sqrt{P.x² +
- * rr = Pn.x^2 + Pn.y^2
+ * P.y² + 1})]
- * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
+ * r² = Pn.x² + Pn.y²
- *                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
+ * \hat{pn} = (1 + k1*r² + k2*r⁴ ) pn + [ 2*k3 pn.x pn.y + k4 (r² + 2 Pn.x²) ;
+ *                      k3 (rr + 2 Pn.y²) + 2*k4 pn.x pn.y  ]
  * pi = K*pn
  */
 class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
+  using This = Cal3Unified;
+  using Base = Cal3DS2_Base;
 
-  typedef Cal3Unified This;
+ private:
-  typedef Cal3DS2_Base Base;
+  double xi_ = 0.0f;  ///< mirror parameter
-
-private:
-
-  double xi_;  // mirror parameter
-
-public:
 
+ public:
   enum { dimension = 10 };
 
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3Unified() : Base(), xi_(0) {}
+  Cal3Unified() = default;
 
-  Cal3Unified(double fx, double fy, double s, double u0, double v0,
+  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) :
+              double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0)
-        Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
+      : Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
 
-  virtual ~Cal3Unified() {}
+  ~Cal3Unified() override {}
 
   /// @}
   /// @name Advanced Constructors
   /// @{
 
-  Cal3Unified(const Vector &v) ;
+  Cal3Unified(const Vector10& v)
+      : Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {}
 
   /// @}
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3Unified& cal);
+
   /// print with optional string
   void print(const std::string& s = "") const override;
 
@@ -85,7 +90,10 @@ public:
   /// @{
 
   /// mirror parameter
-  inline double xi() const { return xi_;}
+  inline double xi() const { return xi_; }
+
+  /// Return all parameters as a vector
+  Vector10 vector() const;
 
   /**
    * convert intrinsic coordinates xy to image coordinates uv
@@ -95,11 +103,12 @@ public:
    * @return point in image coordinates
    */
   Point2 uncalibrate(const Point2& p,
-      OptionalJacobian<2,10> Dcal = boost::none,
+                     OptionalJacobian<2, 10> Dcal = boost::none,
-      OptionalJacobian<2,2> Dp = boost::none) const ;
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Conver a pixel coordinate to ideal coordinate
-  Point2 calibrate(const Point2& p, const double tol=1e-5) const;
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Convert a 3D point to normalized unit plane
   Point2 spaceToNPlane(const Point2& p) const;
@@ -112,41 +121,33 @@ public:
   /// @{
 
   /// Given delta vector, update calibration
-  Cal3Unified retract(const Vector& d) const ;
+  Cal3Unified retract(const Vector& d) const;
 
   /// Given a different calibration, calculate update to obtain it
-  Vector10 localCoordinates(const Cal3Unified& T2) const ;
+  Vector localCoordinates(const Cal3Unified& T2) const;
 
   /// Return dimensions of calibration manifold object
-  virtual size_t dim() const { return dimension ; }
+  size_t dim() const override { return Dim(); }
 
   /// Return dimensions of calibration manifold object
-  static size_t Dim() { return dimension; }
+  inline static size_t Dim() { return dimension; }
-
-  /// Return all parameters as a vector
-  Vector10 vector() const ;
 
   /// @}
 
-private:
+ private:
-
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class Archive>
+  template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/)
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-  {
+    ar& boost::serialization::make_nvp(
-    ar & boost::serialization::make_nvp("Cal3Unified",
+        "Cal3Unified", boost::serialization::base_object<Cal3DS2_Base>(*this));
-        boost::serialization::base_object<Cal3DS2_Base>(*this));
+    ar& BOOST_SERIALIZATION_NVP(xi_);
-    ar & BOOST_SERIALIZATION_NVP(xi_);
   }
-
 };
 
-template<>
+template <>
 struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
 
-template<>
+template <>
 struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
-
 }
-

gtsam/geometry/Cal3_S2.cpp

@@ -22,95 +22,55 @@
 #include <iostream>
 
 namespace gtsam {
-using namespace std;
 
 /* ************************************************************************* */
-Cal3_S2::Cal3_S2(double fov, int w, int h) :
+std::ostream& operator<<(std::ostream& os, const Cal3_S2& cal) {
-    s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) {
+  // Use the base class version since it is identical.
-  double a = fov * M_PI / 360.0; // fov/2 in radians
+  os << (Cal3&)cal;
-  fx_ = (double) w / (2.0 * tan(a)); //    old formula: fx_ = (double) w * tan(a);
-  fy_ = fx_;
-}
-
-/* ************************************************************************* */
-Cal3_S2::Cal3_S2(const std::string &path) :
-    fx_(320), fy_(320), s_(0), u0_(320), v0_(140) {
-
-  char buffer[200];
-  buffer[0] = 0;
-  sprintf(buffer, "%s/calibration_info.txt", path.c_str());
-  std::ifstream infile(buffer, std::ios::in);
-
-  if (infile)
-    infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
-  else {
-    throw std::runtime_error("Cal3_S2: Unable to load the calibration");
-  }
-
-  infile.close();
-}
-
-/* ************************************************************************* */
-ostream& operator<<(ostream& os, const Cal3_S2& cal) {
-  os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px()
-     << ", py:" << cal.py() << "}";
   return os;
 }
 
 /* ************************************************************************* */
 void Cal3_S2::print(const std::string& s) const {
-  gtsam::print((Matrix)matrix(), s);
+  gtsam::print((Matrix)K(), s);
 }
 
 /* ************************************************************************* */
 bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol)
+  return Cal3::equals(K, tol);
-    return false;
-  if (std::abs(fy_ - K.fy_) > tol)
-    return false;
-  if (std::abs(s_ - K.s_) > tol)
-    return false;
-  if (std::abs(u0_ - K.u0_) > tol)
-    return false;
-  if (std::abs(v0_ - K.v0_) > tol)
-    return false;
-  return true;
 }
 
 /* ************************************************************************* */
 Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
-    OptionalJacobian<2, 2> Dp) const {
+                            OptionalJacobian<2, 2> Dp) const {
   const double x = p.x(), y = p.y();
-  if (Dcal)
+  if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
-    *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
+  if (Dp) *Dp << fx_, s_, 0.0, fy_;
-  if (Dp)
-    *Dp << fx_, s_, 0.0, fy_;
   return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
 }
 
 /* ************************************************************************* */
-Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal,
+Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
-                           OptionalJacobian<2,2> Dp) const {
+                          OptionalJacobian<2, 2> Dp) const {
-    const double u = p.x(), v = p.y();
+  const double u = p.x(), v = p.y();
-    double delta_u = u - u0_, delta_v = v - v0_;
+  double delta_u = u - u0_, delta_v = v - v0_;
-    double inv_fx = 1/ fx_, inv_fy = 1/fy_;
+  double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
-    double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
+  double inv_fy_delta_v = inv_fy * delta_v;
-    Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v),
+  double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
-                  inv_fy_delta_v);
+
-    if(Dcal)
+  Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
-       *Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,  -inv_fx * point.y(),
+  if (Dcal) {
-            -inv_fx,  inv_fx_s_inv_fy,
+    *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
-            0, -inv_fy * point.y(), 0,  0, -inv_fy;
+        -inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, -inv_fy * point.y(),
-    if(Dp)
+        0, 0, -inv_fy;
-        *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
+  }
-    return point;
+  if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
+  return point;
 }
 
 /* ************************************************************************* */
-Vector3 Cal3_S2::calibrate(const Vector3& p) const {
+Vector3 Cal3_S2::calibrate(const Vector3& p) const { return inverse() * p; }
-  return matrix_inverse() * p;
-}
 
 /* ************************************************************************* */
 
-} // namespace gtsam
+}  // namespace gtsam

gtsam/geometry/Cal3_S2.h

@@ -21,6 +21,7 @@
 
 #pragma once
 
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -30,31 +31,25 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Cal3_S2 {
+class GTSAM_EXPORT Cal3_S2 : public Cal3 {
-private:
+ public:
-  double fx_, fy_, s_, u0_, v0_;
-
-public:
   enum { dimension = 5 };
-  typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
+
+  ///< shared pointer to calibration object
+  using shared_ptr = boost::shared_ptr<Cal3_S2>;
 
   /// @name Standard Constructors
   /// @{
 
   /// Create a default calibration that leaves coordinates unchanged
-  Cal3_S2() :
+  Cal3_S2() = default;
-      fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {
-  }
 
   /// constructor from doubles
-  Cal3_S2(double fx, double fy, double s, double u0, double v0) :
+  Cal3_S2(double fx, double fy, double s, double u0, double v0)
-      fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
+      : Cal3(fx, fy, s, u0, v0) {}
-  }
 
   /// constructor from vector
-  Cal3_S2(const Vector &d) :
+  Cal3_S2(const Vector5& d) : Cal3(d) {}
-      fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {
-  }
 
   /**
    * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
@@ -62,141 +57,65 @@ public:
    * @param w image width
    * @param h image height
    */
-  Cal3_S2(double fov, int w, int h);
+  Cal3_S2(double fov, int w, int h) : Cal3(fov, w, h) {}
 
-  /// @}
+  /**
-  /// @name Advanced Constructors
+   * Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
-  /// @{
+   * @param p point in intrinsic coordinates
+   * @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in image coordinates
+   */
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
 
-  /// load calibration from location (default name is calibration_info.txt)
+  /**
-  Cal3_S2(const std::string &path);
+   * Convert image coordinates uv to intrinsic coordinates xy
+   * @param p point in image coordinates
+   * @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in intrinsic coordinates
+   */
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
+
+  /**
+   * Convert homogeneous image coordinates to intrinsic coordinates
+   * @param p point in image coordinates
+   * @return point in intrinsic coordinates
+   */
+  Vector3 calibrate(const Vector3& p) const;
 
   /// @}
   /// @name Testable
   /// @{
 
   /// Output stream operator
-  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Cal3_S2& cal);
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3_S2& cal);
 
   /// print with optional string
-  void print(const std::string& s = "Cal3_S2") const;
+  void print(const std::string& s = "Cal3_S2") const override;
 
   /// Check if equal up to specified tolerance
   bool equals(const Cal3_S2& K, double tol = 10e-9) const;
 
-  /// @}
-  /// @name Standard Interface
-  /// @{
-
-  /// focal length x
-  inline double fx() const {
-    return fx_;
-  }
-
-  /// focal length y
-  inline double fy() const {
-    return fy_;
-  }
-
-  /// aspect ratio
-  inline double aspectRatio() const {
-    return fx_/fy_;
-  }
-
-  /// skew
-  inline double skew() const {
-    return s_;
-  }
-
-  /// image center in x
-  inline double px() const {
-    return u0_;
-  }
-
-  /// image center in y
-  inline double py() const {
-    return v0_;
-  }
-
-  /// return the principal point
-  Point2 principalPoint() const {
-    return Point2(u0_, v0_);
-  }
-
-  /// vectorized form (column-wise)
-  Vector5 vector() const {
-    Vector5 v;
-    v << fx_, fy_, s_, u0_, v0_;
-    return v;
-  }
-
-  /// return calibration matrix K
-  Matrix3 K() const {
-    Matrix3 K;
-    K <<  fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
-    return K;
-  }
-
-  /** @deprecated The following function has been deprecated, use K above */
-  Matrix3 matrix() const {
-    return K();
-  }
-
-  /// return inverted calibration matrix inv(K)
-  Matrix3 matrix_inverse() const {
-    const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
-    Matrix3 K_inverse;
-    K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
-        1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0;
-    return K_inverse;
-  }
-
-  /**
-   * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
-   * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
-   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
-   * @return point in image coordinates
-   */
-  Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
-      OptionalJacobian<2,2> Dp = boost::none) const;
-
-  /**
-   * convert image coordinates uv to intrinsic coordinates xy
-   * @param p point in image coordinates
-   * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
-   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
-   * @return point in intrinsic coordinates
-   */
-  Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
-                   OptionalJacobian<2,2> Dp = boost::none) const;
-
-  /**
-   * convert homogeneous image coordinates to intrinsic coordinates
-   * @param p point in image coordinates
-   * @return point in intrinsic coordinates
-   */
-  Vector3 calibrate(const Vector3& p) const;
-
   /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
   inline Cal3_S2 between(const Cal3_S2& q,
-      OptionalJacobian<5,5> H1=boost::none,
+                         OptionalJacobian<5, 5> H1 = boost::none,
-      OptionalJacobian<5,5> H2=boost::none) const {
+                         OptionalJacobian<5, 5> H2 = boost::none) const {
-    if(H1) *H1 = -I_5x5;
+    if (H1) *H1 = -I_5x5;
-    if(H2) *H2 =  I_5x5;
+    if (H2) *H2 = I_5x5;
-    return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
+    return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_,
+                   q.v0_ - v0_);
   }
 
-
   /// @}
   /// @name Manifold
   /// @{
 
   /// return DOF, dimensionality of tangent space
-  inline size_t dim() const { return dimension; }
+  inline static size_t Dim() { return dimension; }
-
-  /// return DOF, dimensionality of tangent space
-  static size_t Dim() { return dimension; }
 
   /// Given 5-dim tangent vector, create new calibration
   inline Cal3_S2 retract(const Vector& d) const {
@@ -212,27 +131,22 @@ public:
   /// @name Advanced Interface
   /// @{
 
-private:
+ private:
-
   /// Serialization function
   friend class boost::serialization::access;
-  template<class Archive>
+  template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/) {
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_NVP(fx_);
+    ar& boost::serialization::make_nvp(
-    ar & BOOST_SERIALIZATION_NVP(fy_);
+        "Cal3_S2", boost::serialization::base_object<Cal3>(*this));
-    ar & BOOST_SERIALIZATION_NVP(s_);
-    ar & BOOST_SERIALIZATION_NVP(u0_);
-    ar & BOOST_SERIALIZATION_NVP(v0_);
   }
 
   /// @}
-
 };
 
-template<>
+template <>
 struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
 
-template<>
+template <>
 struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
 
-} // \ namespace gtsam
+}  // \ namespace gtsam

gtsam/geometry/Cal3_S2Stereo.cpp

@@ -20,20 +20,56 @@
 #include <iostream>
 
 namespace gtsam {
-using namespace std;
+
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3_S2Stereo& cal) {
+  os << (Cal3_S2&)cal;
+  os << ", b: " << cal.baseline();
+  return os;
+}
 
 /* ************************************************************************* */
 void Cal3_S2Stereo::print(const std::string& s) const {
-     K_.print(s+"K: ");
+  std::cout << s << (s != "" ? " " : "");
-     std::cout << s << "Baseline: " << b_ << std::endl;
+  std::cout << "K: " << (Matrix)K() << std::endl;
-   }
+  std::cout << "Baseline: " << b_ << std::endl;
+}
 
 /* ************************************************************************* */
 bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
-  if (std::abs(b_ - other.b_) > tol) return false;
+  const Cal3_S2* base = dynamic_cast<const Cal3_S2*>(&other);
-  return K_.equals(other.K_,tol);
+  return (Cal3_S2::equals(*base, tol) &&
+          std::fabs(b_ - other.baseline()) < tol);
+}
+
+/* ************************************************************************* */
+Point2 Cal3_S2Stereo::uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal,
+                            OptionalJacobian<2, 2> Dp) const {
+  const double x = p.x(), y = p.y();
+  if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, 0.0, y, 0.0, 0.0, 1.0, 0.0;
+  if (Dp) *Dp << fx_, s_, 0.0, fy_;
+  return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
+}
+
+/* ************************************************************************* */
+Point2 Cal3_S2Stereo::calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal,
+                          OptionalJacobian<2, 2> Dp) const {
+  const double u = p.x(), v = p.y();
+  double delta_u = u - u0_, delta_v = v - v0_;
+  double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
+  double inv_fy_delta_v = inv_fy * delta_v;
+  double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
+
+  Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
+  if (Dcal) {
+    *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
+        -inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, 0,
+        -inv_fy * point.y(), 0, 0, -inv_fy, 0;
+  }
+  if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
+  return point;
 }
 
 /* ************************************************************************* */
 
-} // namespace gtsam
+}  // namespace gtsam

gtsam/geometry/Cal3_S2Stereo.h

@@ -22,135 +22,143 @@
 
 namespace gtsam {
 
+/**
+ * @brief The most common 5DOF 3D->2D calibration, stereo version
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
+ private:
+  double b_ = 1.0f;  ///< Stereo baseline.
+
+ public:
+  enum { dimension = 6 };
+
+  ///< shared pointer to stereo calibration object
+  using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
+
+  /// @name Standard Constructors
+  /// @
+
+  /// default calibration leaves coordinates unchanged
+  Cal3_S2Stereo() = default;
+
+  /// constructor from doubles
+  Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
+      : Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
+
+  /// constructor from vector
+  Cal3_S2Stereo(const Vector6& d)
+      : Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
+
+  /// easy constructor; field-of-view in degrees, assumes zero skew
+  Cal3_S2Stereo(double fov, int w, int h, double b)
+      : Cal3_S2(fov, w, h), b_(b) {}
+
   /**
-   * @brief The most common 5DOF 3D->2D calibration, stereo version
+   * Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
-   * @addtogroup geometry
+   * @param p point in intrinsic coordinates
-   * \nosubgrouping
+   * @param Dcal optional 2*6 Jacobian wrpt Cal3_S2Stereo parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in image coordinates
    */
-  class GTSAM_EXPORT Cal3_S2Stereo {
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
-  private:
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
 
-    Cal3_S2 K_;
+  /**
-    double b_;
+   * Convert image coordinates uv to intrinsic coordinates xy
+   * @param p point in image coordinates
+   * @param Dcal optional 2*6 Jacobian wrpt Cal3_S2Stereo parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in intrinsic coordinates
+   */
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
-  public:
+  /**
+   * Convert homogeneous image coordinates to intrinsic coordinates
+   * @param p point in image coordinates
+   * @return point in intrinsic coordinates
+   */
+  Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); }
 
-    enum { dimension = 6 };
+  /// @}
-    typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr;  ///< shared pointer to stereo calibration object
+  /// @name Testable
+  /// @{
 
-    /// @name Standard Constructors
+  /// Output stream operator
-    /// @
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3_S2Stereo& cal);
 
-    /// default calibration leaves coordinates unchanged
+  /// print with optional string
-    Cal3_S2Stereo() :
+  void print(const std::string& s = "") const override;
-      K_(1, 1, 0, 0, 0), b_(1.0) {
-    }
 
-    /// constructor from doubles
+  /// Check if equal up to specified tolerance
-    Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) :
+  bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
-      K_(fx, fy, s, u0, v0), b_(b) {
-    }
 
-    /// constructor from vector
+  /// @}
-    Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){}
+  /// @name Standard Interface
+  /// @{
 
-    /// easy constructor; field-of-view in degrees, assumes zero skew
+  /// return calibration, same for left and right
-    Cal3_S2Stereo(double fov, int w, int h, double b) :
+  const Cal3_S2& calibration() const { return *this; }
-      K_(fov, w, h), b_(b) {
-    }
 
-    /// @}
+  /// return calibration matrix K, same for left and right
-    /// @name Testable
+  Matrix3 K() const override { return Cal3_S2::K(); }
-    /// @{
 
-    void print(const std::string& s = "") const;
+  /// return baseline
+  inline double baseline() const { return b_; }
 
-    /// Check if equal up to specified tolerance
+  /// vectorized form (column-wise)
-    bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
+  Vector6 vector() const {
+    Vector6 v;
+    v << Cal3_S2::vector(), b_;
+    return v;
+  }
 
-   /// @}
+  /// @}
-    /// @name Standard Interface
+  /// @name Manifold
-    /// @{
+  /// @{
 
-    /// return calibration, same for left and right
+  /// return DOF, dimensionality of tangent space
-    const Cal3_S2& calibration() const { return K_;}
+  inline size_t dim() const override { return Dim(); }
 
-    /// return calibration matrix K, same for left and right
+  /// return DOF, dimensionality of tangent space
-    Matrix matrix() const { return K_.matrix();}
+  inline static size_t Dim() { return dimension; }
 
-    /// focal length x
+  /// Given 6-dim tangent vector, create new calibration
-    inline double fx() const { return K_.fx();}
+  inline Cal3_S2Stereo retract(const Vector& d) const {
+    return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3),
+                         py() + d(4), b_ + d(5));
+  }
 
-    /// focal length x
+  /// Unretraction for the calibration
-    inline double fy() const { return K_.fy();}
+  Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
+    return T2.vector() - vector();
+  }
 
-    /// skew
+  /// @}
-    inline double skew() const { return K_.skew();}
+  /// @name Advanced Interface
+  /// @{
 
-    /// image center in x
+ private:
-    inline double px() const { return K_.px();}
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
+    ar& BOOST_SERIALIZATION_NVP(b_);
+  }
+  /// @}
+};
 
-    /// image center in y
+// Define GTSAM traits
-    inline double py() const { return K_.py();}
+template <>
+struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
 
-    /// return the principal point
+template <>
-    Point2 principalPoint() const { return K_.principalPoint();}
+struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
+};
 
-    /// return baseline
+}  // \ namespace gtsam
-    inline double baseline() const { return b_; }
-
-    /// vectorized form (column-wise)
-    Vector6 vector() const {
-      Vector6 v;
-      v << K_.vector(), b_;
-      return v;
-    }
-
-    /// @}
-    /// @name Manifold
-    /// @{
-
-    /// return DOF, dimensionality of tangent space
-    inline size_t dim() const { return dimension; }
-
-    /// return DOF, dimensionality of tangent space
-    static size_t Dim() { return dimension; }
-
-    /// Given 6-dim tangent vector, create new calibration
-    inline Cal3_S2Stereo retract(const Vector& d) const {
-      return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5));
-    }
-
-    /// Unretraction for the calibration
-    Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
-      return T2.vector() - vector();
-    }
-
-
-    /// @}
-    /// @name Advanced Interface
-    /// @{
-
-  private:
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class Archive>
-    void serialize(Archive & ar, const unsigned int /*version*/)
-    {
-      ar & BOOST_SERIALIZATION_NVP(K_);
-      ar & BOOST_SERIALIZATION_NVP(b_);
-    }
-    /// @}
-
-  };
-
-  // Define GTSAM traits
-  template<>
-  struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
-  };
-
-  template<>
-  struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
-  };
-
-} // \ namespace gtsam

gtsam/geometry/CalibratedCamera.h

@@ -119,22 +119,20 @@ public:
   /// @name Standard Constructors
   /// @{
 
-  /** default constructor */
+  /// Default constructor
-  PinholeBase() {
+  PinholeBase() {}
-  }
 
-  /** constructor with pose */
+  /// Constructor with pose
-  explicit PinholeBase(const Pose3& pose) :
+  explicit PinholeBase(const Pose3& pose) : pose_(pose) {}
-      pose_(pose) {
-  }
 
   /// @}
   /// @name Advanced Constructors
   /// @{
 
-  explicit PinholeBase(const Vector &v) :
+  explicit PinholeBase(const Vector& v) : pose_(Pose3::Expmap(v)) {}
-      pose_(Pose3::Expmap(v)) {
+
-  }
+  /// Default destructor
+  virtual ~PinholeBase() = default;
 
   /// @}
   /// @name Testable
@@ -144,7 +142,7 @@ public:
   bool equals(const PinholeBase &camera, double tol = 1e-9) const;
 
   /// print
-  void print(const std::string& s = "PinholeBase") const;
+  virtual void print(const std::string& s = "PinholeBase") const;
 
   /// @}
   /// @name Standard Interface
@@ -324,6 +322,11 @@ public:
   /// Return canonical coordinate
   Vector localCoordinates(const CalibratedCamera& T2) const;
 
+  /// print
+  void print(const std::string& s = "CalibratedCamera") const override {
+    PinholeBase::print(s);
+  }
+
   /// @deprecated
   inline size_t dim() const {
     return dimension;

gtsam/geometry/CameraSet.h

@@ -69,9 +69,12 @@ protected:
 
 public:
 
+  /// Destructor
+  virtual ~CameraSet() = default;
+
   /// Definitions for blocks of F
-  typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
+  using MatrixZD = Eigen::Matrix<double, ZDim, D>;
-  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
+  using FBlocks = std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>;
 
   /**
    * print
@@ -139,6 +142,57 @@ public:
     return ErrorVector(project2(point, Fs, E), measured);
   }
 
+  /**
+     * 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)
+     * Fixed size version
+     */
+    template<int N, int ND> // N = 2 or 3, ND is the camera dimension
+    static SymmetricBlockMatrix SchurComplement(
+        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();
+
+      // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
+      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));
+
+      // Blockwise Schur complement
+      for (size_t i = 0; i < m; i++) { // for each camera
+
+        const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
+        const auto FiT = Fi.transpose();
+        const Eigen::Matrix<double, ZDim, N> Ei_P = //
+            E.block(ZDim * i, 0, ZDim, N) * P;
+
+        // D = (Dx2) * ZDim
+        augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
+        - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
+
+        // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
+        augmentedHessian.setDiagonalBlock(i, FiT
+            * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
+
+        // upper triangular part of the hessian
+        for (size_t j = i + 1; j < m; j++) { // for each camera
+          const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
+
+          // (DxD) = (Dx2) * ( (2x2) * (2xD) )
+          augmentedHessian.setOffDiagonalBlock(i, j, -FiT
+              * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
+        }
+      } // end of for over cameras
+
+      augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
+      return augmentedHessian;
+    }
+
   /**
    * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
    * G = F' * F - F' * E * P * E' * F
@@ -148,45 +202,7 @@ public:
   template<int N> // N = 2 or 3
   static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs,
       const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
-
+    return SchurComplement<N,D>(Fs, E, P, b);
-    // a single point is observed in m cameras
-    size_t m = Fs.size();
-
-    // Create a SymmetricBlockMatrix
-    size_t M1 = D * m + 1;
-    std::vector<DenseIndex> dims(m + 1); // this also includes the b term
-    std::fill(dims.begin(), dims.end() - 1, D);
-    dims.back() = 1;
-    SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
-
-    // Blockwise Schur complement
-    for (size_t i = 0; i < m; i++) { // for each camera
-
-      const MatrixZD& Fi = Fs[i];
-      const auto FiT = Fi.transpose();
-      const Eigen::Matrix<double, ZDim, N> Ei_P = //
-          E.block(ZDim * i, 0, ZDim, N) * P;
-
-      // D = (Dx2) * ZDim
-      augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
-      - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
-
-      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
-      augmentedHessian.setDiagonalBlock(i, FiT
-          * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
-
-      // upper triangular part of the hessian
-      for (size_t j = i + 1; j < m; j++) { // for each camera
-        const MatrixZD& Fj = Fs[j];
-
-        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
-        augmentedHessian.setOffDiagonalBlock(i, j, -FiT
-            * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
-      }
-    } // end of for over cameras
-
-    augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
-    return augmentedHessian;
   }
 
   /// Computes Point Covariance P, with lambda parameter

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/Line3.h

@@ -102,6 +102,27 @@ class Line3 {
    */
   Point3 point(double distance = 0) const;
 
+  /**
+   * Return the rotation of the line.
+   */
+  inline Rot3 R() const {
+    return R_;
+  }
+
+  /**
+   * Return the x-coordinate of the intersection of the line with the xy plane.
+   */
+  inline double a() const {
+    return a_;
+  }
+
+  /**
+   * Return the y-coordinate of the intersection of the line with the xy plane.
+   */
+  inline double b() const {
+    return b_;
+  }
+
   /**
    * Transform a line from world to camera frame
    * @param wTc - Pose3 of camera in world frame