12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1337 from borglab/release/4.2a8

release/4.3a0
Frank Dellaert 2022-11-30 08:28:12 -08:00 committed by GitHub
parent 77008c056e f9a5313ff8
commit 9902ccc0a4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
572 changed files with 26764 additions and 12767 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
.github/scripts/python.sh vendored
View File

@ -43,8 +43,10 @@ if [ -z ${PYTHON_VERSION+x} ]; then
exit 127
fi
PYTHON="python${PYTHON_VERSION}"
export PYTHON="python${PYTHON_VERSION}"
function install_dependencies()
{
if [[ $(uname) == "Darwin" ]]; then
brew install wget
else
@ -52,18 +54,20 @@ else
sudo apt-get install -y wget libicu-dev python3-pip python3-setuptools
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
$PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
}
BUILD_PYBIND="ON"
sudo $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} \
@ -84,5 +88,23 @@ make -j2 install
cd $GITHUB_WORKSPACE/build/python
$PYTHON -m pip install --user .
}
function test()
{
cd $GITHUB_WORKSPACE/python/gtsam/tests
$PYTHON -m unittest discover -v
}
# select between build or test
case $1 in
-d)
install_dependencies
;;
-b)
build
;;
-t)
test
;;
esac

24
.github/workflows/build-linux.yml vendored
View File

@ -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-18.04-gcc-9,
ubuntu-18.04-clang-9,
ubuntu-20.04-gcc-7,
ubuntu-20.04-gcc-9,
ubuntu-20.04-clang-9,
]
build_type: [Debug, Release]
build_unstable: [ON]
include:
- name: ubuntu-18.04-gcc-5
os: ubuntu-18.04
- name: ubuntu-20.04-gcc-7
os: ubuntu-20.04
compiler: gcc
version: "5"
version: "7"
- name: ubuntu-18.04-gcc-9
os: ubuntu-18.04
- name: ubuntu-20.04-gcc-9
os: ubuntu-20.04
compiler: gcc
version: "9"
- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
- name: ubuntu-20.04-clang-9
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

10
.github/workflows/build-macos.yml vendored
View File

@ -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
os: macOS-10.15
- name: macos-11-xcode-13.4.1
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

62
.github/workflows/build-python.yml vendored
View File

@ -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-18.04-gcc-9,
ubuntu-18.04-clang-9,
macOS-10.15-xcode-11.3.1,
ubuntu-18.04-gcc-5-tbb,
ubuntu-20.04-gcc-7,
ubuntu-20.04-gcc-9,
ubuntu-20.04-clang-9,
macOS-11-xcode-13.4.1,
ubuntu-20.04-gcc-7-tbb,
]
build_type: [Debug, Release]
python_version: [3]
include:
- name: ubuntu-18.04-gcc-5
os: ubuntu-18.04
- name: ubuntu-20.04-gcc-7
os: ubuntu-20.04
compiler: gcc
version: "5"
version: "7"
- name: ubuntu-18.04-gcc-9
os: ubuntu-18.04
- name: ubuntu-20.04-gcc-9
os: ubuntu-20.04
compiler: gcc
version: "9"
- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
- name: ubuntu-20.04-clang-9
os: ubuntu-20.04
compiler: clang
version: "9"
# NOTE temporarily added this as it is a required check.
- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
- name: ubuntu-20.04-clang-9
os: ubuntu-20.04
compiler: clang
version: "9"
build_type: Debug
python_version: "3"
- name: macOS-10.15-xcode-11.3.1
os: macOS-10.15
- name: macOS-11-xcode-13.4.1
os: macOS-11
compiler: xcode
version: "11.3.1"
version: "13.4.1"
- name: ubuntu-18.04-gcc-5-tbb
os: ubuntu-18.04
- name: ubuntu-20.04-gcc-7-tbb
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
@ -112,11 +112,17 @@ jobs:
run: |
echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV
echo "GTSAM Uses TBB"
- name: Build (Linux)
- name: Set Swap Space
if: runner.os == 'Linux'
uses: pierotofy/set-swap-space@master
with:
swap-size-gb: 6
- name: Install Dependencies
run: |
bash .github/scripts/python.sh
- name: Build (macOS)
if: runner.os == 'macOS'
bash .github/scripts/python.sh -d
- name: Build
run: |
bash .github/scripts/python.sh
bash .github/scripts/python.sh -b
- name: Test
run: |
bash .github/scripts/python.sh -t

14
.github/workflows/build-special.yml vendored
View File

@ -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

21
.github/workflows/trigger-packaging.yml vendored Normal file
View File

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

4
CMakeLists.txt
View File

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

2
INSTALL.md
View File

@ -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,

25
README.md
View File

@ -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:
```
@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.

4
cmake/Config.cmake.in
View File

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

2347
cmake/FindBoost.cmake
View File

File diff suppressed because it is too large Load Diff

81
cmake/FindEigen3.cmake
View File

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

2
cmake/GtsamBuildTypes.cmake
View File

@ -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:

29
cmake/HandleEigen.cmake
View File

@ -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>
set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
# The actual include directory (for BUILD cmake target interface):
# 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:

1
cmake/HandlePrintConfiguration.cmake
View File

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

6
doc/CMakeLists.txt
View File

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

2729
doc/Doxyfile.in
View File

File diff suppressed because it is too large Load Diff

1
doc/DoxygenLayout.xml
View File

@ -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>

850
doc/ImuFactor.lyx
View File

@ -1,7 +1,9 @@
#LyX 2.0 created this file. For more info see http://www.lyx.org/
\lyxformat 413
#LyX 2.3 created this file. For more info see http://www.lyx.org/
\lyxformat 544
\begin_document
\begin_header
\save_transient_properties true
\origin unavailable
\textclass article
\use_default_options true
\maintain_unincluded_children false
@ -9,16 +11,18 @@
\language_package default
\inputencoding auto
\fontencoding global
\font_roman default
\font_sans default
\font_typewriter default
\font_roman "default" "default"
\font_sans "default" "default"
\font_typewriter "default" "default"
\font_math "auto" "auto"
\font_default_family default
\use_non_tex_fonts false
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100
\font_sf_scale 100 100
\font_tt_scale 100 100
\use_microtype false
\use_dash_ligatures true
\graphics default
\default_output_format default
\output_sync 0
@ -29,16 +33,26 @@
\use_hyperref false
\papersize default
\use_geometry true
\use_amsmath 1
\use_esint 1
\use_mhchem 1
\use_mathdots 1
\use_package amsmath 1
\use_package amssymb 1
\use_package cancel 1
\use_package esint 1
\use_package mathdots 1
\use_package mathtools 1
\use_package mhchem 1
\use_package stackrel 1
\use_package stmaryrd 1
\use_package undertilde 1
\cite_engine basic
\cite_engine_type default
\biblio_style plain
\use_bibtopic false
\use_indices false
\paperorientation portrait
\suppress_date false
\justification true
\use_refstyle 1
\use_minted 0
\index Index
\shortcut idx
\color #008000
@ -51,7 +65,10 @@
\tocdepth 3
\paragraph_separation indent
\paragraph_indentation default
\quotes_language english
\is_math_indent 0
\math_numbering_side default
\quotes_style english
\dynamic_quotes 0
\papercolumns 1
\papersides 1
\paperpagestyle default
@ -65,11 +82,11 @@
\begin_body
\begin_layout Title
The new IMU Factor
The New IMU Factor
\end_layout
\begin_layout Author
Frank Dellaert
Frank Dellaert & Varun Agrawal
\end_layout
\begin_layout Standard
@ -91,6 +108,282 @@ filename "macros.lyx"
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}}
{\mathfrak{\mathbb{R}^{9}}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rninethree}{\mathfrak{\mathbb{R}^{9\times3}}}
{\mathfrak{\mathbb{R}^{9\times3}}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rninesix}{\mathfrak{\mathbb{R}^{9\times6}}}
{\mathfrak{\mathbb{R}^{9\times6}}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rninenine}{\mathfrak{\mathbb{R}^{9\times9}}}
{\mathfrak{\mathbb{R}^{9\times9}}}
\end_inset
\end_layout
\begin_layout Subsubsection*
IMU Factor
\end_layout
\begin_layout Standard
The IMU factor has 2 variants:
\end_layout
\begin_layout Enumerate
ImuFactor is a 5-way factor between the previous pose and velocity, the
current pose and velocity, and the current IMU bias.
\end_layout
\begin_layout Enumerate
ImuFactor2 is a 3-way factor between the previous NavState, the current
NavState and the current IMU bias.
\end_layout
\begin_layout Standard
Both variants take a PreintegratedMeasurements object which encodes all
the IMU measurements between the previous timestep and the current timestep.
\end_layout
\begin_layout Standard
There are also 2 variants of this class:
\end_layout
\begin_layout Enumerate
Manifold Preintegration: This version keeps track of the incremental NavState
\begin_inset Formula $\Delta X_{ij}$
\end_inset
with respect to the previous NavState, on the NavState manifold itself.
It also keeps track of the
\begin_inset Formula $\Rninesix$
\end_inset
Jacobian of
\begin_inset Formula $\Delta X_{ij}$
\end_inset
w.r.t.
the bias.
This corresponds to Forster et.
al.
\begin_inset CommandInset citation
LatexCommand cite
key "Forster15rss"
literal "false"
\end_inset
\end_layout
\begin_layout Enumerate
Tangent Preintegration: This version keeps track of the incremental NavState
in the NavState tangent space instead.
This is a
\begin_inset Formula $\Rnine$
\end_inset
vector
\emph on
preintegrated_
\emph default
.
It also keeps track of the
\begin_inset Formula $\Rninesix$
\end_inset
jacobian of the
\emph on
preintegrated_
\emph default
w.r.t.
the bias.
\end_layout
\begin_layout Standard
The main function of a factor is to calculate an error.
This is done exactly the same in both variants:
\begin_inset Formula
\begin{equation}
e(X_{i},X_{j})=X_{j}\ominus\widehat{X_{j}}\label{eq:imu-factor-error}
\end{equation}
\end_inset
where the predicted NavState
\begin_inset Formula $\widehat{X_{j}}$
\end_inset
at time
\begin_inset Formula $t_{j}$
\end_inset
is a function of the NavState
\begin_inset Formula $X_{i}$
\end_inset
at time
\begin_inset Formula $t_{i}$
\end_inset
and the preintegrated measurements
\begin_inset Formula $PIM$
\end_inset
:
\begin_inset Formula
\[
\widehat{X_{j}}=f(X_{i},PIM)
\]
\end_inset
The noise model associated with this factor is assumed to be zero-mean Gaussian
with a
\begin_inset Formula $9\times9$
\end_inset
covariance matrix
\begin_inset Formula $\Sigma_{ij}$
\end_inset
, which is defined in the tangent space
\begin_inset Formula $T_{X_{j}}\mathcal{N}$
\end_inset
of the NavState manifold at the NavState
\begin_inset Formula $X_{j}$
\end_inset
.
This (discrete-time) covariance matrix is computed in the preintegrated
measurement class, of which there are two variants as discussed above.
\end_layout
\begin_layout Subsubsection*
Combined IMU Factor
\end_layout
\begin_layout Standard
The IMU factor above requires that bias drift over time be modeled as a
separate stochastic process (using a BetweenFactor for example), a crucial
aspect given that the preintegrated measurements depend on these bias values
and are thus correlated.
For this reason, we provide another type of IMU factor which we term the
Combined IMU Factor.
This factor similarly has 2 variants:
\end_layout
\begin_layout Enumerate
CombinedImuFactor is a 6-way factor between the previous pose, velocity
and IMU bias and the current pose, velocity and IMU bias.
\end_layout
\begin_layout Enumerate
CombinedImuFactor2 is a 4-way factor between the previous NavState and IMU
bias and the current NavState and IMU bias.
\end_layout
\begin_layout Standard
Since the Combined IMU Factor has a larger state variable due to the inclusion
of IMU biases, the noise model associated with this factor is assumed to
be a zero mean Gaussian with a
\begin_inset Formula $15\times15$
\end_inset
covariance matrix
\begin_inset Formula $\Sigma$
\end_inset
, similarly defined on the tangent space of the NavState manifold.
\end_layout
\begin_layout Subsubsection*
Covariance Matrices
\end_layout
\begin_layout Standard
For IMU preintegration, it is important to propagate the uncertainty accurately
as well.
As such, we detail the various covariance matrices used in the preintegration
step.
\end_layout
\begin_layout Itemize
Gyroscope Covariance
\begin_inset Formula $Q_{\omega}$
\end_inset
: Measurement uncertainty of the gyroscope.
\end_layout
\begin_layout Itemize
Gyroscope Bias Covariance
\begin_inset Formula $Q_{\Delta b^{\omega}}$
\end_inset
: The covariance associated with the gyroscope bias random walk.
\end_layout
\begin_layout Itemize
Accelerometer Covariance
\begin_inset Formula $Q_{acc}$
\end_inset
: Measurement uncertainty of the accelerometer.
\end_layout
\begin_layout Itemize
Accelerometer Bias Covariance
\begin_inset Formula $Q_{\Delta b^{acc}}$
\end_inset
: The covariance associated with the accelerometer bias random walk.
\end_layout
\begin_layout Itemize
Integration Covariance
\begin_inset Formula $Q_{int}$
\end_inset
: This is the uncertainty due to modeling errors in the integration from
acceleration to velocity and position.
\end_layout
\begin_layout Itemize
Initial Bias Estimate Covariance
\begin_inset Formula $Q_{init}$
\end_inset
: This is the uncertainty associated with the estimation of the bias (since
we jointly estimate the bias as well).
\end_layout
\begin_layout Subsubsection*
Navigation States
\end_layout
@ -285,7 +578,15 @@ acceleration
\end_inset
in the body frame.
We know (from Murray84book) that the derivative of
We know (from
\begin_inset CommandInset citation
LatexCommand cite
key "Murray94book"
literal "false"
\end_inset
) that the derivative of
\begin_inset Formula $R$
\end_inset
@ -592,6 +893,7 @@ Lie Group Methods
\begin_inset CommandInset citation
LatexCommand cite
key "Iserles00an"
literal "true"
\end_inset
@ -707,15 +1009,6 @@ In other words, the vector field
Retractions
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}}
{\mathfrak{\mathbb{R}^{9}}}
\end_inset
\end_layout
\begin_layout Standard
Note that the use of the exponential map in local coordinate mappings is
not obligatory, even in the context of Lie groups.
@ -1000,7 +1293,15 @@ In the IMU factor, we need to predict the NavState
needs to be known in order to compensate properly for the initial velocity
and rotated gravity vector.
Hence, the idea of Lupton was to split up
Hence, the idea of Lupton
\begin_inset CommandInset citation
LatexCommand cite
key "Lupton12tro"
literal "false"
\end_inset
was to split up
\begin_inset Formula $v(t)$
\end_inset
@ -1057,7 +1358,10 @@ p_{g}(t) & = & R_{i}^{T}\frac{gt^{2}}{2}
\end_inset
The recipe for the IMU factor is then, in summary.
The recipe for the IMU factor is then, in summary:
\end_layout
\begin_layout Enumerate
Solve the ordinary differential equations
\begin_inset Formula
\begin{eqnarray*}
@ -1077,6 +1381,9 @@ starting from zero, up to time
\end_inset
at all times.
\end_layout
\begin_layout Enumerate
Form the local coordinate vector as
\begin_inset Formula
\[
@ -1085,6 +1392,10 @@ starting from zero, up to time
\end_inset
\end_layout
\begin_layout Enumerate
Predict the NavState
\begin_inset Formula $X_{j}$
\end_inset
@ -1179,10 +1490,58 @@ where we defined the rotation matrix
\end_layout
\begin_layout Subsubsection*
Noise Propagation
Noise Modeling
\end_layout
\begin_layout Standard
Given the above solutions to the differential equations, we add noise modeling
to account for the various sources of error in the system
\end_layout
\begin_layout Standard
\begin_inset Formula
\begin{eqnarray}
\theta_{k+1} & = & \theta_{k}+H(\theta_{k})^{-1}\,(\omega_{k}^{b}+\epsilon_{k}^{\omega}-b_{k}^{\omega}-\epsilon_{init}^{\omega})\Delta_{t}\nonumber \\
p_{k+1} & = & p_{k}+v_{k}\Delta_{t}+R_{k}(a_{k}^{b}+\epsilon_{k}^{a}-b_{k}^{a}-\epsilon_{init}^{a})\frac{\Delta_{t}^{2}}{2}+\epsilon_{k}^{int}\label{eq:preintegration}\\
v_{k+1} & = & v_{k}+R_{k}(a_{k}^{b}+\epsilon_{k}^{a}-b_{k}^{a}-\epsilon_{init}^{a})\Delta_{t}\nonumber \\
b_{k+1}^{a} & = & b_{k}^{a}+\epsilon_{k}^{b^{a}}\nonumber \\
b_{k+1}^{\omega} & = & b_{k}^{\omega}+\epsilon_{k}^{b^{\omega}}\nonumber
\end{eqnarray}
\end_inset
\end_layout
\begin_layout Standard
which we can write compactly as,
\end_layout
\begin_layout Standard
\begin_inset Formula
\begin{eqnarray}
\theta_{k+1} & = & f_{\theta}(\theta_{k},b_{k}^{w},\epsilon_{k}^{\omega},\epsilon_{init}^{b^{\omega}})\label{eq:compact-preintegration}\\
p_{k+1} & = & f_{p}(p_{k},v_{k},\theta_{k},b_{k}^{a},\epsilon_{k}^{a},\epsilon_{init}^{a},\epsilon_{k}^{int})\nonumber \\
v_{k+1} & = & f_{v}(v_{k,}\theta_{k,}b_{k}^{a},\epsilon_{k}^{a},\epsilon_{init}^{a})\nonumber \\
b_{k+1}^{a} & = & f_{b^{a}}(b_{k}^{a},\epsilon_{k}^{b^{a}})\nonumber \\
b_{k+1}^{\omega} & = & f_{b^{\omega}}(b_{k}^{\omega},\epsilon_{k}^{b^{\omega}})\nonumber
\end{eqnarray}
\end_inset
\end_layout
\begin_layout Subsubsection*
Noise Propagation in IMU Factor
\end_layout
\begin_layout Standard
We wish to compute the ImuFactor covariance matrix
\begin_inset Formula $\Sigma_{ij}$
\end_inset
.
Even when we assume uncorrelated noise on
\begin_inset Formula $\omega^{b}$
\end_inset
@ -1201,11 +1560,12 @@ Even when we assume uncorrelated noise on
\end_inset
appear in multiple places.
To model the noise propagation, let us define
To model the noise propagation, let us define the preintegrated navigation
state
\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k}]$
\end_inset
and rewrite Eqns.
, as a 9D vector on tangent space at and rewrite Eqns.
(
\begin_inset CommandInset ref
LatexCommand ref
@ -1239,7 +1599,7 @@ Then the noise on
propagates as
\begin_inset Formula
\begin{equation}
\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}+C_{k}\Sigma_{\eta}^{gd}C_{k}\label{eq:prop}
\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}^{T}+C_{k}\Sigma_{\eta}^{gd}C_{k}^{T}\label{eq:prop}
\end{equation}
\end_inset
@ -1280,6 +1640,42 @@ where
\begin_inset Formula $\omega^{b}$
\end_inset
.
Note that
\begin_inset Formula $\Sigma_{k},$
\end_inset
\begin_inset Formula $\Sigma_{\eta}^{ad}$
\end_inset
, and
\begin_inset Formula $\Sigma_{\eta}^{gd}$
\end_inset
are discrete time covariances with
\begin_inset Formula $\Sigma_{\eta}^{ad}$
\end_inset
, and
\begin_inset Formula $\Sigma_{\eta}^{gd}$
\end_inset
divided by
\begin_inset Formula $\Delta_{t}$
\end_inset
.
Please see the section on Covariance Discretization
\begin_inset CommandInset ref
LatexCommand vpageref
reference "subsec:Covariance-Discretization"
plural "false"
caps "false"
noprefix "false"
\end_inset
.
\end_layout
@ -1295,7 +1691,7 @@ We start with the noise propagation on
\begin_layout Standard
\begin_inset Formula
\[
\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
\]
\end_inset
@ -1307,7 +1703,7 @@ It can be shown that for small
we have
\begin_inset Formula
\[
\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}-\frac{\Delta t}{2}\Skew{\omega_{k}^{b}}
\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}
\]
\end_inset
@ -1357,9 +1753,9 @@ Putting all this together, we finally obtain
\begin_inset Formula
\[
A_{k}\approx\left[\begin{array}{ccc}
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}\\
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{3\times3}\\
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t}\\
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & & I_{3\times3}
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3}
\end{array}\right]
\]
@ -1384,9 +1780,393 @@ H(\theta_{k})^{-1}\Delta_{t}\\
\end_layout
\begin_layout Subsubsection*
Noise Propagation in Combined IMU Factor
\end_layout
\begin_layout Standard
We can similarly account for bias drift over time, as is commonly seen in
commercial grade IMUs.
\end_layout
\begin_layout Standard
We expand the state vector as
\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k},b_{k}^{a},b_{k}^{\omega}]$
\end_inset
to include the bias terms and define the augmented noise vector
\begin_inset Formula $\epsilon=[\epsilon_{k}^{\omega},\epsilon_{k}^{a},\epsilon_{k}^{b^{a}},\epsilon_{k}^{b^{\omega}},\epsilon_{k}^{int},\epsilon_{init}^{b^{a}},\epsilon_{init}^{b^{\omega}}]$
\end_inset
.
This gives the noise propagation equation as
\end_layout
\begin_layout Standard
\begin_inset Formula
\begin{equation}
\Sigma_{k+1}=F_{k}\Sigma_{k}F_{k}^{T}+G_{k}Q_{k}G_{k}^{T}\label{eq:prop-combined}
\end{equation}
\end_inset
\end_layout
\begin_layout Standard
where
\begin_inset Formula $F_{k}$
\end_inset
is the
\begin_inset Formula $15\times15$
\end_inset
derivative of
\begin_inset Formula $f$
\end_inset
wrpt this new
\begin_inset Formula $\zeta$
\end_inset
, and
\begin_inset Formula $G_{k}$
\end_inset
is the
\begin_inset Formula $15\times21$
\end_inset
matrix for first order uncertainty propagation.
\begin_inset Formula $Q_{k}$
\end_inset
defines the uncertainty of
\begin_inset Formula $\eta$
\end_inset
.
The top-left
\begin_inset Formula $9\times9$
\end_inset
of
\begin_inset Formula $F_{k}$
\end_inset
is the same as
\begin_inset Formula $A_{k}$
\end_inset
, thus we only have the jacobians wrpt the biases left to account for.
\end_layout
\begin_layout Standard
Conveniently, the jacobians of the pose and velocity wrpt the biases are
already computed in the
\emph on
ImuFactor
\emph default
derivation as matrices
\begin_inset Formula $B_{k}$
\end_inset
and
\begin_inset Formula $C_{k}$
\end_inset
, while they are identity matrices wrpt the biases themselves.
Thus, we can easily plug-in the values from the previous section to give
us the final result
\end_layout
\begin_layout Standard
\begin_inset Formula
\[
F_{k}\approx\left[\begin{array}{ccccc}
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & H(\theta_{k})^{-1}\Delta_{t}\\
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t} & R_{k}\frac{\Delta_{t}}{2}^{2} & 0_{3\times3}\\
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3} & R_{k}\Delta_{t} & 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3} & 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3}
\end{array}\right]
\]
\end_inset
\end_layout
\begin_layout Standard
Similarly for
\begin_inset Formula $Q_{k},$
\end_inset
we get
\end_layout
\begin_layout Standard
\begin_inset Formula
\[
Q_{k}=\left[\begin{array}{ccccccc}
\Sigma^{\omega}\\
& \Sigma^{a}\\
& & \Sigma^{b^{a}}\\
& & & \Sigma^{b^{\omega}}\\
& & & & \Sigma^{int}\\
& & & & & \Sigma^{init_{11}} & \Sigma^{init_{12}}\\
& & & & & \Sigma^{init_{21}} & \Sigma^{init_{22}}
\end{array}\right]
\]
\end_inset
\end_layout
\begin_layout Standard
and for
\begin_inset Formula $G_{k}$
\end_inset
we get
\end_layout
\begin_layout Standard
\begin_inset Formula
\[
G_{k}=\left[\begin{array}{ccccccc}
\deriv{\theta}{\epsilon^{\omega}} & \deriv{\theta}{\epsilon^{a}} & \deriv{\theta}{\epsilon^{b^{a}}} & \deriv{\theta}{\epsilon^{b^{\omega}}} & \deriv{\theta}{\epsilon^{int}} & \deriv{\theta}{\epsilon_{init}^{b^{a}}} & \deriv{\theta}{\epsilon_{init}^{b^{\omega}}}\\
\deriv p{\epsilon^{\omega}} & \deriv p{\epsilon^{a}} & \deriv p{\epsilon^{b^{a}}} & \deriv p{\epsilon^{b^{\omega}}} & \deriv p{\epsilon^{int}} & \deriv p{\epsilon_{init}^{b^{a}}} & \deriv p{\epsilon_{init}^{b^{\omega}}}\\
\deriv v{\epsilon^{\omega}} & \deriv v{\epsilon^{a}} & \deriv v{\epsilon^{b^{a}}} & \deriv v{\epsilon^{b^{\omega}}} & \deriv v{\epsilon^{int}} & \deriv v{\epsilon_{init}^{b^{a}}} & \deriv v{\epsilon_{init}^{b^{\omega}}}\\
\deriv{b^{a}}{\epsilon^{\omega}} & \deriv{b^{a}}{\epsilon^{a}} & \deriv{b^{a}}{\epsilon^{b^{a}}} & \deriv{b^{a}}{\epsilon^{b^{\omega}}} & \deriv{b^{a}}{\epsilon^{int}} & \deriv{b^{a}}{\epsilon_{init}^{b^{a}}} & \deriv{b^{a}}{\epsilon_{init}^{b^{\omega}}}\\
\deriv{b^{\omega}}{\epsilon^{\omega}} & \deriv{b^{\omega}}{\epsilon^{a}} & \deriv{b^{\omega}}{\epsilon^{b^{a}}} & \deriv{b^{\omega}}{\epsilon^{b^{\omega}}} & \deriv{b^{\omega}}{\epsilon^{int}} & \deriv{b^{\omega}}{\epsilon_{init}^{b^{a}}} & \deriv{b^{\omega}}{\epsilon_{init}^{b^{\omega}}}
\end{array}\right]=\left[\begin{array}{ccccccc}
\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\\
0 & \deriv p{\epsilon^{a}} & 0 & 0 & \deriv p{\epsilon^{int}} & \deriv p{\eta_{init}^{b^{a}}} & 0\\
0 & \deriv v{\epsilon^{a}} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}} & 0\\
0 & 0 & I_{3\times3} & 0 & 0 & 0 & 0\\
0 & 0 & 0 & I_{3\times3} & 0 & 0 & 0
\end{array}\right]
\]
\end_inset
\end_layout
\begin_layout Standard
We can perform the block-wise computation of
\begin_inset Formula $G_{k}Q_{k}G_{k}^{T}$
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Formula
\[
G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\\
0 & \deriv p{\epsilon^{a}} & 0 & 0 & \deriv p{\epsilon^{int}} & \deriv p{\eta_{init}^{b^{a}}} & 0\\
0 & \deriv v{\epsilon^{a}} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}} & 0\\
0 & 0 & I_{3\times3} & 0 & 0 & 0 & 0\\
0 & 0 & 0 & I_{3\times3} & 0 & 0 & 0
\end{array}\right]\left[\begin{array}{ccccccc}
\Sigma^{\omega}\\
& \Sigma^{a}\\
& & \Sigma^{b^{a}}\\
& & & \Sigma^{b^{\omega}}\\
& & & & \Sigma^{int}\\
& & & & & \Sigma^{init_{11}} & \Sigma^{init_{12}}\\
& & & & & \Sigma^{init_{21}} & \Sigma^{init_{22}}
\end{array}\right]G_{k}^{T}
\]
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Formula
\[
G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega} & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\\
0 & \deriv p{\epsilon^{a}}\Sigma^{a} & 0 & 0 & \deriv p{\epsilon^{int}}\Sigma^{int} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
0 & \deriv v{\epsilon^{a}}\Sigma^{a} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
0 & 0 & \Sigma^{b^{a}} & 0 & 0 & 0 & 0\\
0 & 0 & 0 & \Sigma^{b^{\omega}} & 0 & 0 & 0
\end{array}\right]G_{k}^{T}
\]
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Formula
\begin{multline*}
G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega} & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\\
0 & \deriv p{\epsilon^{a}}\Sigma^{a} & 0 & 0 & \deriv p{\epsilon^{int}}\Sigma^{int} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
0 & \deriv v{\epsilon^{a}}\Sigma^{a} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
0 & 0 & \Sigma^{b^{a}} & 0 & 0 & 0 & 0\\
0 & 0 & 0 & \Sigma^{b^{\omega}} & 0 & 0 & 0
\end{array}\right]\\
\left[\begin{array}{ccccc}
\deriv{\theta}{\epsilon^{\omega}}^{T} & 0 & 0 & 0 & 0\\
0 & \deriv p{\epsilon^{a}}^{T} & \deriv v{\epsilon^{a}}^{T} & 0 & 0\\
0 & 0 & 0 & I_{3\times3} & 0\\
0 & 0 & 0 & 0 & I_{3\times3}\\
0 & \deriv p{\epsilon^{int}}^{T} & 0 & 0 & 0\\
0 & \deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & 0 & 0 & 0
\end{array}\right]
\end{multline*}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Formula
\begin{multline*}
=\\
\left[\begin{array}{ccccc}
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}}^{T}+\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}^{T}\\
& +\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
0 & 0 & 0 & \Sigma^{b^{a}} & 0\\
0 & 0 & 0 & 0 & \Sigma^{b^{\omega}}
\end{array}\right]
\end{multline*}
\end_inset
\end_layout
\begin_layout Standard
which we can break into 3 matrices for clarity, representing the main diagonal
and off-diagonal elements
\end_layout
\begin_layout Standard
\begin_inset Formula
\begin{multline*}
=\\
\left[\begin{array}{ccccc}
\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}}^{T} & 0 & 0 & 0 & 0\\
0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T} & 0 & 0 & 0\\
0 & 0 & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T} & 0 & 0\\
0 & 0 & 0 & \Sigma^{b^{a}} & 0\\
0 & 0 & 0 & 0 & \Sigma^{b^{\omega}}
\end{array}\right]+\\
\left[\begin{array}{ccccc}
\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & 0 & 0 & 0\\
0 & \deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & 0 & 0 & 0\\
0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
0 & 0 & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 0
\end{array}\right]+\\
\left[\begin{array}{ccccc}
0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 0
\end{array}\right]
\end{multline*}
\end_inset
\end_layout
\begin_layout Subsubsection*
Covariance Discretization
\begin_inset CommandInset label
LatexCommand label
name "subsec:Covariance-Discretization"
\end_inset
\end_layout
\begin_layout Standard
So far, all the covariances are assumed to be continuous since the state
and measurement models are considered to be continuous-time stochastic
processes.
However, we sample measurements in a discrete-time fashion, necessitating
the need to convert the covariances to their discrete time equivalents.
\end_layout
\begin_layout Standard
The IMU is modeled as a first order Gauss-Markov process, with a measurement
noise and a process noise.
Following
\begin_inset CommandInset citation
LatexCommand cite
after "Alg. 1 Page 57"
key "Nikolic16thesis"
literal "false"
\end_inset
and
\begin_inset CommandInset citation
LatexCommand cite
after "Eqns 129-130"
key "Trawny05report_IndirectKF"
literal "false"
\end_inset
, the measurement noises
\begin_inset Formula $[\epsilon^{a},\epsilon^{\omega},\epsilon_{init}]$
\end_inset
are simply scaled by
\begin_inset Formula $\frac{1}{\Delta t}$
\end_inset
, and the process noises
\begin_inset Formula $[\epsilon^{int},\epsilon^{b^{a}},\epsilon^{b^{\omega}}]$
\end_inset
are scaled by
\begin_inset Formula $\Delta t$
\end_inset
where
\begin_inset Formula $\Delta t$
\end_inset
is the time interval between 2 consecutive samples.
For a thorough explanation of the discretization process, please refer
to
\begin_inset CommandInset citation
LatexCommand cite
after "Section 8.1"
key "Simon06book"
literal "false"
\end_inset
.
\end_layout
\begin_layout Standard
\begin_inset CommandInset bibtex
LatexCommand bibtex
btprint "btPrintCited"
bibfiles "refs"
options "plain"

BIN
doc/ImuFactor.pdf
View File

Binary file not shown.

BIN
doc/PreintegratedIMUJacobians.pdf Normal file
View File

Binary file not shown.

76
doc/refs.bib
View File

@ -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 N{\o}rsett, Syvert P and Zanna, Antonella},
journal = {Acta Numerica 2000},
volume = {9},
pages = {215--365},
year = {2000},
publisher = {Cambridge Univ Press}
}
publisher = {Cambridge Univ Press},
title = {Lie-group methods},
volume = {9},
year = {2000}}
@book{Murray94book,
author = {Murray, Richard M and Li, Zexiang and Sastry, S Shankar and Sastry, S Shankara},
publisher = {CRC press},
title = {A mathematical introduction to robotic manipulation},
author = {Murray, Richard M and Li, Zexiang and Sastry, S
Shankar and Sastry, S Shankara},
year = {1994},
publisher = {CRC press}
}
year = {1994}}
@book{Spivak65book,
title = {Calculus on manifolds},
author = {Spivak, Michael},
publisher = {WA Benjamin New York},
title = {Calculus on manifolds},
volume = {1},
year = {1965},
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}
}

2
examples/CMakeLists.txt
View File

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

17
examples/CombinedImuFactorsExample.cpp
View File

@ -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")(
"data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
"path to the CSV file with the IMU data")(
"output_filename",
desc.add_options()("help,h", "produce help message") // help message
("data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
"path to the CSV file with the IMU data") // path to the data file
("output_filename",
po::value<string>()->default_value("imuFactorExampleResults.csv"),
"path to the result file to use")("use_isam", po::bool_switch(),
"use ISAM as the optimizer");
"path to the result file to use") // filename to save results to
("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;
}

5
examples/ImuFactorsExample.cpp
View File

@ -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

1
examples/README.md
View File

@ -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

159
examples/TriangulationLOSTExample.cpp Normal file
View File

@ -0,0 +1,159 @@
/* ----------------------------------------------------------------------------
* 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 TriangulationLOSTExample.cpp
* @author Akshay Krishnan
* @brief This example runs triangulation several times using 3 different
* approaches: LOST, DLT, and DLT with optimization. It reports the covariance
* and the runtime for each approach.
*
* @date 2022-07-10
*/
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/triangulation.h>
#include <chrono>
#include <iostream>
#include <random>
using namespace std;
using namespace gtsam;
static std::mt19937 rng(42);
void PrintCovarianceStats(const Matrix& mat, const std::string& method) {
Matrix centered = mat.rowwise() - mat.colwise().mean();
Matrix cov = (centered.adjoint() * centered) / double(mat.rows() - 1);
std::cout << method << " covariance: " << std::endl;
std::cout << cov << std::endl;
std::cout << "Trace sqrt: " << sqrt(cov.trace()) << std::endl << std::endl;
}
void PrintDuration(const std::chrono::nanoseconds dur, double num_samples,
const std::string& method) {
double nanoseconds = dur.count() / num_samples;
std::cout << "Time taken by " << method << ": " << nanoseconds * 1e-3
<< std::endl;
}
void GetLargeCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
std::vector<Pose3>* poses, Point3* point,
Point2Vector* measurements) {
const double minXY = -10, maxXY = 10;
const double minZ = -20, maxZ = 0;
const int nrCameras = 500;
cameras->reserve(nrCameras);
poses->reserve(nrCameras);
measurements->reserve(nrCameras);
*point = Point3(0.0, 0.0, 10.0);
std::uniform_real_distribution<double> rand_xy(minXY, maxXY);
std::uniform_real_distribution<double> rand_z(minZ, maxZ);
Cal3_S2 identityK;
for (int i = 0; i < nrCameras; ++i) {
Point3 wti(rand_xy(rng), rand_xy(rng), rand_z(rng));
Pose3 wTi(Rot3(), wti);
poses->push_back(wTi);
cameras->emplace_back(wTi, identityK);
measurements->push_back(cameras->back().project(*point));
}
}
void GetSmallCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
std::vector<Pose3>* poses, Point3* point,
Point2Vector* measurements) {
Pose3 pose1;
Pose3 pose2(Rot3(), Point3(5., 0., -5.));
Cal3_S2 identityK;
PinholeCamera<Cal3_S2> camera1(pose1, identityK);
PinholeCamera<Cal3_S2> camera2(pose2, identityK);
*point = Point3(0, 0, 1);
cameras->push_back(camera1);
cameras->push_back(camera2);
*poses = {pose1, pose2};
*measurements = {camera1.project(*point), camera2.project(*point)};
}
Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements,
const double measurementSigma) {
std::normal_distribution<double> normal(0.0, measurementSigma);
Point2Vector noisyMeasurements;
noisyMeasurements.reserve(measurements.size());
for (const auto& p : measurements) {
noisyMeasurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng));
}
return noisyMeasurements;
}
/* ************************************************************************* */
int main(int argc, char* argv[]) {
CameraSet<PinholeCamera<Cal3_S2>> cameras;
std::vector<Pose3> poses;
Point3 landmark;
Point2Vector measurements;
GetLargeCamerasDataset(&cameras, &poses, &landmark, &measurements);
// GetSmallCamerasDataset(&cameras, &poses, &landmark, &measurements);
const double measurementSigma = 1e-2;
SharedNoiseModel measurementNoise =
noiseModel::Isotropic::Sigma(2, measurementSigma);
const long int nrTrials = 1000;
Matrix errorsDLT = Matrix::Zero(nrTrials, 3);
Matrix errorsLOST = Matrix::Zero(nrTrials, 3);
Matrix errorsDLTOpt = Matrix::Zero(nrTrials, 3);
double rank_tol = 1e-9;
boost::shared_ptr<Cal3_S2> calib = boost::make_shared<Cal3_S2>();
std::chrono::nanoseconds durationDLT;
std::chrono::nanoseconds durationDLTOpt;
std::chrono::nanoseconds durationLOST;
for (int i = 0; i < nrTrials; i++) {
Point2Vector noisyMeasurements =
AddNoiseToMeasurements(measurements, measurementSigma);
auto lostStart = std::chrono::high_resolution_clock::now();
boost::optional<Point3> estimateLOST = triangulatePoint3<Cal3_S2>(
cameras, noisyMeasurements, rank_tol, false, measurementNoise, true);
durationLOST += std::chrono::high_resolution_clock::now() - lostStart;
auto dltStart = std::chrono::high_resolution_clock::now();
boost::optional<Point3> estimateDLT = triangulatePoint3<Cal3_S2>(
cameras, noisyMeasurements, rank_tol, false, measurementNoise, false);
durationDLT += std::chrono::high_resolution_clock::now() - dltStart;
auto dltOptStart = std::chrono::high_resolution_clock::now();
boost::optional<Point3> estimateDLTOpt = triangulatePoint3<Cal3_S2>(
cameras, noisyMeasurements, rank_tol, true, measurementNoise, false);
durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart;
errorsLOST.row(i) = *estimateLOST - landmark;
errorsDLT.row(i) = *estimateDLT - landmark;
errorsDLTOpt.row(i) = *estimateDLTOpt - landmark;
}
PrintCovarianceStats(errorsLOST, "LOST");
PrintCovarianceStats(errorsDLT, "DLT");
PrintCovarianceStats(errorsDLTOpt, "DLT_OPT");
PrintDuration(durationLOST, nrTrials, "LOST");
PrintDuration(durationDLT, nrTrials, "DLT");
PrintDuration(durationDLTOpt, nrTrials, "DLT_OPT");
}

9
gtsam/CMakeLists.txt
View File

@ -10,6 +10,7 @@ set (gtsam_subdirs
inference
symbolic
discrete
hybrid
linear
nonlinear
sam
@ -116,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
$<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
$<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
)
target_link_libraries(gtsam PUBLIC Eigen3::Eigen)
# MKL include dir:
if (GTSAM_USE_EIGEN_MKL)
target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})

3
gtsam/base/ConcurrentMap.h
View File

@ -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> {
@ -113,6 +113,7 @@ private:
template<class Archive>
void load(Archive& ar, const unsigned int /*version*/)
{
this->clear();
// Load into STL container and then fill our map
FastVector<std::pair<KEY, VALUE> > map;
ar & BOOST_SERIALIZATION_NVP(map);

2
gtsam/base/DSFMap.h
View File

@ -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 {

4
gtsam/base/DSFVector.h
View File

@ -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 {

2
gtsam/base/FastList.h
View File

@ -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> {

2
gtsam/base/FastMap.h
View File

@ -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>,

2
gtsam/base/FastSet.h
View File

@ -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>,

2
gtsam/base/FastVector.h
View File

@ -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 =

8
gtsam/base/Group.h
View File

@ -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());

2
gtsam/base/Lie.h
View File

@ -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

2
gtsam/base/ProductLieGroup.h
View File

@ -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),

2
gtsam/base/SymmetricBlockMatrix.h
View File

@ -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:

2
gtsam/base/Testable.h
View File

@ -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>

2
gtsam/base/ThreadsafeException.h
View File

@ -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

1
gtsam/base/Vector.h
View File

@ -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;

4
gtsam/base/VectorSpace.h
View File

@ -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

2
gtsam/base/VerticalBlockMatrix.h
View File

@ -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:

2
gtsam/base/make_shared.h
View File

@ -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;
}

2
gtsam/base/tests/testGroup.cpp
View File

@ -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();
}

34
gtsam/base/tests/testSerializationBase.cpp
View File

@ -18,6 +18,7 @@
#include <gtsam/inference/Key.h>
#include <gtsam/base/ConcurrentMap.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/MatrixSerialization.h>
#include <gtsam/base/Vector.h>
@ -106,6 +107,39 @@ TEST (Serialization, matrix_vector) {
EXPECT(equalityBinary<Matrix>((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished()));
}
/* ************************************************************************* */
TEST (Serialization, ConcurrentMap) {
ConcurrentMap<int, std::string> map;
map.insert(make_pair(1, "apple"));
map.insert(make_pair(2, "banana"));
std::string binaryPath = "saved_map.dat";
try {
std::ofstream outputStream(binaryPath);
boost::archive::binary_oarchive outputArchive(outputStream);
outputArchive << map;
} catch(...) {
EXPECT(false);
}
// Verify that the existing map contents are replaced by the archive data
ConcurrentMap<int, std::string> mapFromDisk;
mapFromDisk.insert(make_pair(3, "clam"));
EXPECT(mapFromDisk.exists(3));
try {
std::ifstream ifs(binaryPath);
boost::archive::binary_iarchive inputArchive(ifs);
inputArchive >> mapFromDisk;
} catch(...) {
EXPECT(false);
}
EXPECT(mapFromDisk.exists(1));
EXPECT(mapFromDisk.exists(2));
EXPECT(!mapFromDisk.exists(3));
}
/* ************************************************************************* */

2
gtsam/base/timing.h
View File

@ -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_;

4
gtsam/base/treeTraversal-inst.h
View File

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

2
gtsam/base/types.cpp
View File

@ -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>

2
gtsam/base/types.h
View File

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

7
gtsam/basis/Basis.h
View File

@ -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:

6
gtsam/basis/BasisFactors.h
View File

@ -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

4
gtsam/basis/Chebyshev2.h
View File

@ -36,8 +36,6 @@
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/basis/Basis.h>
#include <boost/function.hpp>
namespace gtsam {
/**
@ -134,7 +132,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
* Create matrix of values at Chebyshev points given vector-valued function.
*/
template <size_t M>
static Matrix matrix(boost::function<Eigen::Matrix<double, M, 1>(double)> f,
static Matrix matrix(std::function<Eigen::Matrix<double, M, 1>(double)> f,
size_t N, double a = -1, double b = 1) {
Matrix Xmat(M, N);
for (size_t j = 0; j < N; j++) {

6
gtsam/basis/ParameterMatrix.h
View File

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

2
gtsam/basis/basis.i
View File

@ -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

2
gtsam/basis/tests/testChebyshev2.cpp
View File

@ -118,7 +118,7 @@ TEST(Chebyshev2, InterpolateVector) {
EXPECT(assert_equal(expected, fx(X, actualH), 1e-9));
// Check derivative
boost::function<Vector2(ParameterMatrix<2>)> f = boost::bind(
std::function<Vector2(ParameterMatrix<2>)> f = boost::bind(
&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none);
Matrix numericalH =
numericalDerivative11<Vector2, ParameterMatrix<2>, 2 * N>(f, X);

2
gtsam/basis/tests/testParameterMatrix.cpp
View File

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

2
gtsam/discrete/AlgebraicDecisionTree.h
View File

@ -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> {

26
gtsam/discrete/Assignment.h
View File

@ -18,8 +18,10 @@
#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)
std::cout << "(" << keyValue.first << ", " << keyValue.second << ")";
for (const typename Assignment::value_type& keyValue : *this) {
std::cout << "(" << labelFormatter(keyValue.first) << ", "
<< keyValue.second << ")";
}
std::cout << std::endl;
}

9
gtsam/discrete/DecisionTree-inl.h
View File

@ -39,10 +39,10 @@
#include <string>
#include <vector>
using boost::assign::operator+=;
namespace gtsam {
using boost::assign::operator+=;
/****************************************************************************/
// Node
/****************************************************************************/
@ -291,10 +291,7 @@ namespace gtsam {
}
os << "\"" << this->id() << "\" -> \"" << branch->id() << "\"";
if (B == 2) {
if (i == 0) os << " [style=dashed]";
if (i > 1) os << " [style=bold]";
}
if (B == 2 && i == 0) os << " [style=dashed]";
os << std::endl;
branch->dot(os, labelFormatter, valueFormatter, showZero);
}

6
gtsam/discrete/DecisionTree.h
View File

@ -22,7 +22,7 @@
#include <gtsam/base/types.h>
#include <gtsam/discrete/Assignment.h>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <functional>
#include <iostream>
#include <map>
@ -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 {
@ -220,7 +222,7 @@ namespace gtsam {
/// @{
/// Make virtual
virtual ~DecisionTree() {}
virtual ~DecisionTree() = default;
/// Check if tree is empty.
bool empty() const { return !root_; }

4
gtsam/discrete/DecisionTreeFactor.h
View File

@ -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> {

2
gtsam/discrete/DiscreteBayesNet.h
View File

@ -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:

5
gtsam/discrete/DiscreteBayesTree.h
View File

@ -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:

2
gtsam/discrete/DiscreteConditional.h
View File

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

2
gtsam/discrete/DiscreteDistribution.h
View File

@ -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:

4
gtsam/discrete/DiscreteEliminationTree.h
View File

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

2
gtsam/discrete/DiscreteFactor.h
View File

@ -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 {

14
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -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>,
@ -209,6 +217,10 @@ class GTSAM_EXPORT DiscreteFactorGraph
/// @}
}; // \ DiscreteFactorGraph
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
EliminateForMPE(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys);
/// traits
template <>
struct traits<DiscreteFactorGraph> : public Testable<DiscreteFactorGraph> {};

3
gtsam/discrete/DiscreteJunctionTree.h
View File

@ -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 :

21
gtsam/discrete/DiscreteKey.cpp
View File

@ -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;
}
}

26
gtsam/discrete/DiscreteKey.h
View File

@ -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

1
gtsam/discrete/DiscreteLookupDAG.h
View File

@ -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.

1
gtsam/discrete/DiscreteMarginals.h
View File

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

1
gtsam/discrete/DiscreteValues.h
View File

@ -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:

2
gtsam/discrete/Signature.h
View File

@ -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 {

10
gtsam/discrete/discrete.i
View File

@ -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);

4
gtsam/discrete/tests/testDiscreteBayesTree.cpp
View File

@ -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 =

20
gtsam/discrete/tests/testDiscreteFactor.cpp
View File

@ -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);
}
/* ************************************************************************* */

4
gtsam/geometry/Cal3.h
View File

@ -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 {

2
gtsam/geometry/Cal3Bundler.h
View File

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

2
gtsam/geometry/Cal3DS2.h
View File

@ -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 {

2
gtsam/geometry/Cal3DS2_Base.h
View File

@ -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

2
gtsam/geometry/Cal3Fisheye.h
View File

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

4
gtsam/geometry/Cal3Unified.h
View File

@ -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

4
gtsam/geometry/Cal3_S2.h
View File

@ -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 {

7
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -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
/// @{

4
gtsam/geometry/CalibratedCamera.h
View File

@ -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 {

2
gtsam/geometry/Cyclic.h
View File

@ -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 {

2
gtsam/geometry/Line3.h
View File

@ -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 {

2
gtsam/geometry/OrientedPlane3.h
View File

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

4
gtsam/geometry/PinholeCamera.h
View File

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

6
gtsam/geometry/PinholePose.h
View File

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

2
gtsam/geometry/Point3.h
View File

@ -25,6 +25,7 @@
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h>
#include <gtsam/base/VectorSerialization.h>
#include <boost/serialization/nvp.hpp>
#include <numeric>
@ -33,6 +34,7 @@ namespace gtsam {
/// As of GTSAM 4, in order to make GTSAM more lean,
/// it is now possible to just typedef Point3 to Vector3
typedef Vector3 Point3;
typedef std::vector<Point3, Eigen::aligned_allocator<Point3> > Point3Vector;
// Convenience typedef
using Point3Pair = std::pair<Point3, Point3>;

4
gtsam/geometry/Pose2.h
View File

@ -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;

6
gtsam/geometry/Pose3.cpp
View File

@ -27,7 +27,6 @@
namespace gtsam {
using std::vector;
using Point3Pairs = vector<Point3Pair>;
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3)
@ -489,6 +488,11 @@ boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
}
#endif
/* ************************************************************************* */
Pose3 Pose3::slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const {
return interpolate(*this, other, t, Hx, Hy);
}
/* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
// Both Rot3 and Point3 have ostream definitions so we use them.

14
gtsam/geometry/Pose3.h
View File

@ -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();
}
@ -379,6 +379,14 @@ public:
return std::make_pair(0, 2);
}
/**
* @brief Spherical Linear interpolation between *this and other
* @param s a value between 0 and 1.5
* @param other final point of interpolation geodesic on manifold
*/
Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none,
OptionalJacobian<6, 6> Hy = boost::none) const;
/// Output stream operator
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);

8
gtsam/geometry/Rot2.h
View File

@ -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 */
inline static Rot2 identity() { return Rot2(); }
/** Identity */
inline static Rot2 Identity() { return Rot2(); }
/** The inverse rotation - negative angle */
Rot2 inverse() const { return Rot2(c_, -s_);}

6
gtsam/geometry/Rot3.cpp
View File

@ -228,6 +228,7 @@ double Rot3::yaw(OptionalJacobian<1, 3> H) const {
}
/* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
Vector Rot3::quaternion() const {
gtsam::Quaternion q = toQuaternion();
Vector v(4);
@ -237,6 +238,7 @@ Vector Rot3::quaternion() const {
v(3) = q.z();
return v;
}
#endif
/* ************************************************************************* */
pair<Unit3, double> Rot3::axisAngle() const {
@ -292,8 +294,8 @@ pair<Matrix3, Vector3> RQ(const Matrix3& A, OptionalJacobian<3, 9> H) {
(*H)(1, 8) = yHb22 * cx;
// Next, calculate the derivate of z. We have
// c20 = a10 * cy + a11 * sx * sy + a12 * cx * sy
// c22 = a11 * cx - a12 * sx
// c10 = a10 * cy + a11 * sx * sy + a12 * cx * sy
// c11 = a11 * cx - a12 * sx
const auto c10Hx = (A(1, 1) * cx - A(1, 2) * sx) * sy;
const auto c10Hy = A(1, 2) * cx * cy + A(1, 1) * cy * sx - A(1, 0) * sy;
Vector9 c10HA = c10Hx * H->row(0) + c10Hy * H->row(1);

Some files were not shown because too many files have changed in this diff Show More