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

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

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

10
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/slam
gtsam_unstable/dynamics
gtsam_unstable/nonlinear
gtsam_unstable/partition
gtsam_unstable/slam
gtsam_unstable
)

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

872
doc/ImuFactor.lyx
View File

File diff suppressed because it is too large Load Diff

BIN
doc/ImuFactor.pdf
View File

Binary file not shown.

BIN
doc/PreintegratedIMUJacobians.pdf Normal file
View File

Binary file not shown.

88
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},
journal = {Acta Numerica 2000},
volume = {9},
pages = {215--365},
year = {2000},
publisher = {Cambridge Univ Press}
}
author = {Iserles, Arieh and Munthe-Kaas, Hans Z and N{\o}rsett, Syvert P and Zanna, Antonella},
journal = {Acta Numerica 2000},
pages = {215--365},
publisher = {Cambridge Univ Press},
title = {Lie-group methods},
volume = {9},
year = {2000}}
@book{Murray94book,
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}
}
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},
year = {1994}}
@book{Spivak65book,
title = {Calculus on manifolds},
author = {Spivak, Michael},
volume = {1},
year = {1965},
publisher = {WA Benjamin New York}
}
author = {Spivak, Michael},
publisher = {WA Benjamin New York},
title = {Calculus on manifolds},
volume = {1},
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}")

19
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",
po::value<string>()->default_value("imuFactorExampleResults.csv"),
"path to the result file to use")("use_isam", po::bool_switch(),
"use ISAM as the optimizer");
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") // 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> {

28
gtsam/discrete/Assignment.h
View File

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