Merge branch 'develop' into feature/NoiseModelFactorN_replaceDeprecated

.github/scripts/python.sh

@@ -43,46 +43,68 @@ if [ -z ${PYTHON_VERSION+x} ]; then
     exit 127
 fi
 
-PYTHON="python${PYTHON_VERSION}"
+export PYTHON="python${PYTHON_VERSION}"
 
-if [[ $(uname) == "Darwin" ]]; then
+function install_dependencies()
+{
+  if [[ $(uname) == "Darwin" ]]; then
     brew install wget
-else
+  else
     # Install a system package required by our library
     sudo apt-get install -y wget libicu-dev python3-pip python3-setuptools
-fi
+  fi
 
-PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
+  export PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
 
-[ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
+  [ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
+
+  $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
+}
+
+function build()
+{
+  mkdir $GITHUB_WORKSPACE/build
+  cd $GITHUB_WORKSPACE/build
+
+  BUILD_PYBIND="ON"
+
+  cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
+      -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_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \
+      -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
+      -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \
+      -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \
+      -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install
 
 
-BUILD_PYBIND="ON"
+  # Set to 2 cores so that Actions does not error out during resource provisioning.
+  make -j2 install
 
-sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
+  cd $GITHUB_WORKSPACE/build/python
+  $PYTHON -m pip install --user .
+}
 
-mkdir $GITHUB_WORKSPACE/build
+function test()
-cd $GITHUB_WORKSPACE/build
+{
+  cd $GITHUB_WORKSPACE/python/gtsam/tests
+  $PYTHON -m unittest discover -v
+}
 
-cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
+# select between build or test
-    -DGTSAM_BUILD_TESTS=OFF \
+case $1 in
-    -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
+  -d)
-    -DGTSAM_USE_QUATERNIONS=OFF \
+    install_dependencies
-    -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
+    ;;
-    -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
+  -b)
-    -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
+    build
-    -DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \
+    ;;
-    -DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \
+  -t)
-    -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
+    test
-    -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \
+    ;;
-    -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \
+esac
-    -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install
-
-
-# Set to 2 cores so that Actions does not error out during resource provisioning.
-make -j2 install
-
-cd $GITHUB_WORKSPACE/build/python
-$PYTHON -m pip install --user .
-cd $GITHUB_WORKSPACE/python/gtsam/tests
-$PYTHON -m unittest discover -v

.github/workflows/build-linux.yml

@@ -20,26 +20,26 @@ jobs:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          ubuntu-18.04-gcc-5,
+          ubuntu-20.04-gcc-7,
-          ubuntu-18.04-gcc-9,
+          ubuntu-20.04-gcc-9,
-          ubuntu-18.04-clang-9,
+          ubuntu-20.04-clang-9,
         ]
 
         build_type: [Debug, Release]
         build_unstable: [ON]
         include:
-          - name: ubuntu-18.04-gcc-5
+          - name: ubuntu-20.04-gcc-7
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
-            version: "5"
+            version: "7"
 
-          - name: ubuntu-18.04-gcc-9
+          - name: ubuntu-20.04-gcc-9
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
 
-          - name: ubuntu-18.04-clang-9
+          - name: ubuntu-20.04-clang-9
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: clang
             version: "9"
 
@@ -60,9 +60,9 @@ jobs:
             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-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
+          sudo apt-get -y update
+          sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev
 
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib

.github/workflows/build-macos.yml

@@ -19,16 +19,16 @@ jobs:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          macOS-10.15-xcode-11.3.1,
+          macos-11-xcode-13.4.1,
         ]
 
         build_type: [Debug, Release]
         build_unstable: [ON]
         include:
-          - name: macOS-10.15-xcode-11.3.1
+          - name: macos-11-xcode-13.4.1
-            os: macOS-10.15
+            os: macos-11
             compiler: xcode
-            version: "11.3.1"
+            version: "13.4.1"
 
     steps:
       - name: Checkout
@@ -43,7 +43,7 @@ jobs:
             echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
             echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
-            sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
+            sudo xcode-select -switch /Applications/Xcode.app
             echo "CC=clang" >> $GITHUB_ENV
             echo "CXX=clang++" >> $GITHUB_ENV
           fi

.github/workflows/build-python.yml

@@ -19,48 +19,48 @@ jobs:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          ubuntu-18.04-gcc-5,
+          ubuntu-20.04-gcc-7,
-          ubuntu-18.04-gcc-9,
+          ubuntu-20.04-gcc-9,
-          ubuntu-18.04-clang-9,
+          ubuntu-20.04-clang-9,
-          macOS-10.15-xcode-11.3.1,
+          macOS-11-xcode-13.4.1,
-          ubuntu-18.04-gcc-5-tbb,
+          ubuntu-20.04-gcc-7-tbb,
         ]
 
         build_type: [Debug, Release]
         python_version: [3]
         include:
-          - name: ubuntu-18.04-gcc-5
+          - name: ubuntu-20.04-gcc-7
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
-            version: "5"
+            version: "7"
 
-          - name: ubuntu-18.04-gcc-9
+          - name: ubuntu-20.04-gcc-9
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
 
-          - name: ubuntu-18.04-clang-9
+          - name: ubuntu-20.04-clang-9
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: clang
             version: "9"
 
           # NOTE temporarily added this as it is a required check.
-          - name: ubuntu-18.04-clang-9
+          - name: ubuntu-20.04-clang-9
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: clang
             version: "9"
             build_type: Debug
             python_version: "3"
 
-          - name: macOS-10.15-xcode-11.3.1
+          - name: macOS-11-xcode-13.4.1
-            os: macOS-10.15
+            os: macOS-11
             compiler: xcode
-            version: "11.3.1"
+            version: "13.4.1"
 
-          - name: ubuntu-18.04-gcc-5-tbb
+          - name: ubuntu-20.04-gcc-7-tbb
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
-            version: "5"
+            version: "7"
             flag: tbb
 
     steps:
@@ -79,9 +79,9 @@ jobs:
             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-get -y install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
+          sudo apt-get -y update
+          sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libboost-all-dev
           
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
@@ -103,7 +103,7 @@ jobs:
             echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
             echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
-            sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
+            sudo xcode-select -switch /Applications/Xcode.app
             echo "CC=clang" >> $GITHUB_ENV
             echo "CXX=clang++" >> $GITHUB_ENV
           fi
@@ -117,11 +117,12 @@ jobs:
         uses: pierotofy/set-swap-space@master
         with:
           swap-size-gb: 6
-      - name: Build (Linux)
+      - name: Install Dependencies
-        if: runner.os == 'Linux'
         run: |
-          bash .github/scripts/python.sh
+          bash .github/scripts/python.sh -d
-      - name: Build (macOS)
+      - name: Build
-        if: runner.os == 'macOS'
         run: |
-          bash .github/scripts/python.sh
+          bash .github/scripts/python.sh -b
+      - name: Test
+        run: |
+          bash .github/scripts/python.sh -t

.github/workflows/build-special.yml

@@ -32,31 +32,31 @@ jobs:
 
         include:
           - name: ubuntu-gcc-deprecated
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
             flag: deprecated
 
           - name: ubuntu-gcc-quaternions
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
             flag: quaternions
 
           - name: ubuntu-gcc-tbb
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
             flag: tbb
 
           - name: ubuntu-cayleymap
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
             flag: cayley
 
           - name: ubuntu-system-libs
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
             flag: system-libs
@@ -74,9 +74,9 @@ jobs:
             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-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
+          sudo apt-get -y update
+          sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev
 
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib

.github/workflows/trigger-packaging.yml

@@ -0,0 +1,21 @@
+# This triggers building of packages
+name: Trigger Package Builds
+on:
+  push:
+    branches:
+      - develop
+jobs:
+  trigger-package-build:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Trigger Package Rebuild
+        uses: actions/github-script@v6
+        with:
+          github-token: ${{ secrets.PACKAGING_REPO_ACCESS_TOKEN }}
+          script: |
+            await github.rest.actions.createWorkflowDispatch({
+              owner: 'borglab-launchpad',
+              repo: 'gtsam-packaging',
+              workflow_id: 'main.yaml',
+              ref: 'master'
+            })

CMakeLists.txt

@@ -10,7 +10,7 @@ endif()
 set (GTSAM_VERSION_MAJOR 4)
 set (GTSAM_VERSION_MINOR 2)
 set (GTSAM_VERSION_PATCH 0)
-set (GTSAM_PRERELEASE_VERSION "a7")
+set (GTSAM_PRERELEASE_VERSION "a8")
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 
 if (${GTSAM_VERSION_PATCH} EQUAL 0)
@@ -101,8 +101,6 @@ if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
     # Copy matlab.h to the correct folder.
     configure_file(${PROJECT_SOURCE_DIR}/wrap/matlab.h
                ${PROJECT_BINARY_DIR}/wrap/matlab.h COPYONLY)
-    # Add the include directories so that matlab.h can be found
-    include_directories("${PROJECT_BINARY_DIR}" "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}")
 
     add_subdirectory(wrap)
     list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")

INSTALL.md

@@ -50,7 +50,7 @@ will run up to 10x faster in Release mode! See the end of this document for
 additional debugging tips.
 
 3. GTSAM has Doxygen documentation. To generate, run 'make doc' from your
-build directory.
+build directory after setting the `GTSAM_BUILD_DOCS` and `GTSAM_BUILD_[HTML|LATEX]` cmake flags.
 
 4. The instructions below install the library to the default system install path and
 build all components. From a terminal, starting in the root library folder,

README.md

@@ -31,11 +31,11 @@ In the root library folder execute:
 
 ```sh
 #!bash
-$ mkdir build
+mkdir build
-$ cd build
+cd build
-$ cmake ..
+cmake ..
-$ make check (optional, runs unit tests)
+make check (optional, runs unit tests)
-$ make install
+make install
 ```
 
 Prerequisites:
@@ -64,6 +64,29 @@ GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionali
 
 We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md) wrappers for GTSAM. Please refer to the linked documents for more details.
 
+## Citation
+
+If you are using GTSAM for academic work, please use the following citation:
+
+```bibtex
+@software{gtsam,
+  author       = {Frank Dellaert and Richard Roberts and Varun Agrawal and Alex Cunningham and Chris Beall and Duy-Nguyen Ta and Fan Jiang and lucacarlone and nikai and Jose Luis Blanco-Claraco and Stephen Williams and ydjian and John Lambert and Andy Melim and Zhaoyang Lv and Akshay Krishnan and Jing Dong and Gerry Chen and Krunal Chande and balderdash-devil and DiffDecisionTrees and Sungtae An and mpaluri and Ellon Paiva Mendes and Mike Bosse and Akash Patel and Ayush Baid and Paul Furgale and matthewbroadwaynavenio and roderick-koehle},
+  title        = {borglab/gtsam},
+  month        = may,
+  year         = 2022,
+  publisher    = {Zenodo},
+  version      = {4.2a7},
+  doi          = {10.5281/zenodo.5794541},
+  url          = {https://doi.org/10.5281/zenodo.5794541}
+}
+```
+
+You can also get the latest citation available from Zenodo below:
+
+[![DOI](https://zenodo.org/badge/86362856.svg)](https://doi.org/10.5281/zenodo.5794541)
+
+Specific formats are available in the bottom-right corner of the Zenodo page.
+
 ## The Preintegrated IMU Factor
 
 GTSAM includes a state of the art IMU handling scheme based on
@@ -73,7 +96,7 @@ GTSAM includes a state of the art IMU handling scheme based on
 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)
-- 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)
+- 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.
 

cmake/Config.cmake.in

@@ -21,6 +21,10 @@ else()
 find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
 endif()
 
+if(@GTSAM_USE_SYSTEM_EIGEN@)
+find_dependency(Eigen3 REQUIRED)
+endif()
+
 # Load exports
 include(${OUR_CMAKE_DIR}/@PACKAGE_NAME@-exports.cmake)
 

cmake/FindEigen3.cmake

@@ -1,81 +0,0 @@
-# - Try to find Eigen3 lib
-#
-# This module supports requiring a minimum version, e.g. you can do
-#   find_package(Eigen3 3.1.2)
-# to require version 3.1.2 or newer of Eigen3.
-#
-# Once done this will define
-#
-#  EIGEN3_FOUND - system has eigen lib with correct version
-#  EIGEN3_INCLUDE_DIR - the eigen include directory
-#  EIGEN3_VERSION - eigen version
-
-# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
-# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
-# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
-# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
-
-if(NOT Eigen3_FIND_VERSION)
-  if(NOT Eigen3_FIND_VERSION_MAJOR)
-    set(Eigen3_FIND_VERSION_MAJOR 2)
-  endif(NOT Eigen3_FIND_VERSION_MAJOR)
-  if(NOT Eigen3_FIND_VERSION_MINOR)
-    set(Eigen3_FIND_VERSION_MINOR 91)
-  endif(NOT Eigen3_FIND_VERSION_MINOR)
-  if(NOT Eigen3_FIND_VERSION_PATCH)
-    set(Eigen3_FIND_VERSION_PATCH 0)
-  endif(NOT Eigen3_FIND_VERSION_PATCH)
-
-  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
-endif(NOT Eigen3_FIND_VERSION)
-
-macro(_eigen3_check_version)
-  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
-
-  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
-  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
-  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
-  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
-  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
-  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
-
-  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
-  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-    set(EIGEN3_VERSION_OK FALSE)
-  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-    set(EIGEN3_VERSION_OK TRUE)
-  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-
-  if(NOT EIGEN3_VERSION_OK)
-
-    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
-                   "but at least version ${Eigen3_FIND_VERSION} is required")
-  endif(NOT EIGEN3_VERSION_OK)
-endmacro(_eigen3_check_version)
-
-if (EIGEN3_INCLUDE_DIR)
-
-  # in cache already
-  _eigen3_check_version()
-  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
-
-else (EIGEN3_INCLUDE_DIR)
-
-  find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
-      PATHS
-      ${CMAKE_INSTALL_PREFIX}/include
-      ${KDE4_INCLUDE_DIR}
-      PATH_SUFFIXES eigen3 eigen
-    )
-
-  if(EIGEN3_INCLUDE_DIR)
-    _eigen3_check_version()
-  endif(EIGEN3_INCLUDE_DIR)
-
-  include(FindPackageHandleStandardArgs)
-  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
-
-  mark_as_advanced(EIGEN3_INCLUDE_DIR)
-
-endif(EIGEN3_INCLUDE_DIR)
-

cmake/GtsamBuildTypes.cmake

@@ -190,7 +190,7 @@ if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
 endif()
 
 if (NOT MSVC)
-  option(GTSAM_BUILD_WITH_MARCH_NATIVE  "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" ON)
+  option(GTSAM_BUILD_WITH_MARCH_NATIVE  "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" OFF)
   if(GTSAM_BUILD_WITH_MARCH_NATIVE AND (APPLE AND NOT CMAKE_SYSTEM_PROCESSOR STREQUAL "arm64"))
     # Add as public flag so all dependant projects also use it, as required
     # by Eigen to avid crashes due to SIMD vectorization:

cmake/HandleEigen.cmake

@@ -1,6 +1,5 @@
 ###############################################################################
 # Option for using system Eigen or GTSAM-bundled Eigen
-
 option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
 
 if(NOT GTSAM_USE_SYSTEM_EIGEN)
@@ -11,10 +10,14 @@ endif()
 
 # Switch for using system Eigen or GTSAM-bundled Eigen
 if(GTSAM_USE_SYSTEM_EIGEN)
-    find_package(Eigen3 REQUIRED)
+    # Since Eigen 3.3.0 a Eigen3Config.cmake is available so use it.
+    find_package(Eigen3 CONFIG REQUIRED) # need to find again as REQUIRED
 
-    # Use generic Eigen include paths e.g. <Eigen/Core>
+    # The actual include directory (for BUILD cmake target interface):
-    set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
+    # Note: EIGEN3_INCLUDE_DIR points to some random location on some eigen
+    # versions.  So here I use the target itself to get the proper include
+    # directory (it is generated by cmake, thus has the correct path)
+    get_target_property(GTSAM_EIGEN_INCLUDE_FOR_BUILD Eigen3::Eigen INTERFACE_INCLUDE_DIRECTORIES)
 
     # check if MKL is also enabled - can have one or the other, but not both!
     # Note: Eigen >= v3.2.5 includes our patches
@@ -27,9 +30,6 @@ if(GTSAM_USE_SYSTEM_EIGEN)
     if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4))
         message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.")
     endif()
-
-    # The actual include directory (for BUILD cmake target interface):
-    set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}")
 else()
     # Use bundled Eigen include path.
     # Clear any variables set by FindEigen3
@@ -42,7 +42,20 @@ 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 "${GTSAM_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
+    set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${GTSAM_SOURCE_DIR}/gtsam/3rdparty/Eigen")
+
+    add_library(gtsam_eigen3 INTERFACE)
+
+    target_include_directories(gtsam_eigen3 INTERFACE
+      $<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
+      $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
+    )
+    add_library(Eigen3::Eigen ALIAS gtsam_eigen3)
+
+    install(TARGETS gtsam_eigen3 EXPORT GTSAM-exports PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
+
+    list(APPEND GTSAM_EXPORTED_TARGETS gtsam_eigen3)
+    set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}")
 endif()
 
 # Detect Eigen version:

cmake/HandlePrintConfiguration.cmake

@@ -33,6 +33,7 @@ print_build_options_for_target(gtsam)
 
 print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
 print_config("Use System Metis" "${GTSAM_USE_SYSTEM_METIS}")
+print_config("Using Boost version" "${Boost_VERSION}")
 
 if(GTSAM_USE_TBB)
     print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})")

doc/CMakeLists.txt

@@ -26,6 +26,7 @@ if (GTSAM_BUILD_DOCS)
         gtsam/basis 
         gtsam/discrete
         gtsam/geometry
+        gtsam/hybrid
         gtsam/inference
         gtsam/linear
         gtsam/navigation
@@ -33,7 +34,6 @@ if (GTSAM_BUILD_DOCS)
         gtsam/sam
         gtsam/sfm
         gtsam/slam
-        gtsam/smart
         gtsam/symbolic
         gtsam
     )
@@ -42,10 +42,12 @@ if (GTSAM_BUILD_DOCS)
     set(gtsam_unstable_doc_subdirs
         gtsam_unstable/base
         gtsam_unstable/discrete
+        gtsam_unstable/dynamics
+        gtsam_unstable/geometry
         gtsam_unstable/linear
         gtsam_unstable/nonlinear
+        gtsam_unstable/partition
         gtsam_unstable/slam
-        gtsam_unstable/dynamics 
         gtsam_unstable
     )
     

doc/DoxygenLayout.xml

@@ -18,7 +18,6 @@
       <tab type="files" visible="yes" title="" intro=""/>
       <tab type="globals" visible="yes" title="" intro=""/>
     </tab>
-    <tab type="dirs" visible="yes" title="" intro=""/>
     <tab type="examples" visible="yes" title="" intro=""/>  
   </navindex>
 

doc/refs.bib

@@ -1,26 +1,72 @@
+%% This BibTeX bibliography file was created using BibDesk.
+%% https://bibdesk.sourceforge.io/
+
+%% Created for Varun Agrawal at 2021-09-27 17:39:09 -0400 
+
+
+%% Saved with string encoding Unicode (UTF-8) 
+
+
+
+@article{Lupton12tro,
+	author = {Lupton, Todd and Sukkarieh, Salah},
+	date-added = {2021-09-27 17:38:56 -0400},
+	date-modified = {2021-09-27 17:39:09 -0400},
+	doi = {10.1109/TRO.2011.2170332},
+	journal = {IEEE Transactions on Robotics},
+	number = {1},
+	pages = {61-76},
+	title = {Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions},
+	volume = {28},
+	year = {2012},
+	Bdsk-Url-1 = {https://doi.org/10.1109/TRO.2011.2170332}}
+
+@inproceedings{Forster15rss,
+	author = {Christian Forster and Luca Carlone and Frank Dellaert and Davide Scaramuzza},
+	booktitle = {Robotics: Science and Systems},
+	date-added = {2021-09-26 20:44:41 -0400},
+	date-modified = {2021-09-26 20:45:03 -0400},
+	title = {IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation},
+	year = {2015}}
+
 @article{Iserles00an,
-  title =	 {Lie-group methods},
+	author = {Iserles, Arieh and Munthe-Kaas, Hans Z and N{\o}rsett, Syvert P and Zanna, Antonella},
-  author =	 {Iserles, Arieh and Munthe-Kaas, Hans Z and
+	journal = {Acta Numerica 2000},
-                  N{\o}rsett, Syvert P and Zanna, Antonella},
+	pages = {215--365},
-  journal =	 {Acta Numerica 2000},
+	publisher = {Cambridge Univ Press},
-  volume =	 {9},
+	title = {Lie-group methods},
-  pages =	 {215--365},
+	volume = {9},
-  year =	 {2000},
+	year = {2000}}
-  publisher =	 {Cambridge Univ Press}
-}
 
 @book{Murray94book,
-  title =	 {A mathematical introduction to robotic manipulation},
+	author = {Murray, Richard M and Li, Zexiang and Sastry, S Shankar and Sastry, S Shankara},
-  author =	 {Murray, Richard M and Li, Zexiang and Sastry, S
+	publisher = {CRC press},
-                  Shankar and Sastry, S Shankara},
+	title = {A mathematical introduction to robotic manipulation},
-  year =	 {1994},
+	year = {1994}}
-  publisher =	 {CRC press}
-}
 
 @book{Spivak65book,
-  title =	 {Calculus on manifolds},
+	author = {Spivak, Michael},
-  author =	 {Spivak, Michael},
+	publisher = {WA Benjamin New York},
-  volume =	 {1},
+	title = {Calculus on manifolds},
-  year =	 {1965},
+	volume = {1},
-  publisher =	 {WA Benjamin New York}
+	year = {1965}}
+
+@phdthesis{Nikolic16thesis,
+  title={Characterisation, calibration, and design of visual-inertial sensor systems for robot navigation},
+  author={Nikolic, Janosch},
+  year={2016},
+  school={ETH Zurich}
+}
+
+@book{Simon06book,
+  title={Optimal state estimation: Kalman, H infinity, and nonlinear approaches},
+  author={Simon, Dan},
+  year={2006},
+  publisher={John Wiley \& Sons}
+}
+
+@inproceedings{Trawny05report_IndirectKF,
+  title={Indirect Kalman Filter for 3 D Attitude Estimation},
+  author={Nikolas Trawny and Stergios I. Roumeliotis},
+  year={2005}
 }

examples/CMakeLists.txt

@@ -2,4 +2,4 @@ set (excluded_examples
     elaboratePoint2KalmanFilter.cpp
 )
 
-gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}")
+gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;gtsam_unstable;${Boost_PROGRAM_OPTIONS_LIBRARY}")

examples/CombinedImuFactorsExample.cpp

@@ -60,13 +60,14 @@ namespace po = boost::program_options;
 
 po::variables_map parseOptions(int argc, char* argv[]) {
   po::options_description desc;
-  desc.add_options()("help,h", "produce help message")(
+  desc.add_options()("help,h", "produce help message")  // help message
-      "data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
+      ("data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
-      "path to the CSV file with the IMU data")(
+       "path to the CSV file with the IMU data")  // path to the data file
-      "output_filename",
+      ("output_filename",
-      po::value<string>()->default_value("imuFactorExampleResults.csv"),
+       po::value<string>()->default_value("imuFactorExampleResults.csv"),
-      "path to the result file to use")("use_isam", po::bool_switch(),
+       "path to the result file to use")  // filename to save results to
-                                        "use ISAM as the optimizer");
+      ("use_isam", po::bool_switch(),
+       "use ISAM as the optimizer");  // flag for ISAM optimizer
 
   po::variables_map vm;
   po::store(po::parse_command_line(argc, argv, desc), vm);
@@ -106,7 +107,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
       I_3x3 * 1e-8;  // error committed in integrating position from velocities
   Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
   Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
-  Matrix66 bias_acc_omega_int =
+  Matrix66 bias_acc_omega_init =
       I_6x6 * 1e-5;  // error in the bias used for preintegration
 
   auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
@@ -122,7 +123,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
   // PreintegrationCombinedMeasurements params:
   p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
   p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
-  p->biasAccOmegaInt = bias_acc_omega_int;
+  p->biasAccOmegaInt = bias_acc_omega_init;
 
   return p;
 }

examples/ImuFactorsExample.cpp

@@ -94,7 +94,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
       I_3x3 * 1e-8;  // error committed in integrating position from velocities
   Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
   Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
-  Matrix66 bias_acc_omega_int =
+  Matrix66 bias_acc_omega_init =
       I_6x6 * 1e-5;  // error in the bias used for preintegration
 
   auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
@@ -110,7 +110,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
   // PreintegrationCombinedMeasurements params:
   p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
   p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
-  p->biasAccOmegaInt = bias_acc_omega_int;
+  p->biasAccOmegaInt = bias_acc_omega_init;
 
   return p;
 }
@@ -267,7 +267,6 @@ int main(int argc, char* argv[]) {
 
       if (use_isam) {
         isam2->update(*graph, initial_values);
-        isam2->update();
         result = isam2->calculateEstimate();
 
         // reset the graph

examples/README.md

@@ -33,7 +33,6 @@ The following examples illustrate some concepts from Georgia Tech's research pap
 ## 2D Pose SLAM 
 
 * **LocalizationExample.cpp**: modeling robot motion
-* **LocalizationExample2.cpp**: example with GPS like measurements
 * **Pose2SLAMExample**: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
 * **Pose2SLAMExample_advanced**: same, but uses an Optimizer object
 * **Pose2SLAMwSPCG**: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface

gtsam/CMakeLists.txt

@@ -117,12 +117,10 @@ set_target_properties(gtsam PROPERTIES
     VERSION             ${gtsam_version}
     SOVERSION           ${gtsam_soversion})
 
-# Append Eigen include path, set in top-level CMakeLists.txt to either
+# Append Eigen include path to either
 # system-eigen, or GTSAM eigen path
-target_include_directories(gtsam PUBLIC
+target_link_libraries(gtsam PUBLIC Eigen3::Eigen)
-  $<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
+
-  $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
-)
 # MKL include dir:
 if (GTSAM_USE_EIGEN_MKL)
   target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})

gtsam/base/ConcurrentMap.h

@@ -62,7 +62,7 @@ namespace gtsam {
  * convenience to avoid having lengthy types in the code.  Through timing,
  * we've seen that the fast_pool_allocator can lead to speedups of several
  * percent.
- * @addtogroup base
+ * @ingroup base
  */
 template<typename KEY, typename VALUE>
 class ConcurrentMap : public ConcurrentMapBase<KEY,VALUE> {

gtsam/base/DSFMap.h

@@ -28,7 +28,7 @@ namespace gtsam {
 /**
  * Disjoint set forest using an STL map data structure underneath
  * Uses rank compression and union by rank, iterator version
- * @addtogroup base
+ * @ingroup base
  */
 template <class KEY>
 class DSFMap {

gtsam/base/DSFVector.h

@@ -33,7 +33,7 @@ namespace gtsam {
  * A fast implementation of disjoint set forests that uses vector as underly data structure.
  * This is the absolute minimal DSF data structure, and only allows size_t keys
  * Uses rank compression but not union by rank :-(
- * @addtogroup base
+ * @ingroup base
  */
 class GTSAM_EXPORT DSFBase {
 
@@ -59,7 +59,7 @@ public:
 
 /**
  * DSFVector additionally keeps a vector of keys to support more expensive operations
- * @addtogroup base
+ * @ingroup base
  */
 class GTSAM_EXPORT DSFVector: public DSFBase {
 

gtsam/base/FastList.h

@@ -34,7 +34,7 @@ namespace gtsam {
  * convenience to avoid having lengthy types in the code.  Through timing,
  * we've seen that the fast_pool_allocator can lead to speedups of several
  * percent.
-   * @addtogroup base
+   * @ingroup base
  */
 template<typename VALUE>
 class FastList: public std::list<VALUE, typename internal::FastDefaultAllocator<VALUE>::type> {

gtsam/base/FastMap.h

@@ -31,7 +31,7 @@ namespace gtsam {
  * convenience to avoid having lengthy types in the code.  Through timing,
  * we've seen that the fast_pool_allocator can lead to speedups of several
  * percent.
- * @addtogroup base
+ * @ingroup base
  */
 template<typename KEY, typename VALUE>
 class FastMap : public std::map<KEY, VALUE, std::less<KEY>,

gtsam/base/FastSet.h

@@ -43,7 +43,7 @@ namespace gtsam {
  * fast_pool_allocator instead of the default STL allocator.  This is just a
  * convenience to avoid having lengthy types in the code.  Through timing,
  * we've seen that the fast_pool_allocator can lead to speedups of several %.
- * @addtogroup base
+ * @ingroup base
  */
 template<typename VALUE>
 class FastSet: public std::set<VALUE, std::less<VALUE>,

gtsam/base/FastVector.h

@@ -27,7 +27,7 @@ namespace gtsam {
 /**
  * FastVector is a type alias to a std::vector with a custom memory allocator.
  * The particular allocator depends on GTSAM's cmake configuration.
- * @addtogroup base
+ * @ingroup base
  */
 template <typename T>
 using FastVector =

gtsam/base/Group.h

@@ -95,7 +95,7 @@ template<class Class>
 struct MultiplicativeGroupTraits {
   typedef group_tag structure_category;
   typedef multiplicative_group_tag group_flavor;
-  static Class Identity() { return Class::identity(); }
+  static Class Identity() { return Class::Identity(); }
   static Class Compose(const Class &g, const Class & h) { return g * h;}
   static Class Between(const Class &g, const Class & h) { return g.inverse() * h;}
   static Class Inverse(const Class &g) { return g.inverse();}
@@ -111,7 +111,7 @@ template<class Class>
 struct AdditiveGroupTraits {
   typedef group_tag structure_category;
   typedef additive_group_tag group_flavor;
-  static Class Identity() { return Class::identity(); }
+  static Class Identity() { return Class::Identity(); }
   static Class Compose(const Class &g, const Class & h) { return g + h;}
   static Class Between(const Class &g, const Class & h) { return h - g;}
   static Class Inverse(const Class &g) { return -g;}
@@ -147,7 +147,7 @@ public:
   DirectProduct(const G& g, const H& h):std::pair<G,H>(g,h) {}
 
   // identity
-  static DirectProduct identity() { return DirectProduct(); }
+  static DirectProduct Identity() { return DirectProduct(); }
 
   DirectProduct operator*(const DirectProduct& other) const {
     return DirectProduct(traits<G>::Compose(this->first, other.first),
@@ -181,7 +181,7 @@ public:
   DirectSum(const G& g, const H& h):std::pair<G,H>(g,h) {}
 
   // identity
-  static DirectSum identity() { return DirectSum(); }
+  static DirectSum Identity() { return DirectSum(); }
 
   DirectSum operator+(const DirectSum& other) const {
     return DirectSum(g()+other.g(), h()+other.h());

gtsam/base/Lie.h

@@ -177,7 +177,7 @@ struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
   /// @name Group
   /// @{
   typedef multiplicative_group_tag group_flavor;
-  static Class Identity() { return Class::identity();}
+  static Class Identity() { return Class::Identity();}
   /// @}
 
   /// @name Manifold

gtsam/base/ProductLieGroup.h

@@ -48,7 +48,7 @@ public:
   /// @name Group
   /// @{
   typedef multiplicative_group_tag group_flavor;
-  static ProductLieGroup identity() {return ProductLieGroup();}
+  static ProductLieGroup Identity() {return ProductLieGroup();}
 
   ProductLieGroup operator*(const ProductLieGroup& other) const {
     return ProductLieGroup(traits<G>::Compose(this->first,other.first),

gtsam/base/SymmetricBlockMatrix.h

@@ -47,7 +47,7 @@ namespace gtsam {
   * matrix view.  firstBlock() determines the block that appears to have index 0 for all operations
   * (except re-setting firstBlock()).
   *
-  * @addtogroup base */
+  * @ingroup base */
   class GTSAM_EXPORT SymmetricBlockMatrix
   {
   public:

gtsam/base/Testable.h

@@ -51,7 +51,7 @@ namespace gtsam {
    * tests and in generic algorithms.
    *
    * See macros for details on using this structure
-   * @addtogroup base
+   * @ingroup base
    * @tparam T is the objectype this constrains to be testable - assumes print() and equals()
    */
   template <class T>

gtsam/base/ThreadsafeException.h

@@ -14,7 +14,7 @@
  * @brief    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB
  * @author   Richard Roberts
  * @date     Aug 21, 2010
- * @addtogroup base
+ * @ingroup base
  */
 
 #pragma once

gtsam/base/Vector.h

@@ -60,6 +60,7 @@ GTSAM_MAKE_VECTOR_DEFS(9)
 GTSAM_MAKE_VECTOR_DEFS(10)
 GTSAM_MAKE_VECTOR_DEFS(11)
 GTSAM_MAKE_VECTOR_DEFS(12)
+GTSAM_MAKE_VECTOR_DEFS(15)
 
 typedef Eigen::VectorBlock<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;

gtsam/base/VectorSpace.h

@@ -169,7 +169,7 @@ struct HasVectorSpacePrereqs {
   Vector v;
 
   BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) {
-    p = Class::identity();  // identity
+    p = Class::Identity();  // identity
     q = p + p;              // addition
     q = p - p;              // subtraction
     v = p.vector();         // conversion to vector
@@ -192,7 +192,7 @@ struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
   /// @name Group
   /// @{
   typedef additive_group_tag group_flavor;
-  static Class Identity() { return Class::identity();}
+  static Class Identity() { return Class::Identity();}
   /// @}
 
   /// @name Manifold

gtsam/base/VerticalBlockMatrix.h

@@ -38,7 +38,7 @@ namespace gtsam {
    * row for all operations.  To include all rows, rowEnd() should be set to the number of rows in
    * the matrix (i.e. one after the last true row index).
    *
-   * @addtogroup base */
+   * @ingroup base */
   class GTSAM_EXPORT VerticalBlockMatrix
   {
   public:

gtsam/base/make_shared.h

@@ -26,7 +26,7 @@
 #include <type_traits>
 
 namespace gtsam {
-  /// An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly
+  /// An shorthand alias for accessing the \::type inside std::enable_if that can be used in a template directly
   template<bool B, class T = void>
   using enable_if_t = typename std::enable_if<B, T>::type;
 }

gtsam/base/tests/testGroup.cpp

@@ -29,7 +29,7 @@ class Symmetric: private Eigen::PermutationMatrix<N> {
       Eigen::PermutationMatrix<N>(P) {
   }
 public:
-  static Symmetric identity() { return Symmetric(); }
+  static Symmetric Identity() { return Symmetric(); }
   Symmetric() {
     Eigen::PermutationMatrix<N>::setIdentity();
   }

gtsam/base/timing.h

@@ -147,7 +147,7 @@ namespace gtsam {
       size_t id_;
       size_t t_;
       size_t tWall_;
-      double t2_ ; ///< cache the \sum t_i^2
+      double t2_ ; ///< cache the \f$ \sum t_i^2 \f$
       size_t tIt_;
       size_t tMax_;
       size_t tMin_;

gtsam/base/treeTraversal-inst.h

@@ -221,6 +221,6 @@ void PrintForest(const FOREST& forest, std::string str,
   PrintForestVisitorPre visitor(keyFormatter);
   DepthFirstForest(forest, str, visitor);
 }
-}
+}  // namespace treeTraversal
 
-}
+}  // namespace gtsam

gtsam/base/types.cpp

@@ -14,7 +14,7 @@
  * @brief    Functions for handling type information
  * @author   Varun Agrawal
  * @date     May 18, 2020
- * @addtogroup base
+ * @ingroup base
  */
 
 #include <gtsam/base/types.h>

gtsam/base/types.h

@@ -14,7 +14,7 @@
  * @brief    Typedefs for easier changing of types
  * @author   Richard Roberts
  * @date     Aug 21, 2010
- * @addtogroup base
+ * @ingroup base
  */
 
 #pragma once

gtsam/basis/Basis.h

@@ -78,6 +78,8 @@ using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
  * @tparam M Size of the identity matrix.
  * @param w The weights of the polynomial.
  * @return Mx(M*N) kronecker product [w(0)*I, w(1)*I, ..., w(N-1)*I]
+ *
+ * @ingroup basis
  */
 template <size_t M>
 Matrix kroneckerProductIdentity(const Weights& w) {
@@ -90,7 +92,10 @@ Matrix kroneckerProductIdentity(const Weights& w) {
   return result;
 }
 
-/// CRTP Base class for function bases
+/**
+ * CRTP Base class for function bases
+ *  @ingroup basis
+ */
 template <typename DERIVED>
 class Basis {
  public:

gtsam/basis/BasisFactors.h

@@ -32,6 +32,8 @@ namespace gtsam {
  *
  * Example, degree 8 Chebyshev polynomial measured at x=0.5:
  *  EvaluationFactor<Chebyshev2> factor(key, measured, model, 8, 0.5);
+ *
+ * @ingroup basis
  */
 template <class BASIS>
 class EvaluationFactor : public FunctorizedFactor<double, Vector> {
@@ -86,6 +88,8 @@ class EvaluationFactor : public FunctorizedFactor<double, Vector> {
  *
  * @param BASIS: The basis class to use e.g. Chebyshev2
  * @param M: Size of the evaluated state vector.
+ *
+ * @ingroup basis
  */
 template <class BASIS, int M>
 class VectorEvaluationFactor
@@ -149,6 +153,8 @@ class VectorEvaluationFactor
  *  VectorComponentFactor<BASIS, P> controlPrior(key, measured, model,
  *                                               N, i, t, a, b);
  *  where N is the degree and i is the component index.
+ *
+ * @ingroup basis
  */
 template <class BASIS, size_t P>
 class VectorComponentFactor

gtsam/basis/ParameterMatrix.h

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file ParamaterMatrix.h
+ * @file ParameterMatrix.h
  * @brief Define ParameterMatrix class which is used to store values at
  * interpolation points.
  * @author Varun Agrawal, Frank Dellaert
@@ -189,9 +189,9 @@ class ParameterMatrix {
    * NOTE: The size at compile time is unknown so this identity is zero
    * length and thus not valid.
    */
-  inline static ParameterMatrix identity() {
+  inline static ParameterMatrix Identity() {
     // throw std::runtime_error(
-    //     "ParameterMatrix::identity(): Don't use this function");
+    //     "ParameterMatrix::Identity(): Don't use this function");
     return ParameterMatrix(0);
   }
 

gtsam/basis/basis.i

@@ -137,7 +137,7 @@ class FitBasis {
   static gtsam::GaussianFactorGraph::shared_ptr LinearGraph(
       const std::map<double, double>& sequence,
       const gtsam::noiseModel::Base* model, size_t N);
-  This::Parameters parameters() const;
+  gtsam::This::Parameters parameters() const;
 };
 
 }  // namespace gtsam

gtsam/basis/tests/testParameterMatrix.cpp

@@ -133,7 +133,7 @@ TEST(ParameterMatrix, VectorSpace) {
   // vector
   EXPECT(assert_equal(Vector::Ones(M * N), params.vector()));
   // identity
-  EXPECT(assert_equal(ParameterMatrix<M>::identity(),
+  EXPECT(assert_equal(ParameterMatrix<M>::Identity(),
                       ParameterMatrix<M>(Matrix::Zero(M, 0))));
 }
 

gtsam/discrete/AlgebraicDecisionTree.h

@@ -31,6 +31,8 @@ namespace gtsam {
    * Algebraic Decision Trees fix the range to double
    * Just has some nice constructors and some syntactic sugar
    * TODO: consider eliminating this class altogether?
+   *
+   * @ingroup discrete
    */
   template <typename L>
   class GTSAM_EXPORT AlgebraicDecisionTree : public DecisionTree<L, double> {

gtsam/discrete/Assignment.h

@@ -11,15 +11,17 @@
 
 /**
  * @file    Assignment.h
- * @brief    An assignment from labels to a discrete value index (size_t)
+ * @brief   An assignment from labels to a discrete value index (size_t)
  * @author  Frank Dellaert
  * @date    Feb 5, 2012
  */
 
 #pragma once
 
+#include <functional>
 #include <iostream>
 #include <map>
+#include <sstream>
 #include <utility>
 #include <vector>
 
@@ -29,16 +31,34 @@ namespace gtsam {
  * An assignment from labels to value index (size_t).
  * Assigns to each label a value. Implemented as a simple map.
  * A discrete factor takes an Assignment and returns a value.
+ * @ingroup discrete
  */
 template <class L>
 class Assignment : public std::map<L, size_t> {
+  /**
+   * @brief Default method used by `labelFormatter` or `valueFormatter` when
+   * printing.
+   *
+   * @param x The value passed to format.
+   * @return std::string
+   */
+  static std::string DefaultFormatter(const L& x) {
+    std::stringstream ss;
+    ss << x;
+    return ss.str();
+  }
+
  public:
   using std::map<L, size_t>::operator=;
 
-  void print(const std::string& s = "Assignment: ") const {
+  void print(const std::string& s = "Assignment: ",
+             const std::function<std::string(L)>& labelFormatter =
+                 &DefaultFormatter) const {
     std::cout << s << ": ";
-    for (const typename Assignment::value_type& keyValue : *this)
+    for (const typename Assignment::value_type& keyValue : *this) {
-      std::cout << "(" << keyValue.first << ", " << keyValue.second << ")";
+      std::cout << "(" << labelFormatter(keyValue.first) << ", "
+                << keyValue.second << ")";
+    }
     std::cout << std::endl;
   }
 

gtsam/discrete/DecisionTree.h

@@ -38,6 +38,8 @@ namespace gtsam {
    * Decision Tree
    * L = label for variables
    * Y = function range (any algebra), e.g., bool, int, double
+   *
+   * @ingroup discrete
    */
   template<typename L, typename Y>
   class DecisionTree {

gtsam/discrete/DecisionTreeFactor.h

@@ -36,7 +36,9 @@ namespace gtsam {
   class DiscreteConditional;
 
   /**
-   * A discrete probabilistic factor
+   * A discrete probabilistic factor.
+   *
+   * @ingroup discrete
    */
   class GTSAM_EXPORT DecisionTreeFactor : public DiscreteFactor,
                                           public AlgebraicDecisionTree<Key> {

gtsam/discrete/DiscreteBayesNet.h

@@ -33,7 +33,7 @@ namespace gtsam {
 
 /** 
  * A Bayes net made from discrete conditional distributions. 
- * @addtogroup discrete
+ * @ingroup discrete
  */
 class GTSAM_EXPORT DiscreteBayesNet: public BayesNet<DiscreteConditional> {
  public:

gtsam/discrete/DiscreteBayesTree.h

@@ -62,7 +62,10 @@ class GTSAM_EXPORT DiscreteBayesTreeClique
 };
 
 /* ************************************************************************* */
-/** A Bayes tree representing a Discrete density */
+/**
+ * @brief A Bayes tree representing a Discrete density.
+ * @ingroup discrete
+ */
 class GTSAM_EXPORT DiscreteBayesTree
     : public BayesTree<DiscreteBayesTreeClique> {
  private:

gtsam/discrete/DiscreteConditional.h

@@ -32,6 +32,8 @@ namespace gtsam {
 /**
  * Discrete Conditional Density
  * Derives from DecisionTreeFactor
+ *
+ * @ingroup discrete
  */
 class GTSAM_EXPORT DiscreteConditional
     : public DecisionTreeFactor,

gtsam/discrete/DiscreteDistribution.h

@@ -27,6 +27,8 @@ namespace gtsam {
 /**
  * A prior probability on a set of discrete variables.
  * Derives from DiscreteConditional
+ *
+ * @ingroup discrete
  */
 class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional {
  public:

gtsam/discrete/DiscreteEliminationTree.h

@@ -24,6 +24,10 @@
 
 namespace gtsam {
 
+  /**
+   * @brief Elimination tree for discrete factors.
+   * @ingroup discrete
+   */
   class GTSAM_EXPORT DiscreteEliminationTree :
     public EliminationTree<DiscreteBayesNet, DiscreteFactorGraph>
   {

gtsam/discrete/DiscreteFactor.h

@@ -31,6 +31,8 @@ class DiscreteConditional;
 /**
  * Base class for discrete probabilistic factors
  * The most general one is the derived DecisionTreeFactor
+ *
+ * @ingroup discrete
  */
 class GTSAM_EXPORT DiscreteFactor: public Factor {
 

gtsam/discrete/DiscreteFactorGraph.h

@@ -40,7 +40,14 @@ class DiscreteEliminationTree;
 class DiscreteBayesTree;
 class DiscreteJunctionTree;
 
-/** Main elimination function for DiscreteFactorGraph */
+/**
+ * @brief Main elimination function for DiscreteFactorGraph.
+ * 
+ * @param factors 
+ * @param keys 
+ * @return GTSAM_EXPORT
+ * @ingroup discrete
+ */
 GTSAM_EXPORT std::pair<boost::shared_ptr<DiscreteConditional>, DecisionTreeFactor::shared_ptr>
 EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& keys);
 
@@ -64,6 +71,7 @@ template<> struct EliminationTraits<DiscreteFactorGraph>
 /**
  * A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
  *   Factor == DiscreteFactor
+ * @ingroup discrete
  */
 class GTSAM_EXPORT DiscreteFactorGraph
     : public FactorGraph<DiscreteFactor>,

gtsam/discrete/DiscreteJunctionTree.h

@@ -44,7 +44,8 @@ namespace gtsam {
    * The tree structure and elimination method are exactly analogous to the EliminationTree,
    * except that in the JunctionTree, at each node multiple variables are eliminated at a time.
    *
-   * \addtogroup Multifrontal
+   * \ingroup Multifrontal
+   * @ingroup discrete
    * \nosubgrouping
    */
   class GTSAM_EXPORT DiscreteJunctionTree :

gtsam/discrete/DiscreteKey.cpp

@@ -48,4 +48,25 @@ namespace gtsam {
     return keys & key2;
   }
 
+  void DiscreteKeys::print(const std::string& s,
+                           const KeyFormatter& keyFormatter) const {
+    for (auto&& dkey : *this) {
+      std::cout << DefaultKeyFormatter(dkey.first) << " " << dkey.second
+                << std::endl;
+    }
+  }
+
+  bool DiscreteKeys::equals(const DiscreteKeys& other, double tol) const {
+    if (this->size() != other.size()) {
+      return false;
+    }
+
+    for (size_t i = 0; i < this->size(); i++) {
+      if (this->at(i).first != other.at(i).first ||
+          this->at(i).second != other.at(i).second) {
+        return false;
+      }
+    }
+    return true;
+  }
 }

gtsam/discrete/DiscreteKey.h

@@ -21,6 +21,7 @@
 #include <gtsam/global_includes.h>
 #include <gtsam/inference/Key.h>
 
+#include <boost/serialization/vector.hpp>
 #include <map>
 #include <string>
 #include <vector>
@@ -30,6 +31,7 @@ namespace gtsam {
   /**
    * Key type for discrete variables.
    * Includes Key and cardinality.
+   * @ingroup discrete
    */
   using DiscreteKey = std::pair<Key,size_t>;
 
@@ -69,8 +71,30 @@ namespace gtsam {
       push_back(key);
       return *this;
     }
+
+    /// Print the keys and cardinalities.
+    void print(const std::string& s = "",
+               const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+
+    /// Check equality to another DiscreteKeys object.
+    bool equals(const DiscreteKeys& other, double tol = 0) const;
+
+    /** Serialization function */
+    friend class boost::serialization::access;
+    template <class ARCHIVE>
+    void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+      ar& boost::serialization::make_nvp(
+          "DiscreteKeys",
+          boost::serialization::base_object<std::vector<DiscreteKey>>(*this));
+    }
+
   }; // DiscreteKeys
 
   /// Create a list from two keys
   GTSAM_EXPORT DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2);
-}
+
+  // traits
+  template <>
+  struct traits<DiscreteKeys> : public Testable<DiscreteKeys> {};
+
+  }  // namespace gtsam

gtsam/discrete/DiscreteLookupDAG.h

@@ -32,6 +32,7 @@ class DiscreteBayesNet;
 
 /**
  * @brief DiscreteLookupTable table for max-product
+ * @ingroup discrete
  *
  * Inherits from discrete conditional for convenience, but is not normalized.
  * Is used in the max-product algorithm.

gtsam/discrete/DiscreteMarginals.h

@@ -28,6 +28,7 @@ namespace gtsam {
 
   /**
    * A class for computing marginals of variables in a DiscreteFactorGraph
+   * @ingroup discrete
    */
 class DiscreteMarginals {
 

gtsam/discrete/DiscreteValues.h

@@ -36,6 +36,7 @@ namespace gtsam {
  * Another good thing is we don't need to have the special DiscreteKey which
  * stores cardinality of a Discrete variable. It should be handled naturally in
  * the new class DiscreteValue, as the variable's type (domain)
+ * @ingroup discrete
  */
 class GTSAM_EXPORT DiscreteValues : public Assignment<Key> {
  public:

gtsam/discrete/Signature.h

@@ -48,6 +48,8 @@ namespace gtsam {
    *   (E|T,L) = "F F F 1"
    *   X|E = "95/5 2/98"
    *   (D|E,B) = "9/1 2/8 3/7 1/9"
+   *
+   * @ingroup discrete
    */
   class GTSAM_EXPORT Signature {
 

gtsam/discrete/discrete.i

@@ -219,6 +219,16 @@ class DiscreteBayesTree {
 };
 
 #include <gtsam/discrete/DiscreteLookupDAG.h>
+
+class DiscreteLookupTable : gtsam::DiscreteConditional{
+  DiscreteLookupTable(size_t nFrontals, const gtsam::DiscreteKeys& keys,
+                      const gtsam::DecisionTreeFactor::ADT& potentials);
+  void print(
+    const std::string& s = "Discrete Lookup Table: ",
+    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
+  size_t argmax(const gtsam::DiscreteValues& parentsValues) const;
+};
+
 class DiscreteLookupDAG {
   DiscreteLookupDAG();
   void push_back(const gtsam::DiscreteLookupTable* table);

gtsam/discrete/tests/testDiscreteBayesTree.cpp

@@ -159,6 +159,10 @@ TEST(DiscreteBayesTree, ThinTree) {
       clique->separatorMarginal(EliminateDiscrete);
   DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9);
 
+  DOUBLES_EQUAL(joint_12_14, 0.1875, 1e-9);
+  DOUBLES_EQUAL(joint_8_12_14, 0.0375, 1e-9);
+  DOUBLES_EQUAL(joint_9_12_14, 0.15, 1e-9);
+
   // check separator marginal P(S9), should be P(14)
   clique = (*self.bayesTree)[9];
   DiscreteFactorGraph separatorMarginal9 =

gtsam/discrete/tests/testDiscreteFactor.cpp

@@ -16,14 +16,29 @@
  *  @author Duy-Nguyen Ta
  */
 
-#include <gtsam/base/Testable.h>
-#include <gtsam/discrete/DiscreteFactor.h>
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/serializationTestHelpers.h>
+#include <gtsam/discrete/DiscreteFactor.h>
+
 #include <boost/assign/std/map.hpp>
 using namespace boost::assign;
 
 using namespace std;
 using namespace gtsam;
+using namespace gtsam::serializationTestHelpers;
+
+/* ************************************************************************* */
+TEST(DisreteKeys, Serialization) {
+  DiscreteKeys keys;
+  keys& DiscreteKey(0, 2);
+  keys& DiscreteKey(1, 3);
+  keys& DiscreteKey(2, 4);
+
+  EXPECT(equalsObj<DiscreteKeys>(keys));
+  EXPECT(equalsXML<DiscreteKeys>(keys));
+  EXPECT(equalsBinary<DiscreteKeys>(keys));
+}
 
 /* ************************************************************************* */
 int main() {
@@ -31,4 +46,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-

gtsam/geometry/Cal3.h

@@ -16,7 +16,7 @@
  */
 
 /**
- * @addtogroup geometry
+ * @ingroup geometry
  */
 
 #pragma once
@@ -63,7 +63,7 @@ void calibrateJacobians(const Cal& calibration, const Point2& pn,
 
 /**
  * @brief Common base class for all calibration models.
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3 {

gtsam/geometry/Cal3Bundler.h

@@ -26,7 +26,7 @@ namespace gtsam {
 
 /**
  * @brief Calibration used by Bundler
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3Bundler : public Cal3 {

gtsam/geometry/Cal3DS2.h

@@ -29,7 +29,7 @@ namespace gtsam {
  * @brief Calibration of a camera with radial distortion that also supports
  * Lie-group behaviors for optimization.
  * \sa Cal3DS2_Base
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {

gtsam/geometry/Cal3DS2_Base.h

@@ -27,7 +27,7 @@ namespace gtsam {
 
 /**
  * @brief Calibration of a camera with radial distortion
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  *
  * Uses same distortionmodel as OpenCV, with

gtsam/geometry/Cal3Fisheye.h

@@ -30,7 +30,7 @@ namespace gtsam {
 
 /**
  * @brief Calibration of a fisheye camera
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  *
  * Uses same distortionmodel as OpenCV, with

gtsam/geometry/Cal3Unified.h

@@ -18,7 +18,7 @@
  */
 
 /**
- * @addtogroup geometry
+ * @ingroup geometry
  */
 
 #pragma once
@@ -30,7 +30,7 @@ namespace gtsam {
 /**
  * @brief Calibration of a omni-directional camera with mirror + lens radial
  * distortion
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  *
  * Similar to Cal3DS2, does distortion but has additional mirror parameter xi

gtsam/geometry/Cal3_S2.h

@@ -16,7 +16,7 @@
  */
 
 /**
- * @addtogroup geometry
+ * @ingroup geometry
  */
 
 #pragma once
@@ -28,7 +28,7 @@ namespace gtsam {
 
 /**
  * @brief The most common 5DOF 3D->2D calibration
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3_S2 : public Cal3 {

gtsam/geometry/Cal3_S2Stereo.h

@@ -24,7 +24,7 @@ namespace gtsam {
 
 /**
  * @brief The most common 5DOF 3D->2D calibration, stereo version
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
@@ -38,7 +38,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
   using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
 
   /// @name Standard Constructors
-  /// @
+  /// @{
 
   /// default calibration leaves coordinates unchanged
   Cal3_S2Stereo() = default;
@@ -55,6 +55,8 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
   Cal3_S2Stereo(double fov, int w, int h, double b)
       : Cal3_S2(fov, w, h), b_(b) {}
 
+  /// @}
+
   /**
    * Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
    * @param p point in intrinsic coordinates
@@ -82,7 +84,6 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
    */
   Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); }
 
-  /// @}
   /// @name Testable
   /// @{
 

gtsam/geometry/CalibratedCamera.h

@@ -46,7 +46,7 @@ private:
 
 /**
  * A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT PinholeBase {
@@ -241,7 +241,7 @@ private:
  * A Calibrated camera class [R|-R't], calibration K=I.
  * If calibration is known, it is more computationally efficient
  * to calibrate the measurements rather than try to predict in pixels.
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT CalibratedCamera: public PinholeBase {

gtsam/geometry/Cyclic.h

@@ -38,7 +38,7 @@ public:
   /// Default constructor yields identity
   Cyclic():i_(0) {
   }
-  static Cyclic identity() { return Cyclic();}
+  static Cyclic Identity() { return Cyclic();}
 
   /// Cast to size_t
   operator size_t() const {

gtsam/geometry/Line3.h

@@ -38,7 +38,7 @@ GTSAM_EXPORT Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
 
 /**
  * A 3D line (R,a,b) : (Rot3,Scalar,Scalar)
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT Line3 {

gtsam/geometry/OrientedPlane3.h

@@ -133,8 +133,6 @@ public:
   inline double distance() const {
     return d_;
   }
-
-  /// @}
 };
 
 template<> struct traits<OrientedPlane3> : public internal::Manifold<

gtsam/geometry/PinholeCamera.h

@@ -26,7 +26,7 @@ namespace gtsam {
 /**
  * A pinhole camera class that has a Pose3 and a Calibration.
  * Use PinholePose if you will not be optimizing for Calibration
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 template<typename Calibration>
@@ -213,7 +213,7 @@ public:
   }
 
   /// for Canonical
-  static PinholeCamera identity() {
+  static PinholeCamera Identity() {
     return PinholeCamera(); // assumes that the default constructor is valid
   }
 

gtsam/geometry/PinholePose.h

@@ -27,7 +27,7 @@ namespace gtsam {
 
 /**
  * A pinhole camera class that has a Pose3 and a *fixed* Calibration.
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 template<typename CALIBRATION>
@@ -236,7 +236,7 @@ public:
  * A pinhole camera class that has a Pose3 and a *fixed* Calibration.
  * Instead of using this class, one might consider calibrating the measurements
  * and using CalibratedCamera, which would then be faster.
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 template<typename CALIBRATION>
@@ -412,7 +412,7 @@ public:
   }
 
   /// for Canonical
-  static PinholePose identity() {
+  static PinholePose Identity() {
     return PinholePose(); // assumes that the default constructor is valid
   }
 

gtsam/geometry/Pose2.h

@@ -30,7 +30,7 @@ namespace gtsam {
 
 /**
  * A 2D pose (Point2,Rot2)
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class Pose2: public LieGroup<Pose2, 3> {
@@ -119,7 +119,7 @@ public:
   /// @{
 
   /// identity for group operation
-  inline static Pose2 identity() { return Pose2(); }
+  inline static Pose2 Identity() { return Pose2(); }
 
   /// inverse
   GTSAM_EXPORT Pose2 inverse() const;

gtsam/geometry/Pose3.cpp

@@ -27,7 +27,6 @@
 namespace gtsam {
 
 using std::vector;
-using Point3Pairs = vector<Point3Pair>;
 
 /** instantiate concept checks */
 GTSAM_CONCEPT_POSE_INST(Pose3)

gtsam/geometry/Pose3.h

@@ -31,7 +31,7 @@ class Pose2;
 
 /**
  * A 3D pose (R,t) : (Rot3,Point3)
- * @addtogroup geometry
+ * @ingroup geometry
  * \nosubgrouping
  */
 class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
@@ -83,7 +83,7 @@ public:
    *  A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point
    *  Note this allows for noise on the points but in that case the mapping will not be exact.
    */
-  static boost::optional<Pose3> Align(const std::vector<Point3Pair>& abPointPairs);
+  static boost::optional<Pose3> Align(const Point3Pairs& abPointPairs);
 
   // Version of Pose3::Align that takes 2 matrices.
   static boost::optional<Pose3> Align(const Matrix& a, const Matrix& b);
@@ -103,7 +103,7 @@ public:
   /// @{
 
   /// identity for group operation
-  static Pose3 identity() {
+  static Pose3 Identity() {
     return Pose3();
   }
 

gtsam/geometry/Rot2.h

@@ -30,7 +30,7 @@ namespace gtsam {
   /**
    * Rotation matrix
    * NOTE: the angle theta is in radians unless explicitly stated
-   * @addtogroup geometry
+   * @ingroup geometry
    * \nosubgrouping
    */
   class GTSAM_EXPORT Rot2 : public LieGroup<Rot2, 1> {
@@ -86,7 +86,7 @@ namespace gtsam {
     static Rot2 atan2(double y, double x);
 
     /**
-     * Random, generates random angle \in [-p,pi]
+     * Random, generates random angle \f$\in\f$ [-pi,pi]
      * Example:
      *   std::mt19937 engine(42);
      *   Unit3 unit = Unit3::Random(engine);
@@ -107,8 +107,8 @@ namespace gtsam {
     /// @name Group
     /// @{
 
-    /** identity */
+    /** Identity */
-    inline static Rot2 identity() {  return Rot2(); }
+    inline static Rot2 Identity() {  return Rot2(); }
 
     /** The inverse rotation - negative angle */
     Rot2 inverse() const { return Rot2(c_, -s_);}

gtsam/geometry/Rot3.h

@@ -53,7 +53,7 @@ namespace gtsam {
  * @brief Rot3 is a 3D rotation represented as a rotation matrix if the
  * preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion
  * if it is defined.
- * @addtogroup geometry
+ * @ingroup geometry
  */
 class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
  private:
@@ -129,7 +129,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
     Rot3(double w, double x, double y, double z) : Rot3(Quaternion(w, x, y, z)) {}
 
     /**
-     * Random, generates a random axis, then random angle \in [-p,pi]
+     * Random, generates a random axis, then random angle \f$\in\f$ [-pi,pi]
      * Example:
      *   std::mt19937 engine(42);
      *   Unit3 unit = Unit3::Random(engine);
@@ -297,7 +297,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
     /// @{
 
     /// identity rotation for group operation
-    inline static Rot3 identity() {
+    inline static Rot3 Identity() {
       return Rot3();
     }
 

gtsam/geometry/SO4.h

@@ -74,7 +74,7 @@ GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::non
 
 /**
  * Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q)
- * -> S \in St(3,4).
+ * -> \f$ S \in St(3,4) \f$.
  */
 GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none);
 

gtsam/geometry/SOn.h

@@ -121,7 +121,8 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   /// currently only defined for SO3.
   static SO ClosestTo(const MatrixNN& M);
 
-  /// Named constructor that finds chordal mean = argmin_R \sum sqr(|R-R_i|_F),
+  /// Named constructor that finds chordal mean
+  /// \f$ mu = argmin_R \sum sqr(|R-R_i|_F) \f$,
   /// currently only defined for SO3.
   static SO ChordalMean(const std::vector<SO>& rotations);
 
@@ -178,13 +179,13 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
 
   /// SO<N> identity for N >= 2
   template <int N_ = N, typename = IsFixed<N_>>
-  static SO identity() {
+  static SO Identity() {
     return SO();
   }
 
   /// SO<N> identity for N == Eigen::Dynamic
   template <int N_ = N, typename = IsDynamic<N_>>
-  static SO identity(size_t n = 0) {
+  static SO Identity(size_t n = 0) {
     return SO(n);
   }
 

gtsam/geometry/Similarity2.cpp

@@ -134,7 +134,7 @@ void Similarity2::print(const std::string& s) const {
             << std::endl;
 }
 
-Similarity2 Similarity2::identity() { return Similarity2(); }
+Similarity2 Similarity2::Identity() { return Similarity2(); }
 
 Similarity2 Similarity2::operator*(const Similarity2& S) const {
   return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);

gtsam/geometry/Similarity2.h

@@ -83,7 +83,7 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
   /// @{
 
   /// Return an identity transform
-  static Similarity2 identity();
+  static Similarity2 Identity();
 
   /// Composition
   Similarity2 operator*(const Similarity2& S) const;
@@ -132,7 +132,7 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
    * using the algorithm described here:
    * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
    */
-  static Similarity2 Align(const std::vector<Pose2Pair>& abPosePairs);
+  static Similarity2 Align(const Pose2Pairs& abPosePairs);
 
   /// @}
   /// @name Lie Group

gtsam/geometry/Similarity3.cpp

@@ -122,7 +122,7 @@ void Similarity3::print(const std::string& s) const {
   std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl;
 }
 
-Similarity3 Similarity3::identity() {
+Similarity3 Similarity3::Identity() {
   return Similarity3();
 }
 Similarity3 Similarity3::operator*(const Similarity3& S) const {

gtsam/geometry/Similarity3.h

@@ -84,7 +84,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
   /// @{
 
   /// Return an identity transform
-  static Similarity3 identity();
+  static Similarity3 Identity();
 
   /// Composition
   Similarity3 operator*(const Similarity3& S) const;
@@ -120,7 +120,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
   /**
    *  Create Similarity3 by aligning at least three point pairs
    */
-  static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
+  static Similarity3 Align(const Point3Pairs& abPointPairs);
 
   /**
    * Create the Similarity3 object that aligns at least two pose pairs.