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

Merge branch 'develop' into feature/docker

release/4.3a0
Varun Agrawal 2020-07-04 20:24:58 -04:00
parent 8b66960a42 dde41ebf05
commit 54cc49d880
2979 changed files with 293024 additions and 108653 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
.github/ISSUE_TEMPLATE/bug-report.md vendored Normal file
View File

@ -0,0 +1,35 @@
---
name: "Bug Report"
about: Submit a bug report to help us improve GTSAM
---
<!--Please only submit issues/bug reports that come with enough information to reproduce them, ideally a unit test that fails, and possible ideas on what might be wrong. -->
<!-- Even better yet, fix the bug and/or documentation, add a unit test, and create a pull request! -->
<!-- This is a channel to report bugs/issues, not a support channel to help install/use/debug your own code. We'd love to help, but just don't have the bandwidth. Please post questions in the GTSAM Google group (https://groups.google.com/forum/#!forum/gtsam-users) -->
## Description
<!-- A clear description of the bug -->
## Steps to reproduce
1.
2.
<!-- If you have a code sample, unit test, error messages, stack traces, etc., please provide it here as well -->
## Expected behavior
<!-- A clear and concise description of what you expected to happen. -->
## Environment
<!-- Please provide information about your code environment, things such as OS, language of use (C++, Matlab or Python), version of dependent libraries if using a custom build etc., anything to provide us more information. -->
<!-- The more the information, the faster we can help resolve the issue -->
## Additional information
<!-- Add any other infor or context about the problem here. -->

24
.github/ISSUE_TEMPLATE/feature-request.md vendored Normal file
View File

@ -0,0 +1,24 @@
---
name: "Feature Request"
about: Submit a proposal/request for a new GTSAM feature
---
## Feature
<!-- A clear and concise description of the feature proposal -->
## Motivation
<!-- Please outline the motivation for the proposal. Is your feature request related to a problem? e.g., I'm always frustrated when [...]. If this is related to another GitHub issue, please link here too. -->
## Pitch
<!-- A clear and concise description of what you want to happen. -->
## Alternatives
<!-- A clear and concise description of any alternative solutions or features you've considered, if any. -->
## Additional context
<!-- Add any other context or screenshots about the feature request here. -->

5
.github/ISSUE_TEMPLATE/questions-help-support.md vendored Normal file
View File

@ -0,0 +1,5 @@
---
name: "Questions/Help/Support"
---
Please post questions and support requests in the [GTSAM Google group](https://groups.google.com/forum/#!forum/gtsam-users) and not on Github.

14
.github/workflows/trigger-python.yml vendored Normal file
View File

@ -0,0 +1,14 @@
# This triggers Cython builds on `gtsam-manylinux-build`
name: Trigger Python Builds
on: push
jobs:
triggerCython:
runs-on: ubuntu-latest
steps:
- name: Repository Dispatch
uses: ProfFan/repository-dispatch@master
with:
token: ${{ secrets.PYTHON_CI_REPO_ACCESS_TOKEN }}
repository: borglab/gtsam-manylinux-build
event-type: cython-wrapper
client-payload: '{"ref": "${{ github.ref }}", "sha": "${{ github.sha }}"}'

6
.gitignore vendored
View File

@ -1,4 +1,5 @@
/build*
/debug*
.idea
*.pyc
*.DS_Store
@ -7,7 +8,6 @@
/examples/Data/pose3example-rewritten.txt
*.txt.user
*.txt.user.6d59f0c
/python-build/
*.pydevproject
cython/venv
cython/gtsam.cpp
@ -17,3 +17,7 @@ cython/gtsam.so
cython/gtsam_wrapper.pxd
.vscode
.env
/.vs/
/CMakeSettings.json
# for QtCreator:
CMakeLists.txt.user*

43
.travis.python.sh Normal file
View File

@ -0,0 +1,43 @@
#!/bin/bash
set -x -e
if [ -z ${PYTHON_VERSION+x} ]; then
echo "Please provide the Python version to build against!"
exit 127
fi
PYTHON="python${PYTHON_VERSION}"
if [[ $(uname) == "Darwin" ]]; then
brew install wget
else
# Install a system package required by our library
sudo apt-get install wget libicu-dev python3-pip python3-setuptools
fi
CURRDIR=$(pwd)
sudo $PYTHON -m pip install -r ./cython/requirements.txt
mkdir $CURRDIR/build
cd $CURRDIR/build
cmake $CURRDIR -DCMAKE_BUILD_TYPE=Release \
-DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_UNSTABLE=ON \
-DGTSAM_USE_QUATERNIONS=OFF \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DGTSAM_INSTALL_CYTHON_TOOLBOX=ON \
-DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF \
-DCMAKE_INSTALL_PREFIX=$CURRDIR/../gtsam_install
make -j$(nproc) install
cd $CURRDIR/../gtsam_install/cython
sudo $PYTHON setup.py install
cd $CURRDIR/cython/gtsam/tests
$PYTHON -m unittest discover

114
.travis.sh Executable file
View File

@ -0,0 +1,114 @@
#!/bin/bash
# install TBB with _debug.so files
function install_tbb()
{
TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download
TBB_VERSION=4.4.2
TBB_DIR=tbb44_20151115oss
TBB_SAVEPATH="/tmp/tbb.tgz"
if [ "$TRAVIS_OS_NAME" == "linux" ]; then
OS_SHORT="lin"
TBB_LIB_DIR="intel64/gcc4.4"
SUDO="sudo"
elif [ "$TRAVIS_OS_NAME" == "osx" ]; then
OS_SHORT="lin"
TBB_LIB_DIR=""
SUDO=""
fi
wget "${TBB_BASEURL}/${TBB_VERSION}/${TBB_DIR}_${OS_SHORT}.tgz" -O $TBB_SAVEPATH
tar -C /tmp -xf $TBB_SAVEPATH
TBBROOT=/tmp/$TBB_DIR
# Copy the needed files to the correct places.
# This works correctly for travis builds, instead of setting path variables.
# This is what Homebrew does to install TBB on Macs
$SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/
$SUDO cp -R $TBBROOT/include/ /usr/local/include/
}
# common tasks before either build or test
function configure()
{
set -e # Make sure any error makes the script to return an error code
set -x # echo
SOURCE_DIR=`pwd`
BUILD_DIR=build
#env
git clean -fd || true
rm -fr $BUILD_DIR || true
mkdir $BUILD_DIR && cd $BUILD_DIR
install_tbb
if [ ! -z "$GCC_VERSION" ]; then
export CC=gcc-$GCC_VERSION
export CXX=g++-$GCC_VERSION
fi
# GTSAM_BUILD_WITH_MARCH_NATIVE=OFF: to avoid crashes in builder VMs
cmake $SOURCE_DIR \
-DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \
-DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
-DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DCMAKE_VERBOSE_MAKEFILE=OFF
}
# common tasks after either build or test
function finish ()
{
# Print ccache stats
ccache -s
cd $SOURCE_DIR
}
# compile the code with the intent of populating the cache
function build ()
{
export GTSAM_BUILD_EXAMPLES_ALWAYS=ON
export GTSAM_BUILD_TESTS=OFF
configure
make -j2
finish
}
# run the tests
function test ()
{
export GTSAM_BUILD_EXAMPLES_ALWAYS=OFF
export GTSAM_BUILD_TESTS=ON
configure
# Actual build:
make -j2 check
finish
}
# select between build or test
case $1 in
-b)
build
;;
-t)
test
;;
esac

141
.travis.yml Normal file
View File

@ -0,0 +1,141 @@
language: cpp
cache: ccache
sudo: required
dist: xenial
addons:
apt:
sources:
- ubuntu-toolchain-r-test
- sourceline: 'deb http://apt.llvm.org/xenial/ llvm-toolchain-xenial-9 main'
key_url: 'https://apt.llvm.org/llvm-snapshot.gpg.key'
packages:
- g++-9
- clang-9
- build-essential pkg-config
- cmake
- python3-dev libpython-dev
- python3-numpy
- libboost-all-dev
# before_install:
# - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update; fi
install:
- if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache ; fi
- if [ "$TRAVIS_OS_NAME" == "osx" ]; then export PATH="/usr/local/opt/ccache/libexec:$PATH" ; fi
# We first do the compile stage specified below, then the matrix expansion specified after.
stages:
- compile
- test
- special
env:
global:
- MAKEFLAGS="-j2"
- CCACHE_SLOPPINESS=pch_defines,time_macros
# Compile stage without building examples/tests to populate the caches.
jobs:
# -------- STAGE 1: COMPILE -----------
include:
# on Mac, GCC
- stage: compile
os: osx
compiler: gcc
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -b
- stage: compile
os: osx
compiler: gcc
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -b
# on Mac, CLANG
- stage: compile
os: osx
compiler: clang
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -b
- stage: compile
os: osx
compiler: clang
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -b
# on Linux, GCC
- stage: compile
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -b
- stage: compile
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -b
# on Linux, CLANG
- stage: compile
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -b
- stage: compile
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -b
# on Linux, with deprecated ON to make sure that path still compiles/tests
- stage: special
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON
script: bash .travis.sh -b
# on Linux, with GTSAM_WITH_TBB on to make sure GTSAM still compiles/tests
- stage: special
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=ON
script: bash .travis.sh -t
# -------- STAGE 2: TESTS -----------
# on Mac, GCC
- stage: test
os: osx
compiler: clang
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -t
- stage: test
os: osx
compiler: clang
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -t
- stage: test
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -t
- stage: test
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
script: bash .travis.sh -t
- stage: test
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release
script: bash .travis.sh -t
# on Linux, with quaternions ON to make sure that path still compiles/tests
- stage: special
os: linux
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release GTSAM_BUILD_UNSTABLE=OFF GTSAM_USE_QUATERNIONS=ON
script: bash .travis.sh -t
- stage: special
os: linux
compiler: gcc
env: PYTHON_VERSION=3
script: bash .travis.python.sh
- stage: special
os: osx
compiler: clang
env: PYTHON_VERSION=3
script: bash .travis.python.sh

313
CMakeLists.txt
View File

@ -1,6 +1,5 @@
project(GTSAM CXX C)
cmake_minimum_required(VERSION 2.6)
cmake_minimum_required(VERSION 3.0)
# new feature to Cmake Version > 2.8.12
# Mac ONLY. Define Relative Path on Mac OS
@ -11,7 +10,7 @@ endif()
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 4)
set (GTSAM_VERSION_MINOR 0)
set (GTSAM_VERSION_PATCH 0)
set (GTSAM_VERSION_PATCH 2)
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
@ -23,6 +22,7 @@ set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM
set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
include(GtsamMakeConfigFile)
include(GNUInstallDirs)
# Record the root dir for gtsam - needed during external builds, e.g., ROS
set(GTSAM_SOURCE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR})
@ -47,6 +47,17 @@ else()
set(GTSAM_UNSTABLE_AVAILABLE 0)
endif()
# ----------------------------------------------------------------------------
# Uninstall target, for "make uninstall"
# ----------------------------------------------------------------------------
configure_file(
"${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in"
"${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake"
IMMEDIATE @ONLY)
add_custom_target(uninstall
"${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
###############################################################################
# Set up options
@ -55,41 +66,56 @@ endif()
if(GTSAM_UNSTABLE_AVAILABLE)
option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON)
endif()
option(GTSAM_BUILD_STATIC_LIBRARY "Build a static gtsam library, instead of shared" OFF)
option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON)
option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF)
option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF)
option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON)
option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON)
option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON)
option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF)
option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON)
option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF)
option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typedef Point2 and Point3 to Eigen::Vector equivalents" OFF)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
if(NOT MSVC AND NOT XCODE_VERSION)
option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON)
endif()
if(NOT MSVC AND NOT XCODE_VERSION)
# Set the build type to upper case for downstream use
string(TOUPPER "${CMAKE_BUILD_TYPE}" CMAKE_BUILD_TYPE_UPPER)
# Set the GTSAM_BUILD_TAG variable.
# If build type is Release, set to blank (""), else set to the build type.
if(${CMAKE_BUILD_TYPE_UPPER} STREQUAL "RELEASE")
set(GTSAM_BUILD_TAG "") # Don't create release mode tag on installed directory
else()
set(GTSAM_BUILD_TAG "${CMAKE_BUILD_TYPE}")
endif()
endif()
# Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF)
option(GTSAM_INSTALL_CYTHON_TOOLBOX "Enable/Disable installation of Cython toolbox" OFF)
option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab/cython wrap utility (necessary for matlab/cython interface)" ON)
set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of python to build the cython wrapper for (or Default)")
# Check / set dependent variables for MATLAB wrapper
if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND NOT GTSAM_BUILD_WRAP)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX or GTSAM_INSTALL_CYTHON_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP")
endif()
if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND GTSAM_BUILD_TYPE_POSTFIXES)
set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
endif()
if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_BUILD_STATIC_LIBRARY are both enabled. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of GTSAM_BUILD_STATIC_LIBRARY.")
endif()
if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4)
message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.")
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS)
@ -111,17 +137,34 @@ set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator")
# BOOST_ROOT: path to install prefix for boost
# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT
# If using Boost shared libs, disable auto linking
if(MSVC)
# Some libraries, at least Boost Program Options, rely on this to export DLL symbols
# Disable autolinking
# By default, boost only builds static libraries on windows
set(Boost_USE_STATIC_LIBS ON) # only find static libs
# If we ever reset above on windows and, ...
# If we use Boost shared libs, disable auto linking.
# Some libraries, at least Boost Program Options, rely on this to export DLL symbols.
if(NOT Boost_USE_STATIC_LIBS)
add_definitions(-DBOOST_ALL_NO_LIB)
add_definitions(-DBOOST_ALL_DYN_LINK)
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK)
endif()
# Virtual memory range for PCH exceeded on VS2015
if(MSVC_VERSION LESS 1910) # older than VS2017
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295)
endif()
endif()
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono)
# If building DLLs in MSVC, we need to avoid EIGEN_STATIC_ASSERT()
# or explicit instantiation will generate build errors.
# See: https://bitbucket.org/gtborg/gtsam/issues/417/fail-to-build-on-msvc-2017
#
if(MSVC AND BUILD_SHARED_LIBS)
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
endif()
# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
set(BOOST_FIND_MINIMUM_VERSION 1.43)
set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS})
# Required components
if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR
@ -132,46 +175,40 @@ endif()
option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
set(GTSAM_BOOST_LIBRARIES
${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}
${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY})
Boost::serialization
Boost::system
Boost::filesystem
Boost::thread
Boost::date_time
Boost::regex
)
if (GTSAM_DISABLE_NEW_TIMERS)
message("WARNING: GTSAM timing instrumentation manually disabled")
add_definitions(-DGTSAM_DISABLE_NEW_TIMERS)
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS)
else()
if(Boost_TIMER_LIBRARY)
list(APPEND GTSAM_BOOST_LIBRARIES ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY})
list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono)
else()
list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt
message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.")
endif()
endif()
if(NOT (${Boost_VERSION} LESS 105600))
message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES -DBOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES)
endif()
###############################################################################
# Find TBB
find_package(TBB)
find_package(TBB 4.4 COMPONENTS tbb tbbmalloc)
# Set up variables if we're using TBB
if(TBB_FOUND AND GTSAM_WITH_TBB)
set(GTSAM_USE_TBB 1) # This will go into config.h
include_directories(BEFORE ${TBB_INCLUDE_DIRS})
set(GTSAM_TBB_LIBRARIES "")
if(TBB_DEBUG_LIBRARIES)
foreach(lib ${TBB_LIBRARIES})
list(APPEND GTSAM_TBB_LIBRARIES optimized "${lib}")
endforeach()
foreach(lib ${TBB_DEBUG_LIBRARIES})
list(APPEND GTSAM_TBB_LIBRARIES debug "${lib}")
endforeach()
if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020))
set(TBB_GREATER_EQUAL_2020 1)
else()
set(GTSAM_TBB_LIBRARIES ${TBB_LIBRARIES})
set(TBB_GREATER_EQUAL_2020 0)
endif()
list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${GTSAM_TBB_LIBRARIES})
# all definitions and link requisites will go via imported targets:
# tbb & tbbmalloc
list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
else()
set(GTSAM_USE_TBB 0) # This will go into config.h
endif()
@ -187,6 +224,20 @@ endif()
# Find Google perftools
find_package(GooglePerfTools)
###############################################################################
# Support ccache, if installed
if(NOT MSVC AND NOT XCODE_VERSION)
find_program(CCACHE_FOUND ccache)
if(CCACHE_FOUND)
if(GTSAM_BUILD_WITH_CCACHE)
set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache)
set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache)
else()
set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "")
set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK "")
endif()
endif(CCACHE_FOUND)
endif()
###############################################################################
# Find MKL
@ -195,7 +246,6 @@ find_package(MKL)
if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
set(GTSAM_USE_EIGEN_MKL 1) # This will go into config.h
set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL
include_directories(${MKL_INCLUDE_DIR})
list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES})
# --no-as-needed is required with gcc according to the MKL link advisor
@ -214,7 +264,7 @@ find_package(OpenMP) # do this here to generate correct message if disabled
if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL)
if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP)
set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC ${OpenMP_CXX_FLAGS})
endif()
endif()
@ -226,34 +276,78 @@ endif()
### See: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection)
### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code)
option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" OFF)
# Switch for using system Eigen or GTSAM-bundled Eigen
if(GTSAM_USE_SYSTEM_EIGEN)
find_package(Eigen3 REQUIRED)
include_directories(AFTER "${EIGEN3_INCLUDE_DIR}")
# Use generic Eigen include paths e.g. <Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}")
set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
# check if MKL is also enabled - can have one or the other, but not both!
# Note: Eigen >= v3.2.5 includes our patches
if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5))
message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL")
endif()
# Check for Eigen version which doesn't work with MKL
# See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527 for details.
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
if(EIGEN3_INCLUDE_DIR)
set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
endif()
# Add the bundled version of eigen to the include path so that it can still be included
# with #include <Eigen/Core>
include_directories(BEFORE "gtsam/3rdparty/Eigen/")
# set full path to be used by external projects
# this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in
set(GTSAM_EIGEN_INCLUDE_PREFIX "${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/")
set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/")
# The actual include directory (for BUILD cmake target interface):
set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${CMAKE_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
endif()
# Detect Eigen version:
set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h")
if (EXISTS ${EIGEN_VER_H})
file(READ "${EIGEN_VER_H}" STR_EIGEN_VERSION)
# Extract the Eigen version from the Macros.h file, lines "#define EIGEN_WORLD_VERSION XX", etc...
string(REGEX MATCH "EIGEN_WORLD_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${STR_EIGEN_VERSION}")
string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${GTSAM_EIGEN_VERSION_WORLD}")
string(REGEX MATCH "EIGEN_MAJOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${STR_EIGEN_VERSION}")
string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${GTSAM_EIGEN_VERSION_MAJOR}")
string(REGEX MATCH "EIGEN_MINOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${STR_EIGEN_VERSION}")
string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${GTSAM_EIGEN_VERSION_MINOR}")
set(GTSAM_EIGEN_VERSION "${GTSAM_EIGEN_VERSION_WORLD}.${GTSAM_EIGEN_VERSION_MAJOR}.${GTSAM_EIGEN_VERSION_MINOR}")
message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}")
else()
message(WARNING "Cannot determine Eigen version, missing file: `${EIGEN_VER_H}`")
endif ()
if (MSVC)
if (BUILD_SHARED_LIBS)
# mute eigen static assert to avoid errors in shared lib
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
endif()
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen
endif()
if (APPLE AND BUILD_SHARED_LIBS)
# Set the default install directory on macOS
set(CMAKE_INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib")
endif()
###############################################################################
@ -294,51 +388,29 @@ elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc")
list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc")
endif()
# Include boost - use 'BEFORE' so that a specific boost specified to CMake
# takes precedence over a system-installed one.
include_directories(BEFORE SYSTEM ${Boost_INCLUDE_DIR})
if(GTSAM_SUPPORT_NESTED_DISSECTION)
set(METIS_INCLUDE_DIRECTORIES
gtsam/3rdparty/metis/include
gtsam/3rdparty/metis/libmetis
gtsam/3rdparty/metis/GKlib)
else()
set(METIS_INCLUDE_DIRECTORIES)
endif()
# Add includes for source directories 'BEFORE' boost and any system include
# paths so that the compiler uses GTSAM headers in our source directory instead
# of any previously installed GTSAM headers.
include_directories(BEFORE
gtsam/3rdparty/SuiteSparse_config
gtsam/3rdparty/CCOLAMD/Include
${METIS_INCLUDE_DIRECTORIES}
${PROJECT_SOURCE_DIR}
${PROJECT_BINARY_DIR} # So we can include generated config header files
CppUnitLite)
if(MSVC)
add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS)
add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS)
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /bigobj) # Allow large object files for template-based code
endif()
# GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8)
add_definitions(-Wno-unused-local-typedefs)
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs)
endif()
endif()
# As of XCode 7, clang also complains about this
if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7.0)
add_definitions(-Wno-unused-local-typedefs)
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs)
endif()
endif()
if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS)
# This should be made PUBLIC if GTSAM_EXTRA_CONSISTENCY_CHECKS is someday used in a public .h
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS)
endif()
###############################################################################
@ -350,6 +422,10 @@ add_subdirectory(CppUnitLite)
# Build wrap
if (GTSAM_BUILD_WRAP)
add_subdirectory(wrap)
# suppress warning of cython line being too long
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation")
endif()
endif(GTSAM_BUILD_WRAP)
# Build GTSAM library
@ -367,27 +443,13 @@ add_subdirectory(timing)
# Build gtsam_unstable
if (GTSAM_BUILD_UNSTABLE)
add_subdirectory(gtsam_unstable)
endif(GTSAM_BUILD_UNSTABLE)
endif()
# Matlab toolbox
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
add_subdirectory(matlab)
endif()
# Python wrap
if (GTSAM_BUILD_PYTHON)
include(GtsamPythonWrap)
# NOTE: The automatic generation of python wrapper from the gtsampy.h interface is
# not working yet, so we're using a handwritten wrapper files on python/handwritten.
# Once the python wrapping from the interface file is working, you can _swap_ the
# comments on the next lines
# wrap_and_install_python(gtsampy.h "${GTSAM_ADDITIONAL_LIBRARIES}" "")
add_subdirectory(python)
endif()
# Cython wrap
if (GTSAM_INSTALL_CYTHON_TOOLBOX)
set(GTSAM_INSTALL_CYTHON_TOOLBOX 1)
@ -450,6 +512,9 @@ message(STATUS "==============================================================="
message(STATUS "================ Configuration Options ======================")
message(STATUS " CMAKE_CXX_COMPILER_ID type : ${CMAKE_CXX_COMPILER_ID}")
message(STATUS " CMAKE_CXX_COMPILER_VERSION : ${CMAKE_CXX_COMPILER_VERSION}")
message(STATUS " CMake version : ${CMAKE_VERSION}")
message(STATUS " CMake generator : ${CMAKE_GENERATOR}")
message(STATUS " CMake build tool : ${CMAKE_BUILD_TOOL}")
message(STATUS "Build flags ")
print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ")
print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all' ")
@ -457,22 +522,23 @@ print_config_flag(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts wit
if (DOXYGEN_FOUND)
print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ")
endif()
print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM library instead of shared")
print_config_flag(${BUILD_SHARED_LIBS} "Build shared GTSAM libraries ")
print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name ")
if(GTSAM_UNSTABLE_AVAILABLE)
print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ")
endif()
string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper)
if(NOT MSVC AND NOT XCODE_VERSION)
print_config_flag(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ")
message(STATUS " Build type : ${CMAKE_BUILD_TYPE}")
message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}")
message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}")
endif()
if(GTSAM_USE_SYSTEM_EIGEN)
message(STATUS " Use System Eigen : Yes")
else()
message(STATUS " Use System Eigen : No")
message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
endif()
print_build_options_for_target(gtsam)
message(STATUS " Use System Eigen : ${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
if(GTSAM_USE_TBB)
message(STATUS " Use Intel TBB : Yes")
elseif(TBB_FOUND)
@ -500,6 +566,21 @@ else()
endif()
message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}")
if(GTSAM_THROW_CHEIRALITY_EXCEPTION)
message(STATUS " Cheirality exceptions enabled : YES")
else()
message(STATUS " Cheirality exceptions enabled : NO")
endif()
if(NOT MSVC AND NOT XCODE_VERSION)
if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
message(STATUS " Build with ccache : Yes")
elseif(CCACHE_FOUND)
message(STATUS " Build with ccache : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
else()
message(STATUS " Build with ccache : No")
endif()
endif()
message(STATUS "Packaging flags ")
message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}")
@ -514,39 +595,27 @@ print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 al
print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")
print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")
print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
message(STATUS "Python module flags ")
if(GTSAM_PYTHON_WARNINGS)
message(STATUS " Build python module : No - dependencies missing")
else()
print_config_flag(${GTSAM_BUILD_PYTHON} "Build python module ")
endif()
if(GTSAM_BUILD_PYTHON)
message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}")
endif()
message(STATUS "Cython toolbox flags ")
print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ")
print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
if(GTSAM_INSTALL_CYTHON_TOOLBOX)
message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}")
endif()
message(STATUS "===============================================================")
# Print warnings at the end
if(GTSAM_WITH_TBB AND NOT TBB_FOUND)
message(WARNING "TBB was not found - this is ok, but note that GTSAM parallelization will be disabled. Set GTSAM_WITH_TBB to 'Off' to avoid this warning.")
message(WARNING "TBB 4.4 or newer was not found - this is ok, but note that GTSAM parallelization will be disabled. Set GTSAM_WITH_TBB to 'Off' to avoid this warning.")
endif()
if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND)
message(WARNING "MKL was not found - this is ok, but note that MKL yields better performance. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning.")
message(WARNING "MKL was not found - this is ok, but note that MKL will be disabled. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning. See INSTALL.md for notes on performance.")
endif()
if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND)
message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.")
endif()
if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS)
message(WARNING "${GTSAM_PYTHON_WARNINGS}")
message(WARNING "Your compiler does not support OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.")
endif()
# Include CPack *after* all flags

5
CppUnitLite/CMakeLists.txt
View File

@ -6,11 +6,12 @@ file(GLOB cppunitlite_src "*.cpp")
add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers})
list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
target_link_libraries(CppUnitLite PUBLIC Boost::boost) # boost/lexical_cast.h
gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure
option(GTSAM_INSTALL_CPPUNITLITE "Enable/Disable installation of CppUnitLite library" ON)
if (GTSAM_INSTALL_CPPUNITLITE)
install(FILES ${cppunitlite_headers} DESTINATION include/CppUnitLite)
install(TARGETS CppUnitLite EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
install(FILES ${cppunitlite_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/CppUnitLite)
install(TARGETS CppUnitLite EXPORT GTSAM-exports ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR})
endif(GTSAM_INSTALL_CPPUNITLITE)

19
DEVELOP
View File

@ -1,19 +0,0 @@
Information for developers
Coding Conventions:
* Classes are Uppercase, methods and functions lowerMixedCase
* We use a modified K&R Style, with 2-space tabs, inserting spaces for tabs
* Use meaningful variable names, e.g., measurement not msm
Windows:
On Windows it is necessary to explicitly export all functions from the library
which should be externally accessible. To do this, include the macro
GTSAM_EXPORT in your class or function definition.
For example:
class GTSAM_EXPORT MyClass { ... };
GTSAM_EXPORT myFunction();

19
DEVELOP.md Normal file
View File

@ -0,0 +1,19 @@
# Information for Developers
### Coding Conventions
* Classes are Uppercase, methods and functions lowerMixedCase.
* We use a modified K&R Style, with 2-space tabs, inserting spaces for tabs.
* Use meaningful variable names, e.g. `measurement` not `msm`.
### Windows
On Windows it is necessary to explicitly export all functions from the library which should be externally accessible. To do this, include the macro `GTSAM_EXPORT` in your class or function definition.
For example:
```cpp
class GTSAM_EXPORT MyClass { ... };
GTSAM_EXPORT myFunction();
```

14
GTSAM-Concepts.md
View File

@ -97,12 +97,24 @@ Note that in the Lie group case, the usual valid expressions for Retract and Loc
For Lie groups, the `exponential map` above is the most obvious mapping: it
associates straight lines in the tangent space with geodesics on the manifold
(and it's inverse, the log map). However, there are two cases in which we deviate from this:
(and it's inverse, the log map). However, there are several cases in which we deviate from this:
However, the exponential map is unnecessarily expensive for use in optimization. Hence, in GTSAM there is the option to provide a cheaper chart by means of the `ChartAtOrigin` struct in a class. This is done for *SE(2)*, *SO(3)* and *SE(3)* (see `Pose2`, `Rot3`, `Pose3`)
Most Lie groups we care about are *Matrix groups*, continuous sub-groups of *GL(n)*, the group of *n x n* invertible matrices. In this case, a lot of the derivatives calculations needed can be standardized, and this is done by the `LieGroup` superclass. You only need to provide an `AdjointMap` method.
A CRTP helper class `LieGroup` is available that can take a class and create some of the Lie group methods automatically. The class needs:
* operator* : implements group operator
* inverse: implements group inverse
* AdjointMap: maps tangent vectors according to group element
* Expmap/Logmap: exponential map and its inverse
* ChartAtOrigin: struct where you define Retract/Local at origin
To use, simply derive, but also say `using LieGroup<Class,N>::inverse` so you get an inverse with a derivative.
Finally, to create the traits automatically you can use `internal::LieGroupTraits<Class>`
Vector Space
------------

146
INSTALL
View File

@ -1,146 +0,0 @@
Quickstart
In the root library folder execute:
$] mkdir build
$] cd build
$] cmake ..
$] make check (optional, runs unit tests)
$] make install
Important Installation Notes
----------------------------
1)
GTSAM requires the following libraries to be installed on your system:
- BOOST version 1.43 or greater (install through Linux repositories or MacPorts)
- Cmake version 2.6 or higher
- Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher
Optional dependent libraries:
- If TBB is installed and detectable by CMake GTSAM will use it automatically.
Ensure that CMake prints "Use Intel TBB : Yes". To disable the use of TBB,
disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB
may be installed from the Ubuntu repositories, and for other platforms it
may be downloaded from https://www.threadingbuildingblocks.org/
Tested compilers:
- GCC 4.2-4.7
- OSX Clang 2.9-5.0
- OSX GCC 4.2
- MSVC 2010, 2012
Tested systems:
- Ubuntu 11.04 - 13.10
- MacOS 10.6 - 10.9
- Windows 7, 8, 8.1
Known issues:
- MSVC 2013 is not yet supported because it cannot build the serialization module
of Boost 1.55 (or earlier).
2)
GTSAM makes extensive use of debug assertions, and we highly recommend you work
in Debug mode while developing (enabled by default). Likewise, it is imperative
that you switch to release mode when running finished code and for timing. GTSAM
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.
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,
execute commands as follows for an out-of-source build:
$] mkdir build
$] cd build
$] cmake ..
$] make check (optional, runs unit tests)
$] make install
This will build the library and unit tests, run all of the unit tests,
and then install the library itself.
- CMake Configuration Options and Details
GTSAM has a number of options that can be configured, which is best done with
one of the following:
ccmake the curses GUI for cmake
cmake-gui a real GUI for cmake
Important Options:
CMAKE_BUILD_TYPE: We support several build configurations for GTSAM (case insensitive)
Debug (default) All error checking options on, no optimization. Use for development.
Release Optimizations turned on, no debug symbols.
Timing Adds ENABLE_TIMING flag to provide statistics on operation
Profiling Standard configuration for use during profiling
RelWithDebInfo Same as Release, but with the -g flag for debug symbols
CMAKE_INSTALL_PREFIX: The install folder. The default is typically /usr/local/
To configure to install to your home directory, you could execute:
$] cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME ..
GTSAM_TOOLBOX_INSTALL_PATH: The Matlab toolbox will be installed in a subdirectory
of this folder, called 'gtsam'.
$] cmake -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=$HOME/toolbox ..
GTSAM_BUILD_CONVENIENCE_LIBRARIES: This is a build option to allow for tests in
subfolders to be linked against convenience libraries rather than the full libgtsam.
Set with the command line as follows:
$] cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..
ON (Default) This builds convenience libraries and links tests against them. This
option is suggested for gtsam developers, as it is possible to build
and run tests without first building the rest of the library, and
speeds up compilation for a single test. The downside of this option
is that it will build the entire library again to build the full
libgtsam library, so build/install will be slower.
OFF This will build all of libgtsam before any of the tests, and then
link all of the tests at once. This option is best for users of GTSAM,
as it avoids rebuilding the entirety of gtsam an extra time.
GTSAM_BUILD_UNSTABLE: Enable build and install for libgtsam_unstable library.
Set with the command line as follows:
$] cmake -DGTSAM_BUILD_UNSTABLE:OPTION=ON ..
ON When enabled, libgtsam_unstable will be built and installed with the
same options as libgtsam. In addition, if tests are enabled, the
unit tests will be built as well. The Matlab toolbox will also
be generated if the matlab toolbox is enabled, installing into a
folder called "gtsam_unstable".
OFF (Default) If disabled, no gtsam_unstable code will be included in build or install.
Check
"make check" will build and run all of the tests. Note that the tests will only be
built when using the "check" targets, to prevent "make install" from building the tests
unnecessarily. You can also run "make timing" to build all of the timing scripts.
To run check on a particular module only, run "make check.[subfolder]", so to run
just the geometry tests, run "make check.geometry". Individual tests can be run by
appending ".run" to the name of the test, for example, to run testMatrix, run
"make testMatrix.run".
MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your
shell's PATH environment variable. mex is installed with matlab at
$MATLABROOT/bin/mex
$MATLABROOT can be found by executing the command 'matlabroot' in MATLAB
Debugging tips:
Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks
and safe containers in the standard C++ library and makes problems much easier
to find.
NOTE: The native Snow Leopard g++ compiler/library contains a bug that makes
it impossible to use _GLIBCXX_DEBUG. MacPorts g++ compilers do work with it though.
NOTE: If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against
gtsam will need to be compiled with _GLIBCXX_DEBUG as well, due to the use of
header-only Eigen.

195
INSTALL.md Normal file
View File

@ -0,0 +1,195 @@
# Quickstart
In the root library folder execute:
```sh
$ mkdir build
$ cd build
$ cmake ..
$ make check # (optional, runs unit tests)
$ make install
```
## Important Installation Notes
1. GTSAM requires the following libraries to be installed on your system:
- BOOST version 1.43 or greater (install through Linux repositories or MacPorts)
- Cmake version 3.0 or higher
- Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher
Optional dependent libraries:
- If TBB is installed and detectable by CMake GTSAM will use it automatically.
Ensure that CMake prints "Use Intel TBB : Yes". To disable the use of TBB,
disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB
may be installed from the Ubuntu repositories, and for other platforms it
may be downloaded from https://www.threadingbuildingblocks.org/
- GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and
`GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually
achieved with MKL disabled. We therefore advise you to benchmark your problem
before using MKL.
Tested compilers:
- GCC 4.2-7.3
- OS X Clang 2.9-10.0
- OS X GCC 4.2
- MSVC 2010, 2012, 2017
Tested systems:
- Ubuntu 16.04 - 18.04
- MacOS 10.6 - 10.14
- Windows 7, 8, 8.1, 10
Known issues:
- MSVC 2013 is not yet supported because it cannot build the serialization module
of Boost 1.55 (or earlier).
2. GTSAM makes extensive use of debug assertions, and we highly recommend you work
in Debug mode while developing (enabled by default). Likewise, it is imperative
that you switch to release mode when running finished code and for timing. GTSAM
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.
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,
execute commands as follows for an out-of-source build:
```sh
$ mkdir build
$ cd build
$ cmake ..
$ make check (optional, runs unit tests)
$ make install
```
This will build the library and unit tests, run all of the unit tests,
and then install the library itself.
## CMake Configuration Options and Details
GTSAM has a number of options that can be configured, which is best done with
one of the following:
- ccmake the curses GUI for cmake
- cmake-gui a real GUI for cmake
### Important Options:
#### CMAKE_BUILD_TYPE
We support several build configurations for GTSAM (case insensitive)
```cmake -DCMAKE_BUILD_TYPE=[Option] ..```
- Debug (default) All error checking options on, no optimization. Use for development.
- Release Optimizations turned on, no debug symbols.
- Timing Adds ENABLE_TIMING flag to provide statistics on operation
- Profiling Standard configuration for use during profiling
- RelWithDebInfo Same as Release, but with the -g flag for debug symbols
#### CMAKE_INSTALL_PREFIX
The install folder. The default is typically `/usr/local/`.
To configure to install to your home directory, you could execute:
```cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME ..```
#### GTSAM_TOOLBOX_INSTALL_PATH
The Matlab toolbox will be installed in a subdirectory
of this folder, called 'gtsam'.
```cmake -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=$HOME/toolbox ..```
#### GTSAM_BUILD_CONVENIENCE_LIBRARIES
This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam.
Set with the command line as follows:
```cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..```
- ON (Default): This builds convenience libraries and links tests against them. This option is suggested for gtsam developers, as it is possible to build and run tests without first building the rest of the library, and speeds up compilation for a single test. The downside of this option is that it will build the entire library again to build the full libgtsam library, so build/install will be slower.
- OFF: This will build all of libgtsam before any of the tests, and then link all of the tests at once. This option is best for users of GTSAM, as it avoids rebuilding the entirety of gtsam an extra time.
#### GTSAM_BUILD_UNSTABLE
Enable build and install for libgtsam_unstable library.
Set with the command line as follows:
```cmake -DGTSAM_BUILD_UNSTABLE:OPTION=ON ..```
ON: When enabled, libgtsam_unstable will be built and installed with the same options as libgtsam. In addition, if tests are enabled, the unit tests will be built as well. The Matlab toolbox will also be generated if the matlab toolbox is enabled, installing into a folder called `gtsam_unstable`.
OFF (Default) If disabled, no `gtsam_unstable` code will be included in build or install.
## Check
`make check` will build and run all of the tests. Note that the tests will only be
built when using the "check" targets, to prevent `make install` from building the tests
unnecessarily. You can also run `make timing` to build all of the timing scripts.
To run check on a particular module only, run `make check.[subfolder]`, so to run
just the geometry tests, run `make check.geometry`. Individual tests can be run by
appending `.run` to the name of the test, for example, to run testMatrix, run
`make testMatrix.run`.
MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your shell's PATH environment variable. mex is installed with matlab at `$MATLABROOT/bin/mex`
$MATLABROOT can be found by executing the command `matlabroot` in MATLAB
## Performance
Here are some tips to get the best possible performance out of GTSAM.
1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode.
2. Enable TBB. On modern processors with multiple cores, this can easily speed up
optimization by 30-50%. Please note that this may not be true for very small
problems where the overhead of dispatching work to multiple threads outweighs
the benefit. We recommend that you benchmark your problem with/without TBB.
3. Add `-march=native` to `GTSAM_CMAKE_CXX_FLAGS`. A performance gain of
25-30% can be expected on modern processors. Note that this affects the portability
of your executable. It may not run when copied to another system with older/different
processor architecture.
Also note that all dependent projects *must* be compiled with the same flag, or
seg-faults and other undefined behavior may result.
4. Possibly enable MKL. Please note that our benchmarks have shown that this helps only
in very limited cases, and actually hurts performance in the usual case. We therefore
recommend that you do *not* enable MKL, unless you have benchmarked it on
your problem and have verified that it improves performance.
## Debugging tips
Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks and safe containers in the standard C++ library and makes problems much easier to find.
NOTE: The native Snow Leopard g++ compiler/library contains a bug that makes it impossible to use _GLIBCXX_DEBUG. MacPorts g++ compilers do work with it though.
NOTE: If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against gtsam will need to be compiled with _GLIBCXX_DEBUG as well, due to the use of header-only Eigen.
## Installing MKL on Linux
Intel has a guide for installing MKL on Linux through APT repositories at <https://software.intel.com/en-us/articles/installing-intel-free-libs-and-python-apt-repo>.
After following the instructions, add the following to your `~/.bashrc` (and afterwards, open a new terminal before compiling GTSAM):
`LD_PRELOAD` need only be set if you are building the cython wrapper to use GTSAM from python.
```sh
source /opt/intel/mkl/bin/mklvars.sh intel64
export LD_PRELOAD="$LD_PRELOAD:/opt/intel/mkl/lib/intel64/libmkl_core.so:/opt/intel/mkl/lib/intel64/libmkl_sequential.so"
```
To use MKL in GTSAM pass the flag `-DGTSAM_WITH_EIGEN_MKL=ON` to cmake.
The `LD_PRELOAD` fix seems to be related to a well known problem with MKL which leads to lots of undefined symbol errors, for example:
- <https://software.intel.com/en-us/forums/intel-math-kernel-library/topic/300857>
- <https://software.intel.com/en-us/forums/intel-distribution-for-python/topic/628976>
- <https://groups.google.com/a/continuum.io/forum/#!topic/anaconda/J3YGoef64z8>
Failing to specify `LD_PRELOAD` may lead to errors such as:
`ImportError: /opt/intel/mkl/lib/intel64/libmkl_vml_avx2.so: undefined symbol: mkl_serv_getenv`
or
`Intel MKL FATAL ERROR: Cannot load libmkl_avx2.so or libmkl_def.so.`
when importing GTSAM using the cython wrapper in python.

25
LICENSE
View File

@ -1,18 +1,25 @@
GTSAM is released under the simplified BSD license, reproduced in the file
LICENSE.BSD in this directory.
GTSAM contains two third party libraries, with documentation of licensing and
modifications as follows:
GTSAM contains several third party libraries, with documentation of licensing
and modifications as follows:
- CCOLAMD 2.9.3: Tim Davis' constrained column approximate minimum degree
- CCOLAMD 2.9.6: Tim Davis' constrained column approximate minimum degree
ordering library
- Included unmodified in gtsam/3rdparty/CCOLAMD and gtsam/3rdparty/UFconfig
- Included unmodified in gtsam/3rdparty/CCOLAMD and
gtsam/3rdparty/SuiteSparse_config
- http://faculty.cse.tamu.edu/davis/suitesparse.html
- Licenced under BSD-3, provided in gtsam/3rdparty/CCOLAMD/Doc/License.txt
- Eigen 3.2: General C++ matrix and linear algebra library
- Modified with 3 patches that have been contributed back to the Eigen team:
- http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection)
- http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code)
- http://eigen.tuxfamily.org/bz/show_bug.cgi?id=716 (Improved comma initialization)
- ceres: Google's nonlinear least-squares optimization library
- Includes only auto-diff/jet code, with minor modifications to includes
- http://ceres-solver.org/license.html
- Eigen 3.3.7: General C++ matrix and linear algebra library
- Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README
- Some code that is 3rd-party to Eigen is BSD and LGPL
- GeographicLib 1.35: Charles Karney's geographic conversion utility library
- Included unmodified in gtsam/3rdparty/GeographicLib
- Licenced under MIT, provided in gtsam/3rdparty/GeographicLib/LICENSE.txt
- METIS 5.1.0: Graph partitioning and fill-reducing matrix ordering library
- Included unmodified in gtsam/3rdparty/metis
- Licenced under Apache License v 2.0, provided in
gtsam/3rdparty/metis/LICENSE.txt

63
README.md
View File

@ -1,24 +1,26 @@
README - Georgia Tech Smoothing and Mapping library
===================================================
# README - Georgia Tech Smoothing and Mapping Library
What is GTSAM?
--------------
## What is GTSAM?
GTSAM is a library of C++ classes that implement smoothing and
mapping (SAM) in robotics and vision, using factor graphs and Bayes
networks as the underlying computing paradigm rather than sparse
GTSAM is a C++ library that implements smoothing and
mapping (SAM) in robotics and vision, using Factor Graphs and Bayes
Networks as the underlying computing paradigm rather than sparse
matrices.
On top of the C++ library, GTSAM includes a MATLAB interface (enable
GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
is under development.
| Platform | Build Status |
|:---------:|:-------------:|
| gcc/clang | [![Build Status](https://travis-ci.com/borglab/gtsam.svg?branch=develop)](https://travis-ci.com/borglab/gtsam/) |
| MSVC | [![Build status](https://ci.appveyor.com/api/projects/status/3enllitj52jsxwfg/branch/develop?svg=true)](https://ci.appveyor.com/project/dellaert/gtsam) |
Quickstart
----------
On top of the C++ library, GTSAM includes [wrappers for MATLAB & Python](##Wrappers).
## Quickstart
In the root library folder execute:
```
```sh
#!bash
$ mkdir build
$ cd build
@ -30,40 +32,43 @@ $ make install
Prerequisites:
- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`)
- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
Optional prerequisites - used automatically if findable by CMake:
- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) (Ubuntu: [installing using APT](https://software.intel.com/en-us/articles/installing-intel-free-libs-and-python-apt-repo))
- See [INSTALL.md](INSTALL.md) for more installation information
- Note that MKL may not provide a speedup in all cases. Make sure to benchmark your problem with and without MKL.
GTSAM 4 Compatibility
---------------------
## GTSAM 4 Compatibility
GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
GTSAM 4 introduces several new features, most notably Expressions and a Python toolbox. We also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect.
Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 is deprecated, so please be aware that this might render functions using their default constructor incorrect.
The Preintegrated IMU Factor
----------------------------
## Wrappers
We provide support for [MATLAB](matlab/README.md) and [Python](cython/README.md) wrappers for GTSAM. Please refer to the linked documents for more details.
## The Preintegrated IMU Factor
GTSAM includes a state of the art IMU handling scheme based on
- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. [[link]](https://ieeexplore.ieee.org/document/6092505)
Our implementation improves on this using integration on the manifold, as detailed in
- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014.
- 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.
- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483)
- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf)
If you are using the factor in academic work, please cite the publications above.
In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in [this document](doc/ImuFactor.pdf), is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
Additional Information
----------------------
## Additional Information
There is a [`GTSAM users Google group`](https://groups.google.com/forum/#!forum/gtsam-users) for general discussion.
@ -71,10 +76,10 @@ Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. A primer on GTS
which support (superfast) automatic differentiation,
can be found on the [GTSAM wiki on BitBucket](https://bitbucket.org/gtborg/gtsam/wiki/Home).
See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
See the [`INSTALL`](INSTALL.md) file for more detailed installation instructions.
GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).
GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS.md).

2
THANKS → THANKS.md
View File

@ -1,5 +1,6 @@
GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech:
* Jeremy Aguilon, Facebook
* Sungtae An
* Doru Balcan, Bank of America
* Chris Beall
@ -26,6 +27,7 @@ GTSAM was made possible by the efforts of many collaborators at Georgia Tech, li
* Natesh Srinivasan
* Alex Trevor
* Stephen Williams, BossaNova
* Matthew Broadway
at ETH, Zurich

54
USAGE.md
View File

@ -1,48 +1,42 @@
USAGE - Georgia Tech Smoothing and Mapping library
===================================
What is this file?
# GTSAM USAGE
This file explains how to make use of the library for common SLAM tasks,
using a visual SLAM implementation as an example.
This file explains how to make use of the library for common SLAM tasks, using a visual SLAM implementation as an example.
## Getting Started
Getting Started
---------------------------------------------------
Install:
Follow the installation instructions in the README file to build and
install gtsam, as well as running tests to ensure the library is working
properly.
### Install
Compiling/Linking with gtsam:
The installation creates a binary "libgtsam" at the installation prefix,
and an include folder "gtsam". These are the only required includes, but
the library has also been designed to make use of XML serialization through
the Boost.serialization library, which requires the the Boost.serialization
headers and binaries to be linked.
Follow the installation instructions in the README file to build and install gtsam, as well as running tests to ensure the library is working properly.
If you use CMake for your project, you can use the CMake scripts in the
cmake folder for finding GTSAM, CppUnitLite, and Wrap.
### Compiling/Linking with GTSAM
Examples:
To see how the library works, examine the unit tests provided.
The installation creates a binary `libgtsam` at the installation prefix, and an include folder `gtsam`. These are the only required includes, but the library has also been designed to make use of XML serialization through the `Boost.serialization` library, which requires the the Boost.serialization headers and binaries to be linked.
If you use CMake for your project, you can use the CMake scripts in the cmake folder for finding `GTSAM`, `CppUnitLite`, and `Wrap`.
Overview
---------------------------------------------------
The GTSAM library has three primary components necessary for the construction
of factor graph representation and optimization which users will need to
adapt to their particular problem.
### Examples
To see how the library works, examine the unit tests provided.
## Overview
The GTSAM library has three primary components necessary for the construction of factor graph representation and optimization which users will need to adapt to their particular problem.
* FactorGraph
* FactorGraph:
A factor graph contains a set of variables to solve for (i.e., robot poses, landmark poses, etc.) and a set of constraints between these variables, which make up factors.
* Values:
Values is a single object containing labeled values for all of the variables. Currently, all variables are labeled with strings, but the type or organization of the variables can change
* Factors:
Values is a single object containing labeled values for all of the variables. Currently, all variables are labeled with strings, but the type or organization of the variables can change.
* Factors
A nonlinear factor expresses a constraint between variables, which in the SLAM example, is a measurement such as a visual reading on a landmark or odometry.
The library is organized according to the following directory structure:
3rdparty local copies of third party libraries - Eigen3 and CCOLAMD
3rdparty local copies of third party libraries e.g. Eigen3 and CCOLAMD
base provides some base Math and data structures, as well as test-related utilities
geometry points, poses, tensors, etc
inference core graphical model inference such as factor graphs, junction trees, Bayes nets, Bayes trees

37
Using-GTSAM-EXPORT.md Normal file
View File

@ -0,0 +1,37 @@
# Using GTSAM_EXPORT
To create a DLL in windows, the `GTSAM_EXPORT` keyword has been created and needs to be applied to different methods and classes in the code to expose this code outside of the DLL. However, there are several intricacies that make this more difficult than it sounds. In general, if you follow the following three rules, GTSAM_EXPORT should work properly. The rest of the document also describes (1) the common error message encountered when you are not following these rules and (2) the reasoning behind these usage rules.
## Usage Rules
1. Put `GTSAM_EXPORT` in front of any function that you want exported in the DLL _if and only if_ that function is declared in a .cpp file, not just a .h file.
2. Use `GTSAM_EXPORT` in a class definition (i.e. `class GSTAM_EXPORT MyClass {...}`) only if:
* At least one of the functions inside that class is declared in a .cpp file and not just the .h file.
* You can `GTSAM_EXPORT` any class it inherits from as well. (Note that this implictly requires the class does not derive from a "header-only" class. Note that Eigen is a "header-only" library, so if your class derives from Eigen, _do not_ use `GTSAM_EXPORT` in the class definition!)
3. If you have defined a class using `GTSAM_EXPORT`, do not use `GTSAM_EXPORT` in any of its individual function declarations. (Note that you _can_ put `GTSAM_EXPORT` in the definition of individual functions within a class as long as you don't put `GTSAM_EXPORT` in the class definition.)
## When is GTSAM_EXPORT being used incorrectly
Unfortunately, using `GTSAM_EXPORT` incorrectly often does not cause a compiler or linker error in the library that is being compiled, but only when you try to use that DLL in a different library. For example, an error in gtsam/base will often show up when compiling the check_base_program or the MATLAB wrapper, but not when compiling/linking gtsam itself. The most common errors will say something like:
```
Error LNK2019 unresolved external symbol "public: void __cdecl gtsam::SO3::print(class std::basic_string<char,struct std::char_traits<char>,class std::allocator<char> > const &)const " (?print@SO3@gtsam@@QEBAXAEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@@Z) referenced in function "public: static void __cdecl gtsam::Testable<class gtsam::SO3>::Print(class gtsam::SO3 const &,class std::basic_string<char,struct std::char_traits<char>,class std::allocator<char> > const &)" (?Print@?$Testable@VSO3@gtsam@@@gtsam@@SAXAEBVSO3@2@AEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@@Z) check_geometry_program C:\AFIT\lib\gtsam\build\gtsam\geometry\tests\testSO3.obj
```
Let's analyze this error statement. First, there is an unresolved symbol `gtsam::SO3::print`. This can occur because _either_ `GTSAM_EXPORT` was not added to the print function definition when it should have been, _OR_ because `GTSAM_EXPORT` was added to the print function definition when it is fully declared in the header. This error was detected while compiling `check_geometry_program` and pulling in `...\testSO3.obj`. Specifically, within the function call `gtsam::Testable<class gtsam::SO3>::Print (...)`. Note that this error did _not_ occur when compiling the library that actually has SO3 inside of it.
## But Why?
I believe that how the compiler/linker interacts with GTSAM_EXPORT can be explained by understanding the rules that MSVC operates under.
But first, we need to understand exactly what `GTSAM_EXPORT` is. `GTSAM_EXPORT` is a `#define` macro that is created by CMAKE when GTSAM is being compiled on a Windows machine. Inside the GTSAM project, GTSAM export will be set to a "dllexport" command. A "dllexport" command tells the compiler that this function should go into the DLL and be visible externally. In any other library, `GTSAM_EXPORT` will be set to a "dllimport" command, telling the linker it should find this function in a DLL somewhere. This leads to the first rule the compiler uses. (Note that I say "compiler rules" when the rules may actually be in the linker, but I am conflating the two terms here when I speak of the "compiler rules".)
***Compiler Rule #1*** If a `dllimport` command is used in defining a function or class, that function or class _must_ be found in a DLL.
Rule #1 doesn't seem very bad, until you combine it with rule #2
***Compiler Rule #2*** Anything declared in a header file is not included in a DLL.
When these two rules are combined, you get some very confusing results. For example, a class which is completely defined in a header (e.g. LieMatrix) cannot use `GTSAM_EXPORT` in its definition. If LieMatrix is defined with `GTSAM_EXPORT`, then the compiler _must_ find LieMatrix in a DLL. Because LieMatrix is a header-only class, however, it can't find it, leading to a very confusing "I can't find this symbol" type of error. Note that the linker says it can't find the symbol even though the compiler found the header file that completely defines the class.
Also note that when a class that you want to export inherits from another class that is not exportable, this can cause significant issues. According to this [MSVC Warning page](https://docs.microsoft.com/en-us/cpp/error-messages/compiler-warnings/compiler-warning-level-2-c4275?view=vs-2019), it may not strictly be a rule, but we have seen several linker errors when a class that is defined with `GTSAM_EXPORT` extended an Eigen class. In general, it appears that any inheritance of non-exportable class by an exportable class is a bad idea.
## Conclusion
Hopefully, this little document clarifies when `GTSAM_EXPORT` should and should not be used whenever future GTSAM code is being written. Following the usage rules above, we have been able to get all of the libraries, together with their test and wrapping libraries, to compile/link successfully.

33
appveyor.yml Normal file
View File

@ -0,0 +1,33 @@
# version format
version: 4.0.2-{branch}-build{build}
os: Visual Studio 2019
clone_folder: c:\projects\gtsam
platform: x64
configuration: Release
environment:
CTEST_OUTPUT_ON_FAILURE: 1
BOOST_ROOT: C:/Libraries/boost_1_71_0
build_script:
- cd c:\projects\gtsam\build
# As of Dec 2019, not all unit tests build cleanly for MSVC, so we'll just
# check that parts of GTSAM build correctly:
#- cmake --build .
- cmake --build . --config Release --target gtsam
- cmake --build . --config Release --target gtsam_unstable
- cmake --build . --config Release --target wrap
#- cmake --build . --target check
- cmake --build . --config Release --target check.base
- cmake --build . --config Release --target check.base_unstable
- cmake --build . --config Release --target check.linear
before_build:
- cd c:\projects\gtsam
- mkdir build
- cd build
# Disable examples to avoid AppVeyor timeout
- cmake -G "Visual Studio 16 2019" .. -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF

15
bitbucket-pipelines.yml
View File

@ -1,15 +0,0 @@
# Built from sample configuration for C++ – Make.
# Check https://confluence.atlassian.com/x/5Q4SMw for more examples.
# -----
# Our custom docker image from Docker Hub as the build environment.
image: dellaert/ubuntu-boost-tbb-eigen3:bionic
pipelines:
default:
- step:
script: # Modify the commands below to build your repository.
- mkdir build
- cd build
- cmake -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_USE_EIGEN_MKL=OFF ..
- make -j2
- make -j2 check

1
cmake/CMakeLists.txt
View File

@ -20,6 +20,7 @@ install(FILES
GtsamPythonWrap.cmake
GtsamCythonWrap.cmake
GtsamTesting.cmake
GtsamPrinting.cmake
FindCython.cmake
FindNumPy.cmake
README.html

9
cmake/Config.cmake.in
View File

@ -12,6 +12,15 @@ else()
set(@PACKAGE_NAME@_INCLUDE_DIR "${OUR_CMAKE_DIR}/@CONF_REL_INCLUDE_DIR@" CACHE PATH "@PACKAGE_NAME@ include directory")
endif()
# Find dependencies, required by cmake exported targets:
include(CMakeFindDependencyMacro)
# Allow using cmake < 3.8
if(${CMAKE_VERSION} VERSION_LESS "3.8.0")
find_package(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
else()
find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
endif()
# Load exports
include(${OUR_CMAKE_DIR}/@PACKAGE_NAME@-exports.cmake)

2347
cmake/FindBoost.cmake Normal file
View File

File diff suppressed because it is too large Load Diff

11
cmake/FindCython.cmake
View File

@ -29,10 +29,15 @@
# Use the Cython executable that lives next to the Python executable
# if it is a local installation.
find_package( PythonInterp )
if(GTSAM_PYTHON_VERSION STREQUAL "Default")
find_package(PythonInterp)
else()
find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT)
endif()
if ( PYTHONINTERP_FOUND )
execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c"
"import Cython; print Cython.__path__"
"import Cython; print(Cython.__path__[0])"
RESULT_VARIABLE RESULT
OUTPUT_VARIABLE CYTHON_PATH
OUTPUT_STRIP_TRAILING_WHITESPACE
@ -51,7 +56,7 @@ endif ()
# RESULT=0 means ok
if ( NOT RESULT )
execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c"
"import Cython; print Cython.__version__"
"import Cython; print(Cython.__version__)"
RESULT_VARIABLE RESULT
OUTPUT_VARIABLE CYTHON_VAR_OUTPUT
ERROR_VARIABLE CYTHON_VAR_OUTPUT

10
cmake/FindMKL.cmake
View File

@ -206,6 +206,15 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
)
ENDIF()
IF(NOT MKL_LAPACK_LIBRARY)
FIND_LIBRARY(MKL_LAPACK_LIBRARY
mkl_intel_lp64
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
ENDIF()
# iomp5
IF("${MKL_ARCH_DIR}" STREQUAL "32")
IF(UNIX AND NOT APPLE)
@ -222,6 +231,7 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
FIND_LIBRARY(MKL_IOMP5_LIBRARY
iomp5
PATHS
${MKL_ROOT_DIR}/lib/intel64
${MKL_ROOT_DIR}/../lib/intel64
)
ELSE()

8
cmake/FindNumPy.cmake
View File

@ -40,9 +40,17 @@
# Finding NumPy involves calling the Python interpreter
if(NumPy_FIND_REQUIRED)
if(GTSAM_PYTHON_VERSION STREQUAL "Default")
find_package(PythonInterp REQUIRED)
else()
find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
endif()
else()
if(GTSAM_PYTHON_VERSION STREQUAL "Default")
find_package(PythonInterp)
else()
find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT)
endif()
endif()
if(NOT PYTHONINTERP_FOUND)

540
cmake/FindTBB.cmake
View File

@ -1,13 +1,6 @@
# Locate Intel Threading Building Blocks include paths and libraries
# FindTBB.cmake can be found at https://code.google.com/p/findtbb/
# Written by Hannes Hofmann <hannes.hofmann _at_ informatik.uni-erlangen.de>
# Improvements by Gino van den Bergen <gino _at_ dtecta.com>,
# Florian Uhlig <F.Uhlig _at_ gsi.de>,
# Jiri Marsik <jiri.marsik89 _at_ gmail.com>
# The MIT License
# The MIT License (MIT)
#
# Copyright (c) 2011 Hannes Hofmann
# Copyright (c) 2015 Justus Calvin
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
@ -16,295 +9,306 @@
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
# GvdB: This module uses the environment variable TBB_ARCH_PLATFORM which defines architecture and compiler.
# e.g. "ia32/vc8" or "em64t/cc4.1.0_libc2.4_kernel2.6.16.21"
# TBB_ARCH_PLATFORM is set by the build script tbbvars[.bat|.sh|.csh], which can be found
# in the TBB installation directory (TBB_INSTALL_DIR).
#
# GvdB: Mac OS X distribution places libraries directly in lib directory.
# FindTBB
# -------
#
# For backwards compatibility, you may explicitely set the CMake variables TBB_ARCHITECTURE and TBB_COMPILER.
# TBB_ARCHITECTURE [ ia32 | em64t | itanium ]
# which architecture to use
# TBB_COMPILER e.g. vc9 or cc3.2.3_libc2.3.2_kernel2.4.21 or cc4.0.1_os10.4.9
# which compiler to use (detected automatically on Windows)
# Find TBB include directories and libraries.
#
# Usage:
#
# find_package(TBB [major[.minor]] [EXACT]
# [QUIET] [REQUIRED]
# [[COMPONENTS] [components...]]
# [OPTIONAL_COMPONENTS components...])
#
# where the allowed components are tbbmalloc and tbb_preview. Users may modify
# the behavior of this module with the following variables:
#
# * TBB_ROOT_DIR - The base directory the of TBB installation.
# * TBB_INCLUDE_DIR - The directory that contains the TBB headers files.
# * TBB_LIBRARY - The directory that contains the TBB library files.
# * TBB_<library>_LIBRARY - The path of the TBB the corresponding TBB library.
# These libraries, if specified, override the
# corresponding library search results, where <library>
# may be tbb, tbb_debug, tbbmalloc, tbbmalloc_debug,
# tbb_preview, or tbb_preview_debug.
# * TBB_USE_DEBUG_BUILD - The debug version of tbb libraries, if present, will
# be used instead of the release version.
#
# Users may modify the behavior of this module with the following environment
# variables:
#
# * TBB_INSTALL_DIR
# * TBBROOT
# * LIBRARY_PATH
#
# This module will set the following variables:
#
# * TBB_FOUND - Set to false, or undefined, if we haven’t found, or
# don’t want to use TBB.
# * TBB_<component>_FOUND - If False, optional <component> part of TBB sytem is
# not available.
# * TBB_VERSION - The full version string
# * TBB_VERSION_MAJOR - The major version
# * TBB_VERSION_MINOR - The minor version
# * TBB_INTERFACE_VERSION - The interface version number defined in
# tbb/tbb_stddef.h.
# * TBB_<library>_LIBRARY_RELEASE - The path of the TBB release version of
# <library>, where <library> may be tbb, tbb_debug,
# tbbmalloc, tbbmalloc_debug, tbb_preview, or
# tbb_preview_debug.
# * TBB_<library>_LIBRARY_DEGUG - The path of the TBB release version of
# <library>, where <library> may be tbb, tbb_debug,
# tbbmalloc, tbbmalloc_debug, tbb_preview, or
# tbb_preview_debug.
#
# The following varibles should be used to build and link with TBB:
#
# * TBB_INCLUDE_DIRS - The include directory for TBB.
# * TBB_LIBRARIES - The libraries to link against to use TBB.
# * TBB_LIBRARIES_RELEASE - The release libraries to link against to use TBB.
# * TBB_LIBRARIES_DEBUG - The debug libraries to link against to use TBB.
# * TBB_DEFINITIONS - Definitions to use when compiling code that uses
# TBB.
# * TBB_DEFINITIONS_RELEASE - Definitions to use when compiling release code that
# uses TBB.
# * TBB_DEFINITIONS_DEBUG - Definitions to use when compiling debug code that
# uses TBB.
#
# This module will also create the "tbb" target that may be used when building
# executables and libraries.
# This module respects
# TBB_INSTALL_DIR or $ENV{TBB21_INSTALL_DIR} or $ENV{TBB_INSTALL_DIR}
include(FindPackageHandleStandardArgs)
# This module defines
# TBB_INCLUDE_DIRS, where to find task_scheduler_init.h, etc.
# TBB_LIBRARY_DIRS, where to find libtbb, libtbbmalloc
# TBB_DEBUG_LIBRARY_DIRS, where to find libtbb_debug, libtbbmalloc_debug
# TBB_INSTALL_DIR, the base TBB install directory
# TBB_LIBRARIES, the libraries to link against to use TBB.
# TBB_DEBUG_LIBRARIES, the libraries to link against to use TBB with debug symbols.
# TBB_FOUND, If false, don't try to use TBB.
# TBB_INTERFACE_VERSION, as defined in tbb/tbb_stddef.h
if(NOT TBB_FOUND)
##################################
# Check the build type
##################################
if (WIN32)
# has em64t/vc8 em64t/vc9
# has ia32/vc7.1 ia32/vc8 ia32/vc9
set(_TBB_DEFAULT_INSTALL_DIR "C:/Program Files/Intel/TBB")
set(_TBB_LIB_NAME "tbb")
set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
if (MSVC71)
set (_TBB_COMPILER "vc7.1")
set (TBB_COMPILER "vc7.1")
endif(MSVC71)
if (MSVC80)
set(_TBB_COMPILER "vc8")
set(TBB_COMPILER "vc8")
endif(MSVC80)
if (MSVC90)
set(_TBB_COMPILER "vc9")
set(TBB_COMPILER "vc9")
endif(MSVC90)
if(MSVC10)
set(_TBB_COMPILER "vc10")
set(TBB_COMPILER "vc10")
endif(MSVC10)
if(MSVC11)
set(_TBB_COMPILER "vc11")
set(TBB_COMPILER "vc11")
endif(MSVC11)
if(MSVC14)
set(_TBB_COMPILER "vc14")
set(TBB_COMPILER "vc14")
endif(MSVC14)
# Todo: add other Windows compilers such as ICL.
if(TBB_ARCHITECTURE)
set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
elseif("$ENV{TBB_ARCH_PLATFORM}" STREQUAL "")
# Try to guess the architecture
if(CMAKE_CL_64)
set(_TBB_ARCHITECTURE intel64)
set(TBB_ARCHITECTURE intel64)
if(NOT DEFINED TBB_USE_DEBUG_BUILD)
if(CMAKE_BUILD_TYPE MATCHES "(Debug|DEBUG|debug|RelWithDebInfo|RELWITHDEBINFO|relwithdebinfo)")
set(TBB_BUILD_TYPE DEBUG)
else()
set(_TBB_ARCHITECTURE ia32)
set(TBB_ARCHITECTURE ia32)
set(TBB_BUILD_TYPE RELEASE)
endif()
elseif(TBB_USE_DEBUG_BUILD)
set(TBB_BUILD_TYPE DEBUG)
else()
set(TBB_BUILD_TYPE RELEASE)
endif()
##################################
# Set the TBB search directories
##################################
# Define search paths based on user input and environment variables
set(TBB_SEARCH_DIR ${TBB_ROOT_DIR} $ENV{TBB_INSTALL_DIR} $ENV{TBBROOT})
# Define the search directories based on the current platform
if(CMAKE_SYSTEM_NAME STREQUAL "Windows")
set(TBB_DEFAULT_SEARCH_DIR "C:/Program Files/Intel/TBB"
"C:/Program Files (x86)/Intel/TBB")
# Set the target architecture
if(CMAKE_SIZEOF_VOID_P EQUAL 8)
set(TBB_ARCHITECTURE "intel64")
else()
set(TBB_ARCHITECTURE "ia32")
endif()
# Set the TBB search library path search suffix based on the version of VC
if(WINDOWS_STORE)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11_ui")
elseif(MSVC14)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc14")
elseif(MSVC12)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc12")
elseif(MSVC11)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11")
elseif(MSVC10)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc10")
endif()
# Add the library path search suffix for the VC independent version of TBB
list(APPEND TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc_mt")
elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
# OS X
set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb")
# TODO: Check to see which C++ library is being used by the compiler.
if(NOT ${CMAKE_SYSTEM_VERSION} VERSION_LESS 13.0)
# The default C++ library on OS X 10.9 and later is libc++
set(TBB_LIB_PATH_SUFFIX "lib/libc++" "lib")
else()
set(TBB_LIB_PATH_SUFFIX "lib")
endif()
elseif(CMAKE_SYSTEM_NAME STREQUAL "Linux")
# Linux
set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb")
# TODO: Check compiler version to see the suffix should be <arch>/gcc4.1 or
# <arch>/gcc4.1. For now, assume that the compiler is more recent than
# gcc 4.4.x or later.
if(CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64")
set(TBB_LIB_PATH_SUFFIX "lib/intel64/gcc4.4")
elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "^i.86$")
set(TBB_LIB_PATH_SUFFIX "lib/ia32/gcc4.4")
endif()
endif()
endif (WIN32)
if (UNIX)
if (APPLE)
# MAC
set(_TBB_DEFAULT_INSTALL_DIR "/Library/Frameworks/Intel_TBB.framework/Versions")
# libs: libtbb.dylib, libtbbmalloc.dylib, *_debug
set(_TBB_LIB_NAME "tbb")
set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
# default flavor on apple: ia32/cc4.0.1_os10.4.9
# Jiri: There is no reason to presume there is only one flavor and
# that user's setting of variables should be ignored.
if(NOT TBB_COMPILER)
set(_TBB_COMPILER "cc4.0.1_os10.4.9")
elseif (NOT TBB_COMPILER)
set(_TBB_COMPILER ${TBB_COMPILER})
endif(NOT TBB_COMPILER)
if(NOT TBB_ARCHITECTURE)
set(_TBB_ARCHITECTURE "ia32")
elseif(NOT TBB_ARCHITECTURE)
set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
endif(NOT TBB_ARCHITECTURE)
else (APPLE)
# LINUX
set(_TBB_DEFAULT_INSTALL_DIR "/opt/intel/tbb" "/usr/local/include" "/usr/include")
set(_TBB_LIB_NAME "tbb")
set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
# has em64t/cc3.2.3_libc2.3.2_kernel2.4.21 em64t/cc3.3.3_libc2.3.3_kernel2.6.5 em64t/cc3.4.3_libc2.3.4_kernel2.6.9 em64t/cc4.1.0_libc2.4_kernel2.6.16.21
# has ia32/*
# has itanium/*
set(_TBB_COMPILER ${TBB_COMPILER})
set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
endif (APPLE)
endif (UNIX)
##################################
# Find the TBB include dir
##################################
if (CMAKE_SYSTEM MATCHES "SunOS.*")
# SUN
# not yet supported
# has em64t/cc3.4.3_kernel5.10
# has ia32/*
endif (CMAKE_SYSTEM MATCHES "SunOS.*")
find_path(TBB_INCLUDE_DIRS tbb/tbb.h
HINTS ${TBB_INCLUDE_DIR} ${TBB_SEARCH_DIR}
PATHS ${TBB_DEFAULT_SEARCH_DIR}
PATH_SUFFIXES include)
##################################
# Set version strings
##################################
#-- Clear the public variables
set (TBB_FOUND "NO")
if(TBB_INCLUDE_DIRS)
file(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _tbb_version_file)
string(REGEX REPLACE ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1"
TBB_VERSION_MAJOR "${_tbb_version_file}")
string(REGEX REPLACE ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1"
TBB_VERSION_MINOR "${_tbb_version_file}")
string(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1"
TBB_INTERFACE_VERSION "${_tbb_version_file}")
set(TBB_VERSION "${TBB_VERSION_MAJOR}.${TBB_VERSION_MINOR}")
endif()
##################################
# Find TBB components
##################################
#-- Find TBB install dir and set ${_TBB_INSTALL_DIR} and cached ${TBB_INSTALL_DIR}
# first: use CMake variable TBB_INSTALL_DIR
if (TBB_INSTALL_DIR)
set (_TBB_INSTALL_DIR ${TBB_INSTALL_DIR})
endif (TBB_INSTALL_DIR)
# second: use environment variable
if (NOT _TBB_INSTALL_DIR)
if (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "")
set (_TBB_INSTALL_DIR $ENV{TBB_INSTALL_DIR})
endif (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "")
# Intel recommends setting TBB21_INSTALL_DIR
if (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "")
set (_TBB_INSTALL_DIR $ENV{TBB21_INSTALL_DIR})
endif (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "")
if (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "")
set (_TBB_INSTALL_DIR $ENV{TBB22_INSTALL_DIR})
endif (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "")
if (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "")
set (_TBB_INSTALL_DIR $ENV{TBB30_INSTALL_DIR})
endif (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "")
endif (NOT _TBB_INSTALL_DIR)
# third: try to find path automatically
if (NOT _TBB_INSTALL_DIR)
if (_TBB_DEFAULT_INSTALL_DIR)
set (_TBB_INSTALL_DIR ${_TBB_DEFAULT_INSTALL_DIR})
endif (_TBB_DEFAULT_INSTALL_DIR)
endif (NOT _TBB_INSTALL_DIR)
# sanity check
if (NOT _TBB_INSTALL_DIR)
message (STATUS "TBB: Unable to find Intel TBB install directory. ${_TBB_INSTALL_DIR}")
else (NOT _TBB_INSTALL_DIR)
# finally: set the cached CMake variable TBB_INSTALL_DIR
if (NOT TBB_INSTALL_DIR)
set (TBB_INSTALL_DIR ${_TBB_INSTALL_DIR} CACHE PATH "Intel TBB install directory")
mark_as_advanced(TBB_INSTALL_DIR)
endif (NOT TBB_INSTALL_DIR)
if(TBB_VERSION VERSION_LESS 4.3)
set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc tbb)
else()
set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc_proxy tbbmalloc tbb)
endif()
# Find each component
foreach(_comp ${TBB_SEARCH_COMPOMPONENTS})
if(";${TBB_FIND_COMPONENTS};tbb;" MATCHES ";${_comp};")
#-- A macro to rewrite the paths of the library. This is necessary, because
# find_library() always found the em64t/vc9 version of the TBB libs
macro(TBB_CORRECT_LIB_DIR var_name)
# if (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t")
string(REPLACE em64t "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}})
# endif (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t")
string(REPLACE ia32 "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}})
string(REPLACE vc7.1 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
string(REPLACE vc8 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
string(REPLACE vc9 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
string(REPLACE vc10 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
string(REPLACE vc11 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
endmacro(TBB_CORRECT_LIB_DIR var_content)
# Search for the libraries
find_library(TBB_${_comp}_LIBRARY_RELEASE ${_comp}
HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR}
PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH
PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX})
find_library(TBB_${_comp}_LIBRARY_DEBUG ${_comp}_debug
HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR}
PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH
PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX})
#-- Look for include directory and set ${TBB_INCLUDE_DIR}
set (TBB_INC_SEARCH_DIR ${_TBB_INSTALL_DIR}/include)
# Jiri: tbbvars now sets the CPATH environment variable to the directory
# containing the headers.
find_path(TBB_INCLUDE_DIR
tbb/task_scheduler_init.h
PATHS ${TBB_INC_SEARCH_DIR} ENV CPATH
)
mark_as_advanced(TBB_INCLUDE_DIR)
if(TBB_${_comp}_LIBRARY_DEBUG)
list(APPEND TBB_LIBRARIES_DEBUG "${TBB_${_comp}_LIBRARY_DEBUG}")
endif()
if(TBB_${_comp}_LIBRARY_RELEASE)
list(APPEND TBB_LIBRARIES_RELEASE "${TBB_${_comp}_LIBRARY_RELEASE}")
endif()
if(TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE} AND NOT TBB_${_comp}_LIBRARY)
set(TBB_${_comp}_LIBRARY "${TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE}}")
endif()
if(TBB_${_comp}_LIBRARY AND EXISTS "${TBB_${_comp}_LIBRARY}")
set(TBB_${_comp}_FOUND TRUE)
else()
set(TBB_${_comp}_FOUND FALSE)
endif()
#-- Look for libraries
# GvdB: $ENV{TBB_ARCH_PLATFORM} is set by the build script tbbvars[.bat|.sh|.csh]
if (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "")
set (_TBB_LIBRARY_DIR
${_TBB_INSTALL_DIR}/lib/$ENV{TBB_ARCH_PLATFORM}
${_TBB_INSTALL_DIR}/$ENV{TBB_ARCH_PLATFORM}/lib
# Mark internal variables as advanced
mark_as_advanced(TBB_${_comp}_LIBRARY_RELEASE)
mark_as_advanced(TBB_${_comp}_LIBRARY_DEBUG)
mark_as_advanced(TBB_${_comp}_LIBRARY)
endif()
endforeach()
##################################
# Set compile flags and libraries
##################################
set(TBB_DEFINITIONS_RELEASE "")
set(TBB_DEFINITIONS_DEBUG "-DTBB_USE_DEBUG=1")
if(TBB_LIBRARIES_${TBB_BUILD_TYPE})
set(TBB_DEFINITIONS "${TBB_DEFINITIONS_${TBB_BUILD_TYPE}}")
set(TBB_LIBRARIES "${TBB_LIBRARIES_${TBB_BUILD_TYPE}}")
elseif(TBB_LIBRARIES_RELEASE)
set(TBB_DEFINITIONS "${TBB_DEFINITIONS_RELEASE}")
set(TBB_LIBRARIES "${TBB_LIBRARIES_RELEASE}")
elseif(TBB_LIBRARIES_DEBUG)
set(TBB_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}")
set(TBB_LIBRARIES "${TBB_LIBRARIES_DEBUG}")
endif()
find_package_handle_standard_args(TBB
REQUIRED_VARS TBB_INCLUDE_DIRS TBB_LIBRARIES
HANDLE_COMPONENTS
VERSION_VAR TBB_VERSION)
##################################
# Create targets
##################################
if(NOT CMAKE_VERSION VERSION_LESS 3.0 AND TBB_FOUND)
# Start fix to support different targets for tbb, tbbmalloc, etc.
# (Jose Luis Blanco, Jan 2019)
# Iterate over tbb, tbbmalloc, etc.
foreach(libname ${TBB_SEARCH_COMPOMPONENTS})
if ((NOT TBB_${libname}_LIBRARY_RELEASE) AND (NOT TBB_${libname}_LIBRARY_DEBUG))
continue()
endif()
add_library(${libname} SHARED IMPORTED)
set_target_properties(${libname} PROPERTIES
INTERFACE_INCLUDE_DIRECTORIES ${TBB_INCLUDE_DIRS}
IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_RELEASE})
if(TBB_${libname}_LIBRARY_RELEASE AND TBB_${libname}_LIBRARY_DEBUG)
set_target_properties(${libname} PROPERTIES
INTERFACE_COMPILE_DEFINITIONS "$<$<OR:$<CONFIG:Debug>,$<CONFIG:RelWithDebInfo>>:TBB_USE_DEBUG=1>"
IMPORTED_LOCATION_DEBUG ${TBB_${libname}_LIBRARY_DEBUG}
IMPORTED_LOCATION_RELWITHDEBINFO ${TBB_${libname}_LIBRARY_DEBUG}
IMPORTED_LOCATION_RELEASE ${TBB_${libname}_LIBRARY_RELEASE}
IMPORTED_LOCATION_MINSIZEREL ${TBB_${libname}_LIBRARY_RELEASE}
)
endif (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "")
# Jiri: This block isn't mutually exclusive with the previous one
# (hence no else), instead I test if the user really specified
# the variables in question.
if ((NOT ${TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${TBB_COMPILER} STREQUAL ""))
# HH: deprecated
message(STATUS "[Warning] FindTBB.cmake: The use of TBB_ARCHITECTURE and TBB_COMPILER is deprecated and may not be supported in future versions. Please set \$ENV{TBB_ARCH_PLATFORM} (using tbbvars.[bat|csh|sh]).")
# Jiri: It doesn't hurt to look in more places, so I store the hints from
# ENV{TBB_ARCH_PLATFORM} and the TBB_ARCHITECTURE and TBB_COMPILER
# variables and search them both.
set (
_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}/${_TBB_ARCHITECTURE}/${_TBB_COMPILER}/lib" ${_TBB_LIBRARY_DIR}
_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}/lib/${_TBB_ARCHITECTURE}/${_TBB_COMPILER}" ${_TBB_LIBRARY_DIR}
elseif(TBB_${libname}_LIBRARY_RELEASE)
set_target_properties(${libname} PROPERTIES IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_RELEASE})
else()
set_target_properties(${libname} PROPERTIES
INTERFACE_COMPILE_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}"
IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_DEBUG}
)
endif ((NOT ${TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${TBB_COMPILER} STREQUAL ""))
endif()
endforeach()
# End of fix to support different targets
endif()
# GvdB: Mac OS X distribution places libraries directly in lib directory.
list(APPEND _TBB_LIBRARY_DIR ${_TBB_INSTALL_DIR}/lib)
mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARIES)
# Jiri: No reason not to check the default paths. From recent versions,
# tbbvars has started exporting the LIBRARY_PATH and LD_LIBRARY_PATH
# variables, which now point to the directories of the lib files.
# It all makes more sense to use the ${_TBB_LIBRARY_DIR} as a HINTS
# argument instead of the implicit PATHS as it isn't hard-coded
# but computed by system introspection. Searching the LIBRARY_PATH
# and LD_LIBRARY_PATH environment variables is now even more important
# that tbbvars doesn't export TBB_ARCH_PLATFORM and it facilitates
# the use of TBB built from sources.
find_library(TBB_LIBRARY ${_TBB_LIB_NAME} HINTS ${_TBB_LIBRARY_DIR}
PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
find_library(TBB_MALLOC_LIBRARY ${_TBB_LIB_MALLOC_NAME} HINTS ${_TBB_LIBRARY_DIR}
PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
unset(TBB_ARCHITECTURE)
unset(TBB_BUILD_TYPE)
unset(TBB_LIB_PATH_SUFFIX)
unset(TBB_DEFAULT_SEARCH_DIR)
#Extract path from TBB_LIBRARY name
get_filename_component(TBB_LIBRARY_DIR ${TBB_LIBRARY} PATH)
#TBB_CORRECT_LIB_DIR(TBB_LIBRARY)
#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY)
mark_as_advanced(TBB_LIBRARY TBB_MALLOC_LIBRARY)
#-- Look for debug libraries
# Jiri: Changed the same way as for the release libraries.
find_library(TBB_LIBRARY_DEBUG ${_TBB_LIB_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR}
PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
find_library(TBB_MALLOC_LIBRARY_DEBUG ${_TBB_LIB_MALLOC_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR}
PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
# Jiri: Self-built TBB stores the debug libraries in a separate directory.
# Extract path from TBB_LIBRARY_DEBUG name
get_filename_component(TBB_LIBRARY_DEBUG_DIR ${TBB_LIBRARY_DEBUG} PATH)
#TBB_CORRECT_LIB_DIR(TBB_LIBRARY_DEBUG)
#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY_DEBUG)
mark_as_advanced(TBB_LIBRARY_DEBUG TBB_MALLOC_LIBRARY_DEBUG)
if (TBB_INCLUDE_DIR)
if (TBB_LIBRARY)
set (TBB_FOUND "YES")
set (TBB_LIBRARIES ${TBB_LIBRARY} ${TBB_MALLOC_LIBRARY} ${TBB_LIBRARIES})
set (TBB_DEBUG_LIBRARIES ${TBB_LIBRARY_DEBUG} ${TBB_MALLOC_LIBRARY_DEBUG} ${TBB_DEBUG_LIBRARIES})
set (TBB_INCLUDE_DIRS ${TBB_INCLUDE_DIR} CACHE PATH "TBB include directory" FORCE)
set (TBB_LIBRARY_DIRS ${TBB_LIBRARY_DIR} CACHE PATH "TBB library directory" FORCE)
# Jiri: Self-built TBB stores the debug libraries in a separate directory.
set (TBB_DEBUG_LIBRARY_DIRS ${TBB_LIBRARY_DEBUG_DIR} CACHE PATH "TBB debug library directory" FORCE)
mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARY_DIRS TBB_DEBUG_LIBRARY_DIRS TBB_LIBRARIES TBB_DEBUG_LIBRARIES)
message(STATUS "Found Intel TBB")
endif (TBB_LIBRARY)
endif (TBB_INCLUDE_DIR)
if (NOT TBB_FOUND)
message(STATUS "TBB: Intel TBB NOT found!")
message(STATUS "TBB: Looked for Threading Building Blocks in ${_TBB_INSTALL_DIR}")
# do only throw fatal, if this pkg is REQUIRED
if (TBB_FIND_REQUIRED)
message(FATAL_ERROR "Could NOT find TBB library.")
endif (TBB_FIND_REQUIRED)
endif (NOT TBB_FOUND)
endif (NOT _TBB_INSTALL_DIR)
if (TBB_FOUND)
set(TBB_INTERFACE_VERSION 0)
FILE(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _TBB_VERSION_CONTENTS)
STRING(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1" TBB_INTERFACE_VERSION "${_TBB_VERSION_CONTENTS}")
set(TBB_INTERFACE_VERSION "${TBB_INTERFACE_VERSION}")
endif (TBB_FOUND)
endif()

27
cmake/GtsamAddPch.cmake Normal file
View File

@ -0,0 +1,27 @@
###############################################################################
# Macro:
#
# gtsamAddPch(precompiledHeader precompiledSource sources)
#
# Adds a precompiled header to compile all sources with. Currently only on MSVC.
# Inspired by https://stackoverflow.com/questions/148570/
#
# Arguments:
# precompiledHeader: the header file that includes headers to be precompiled.
# precompiledSource: the source file that simply includes that header above.
# sources: the list of source files to apply this to.
#
macro(gtsamAddPch precompiledHeader precompiledSource sources)
get_filename_component(pchBasename ${precompiledHeader} NAME_WE)
SET(precompiledBinary "${CMAKE_CURRENT_BINARY_DIR}/${pchBasename}.pch")
IF(MSVC)
message(STATUS "Adding precompiled header for MSVC")
set_source_files_properties(${precompiledSource}
PROPERTIES COMPILE_FLAGS "/Yc\"${precompiledHeader}\" /Fp\"${precompiledBinary}\""
OBJECT_OUTPUTS "${precompiledBinary}")
set_source_files_properties(${sources}
PROPERTIES COMPILE_FLAGS "/Yu\"${precompiledHeader}\" /FI\"${precompiledHeader}\" /Fp\"${precompiledBinary}\""
OBJECT_DEPENDS "${precompiledBinary}")
ENDIF(MSVC)
endmacro(gtsamAddPch)

197
cmake/GtsamBuildTypes.cmake
View File

@ -1,7 +1,45 @@
# function: list_append_cache(var [new_values ...])
# Like "list(APPEND ...)" but working for CACHE variables.
# -----------------------------------------------------------
function(list_append_cache var)
set(cur_value ${${var}})
list(APPEND cur_value ${ARGN})
get_property(MYVAR_DOCSTRING CACHE ${var} PROPERTY HELPSTRING)
set(${var} "${cur_value}" CACHE STRING "${MYVAR_DOCSTRING}" FORCE)
endfunction()
# function: append_config_if_not_empty(TARGET_VARIABLE build_type)
# Auxiliary function used to merge configuration-specific flags into the
# global variables that will actually be send to cmake targets.
# -----------------------------------------------------------
function(append_config_if_not_empty TARGET_VARIABLE_ build_type)
string(TOUPPER "${build_type}" build_type_toupper)
set(flags_variable_name "${TARGET_VARIABLE_}_${build_type_toupper}")
set(flags_ ${${flags_variable_name}})
if (NOT "${flags_}" STREQUAL "")
if (${build_type_toupper} STREQUAL "COMMON")
# Special "COMMON" configuration type, just append without CMake expression:
list_append_cache(${TARGET_VARIABLE_} "${flags_}")
else()
# Regular configuration type:
list_append_cache(${TARGET_VARIABLE_} "$<$<CONFIG:${build_type}>:${flags_}>")
endif()
endif()
endfunction()
# Add install prefix to search path
list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}")
# Set up build types for MSVC and XCode
set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel
CACHE STRING "Build types available to MSVC and XCode")
mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES)
set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES} CACHE STRING "Build configurations" FORCE)
# Default to Release mode
if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING
@ -13,39 +51,85 @@ endif()
# Add option for using build type postfixes to allow installing multiple build modes
option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON)
# Set custom compilation flags.
# NOTE: We set all the CACHE variables with a GTSAM prefix, and then set a normal local variable below
# so that we don't "pollute" the global variable namespace in the cmake cache.
# Set all CMAKE_BUILD_TYPE flags:
# (see https://cmake.org/Wiki/CMake_Useful_Variables#Compilers_and_Tools)
# Define all cache variables, to be populated below depending on the OS/compiler:
set(GTSAM_COMPILE_OPTIONS_PRIVATE "" CACHE STRING "(Do not edit) Private compiler flags for all build configurations." FORCE)
set(GTSAM_COMPILE_OPTIONS_PUBLIC "" CACHE STRING "(Do not edit) Public compiler flags (exported to user projects) for all build configurations." FORCE)
set(GTSAM_COMPILE_DEFINITIONS_PRIVATE "" CACHE STRING "(Do not edit) Private preprocessor macros for all build configurations." FORCE)
set(GTSAM_COMPILE_DEFINITIONS_PUBLIC "" CACHE STRING "(Do not edit) Public preprocessor macros for all build configurations." FORCE)
mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE)
mark_as_advanced(GTSAM_COMPILE_OPTIONS_PUBLIC)
mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE)
mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PUBLIC)
foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_toupper)
# Define empty cache variables for "public". "private" are creaed below.
set(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper} "" CACHE STRING "(User editable) Public compiler flags (exported to user projects) for `${build_type_toupper}` configuration.")
set(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper} "" CACHE STRING "(User editable) Public preprocessor macros for `${build_type_toupper}` configuration.")
endforeach()
# Common preprocessor macros for each configuration:
set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_DEBUG "_DEBUG;EIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "(User editable) Private preprocessor macros for Debug configuration.")
set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELWITHDEBINFO "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for RelWithDebInfo configuration.")
set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELEASE "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for Release configuration.")
set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_PROFILING "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for Profiling configuration.")
set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_TIMING "NDEBUG;ENABLE_TIMING" CACHE STRING "(User editable) Private preprocessor macros for Timing configuration.")
if(MSVC)
set(GTSAM_CMAKE_C_FLAGS "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.")
set(GTSAM_CMAKE_CXX_FLAGS "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.")
set(GTSAM_CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
set(GTSAM_CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
else()
set(GTSAM_CMAKE_C_FLAGS "-std=c11 -Wall" CACHE STRING "Flags used by the compiler for all builds.")
set(GTSAM_CMAKE_CXX_FLAGS "-std=c++11 -Wall" CACHE STRING "Flags used by the compiler for all builds.")
set(GTSAM_CMAKE_C_FLAGS_DEBUG "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
set(GTSAM_CMAKE_C_FLAGS_RELEASE " -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
set(GTSAM_CMAKE_CXX_FLAGS_RELEASE " -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
# Common to all configurations:
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE
WINDOWS_LEAN_AND_MEAN
NOMINMAX
)
# Avoid literally hundreds to thousands of warnings:
list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC
/wd4267 # warning C4267: 'initializing': conversion from 'size_t' to 'int', possible loss of data
)
endif()
# Other (non-preprocessor macros) compiler flags:
if(MSVC)
# Common to all configurations, next for each configuration:
set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON /W3 /GR /EHsc /MP CACHE STRING "(User editable) Private compiler flags for all configurations.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG /MDd /Zi /Ob0 /Od /RTC1 CACHE STRING "(User editable) Private compiler flags for Debug configuration.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO /MD /O2 /D /Zi /d2Zi+ CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE /MD /O2 CACHE STRING "(User editable) Private compiler flags for Release configuration.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING /MD /O2 /Zi CACHE STRING "(User editable) Private compiler flags for Profiling configuration.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING /MD /O2 CACHE STRING "(User editable) Private compiler flags for Timing configuration.")
else()
# Common to all configurations, next for each configuration:
# "-fPIC" is to ensure proper code generation for shared libraries
set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON -Wall -fPIC CACHE STRING "(User editable) Private compiler flags for all configurations.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG -g -fno-inline CACHE STRING "(User editable) Private compiler flags for Debug configuration.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO -g -O3 CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE -O3 CACHE STRING "(User editable) Private compiler flags for Release configuration.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING -O3 CACHE STRING "(User editable) Private compiler flags for Profiling configuration.")
set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING -g -O3 CACHE STRING "(User editable) Private compiler flags for Timing configuration.")
endif()
# Enable C++11:
if (NOT CMAKE_VERSION VERSION_LESS 3.8)
set(GTSAM_COMPILE_FEATURES_PUBLIC "cxx_std_11" CACHE STRING "CMake compile features property for all gtsam targets.")
# See: https://cmake.org/cmake/help/latest/prop_tgt/CXX_EXTENSIONS.html
# This is to enable -std=c++11 instead of -std=g++11
set(CMAKE_CXX_EXTENSIONS OFF)
else()
# Old cmake versions:
if (NOT MSVC)
list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC $<$<COMPILE_LANGUAGE:CXX>:-std=c++11>)
endif()
endif()
# Merge all user-defined flags into the variables that are to be actually used by CMake:
foreach(build_type "common" ${GTSAM_CMAKE_CONFIGURATION_TYPES})
append_config_if_not_empty(GTSAM_COMPILE_OPTIONS_PRIVATE ${build_type})
append_config_if_not_empty(GTSAM_COMPILE_OPTIONS_PUBLIC ${build_type})
append_config_if_not_empty(GTSAM_COMPILE_DEFINITIONS_PRIVATE ${build_type})
append_config_if_not_empty(GTSAM_COMPILE_DEFINITIONS_PUBLIC ${build_type})
endforeach()
# Linker flags:
set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
set(GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
@ -54,26 +138,11 @@ set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEA
set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
set(GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING
mark_as_advanced(GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING
GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING
GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_CMAKE_CXX_FLAGS_PROFILING GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING
GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_ GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING
GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING)
# Apply the gtsam specific build flags as normal variables. This makes it so that they only
# apply to the gtsam part of the build if gtsam is built as a subproject
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${GTSAM_CMAKE_C_FLAGS}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GTSAM_CMAKE_CXX_FLAGS}")
set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} ${GTSAM_CMAKE_C_FLAGS_DEBUG}")
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${GTSAM_CMAKE_CXX_FLAGS_DEBUG}")
set(CMAKE_C_FLAGS_RELWITHDEBINFO "${CMAKE_C_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}")
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO}")
set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} ${GTSAM_CMAKE_C_FLAGS_RELEASE}")
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${GTSAM_CMAKE_CXX_FLAGS_RELEASE}")
set(CMAKE_C_FLAGS_PROFILING "${CMAKE_C_FLAGS_PROFILING} ${GTSAM_CMAKE_C_FLAGS_PROFILING}")
set(CMAKE_CXX_FLAGS_PROFILING "${CMAKE_CXX_FLAGS_PROFILING} ${GTSAM_CMAKE_CXX_FLAGS_PROFILING}")
set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_TIMING} ${GTSAM_CMAKE_C_FLAGS_TIMING}")
set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_TIMING} ${GTSAM_CMAKE_CXX_FLAGS_TIMING}")
set(CMAKE_SHARED_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING})
set(CMAKE_MODULE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING})
set(CMAKE_EXE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING})
@ -86,7 +155,16 @@ set(CMAKE_EXE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING})
if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
# Apple Clang before 5.0 does not support -ftemplate-depth.
if(NOT (APPLE AND "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0"))
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-ftemplate-depth=1024")
endif()
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)
if(GTSAM_BUILD_WITH_MARCH_NATIVE)
# Add as public flag so all dependant projects also use it, as required
# by Eigen to avid crashes due to SIMD vectorization:
list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-march=native")
endif()
endif()
@ -112,12 +190,6 @@ if(NOT "${CMAKE_BUILD_TYPE}" STREQUAL "")
endif()
endif()
# Set up build types for MSVC and XCode
set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel
CACHE STRING "Build types available to MSVC and XCode")
mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES)
set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES})
# Check build types
string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower)
if( NOT cmake_build_type_tolower STREQUAL ""
@ -153,3 +225,20 @@ function(gtsam_assign_all_source_folders)
gtsam_assign_source_folders("${all_c_srcs};${all_cpp_srcs};${all_headers}")
endfunction()
# Applies the per-config build flags to the given target (e.g. gtsam, wrap_lib)
function(gtsam_apply_build_flags target_name_)
# To enable C++11: the use of target_compile_features() is preferred since
# it will be not in conflict with a more modern C++ standard, if used in a
# client program.
if (NOT "${GTSAM_COMPILE_FEATURES_PUBLIC}" STREQUAL "")
target_compile_features(${target_name_} PUBLIC ${GTSAM_COMPILE_FEATURES_PUBLIC})
endif()
target_compile_definitions(${target_name_} PRIVATE ${GTSAM_COMPILE_DEFINITIONS_PRIVATE})
target_compile_definitions(${target_name_} PUBLIC ${GTSAM_COMPILE_DEFINITIONS_PUBLIC})
if (NOT "${GTSAM_COMPILE_OPTIONS_PUBLIC}" STREQUAL "")
target_compile_options(${target_name_} PUBLIC ${GTSAM_COMPILE_OPTIONS_PUBLIC})
endif()
target_compile_options(${target_name_} PRIVATE ${GTSAM_COMPILE_OPTIONS_PRIVATE})
endfunction(gtsam_apply_build_flags)

53
cmake/GtsamCythonWrap.cmake
View File

@ -3,8 +3,27 @@
# in the current environment are different from the cached!
unset(PYTHON_EXECUTABLE CACHE)
unset(CYTHON_EXECUTABLE CACHE)
unset(PYTHON_INCLUDE_DIR CACHE)
unset(PYTHON_MAJOR_VERSION CACHE)
unset(PYTHON_LIBRARY CACHE)
# Allow override from command line
if(NOT DEFINED GTSAM_USE_CUSTOM_PYTHON_LIBRARY)
if(GTSAM_PYTHON_VERSION STREQUAL "Default")
find_package(PythonInterp REQUIRED)
find_package(PythonLibs REQUIRED)
else()
find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
endif()
endif()
find_package(Cython 0.25.2 REQUIRED)
execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c"
"from __future__ import print_function;import sys;print(sys.version[0], end='')"
OUTPUT_VARIABLE PYTHON_MAJOR_VERSION
)
# User-friendly Cython wrapping and installing function.
# Builds a Cython module from the provided interface_header.
# For example, for the interface header gtsam.h,
@ -29,12 +48,12 @@ endfunction()
function(set_up_required_cython_packages)
# Set up building of cython module
find_package(PythonLibs 2.7 REQUIRED)
include_directories(${PYTHON_INCLUDE_DIRS})
find_package(NumPy REQUIRED)
include_directories(${NUMPY_INCLUDE_DIRS})
endfunction()
# Convert pyx to cpp by executing cython
# This is the first step to compile cython from the command line
# as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html
@ -52,7 +71,7 @@ function(pyx_to_cpp target pyx_file generated_cpp include_dirs)
add_custom_command(
OUTPUT ${generated_cpp}
COMMAND
${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus -${PYTHON_MAJOR_VERSION} ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
VERBATIM)
add_custom_target(${target} ALL DEPENDS ${generated_cpp})
endfunction()
@ -68,11 +87,25 @@ endfunction()
# - output_dir: The output directory
function(build_cythonized_cpp target cpp_file output_lib_we output_dir)
add_library(${target} MODULE ${cpp_file})
if(WIN32)
# Use .pyd extension instead of .dll on Windows
set_target_properties(${target} PROPERTIES SUFFIX ".pyd")
# Add full path to the Python library
target_link_libraries(${target} ${PYTHON_LIBRARIES})
endif()
if(APPLE)
set(link_flags "-undefined dynamic_lookup")
endif()
set_target_properties(${target} PROPERTIES COMPILE_FLAGS "-w" LINK_FLAGS "${link_flags}"
OUTPUT_NAME ${output_lib_we} PREFIX "" LIBRARY_OUTPUT_DIRECTORY ${output_dir})
set_target_properties(${target}
PROPERTIES COMPILE_FLAGS "-w"
LINK_FLAGS "${link_flags}"
OUTPUT_NAME ${output_lib_we}
PREFIX ""
${CMAKE_BUILD_TYPE_UPPER}_POSTFIX ""
LIBRARY_OUTPUT_DIRECTORY ${output_dir})
endfunction()
# Cythonize a pyx from the command line as described at
@ -148,7 +181,11 @@ function(install_cython_wrapped_library interface_header generated_files_path in
# NOTE: only installs .pxd and .pyx and binary files (not .cpp) - the trailing slash on the directory name
# here prevents creating the top-level module name directory in the destination.
message(STATUS "Installing Cython Toolbox to ${install_path}") #${GTSAM_CYTHON_INSTALL_PATH}")
# Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH/subdirectory if there is one
get_filename_component(location "${install_path}" PATH)
get_filename_component(name "${install_path}" NAME)
message(STATUS "Installing Cython Toolbox to ${location}${GTSAM_BUILD_TAG}/${name}") #${GTSAM_CYTHON_INSTALL_PATH}"
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
@ -157,10 +194,8 @@ function(install_cython_wrapped_library interface_header generated_files_path in
else()
set(build_type_tag "${build_type}")
endif()
# Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH if there is one
get_filename_component(location "${install_path}" PATH)
get_filename_component(name "${install_path}" NAME)
install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}/${name}${build_type_tag}"
install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}${build_type_tag}/${name}"
CONFIGURATIONS "${build_type}"
PATTERN "build" EXCLUDE
PATTERN "CMakeFiles" EXCLUDE

30
cmake/GtsamMakeConfigFile.cmake
View File

@ -15,18 +15,42 @@ function(GtsamMakeConfigFile PACKAGE_NAME)
get_filename_component(name "${ARGV1}" NAME_WE)
set(EXTRA_FILE "${name}.cmake")
configure_file(${ARGV1} "${PROJECT_BINARY_DIR}/${EXTRA_FILE}" @ONLY)
install(FILES "${PROJECT_BINARY_DIR}/${EXTRA_FILE}" DESTINATION "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}")
install(FILES "${PROJECT_BINARY_DIR}/${EXTRA_FILE}" DESTINATION "${DEF_INSTALL_CMAKE_DIR}")
else()
set(EXTRA_FILE "_does_not_exist_")
endif()
# GTSAM defines its version variable as GTSAM_VERSION_STRING while other
# projects may define it as <ProjectName>_VERSION. Since this file is
# installed on the system as part of GTSAMCMakeTools and may be useful in
# external projects, we need to handle both cases of version variable naming
# here.
if(NOT DEFINED ${PACKAGE_NAME}_VERSION AND DEFINED ${PACKAGE_NAME}_VERSION_STRING)
set(${PACKAGE_NAME}_VERSION ${${PACKAGE_NAME}_VERSION_STRING})
endif()
# Version file
include(CMakePackageConfigHelpers)
write_basic_package_version_file(
"${PROJECT_BINARY_DIR}/${PACKAGE_NAME}ConfigVersion.cmake"
VERSION ${${PACKAGE_NAME}_VERSION}
COMPATIBILITY SameMajorVersion
)
# Config file
file(RELATIVE_PATH CONF_REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/include")
file(RELATIVE_PATH CONF_REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/lib")
configure_file(${GTSAM_CONFIG_TEMPLATE_PATH}/Config.cmake.in "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake" @ONLY)
message(STATUS "Wrote ${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake")
# Install config and exports files (for find scripts)
install(FILES "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake" DESTINATION "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}")
# Install config, version and exports files (for find scripts)
install(
FILES
"${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake"
"${PROJECT_BINARY_DIR}/${PACKAGE_NAME}ConfigVersion.cmake"
DESTINATION
"${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}"
)
install(EXPORT ${PACKAGE_NAME}-exports DESTINATION ${DEF_INSTALL_CMAKE_DIR})
endfunction()

10
cmake/GtsamMatlabWrap.cmake
View File

@ -35,7 +35,11 @@ mark_as_advanced(FORCE MEX_COMMAND)
# Now that we have mex, trace back to find the Matlab installation root
get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
get_filename_component(mex_path "${MEX_COMMAND}" PATH)
get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
if(mex_path MATCHES ".*/win64$")
get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE)
else()
get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
endif()
set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
@ -99,7 +103,8 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
message(STATUS "Building wrap module ${moduleName}")
# Find matlab.h in GTSAM
if("${PROJECT_NAME}" STREQUAL "GTSAM")
if(("${PROJECT_NAME}" STREQUAL "gtsam") OR
("${PROJECT_NAME}" STREQUAL "gtsam_unstable"))
set(matlab_h_path "${PROJECT_SOURCE_DIR}")
else()
if(NOT GTSAM_INCLUDE_DIR)
@ -221,6 +226,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
string(REPLACE ";" " " mexFlagsSpaced "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
add_library(${moduleName}_matlab_wrapper MODULE ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects})
target_link_libraries(${moduleName}_matlab_wrapper ${correctedOtherLibraries})
target_link_libraries(${moduleName}_matlab_wrapper ${moduleName})
set_target_properties(${moduleName}_matlab_wrapper PROPERTIES
OUTPUT_NAME "${moduleName}_wrapper"
PREFIX ""

43
cmake/GtsamPrinting.cmake
View File

@ -7,4 +7,45 @@ function(print_config_flag flag msg)
else ()
message(STATUS " ${msg}: Disabled")
endif ()
endfunction(print_config_flag)
endfunction()
# Based on https://github.com/jimbraun/XCDF/blob/master/cmake/CMakePadString.cmake
function(string_pad RESULT_NAME DESIRED_LENGTH VALUE)
string(LENGTH "${VALUE}" VALUE_LENGTH)
math(EXPR REQUIRED_PADS "${DESIRED_LENGTH} - ${VALUE_LENGTH}")
set(PAD ${VALUE})
if(REQUIRED_PADS GREATER 0)
math(EXPR REQUIRED_MINUS_ONE "${REQUIRED_PADS} - 1")
foreach(FOO RANGE ${REQUIRED_MINUS_ONE})
set(PAD "${PAD} ")
endforeach()
endif()
set(${RESULT_NAME} "${PAD}" PARENT_SCOPE)
endfunction()
set(GTSAM_PRINT_SUMMARY_PADDING_LENGTH 50 CACHE STRING "Padding of cmake summary report lines after configuring.")
mark_as_advanced(GTSAM_PRINT_SUMMARY_PADDING_LENGTH)
# Print " var: ${var}" padding with spaces as needed
function(print_padded variable_name)
string_pad(padded_prop ${GTSAM_PRINT_SUMMARY_PADDING_LENGTH} " ${variable_name}")
message(STATUS "${padded_prop}: ${${variable_name}}")
endfunction()
# Prints all the relevant CMake build options for a given target:
function(print_build_options_for_target target_name_)
print_padded(GTSAM_COMPILE_FEATURES_PUBLIC)
print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE)
print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC)
print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE)
print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC)
foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_toupper)
print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE_${build_type_toupper})
print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper})
print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE_${build_type_toupper})
print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper})
endforeach()
endfunction()

65
cmake/GtsamTesting.cmake
View File

@ -88,29 +88,36 @@ enable_testing()
option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON)
option(GTSAM_BUILD_EXAMPLES_ALWAYS "Build examples with 'make all' (build with 'make examples' if not)" ON)
option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF)
option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF)
# Add option for combining unit tests
if(MSVC OR XCODE_VERSION)
# Add option for combining unit tests
if(MSVC OR XCODE_VERSION)
option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" ON)
else()
else()
option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" OFF)
endif()
mark_as_advanced(GTSAM_SINGLE_TEST_EXE)
endif()
mark_as_advanced(GTSAM_SINGLE_TEST_EXE)
# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
if(GTSAM_BUILD_TESTS)
# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
if(GTSAM_BUILD_TESTS)
add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
# Also add alternative checks using valgrind.
# We don't look for valgrind being installed in the system, since these
# targets are not invoked unless directly instructed by the user.
if (UNIX)
# Run all tests using valgrind:
add_custom_target(check_valgrind)
endif()
# Add target to build tests without running
add_custom_target(all.tests)
endif()
endif()
# Add examples target
add_custom_target(examples)
# Add examples target
add_custom_target(examples)
# Add timing target
add_custom_target(timing)
# Add timing target
add_custom_target(timing)
# Implementations of this file's macros:
@ -120,6 +127,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
# Add group target if it doesn't already exist
if(NOT TARGET check.${groupName})
add_custom_target(check.${groupName} COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
set_property(TARGET check.${groupName} PROPERTY FOLDER "Unit tests")
endif()
# Get all script files
@ -161,17 +169,32 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
add_executable(${script_name} ${script_src} ${script_headers})
target_link_libraries(${script_name} CppUnitLite ${linkLibraries})
# Apply user build flags from CMake cache variables:
gtsam_apply_build_flags(${script_name})
# Add target dependencies
add_test(NAME ${script_name} COMMAND ${script_name})
add_dependencies(check.${groupName} ${script_name})
add_dependencies(check ${script_name})
add_dependencies(all.tests ${script_name})
if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name})
# Regular test run:
add_custom_target(${script_name}.run
COMMAND ${EXECUTABLE_OUTPUT_PATH}${script_name}
DEPENDS ${script_name}
)
# Run with valgrind:
set(GENERATED_EXE "$<TARGET_FILE:${script_name}>")
add_custom_target(${script_name}.valgrind
COMMAND "valgrind" "--error-exitcode=1" ${GENERATED_EXE}
DEPENDS ${script_name}
)
add_dependencies(check_valgrind ${script_name}.valgrind)
endif()
# Add TOPSRCDIR
set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"")
# Exclude from 'make all' and 'make install'
set_target_properties(${script_name} PROPERTIES EXCLUDE_FROM_ALL ON)
@ -193,6 +216,11 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
add_executable(${target_name} "${script_srcs}" ${script_headers})
target_link_libraries(${target_name} CppUnitLite ${linkLibraries})
# Apply user build flags from CMake cache variables:
gtsam_apply_build_flags(${target_name})
set_property(TARGET check_${groupName}_program PROPERTY FOLDER "Unit tests")
# Only have a main function in one script - use preprocessor
set(rest_script_srcs ${script_srcs})
list(REMOVE_AT rest_script_srcs 0)
@ -207,7 +235,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
endif()
# Add TOPSRCDIR
set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"")
# Exclude from 'make all' and 'make install'
set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON)
@ -258,6 +286,9 @@ macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName b
add_executable(${script_name} ${script_src} ${script_headers})
target_link_libraries(${script_name} ${linkLibraries})
# Apply user build flags from CMake cache variables:
gtsam_apply_build_flags(${script_name})
# Add target dependencies
add_dependencies(${groupName} ${script_name})
if(NOT MSVC AND NOT XCODE_VERSION)
@ -265,7 +296,7 @@ macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName b
endif()
# Add TOPSRCDIR
set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"")
# Exclude from all or not - note weird variable assignment because we're in a macro
set(buildWithAll_on ${buildWithAll})

15
cmake/README.md
View File

@ -1,5 +1,4 @@
GTSAMCMakeTools
===============
# GTSAMCMakeTools
This is the collection of GTSAM CMake tools that may be useful in external projects. The way to use this collection is by first making a find_package call:
@ -7,8 +6,7 @@ This is the collection of GTSAM CMake tools that may be useful in external proje
which will add a directory containing the GTSAM CMake tools to the CMAKE_MODULE_PATH variable. After that, you may include the files you would like to use. These files and the functions they define are explained below.
GtsamBuildTypes
---------------
## GtsamBuildTypes
include(GtsamBuildTypes)
@ -30,8 +28,7 @@ It defines the following functions:
* `gtsam_assign_source_folders( [files] )` Organizes files in the IDE into folders to reflect the actual directory structure of those files. Folders will be determined relative to the current source folder when this function is called.
* `gtsam_assign_all_source_folders()` Calls `gtsam_assign_source_folders` on all cpp, c, and h files recursively in the current source folder.
GtsamTesting
------------
## GtsamTesting
include(GtsamTesting)
@ -70,8 +67,7 @@ Defines two useful functions for creating CTest unit tests. Also immediately cr
an empty string "" if nothing needs to be excluded.
linkLibraries: The list of libraries to link to.
GtsamMatlabWrap
---------------
## GtsamMatlabWrap
include(GtsamMatlabWrap)
@ -97,8 +93,7 @@ Defines functions for generating MATLAB wrappers. Also immediately creates seve
extraMexFlags: Any *additional* flags to pass to the compiler when building
the wrap code. Normally, leave this empty.
GtsamMakeConfigFile
-------------------
## GtsamMakeConfigFile
include(GtsamMakeConfigFile)

27
cmake/cmake_uninstall.cmake.in Normal file
View File

@ -0,0 +1,27 @@
# -----------------------------------------------
# File that provides "make uninstall" target
# We use the file 'install_manifest.txt'
# -----------------------------------------------
if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
message(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"")
endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files)
string(REGEX REPLACE "\n" ";" files "${files}")
foreach(file ${files})
message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"")
if(EXISTS "$ENV{DESTDIR}${file}")
exec_program(
"@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\""
OUTPUT_VARIABLE rm_out
RETURN_VALUE rm_retval
)
if(NOT "${rm_retval}" STREQUAL 0)
message(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"")
endif(NOT "${rm_retval}" STREQUAL 0)
else(EXISTS "$ENV{DESTDIR}${file}")
message(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.")
endif(EXISTS "$ENV{DESTDIR}${file}")
endforeach(file)

23
cmake/dllexport.h.in
View File

@ -26,21 +26,30 @@
// class __declspec(dllexport) MyClass { ... };
// When included while compiling other code against GTSAM:
// class __declspec(dllimport) MyClass { ... };
#pragma once
// Whether GTSAM is compiled as static or DLL in windows.
// This will be used to decide whether include __declspec(dllimport) or not in headers
#cmakedefine BUILD_SHARED_LIBS
#ifdef _WIN32
# ifndef BUILD_SHARED_LIBS
# define @library_name@_EXPORT
# define @library_name@_EXTERN_EXPORT extern
# else
# ifdef @library_name@_EXPORTS
# define @library_name@_EXPORT __declspec(dllexport)
# define @library_name@_EXTERN_EXPORT __declspec(dllexport) extern
# else
# ifndef @library_name@_IMPORT_STATIC
# define @library_name@_EXPORT __declspec(dllimport)
# define @library_name@_EXTERN_EXPORT __declspec(dllimport)
# else /* @library_name@_IMPORT_STATIC */
# define @library_name@_EXPORT
# define @library_name@_EXTERN_EXPORT extern
# endif /* @library_name@_IMPORT_STATIC */
# endif /* @library_name@_EXPORTS */
#else /* _WIN32 */
# endif
# endif
#else
# define @library_name@_EXPORT
# define @library_name@_EXTERN_EXPORT extern
#endif
#undef BUILD_SHARED_LIBS

17
cmake/example_cmake_find_gtsam/CMakeLists.txt Normal file
View File

@ -0,0 +1,17 @@
# This file shows how to build and link a user project against GTSAM using CMake
###################################################################################
# To create your own project, replace "example" with the actual name of your project
cmake_minimum_required(VERSION 3.0)
project(example CXX)
# Find GTSAM, either from a local build, or from a Debian/Ubuntu package.
find_package(GTSAM REQUIRED)
add_executable(example
main.cpp
)
# By using CMake exported targets, a simple "link" dependency introduces the
# include directories (-I) flags, links against Boost, and add any other
# required build flags (e.g. C++11, etc.)
target_link_libraries(example PRIVATE gtsam)

126
cmake/example_cmake_find_gtsam/main.cpp Normal file
View File

@ -0,0 +1,126 @@
/* ----------------------------------------------------------------------------
* 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 Pose2SLAMExample.cpp
* @brief A 2D Pose SLAM example
* @date Oct 21, 2010
* @author Yong Dian Jian
*/
/**
* A simple 2D pose slam example
* - The robot moves in a 2 meter square
* - The robot moves 2 meters each step, turning 90 degrees after each step
* - The robot initially faces along the X axis (horizontal, to the right in 2D)
* - We have full odometry between pose
* - We have a loop closure constraint when the robot returns to the first position
*/
// In planar SLAM example we use Pose2 variables (x, y, theta) to represent the robot poses
#include <gtsam/geometry/Pose2.h>
// We will use simple integer Keys to refer to the robot poses.
#include <gtsam/inference/Key.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Between factors for the relative motion described by odometry measurements.
// We will also use a Between Factor to encode the loop closure constraint
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/BetweenFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// Finally, once all of the factors have been added to our factor graph, we will want to
// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the
// a Gauss-Newton solver
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
// Once the optimized values have been calculated, we can also calculate the marginal covariance
// of desired variables
#include <gtsam/nonlinear/Marginals.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0 ), model);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
// 2c. Add the loop closure constraint
// This factor encodes the fact that we have returned to the same pose. In real systems,
// these constraints may be identified in many ways, such as appearance-based techniques
// with camera images. We will use another Between Factor to enforce this constraint:
graph.emplace_shared<BetweenFactor<Pose2> >(5, 2, Pose2(2, 0, M_PI_2), model);
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 ));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 ));
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI ));
initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
// The optimizer accepts an optional set of configuration parameters,
// controlling things like convergence criteria, the type of linear
// system solver to use, and the amount of information displayed during
// optimization. We will set a few parameters as a demonstration.
GaussNewtonParams parameters;
// Stop iterating once the change in error between steps is less than this value
parameters.relativeErrorTol = 1e-5;
// Do not perform more than N iteration steps
parameters.maxIterations = 100;
// Create the optimizer ...
GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
// ... and optimize
Values result = optimizer.optimize();
result.print("Final Result:\n");
// 5. Calculate and print marginal covariances for all variables
cout.precision(3);
Marginals marginals(graph, result);
cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
cout << "x4 covariance:\n" << marginals.marginalCovariance(4) << endl;
cout << "x5 covariance:\n" << marginals.marginalCovariance(5) << endl;
return 0;
}

12
cmake/example_project/CMakeLists.txt
View File

@ -7,7 +7,7 @@
###################################################################################
# To create your own project, replace "example" with the actual name of your project
cmake_minimum_required(VERSION 2.6)
cmake_minimum_required(VERSION 3.0)
project(example CXX C)
# Include GTSAM CMake tools
@ -22,14 +22,20 @@ include_directories(BEFORE "${PROJECT_SOURCE_DIR}")
###################################################################################
# Find GTSAM components
find_package(GTSAM REQUIRED) # Uses installed package
include_directories(${GTSAM_INCLUDE_DIR})
# Note: Since Jan-2019, GTSAMConfig.cmake defines exported CMake targets
# that automatically do include the include_directories() without the need
# to call include_directories(), just target_link_libraries(NAME gtsam)
#include_directories(${GTSAM_INCLUDE_DIR})
###################################################################################
# Build static library from common sources
set(CONVENIENCE_LIB_NAME ${PROJECT_NAME})
add_library(${CONVENIENCE_LIB_NAME} STATIC example/PrintExamples.h example/PrintExamples.cpp)
add_library(${CONVENIENCE_LIB_NAME} SHARED example/PrintExamples.h example/PrintExamples.cpp)
target_link_libraries(${CONVENIENCE_LIB_NAME} gtsam)
# Install library
install(TARGETS ${CONVENIENCE_LIB_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
###################################################################################
# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library)
gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}")

32
cmake/example_project/README.md Normal file
View File

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

3
cmake/example_project/example.h
View File

@ -18,9 +18,12 @@
// This is an interface file for automatic MATLAB wrapper generation. See
// gtsam.h for full documentation and more examples.
#include <example/PrintExamples.h>
namespace example {
class PrintExamples {
PrintExamples();
void sayHello() const;
void sayGoodbye() const;
};

1
cmake/example_project/example/PrintExamples.h
View File

@ -17,6 +17,7 @@
#pragma once
#include <iostream>
#include <string>
namespace example {

88
cmake/obsolete/FindGTSAM.cmake
View File

@ -1,88 +0,0 @@
# This is FindGTSAM.cmake
# DEPRECIATED: Use config file approach to pull in targets from gtsam
# CMake module to locate the GTSAM package
#
# The following cache variables may be set before calling this script:
#
# GTSAM_DIR (or GTSAM_ROOT): (Optional) The install prefix OR source tree of gtsam (e.g. /usr/local or src/gtsam)
# GTSAM_BUILD_NAME: (Optional) If compiling against a source tree, the name of the build directory
# within it (e.g build-debug). Without this defined, this script tries to
# intelligently find the build directory based on the project's build directory name
# or based on the build type (Debug/Release/etc).
#
# The following variables will be defined:
#
# GTSAM_FOUND : TRUE if the package has been successfully found
# GTSAM_INCLUDE_DIR : paths to GTSAM's INCLUDE directories
# GTSAM_LIBS : paths to GTSAM's libraries
#
# NOTES on compiling against an uninstalled GTSAM build tree:
# - A GTSAM source tree will be automatically searched for in the directory
# 'gtsam' next to your project directory, after searching
# CMAKE_INSTALL_PREFIX and $HOME, but before searching /usr/local and /usr.
# - The build directory will be searched first with the same name as your
# project's build directory, e.g. if you build from 'MyProject/build-optimized',
# 'gtsam/build-optimized' will be searched first. Next, a build directory for
# your project's build type, e.g. if CMAKE_BUILD_TYPE in your project is
# 'Release', then 'gtsam/build-release' will be searched next. Finally, plain
# 'gtsam/build' will be searched.
# - You can control the gtsam build directory name directly by defining the CMake
# cache variable 'GTSAM_BUILD_NAME', then only 'gtsam/${GTSAM_BUILD_NAME} will
# be searched.
# - Use the standard CMAKE_PREFIX_PATH, or GTSAM_DIR, to find a specific gtsam
# directory.
# Get path suffixes to help look for gtsam
if(GTSAM_BUILD_NAME)
set(gtsam_build_names "${GTSAM_BUILD_NAME}/gtsam")
else()
# lowercase build type
string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_suffix)
# build suffix of this project
get_filename_component(my_build_name "${CMAKE_BINARY_DIR}" NAME)
set(gtsam_build_names "${my_build_name}/gtsam" "build-${build_type_suffix}/gtsam" "build/gtsam")
endif()
# Use GTSAM_ROOT or GTSAM_DIR equivalently
if(GTSAM_ROOT AND NOT GTSAM_DIR)
set(GTSAM_DIR "${GTSAM_ROOT}")
endif()
if(GTSAM_DIR)
# Find include dirs
find_path(GTSAM_INCLUDE_DIR gtsam/inference/FactorGraph.h
PATHS "${GTSAM_DIR}/include" "${GTSAM_DIR}" NO_DEFAULT_PATH
DOC "GTSAM include directories")
# Find libraries
find_library(GTSAM_LIBS NAMES gtsam
HINTS "${GTSAM_DIR}/lib" "${GTSAM_DIR}" NO_DEFAULT_PATH
PATH_SUFFIXES ${gtsam_build_names}
DOC "GTSAM libraries")
else()
# Find include dirs
set(extra_include_paths ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/include /usr/include)
find_path(GTSAM_INCLUDE_DIR gtsam/inference/FactorGraph.h
PATHS ${extra_include_paths}
DOC "GTSAM include directories")
if(NOT GTSAM_INCLUDE_DIR)
message(STATUS "Searched for gtsam headers in default paths plus ${extra_include_paths}")
endif()
# Find libraries
find_library(GTSAM_LIBS NAMES gtsam
HINTS ${CMAKE_INSTALL_PREFIX}/lib "$ENV{HOME}/lib" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/lib /usr/lib
PATH_SUFFIXES ${gtsam_build_names}
DOC "GTSAM libraries")
endif()
# handle the QUIETLY and REQUIRED arguments and set GTSAM_FOUND to TRUE
# if all listed variables are TRUE
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(GTSAM DEFAULT_MSG
GTSAM_LIBS GTSAM_INCLUDE_DIR)

88
cmake/obsolete/FindGTSAM_UNSTABLE.cmake
View File

@ -1,88 +0,0 @@
# This is FindGTSAM_UNSTABLE.cmake
# DEPRECIATED: Use config file approach to pull in targets from gtsam
# CMake module to locate the GTSAM_UNSTABLE package
#
# The following cache variables may be set before calling this script:
#
# GTSAM_UNSTABLE_DIR (or GTSAM_UNSTABLE_ROOT): (Optional) The install prefix OR source tree of gtsam_unstable (e.g. /usr/local or src/gtsam_unstable)
# GTSAM_UNSTABLE_BUILD_NAME: (Optional) If compiling against a source tree, the name of the build directory
# within it (e.g build-debug). Without this defined, this script tries to
# intelligently find the build directory based on the project's build directory name
# or based on the build type (Debug/Release/etc).
#
# The following variables will be defined:
#
# GTSAM_UNSTABLE_FOUND : TRUE if the package has been successfully found
# GTSAM_UNSTABLE_INCLUDE_DIR : paths to GTSAM_UNSTABLE's INCLUDE directories
# GTSAM_UNSTABLE_LIBS : paths to GTSAM_UNSTABLE's libraries
#
# NOTES on compiling against an uninstalled GTSAM_UNSTABLE build tree:
# - A GTSAM_UNSTABLE source tree will be automatically searched for in the directory
# 'gtsam_unstable' next to your project directory, after searching
# CMAKE_INSTALL_PREFIX and $HOME, but before searching /usr/local and /usr.
# - The build directory will be searched first with the same name as your
# project's build directory, e.g. if you build from 'MyProject/build-optimized',
# 'gtsam_unstable/build-optimized' will be searched first. Next, a build directory for
# your project's build type, e.g. if CMAKE_BUILD_TYPE in your project is
# 'Release', then 'gtsam_unstable/build-release' will be searched next. Finally, plain
# 'gtsam_unstable/build' will be searched.
# - You can control the gtsam build directory name directly by defining the CMake
# cache variable 'GTSAM_UNSTABLE_BUILD_NAME', then only 'gtsam/${GTSAM_UNSTABLE_BUILD_NAME} will
# be searched.
# - Use the standard CMAKE_PREFIX_PATH, or GTSAM_UNSTABLE_DIR, to find a specific gtsam
# directory.
# Get path suffixes to help look for gtsam_unstable
if(GTSAM_UNSTABLE_BUILD_NAME)
set(gtsam_unstable_build_names "${GTSAM_UNSTABLE_BUILD_NAME}/gtsam_unstable")
else()
# lowercase build type
string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_suffix)
# build suffix of this project
get_filename_component(my_build_name "${CMAKE_BINARY_DIR}" NAME)
set(gtsam_unstable_build_names "${my_build_name}/gtsam_unstable" "build-${build_type_suffix}/gtsam_unstable" "build/gtsam_unstable")
endif()
# Use GTSAM_UNSTABLE_ROOT or GTSAM_UNSTABLE_DIR equivalently
if(GTSAM_UNSTABLE_ROOT AND NOT GTSAM_UNSTABLE_DIR)
set(GTSAM_UNSTABLE_DIR "${GTSAM_UNSTABLE_ROOT}")
endif()
if(GTSAM_UNSTABLE_DIR)
# Find include dirs
find_path(GTSAM_UNSTABLE_INCLUDE_DIR gtsam_unstable/base/DSF.h
PATHS "${GTSAM_UNSTABLE_DIR}/include" "${GTSAM_UNSTABLE_DIR}" NO_DEFAULT_PATH
DOC "GTSAM_UNSTABLE include directories")
# Find libraries
find_library(GTSAM_UNSTABLE_LIBS NAMES gtsam_unstable
HINTS "${GTSAM_UNSTABLE_DIR}/lib" "${GTSAM_UNSTABLE_DIR}" NO_DEFAULT_PATH
PATH_SUFFIXES ${gtsam_unstable_build_names}
DOC "GTSAM_UNSTABLE libraries")
else()
# Find include dirs
set(extra_include_paths ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/include /usr/include)
find_path(GTSAM_UNSTABLE_INCLUDE_DIR gtsam_unstable/base/DSF.h
PATHS ${extra_include_paths}
DOC "GTSAM_UNSTABLE include directories")
if(NOT GTSAM_UNSTABLE_INCLUDE_DIR)
message(STATUS "Searched for gtsam_unstable headers in default paths plus ${extra_include_paths}")
endif()
# Find libraries
find_library(GTSAM_UNSTABLE_LIBS NAMES gtsam_unstable
HINTS ${CMAKE_INSTALL_PREFIX}/lib "$ENV{HOME}/lib" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/lib /usr/lib
PATH_SUFFIXES ${gtsam_unstable_build_names}
DOC "GTSAM_UNSTABLE libraries")
endif()
# handle the QUIETLY and REQUIRED arguments and set GTSAM_UNSTABLE_FOUND to TRUE
# if all listed variables are TRUE
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(GTSAM_UNSTABLE DEFAULT_MSG
GTSAM_UNSTABLE_LIBS GTSAM_UNSTABLE_INCLUDE_DIR)

10
cmake/obsolete/GtsamTestingObsolete.cmake
View File

@ -14,7 +14,7 @@ function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests)
set(is_test TRUE)
# Put check target in Visual Studio solution folder
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
file(RELATIVE_PATH relative_path "${GTSAM_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET check.${subdir} PROPERTY FOLDER "${relative_path}")
# Link with CppUnitLite - pulled from gtsam installation
@ -125,7 +125,7 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l
endif()
# Add TOPSRCDIR
set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"")
# Disable building during make all/install
if (GTSAM_DISABLE_TESTS_ON_INSTALL)
@ -149,7 +149,7 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l
endif()
# Set up Visual Studio folders
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
file(RELATIVE_PATH relative_path "${GTSAM_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
endif()
endforeach(script_src)
@ -173,7 +173,7 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l
set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=static no_main")
# Add TOPSRCDIR
set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"")
# Add test
add_dependencies(${target_prefix}.${group} ${script_bin})
@ -187,7 +187,7 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l
# Set up Visual Studio folders
if(MSVC)
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
file(RELATIVE_PATH relative_path "${GTSAM_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
source_group("" FILES ${script_srcs} ${script_headers})
endif()

20
cython/CMakeLists.txt
View File

@ -7,6 +7,11 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
add_subdirectory(gtsam_eigency)
include_directories(${PROJECT_BINARY_DIR}/cython/gtsam_eigency)
# Fix for error "C1128: number of sections exceeded object file format limit"
if(MSVC)
add_compile_options(/bigobj)
endif()
# wrap gtsam
add_custom_target(gtsam_header DEPENDS "../gtsam.h")
wrap_and_install_library_cython("../gtsam.h" # interface_header
@ -19,22 +24,25 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
# wrap gtsam_unstable
if(GTSAM_BUILD_UNSTABLE)
add_custom_target(gtsam_unstable_header DEPENDS "../gtsam_unstable/gtsam_unstable.h")
set(GTSAM_UNSTABLE_IMPORT "from gtsam_unstable import *")
wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header
"from gtsam.gtsam cimport *" # extra imports
"${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path
"${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" # install path
gtsam_unstable # library to link with
"gtsam_unstable;gtsam_unstable_header;cythonize_gtsam" # dependencies to be built before wrapping
)
# for some reasons cython gtsam_unstable can't find gtsam/gtsam.pxd without doing this
file(WRITE ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py "")
endif()
file(READ "${PROJECT_SOURCE_DIR}/cython/requirements.txt" CYTHON_INSTALL_REQUIREMENTS)
file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS)
# Install the custom-generated __init__.py
# This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable
configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py)
install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam")
configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py COPYONLY)
configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py COPYONLY)
configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py)
install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}")
# install scripts and tests
install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
endif ()

145
cython/README.md
View File

@ -1,27 +1,36 @@
# Python Wrapper
This is the Cython/Python wrapper around the GTSAM C++ library.
INSTALL
=======
## Install
- if you want to build the gtsam python library for a specific python version (eg 2.7), use the `-DGTSAM_PYTHON_VERSION=2.7` option when running `cmake` otherwise the default interpreter will be used.
- If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment) then the environment should be active while building gtsam.
- This wrapper needs Cython(>=0.25.2), backports_abc>=0.5, and numpy. These can be installed as follows:
```bash
pip install -r <gtsam_folder>/cython/requirements.txt
```
- For compatiblity with gtsam's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git),
- For compatibility with gtsam's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git),
named **gtsam_eigency**, to interface between C++'s Eigen and Python's numpy.
- Build and install gtsam using cmake with GTSAM_INSTALL_CYTHON_TOOLBOX enabled.
The wrapped module will be installed to GTSAM_CYTHON_INSTALL_PATH, which is
by default: <your CMAKE_INSTALL_PREFIX>/cython
- Build and install gtsam using cmake with `GTSAM_INSTALL_CYTHON_TOOLBOX` enabled.
The wrapped module will be installed to `GTSAM_CYTHON_INSTALL_PATH`, which is
by default: `<your CMAKE_INSTALL_PREFIX>/cython`
- Modify your PYTHONPATH to include the GTSAM_CYTHON_INSTALL_PATH:
- To use the library without installing system-wide: modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`:
```bash
export PYTHONPATH=$PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH>
```
- To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install`
- (the same command can be used to install into a virtual environment if it is active)
- note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install gtsam to a subdirectory of the build directory.
- if you run `setup.py` from the build directory rather than the installation directory, the script will warn you with the message: `setup.py is being run from an unexpected location`.
Before `make install` is run, not all the components of the package have been copied across, so running `setup.py` from the build directory would result in an incomplete package.
## Unit Tests
UNIT TESTS
==========
The Cython toolbox also has a small set of unit tests located in the
test directory. To run them:
@ -30,11 +39,11 @@ test directory. To run them:
python -m unittest discover
```
WRITING YOUR OWN SCRIPTS
========================
## Writing Your Own Scripts
See the tests for examples.
## Some important notes:
### Some Important Notes:
- Vector/Matrix:
+ GTSAM expects double-precision floating point vectors and matrices.
@ -55,12 +64,12 @@ Examples: noiseModel_Gaussian, noiseModel_mEstimator_Tukey
- Casting from a base class to a derive class must be done explicitly.
Examples:
```Python
noiseBase = factor.get_noiseModel()
noiseBase = factor.noiseModel()
noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase)
```
WRAPPING YOUR OWN PROJECT THAT USES GTSAM
=========================================
## Wrapping Your Own Project That Uses GTSAM
- Set PYTHONPATH to include ${GTSAM_CYTHON_INSTALL_PATH}
+ so that it can find gtsam Cython header: gtsam/gtsam.pxd
@ -81,8 +90,8 @@ wrap_and_install_library_cython("your_project_interface.h"
#Optional: install_cython_scripts and install_cython_files. See GtsamCythonWrap.cmake.
```
KNOWN ISSUES
============
## KNOWN ISSUES
- Doesn't work with python3 installed from homebrew
- size-related issue: can only wrap up to a certain number of classes: up to mEstimator!
- Guess: 64 vs 32b? disutils Compiler flags?
@ -92,63 +101,55 @@ KNOWN ISSUES
- support these constructors by default and declare "delete" for special classes?
TODO
=====
☐ allow duplication of parent' functions in child classes. Not allowed for now due to conflicts in Cython.
☐ a common header for boost shared_ptr? (Or wait until everything is switched to std::shared_ptr in gtsam?)
☐ inner namespaces ==> inner packages?
☐ Wrap fixed-size Matrices/Vectors?
### TODO
- [ ] allow duplication of parent' functions in child classes. Not allowed for now due to conflicts in Cython.
- [ ] a common header for boost shared_ptr? (Or wait until everything is switched to std::shared_ptr in gtsam?)
- [ ] inner namespaces ==> inner packages?
- [ ] Wrap fixed-size Matrices/Vectors?
Completed/Cancelled:
=====
✔ Fix Python tests: don't use " import <package> * ": Bad style!!! @done (18-03-17 19:50)
✔ Unit tests for cython wrappers @done (18-03-17 18:45) -- simply compare generated files
✔ Wrap unstable @done (18-03-17 15:30)
✔ Unify cython/gtsam.h and the original gtsam.h @done (18-03-17 15:30)
✔ 18-03-17: manage to unify the two versions by removing std container stubs from the matlab version,and keeping KeyList/KeyVector/KeySet as in the matlab version. Probably Cython 0.25 fixes the casting problem.
✔ 06-03-17: manage to remove the requirements for default and copy constructors
✘ 25-11-16:
Try to unify but failed. Main reasons are: Key/size_t, std containers, KeyVector/KeyList/KeySet.
Matlab doesn't need to know about Key, but I can't make Cython to ignore Key as it couldn't cast KeyVector, i.e. FastVector<Key>, to FastVector<size_t>.
✘ Marginal and JointMarginal: revert changes @failed (17-03-17 11:00) -- Cython does need a default constructor! It produces cpp code like this: ```gtsam::JointMarginal __pyx_t_1;``` Users don't have to wrap this constructor, however.
✔ Convert input numpy Matrix/Vector to float dtype and storage order 'F' automatically, cannot crash! @done (15-03-17 13:00)
✔ Remove requirements.txt - Frank: don't bother with only 2 packages and a special case for eigency! @done (08-03-17 10:30)
✔ CMake install script @done (25-11-16 02:30)
✘ [REFACTOR] better name for uninstantiateClass: very vague!! @cancelled (25-11-16 02:30) -- lazy
✘ forward declaration? @cancelled (23-11-16 13:00) - nothing to do, seem to work?
✔ wrap VariableIndex: why is it in inference? If need to, shouldn't have constructors to specific FactorGraphs @done (23-11-16 13:00)
✔ Global functions @done (22-11-16 21:00)
✔ [REFACTOR] typesEqual --> isSameSignature @done (22-11-16 21:00)
✔ Proper overloads (constructors, static methods, methods) @done (20-11-16 21:00)
✔ Allow overloading methods. The current solution is annoying!!! @done (20-11-16 21:00)
✔ Casting from parent and grandparents @done (16-11-16 17:00)
✔ Allow overloading constructors. The current solution is annoying!!! @done (16-11-16 17:00)
✔ Support "print obj" @done (16-11-16 17:00)
✔ methods for FastVector: at, [], ... @done (16-11-16 17:00)
✔ Cython: Key and size_t: traits<size_t> doesn't exist @done (16-09-12 18:34)
✔ KeyVector, KeyList, KeySet... @done (16-09-13 17:19)
✔ [Nice to have] parse typedef @done (16-09-13 17:19)
✔ ctypedef at correct places @done (16-09-12 18:34)
✔ expand template variable type in constructor/static methods? @done (16-09-12 18:34)
✔ NonlinearOptimizer: copy constructor deleted!!! @done (16-09-13 17:20)
✔ Value: no default constructor @done (16-09-13 17:20)
✔ ctypedef PriorFactor[Vector] PriorFactorVector @done (16-09-19 12:25)
✔ Delete duplicate methods in derived class @done (16-09-12 13:38)
✔ Fix return properly @done (16-09-11 17:14)
✔ handle pair @done (16-09-11 17:14)
✔ Eigency: ambiguous call: A(const T&) A(const Vector& v) and Eigency A(Map[Vector]& v) @done (16-09-11 07:59)
✔ Eigency: Constructor: ambiguous construct from Vector/Matrix @done (16-09-11 07:59)
✔ Eigency: Fix method template of Vector/Matrix: template argument is [Vector] while arugment is Map[Vector] @done (16-09-11 08:22)
✔ Robust noise: copy assignment operator is deleted because of shared_ptr of the abstract Base class @done (16-09-10 09:05)
✘ Cython: Constructor: generate default constructor? (hack: if it's serializable?) @cancelled (16-09-13 17:20)
✘ Eigency: Map[] to Block @created(16-09-10 07:59) @cancelled (16-09-11 08:28)
### Completed/Cancelled:
- [x] Fix Python tests: don't use " import <package> * ": Bad style!!! (18-03-17 19:50)
- [x] Unit tests for cython wrappers @done (18-03-17 18:45) -- simply compare generated files
- [x] Wrap unstable @done (18-03-17 15:30)
- [x] Unify cython/gtsam.h and the original gtsam.h @done (18-03-17 15:30)
- [x] 18-03-17: manage to unify the two versions by removing std container stubs from the matlab version,and keeping KeyList/KeyVector/KeySet as in the matlab version. Probably Cython 0.25 fixes the casting problem.
- [x] 06-03-17: manage to remove the requirements for default and copy constructors
- [ ] 25-11-16: Try to unify but failed. Main reasons are: Key/size_t, std containers, KeyVector/KeyList/KeySet. Matlab doesn't need to know about Key, but I can't make Cython to ignore Key as it couldn't cast KeyVector, i.e. FastVector<Key>, to FastVector<size_t>.
- [ ] Marginal and JointMarginal: revert changes @failed (17-03-17 11:00) -- Cython does need a default constructor! It produces cpp code like this: ```gtsam::JointMarginal __pyx_t_1;``` Users don't have to wrap this constructor, however.
- [x] Convert input numpy Matrix/Vector to float dtype and storage order 'F' automatically, cannot crash! @done (15-03-17 13:00)
- [x] Remove requirements.txt - Frank: don't bother with only 2 packages and a special case for eigency! @done (08-03-17 10:30)
- [x] CMake install script @done (25-11-16 02:30)
- [ ] [REFACTOR] better name for uninstantiateClass: very vague!! @cancelled (25-11-16 02:30) -- lazy
- [ ] forward declaration? @cancelled (23-11-16 13:00) - nothing to do, seem to work?
- [x] wrap VariableIndex: why is it in inference? If need to, shouldn't have constructors to specific FactorGraphs @done (23-11-16 13:00)
- [x] Global functions @done (22-11-16 21:00)
- [x] [REFACTOR] typesEqual --> isSameSignature @done (22-11-16 21:00)
- [x] Proper overloads (constructors, static methods, methods) @done (20-11-16 21:00)
- [x] Allow overloading methods. The current solution is annoying!!! @done (20-11-16 21:00)
- [x] Casting from parent and grandparents @done (16-11-16 17:00)
- [x] Allow overloading constructors. The current solution is annoying!!! @done (16-11-16 17:00)
- [x] Support "print obj" @done (16-11-16 17:00)
- [x] methods for FastVector: at, [], ... @done (16-11-16 17:00)
- [x] Cython: Key and size_t: traits<size_t> doesn't exist @done (16-09-12 18:34)
- [x] KeyVector, KeyList, KeySet... @done (16-09-13 17:19)
- [x] [Nice to have] parse typedef @done (16-09-13 17:19)
- [x] ctypedef at correct places @done (16-09-12 18:34)
- [x] expand template variable type in constructor/static methods? @done (16-09-12 18:34)
- [x] NonlinearOptimizer: copy constructor deleted!!! @done (16-09-13 17:20)
- [x] Value: no default constructor @done (16-09-13 17:20)
- [x] ctypedef PriorFactor[Vector] PriorFactorVector @done (16-09-19 12:25)
- [x] Delete duplicate methods in derived class @done (16-09-12 13:38)
- [x] Fix return properly @done (16-09-11 17:14)
- [x] handle pair @done (16-09-11 17:14)
- [x] Eigency: ambiguous call: A(const T&) A(const Vector& v) and Eigency A(Map[Vector]& v) @done (16-09-11 07:59)
- [x] Eigency: Constructor: ambiguous construct from Vector/Matrix @done (16-09-11 07:59)
- [x] Eigency: Fix method template of Vector/Matrix: template argument is [Vector] while arugment is Map[Vector] @done (16-09-11 08:22)
- [x] Robust noise: copy assignment operator is deleted because of shared_ptr of the abstract Base class @done (16-09-10 09:05)
- [ ] Cython: Constructor: generate default constructor? (hack: if it's serializable?) @cancelled (16-09-13 17:20)
- [ ] Eigency: Map[] to Block @created(16-09-10 07:59) @cancelled (16-09-11 08:28)
- inference before symbolic/linear
- what's the purpose of "virtual" ??
Installation:
☐ Prerequisite:
- Users create venv and pip install requirements before compiling
- Wrap cython script in gtsam/cython folder
☐ Install built module into venv?

26
cython/gtsam/__init__.py Normal file
View File

@ -0,0 +1,26 @@
from .gtsam import *
try:
import gtsam_unstable
def _deprecated_wrapper(item, name):
def wrapper(*args, **kwargs):
from warnings import warn
message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) +
'Please import it from gtsam_unstable.')
warn(message)
return item(*args, **kwargs)
return wrapper
for name in dir(gtsam_unstable):
if not name.startswith('__'):
item = getattr(gtsam_unstable, name)
if callable(item):
item = _deprecated_wrapper(item, name)
globals()[name] = item
except ImportError:
pass

2
cython/gtsam/__init__.py.in
View File

@ -1,2 +0,0 @@
from gtsam import *
${GTSAM_UNSTABLE_IMPORT}

118
cython/gtsam/examples/DogLegOptimizerExample.py Normal file
View File

@ -0,0 +1,118 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Example comparing DoglegOptimizer with Levenberg-Marquardt.
Author: Frank Dellaert
"""
# pylint: disable=no-member, invalid-name
import math
import argparse
import gtsam
import matplotlib.pyplot as plt
import numpy as np
def run(args):
"""Test Dogleg vs LM, inspired by issue #452."""
# print parameters
print("num samples = {}, deltaInitial = {}".format(
args.num_samples, args.delta))
# Ground truth solution
T11 = gtsam.Pose2(0, 0, 0)
T12 = gtsam.Pose2(1, 0, 0)
T21 = gtsam.Pose2(0, 1, 0)
T22 = gtsam.Pose2(1, 1, 0)
# Factor graph
graph = gtsam.NonlinearFactorGraph()
# Priors
prior = gtsam.noiseModel_Isotropic.Sigma(3, 1)
graph.add(gtsam.PriorFactorPose2(11, T11, prior))
graph.add(gtsam.PriorFactorPose2(21, T21, prior))
# Odometry
model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.01, 0.01, 0.3]))
graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model))
graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model))
# Range
model_rho = gtsam.noiseModel_Isotropic.Sigma(1, 0.01)
graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho))
params = gtsam.DoglegParams()
params.setDeltaInitial(args.delta) # default is 10
# Add progressively more noise to ground truth
sigmas = [0.01, 0.1, 0.2, 0.5, 1, 2, 5, 10, 20]
n = len(sigmas)
p_dl, s_dl, p_lm, s_lm = [0]*n, [0]*n, [0]*n, [0]*n
for i, sigma in enumerate(sigmas):
dl_fails, lm_fails = 0, 0
# Attempt num_samples optimizations for both DL and LM
for _attempt in range(args.num_samples):
initial = gtsam.Values()
initial.insert(11, T11.retract(np.random.normal(0, sigma, 3)))
initial.insert(12, T12.retract(np.random.normal(0, sigma, 3)))
initial.insert(21, T21.retract(np.random.normal(0, sigma, 3)))
initial.insert(22, T22.retract(np.random.normal(0, sigma, 3)))
# Run dogleg optimizer
dl = gtsam.DoglegOptimizer(graph, initial, params)
result = dl.optimize()
dl_fails += graph.error(result) > 1e-9
# Run
lm = gtsam.LevenbergMarquardtOptimizer(graph, initial)
result = lm.optimize()
lm_fails += graph.error(result) > 1e-9
# Calculate Bayes estimate of success probability
# using a beta prior of alpha=0.5, beta=0.5
alpha, beta = 0.5, 0.5
v = args.num_samples+alpha+beta
p_dl[i] = (args.num_samples-dl_fails+alpha)/v
p_lm[i] = (args.num_samples-lm_fails+alpha)/v
def stddev(p):
"""Calculate standard deviation."""
return math.sqrt(p*(1-p)/(1+v))
s_dl[i] = stddev(p_dl[i])
s_lm[i] = stddev(p_lm[i])
fmt = "sigma= {}:\tDL success {:.2f}% +/- {:.2f}%, LM success {:.2f}% +/- {:.2f}%"
print(fmt.format(sigma,
100*p_dl[i], 100*s_dl[i],
100*p_lm[i], 100*s_lm[i]))
if args.plot:
fig, ax = plt.subplots()
dl_plot = plt.errorbar(sigmas, p_dl, yerr=s_dl, label="Dogleg")
lm_plot = plt.errorbar(sigmas, p_lm, yerr=s_lm, label="LM")
plt.title("Dogleg emprical success vs. LM")
plt.legend(handles=[dl_plot, lm_plot])
ax.set_xlim(0, sigmas[-1]+1)
ax.set_ylim(0, 1)
plt.show()
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Compare Dogleg and LM success rates")
parser.add_argument("-n", "--num_samples", type=int, default=1000,
help="Number of samples for each sigma")
parser.add_argument("-d", "--delta", type=float, default=10.0,
help="Initial delta for dogleg")
parser.add_argument("-p", "--plot", action="store_true",
help="Flag to plot results")
args = parser.parse_args()
run(args)

56
cython/gtsam/examples/GPSFactorExample.py Normal file
View File

@ -0,0 +1,56 @@
"""
GTSAM Copyright 2010-2018, 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
Simple robot motion example, with prior and one GPS measurements
Author: Mandy Xie
"""
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import numpy as np
import gtsam
import matplotlib.pyplot as plt
import gtsam.utils.plot as gtsam_plot
# ENU Origin is where the plane was in hold next to runway
lat0 = 33.86998
lon0 = -84.30626
h0 = 274
# Create noise models
GPS_NOISE = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
PRIOR_NOISE = gtsam.noiseModel_Isotropic.Sigma(6, 0.25)
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
# Add a prior on the first point, setting it to the origin
# A prior factor consists of a mean and a noise model (covariance matrix)
priorMean = gtsam.Pose3() # prior at origin
graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE))
# Add GPS factors
gps = gtsam.Point3(lat0, lon0, h0)
graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE))
print("\nFactor Graph:\n{}".format(graph))
# Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values
initial = gtsam.Values()
initial.insert(1, gtsam.Pose3())
print("\nInitial Estimate:\n{}".format(initial))
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))

86
python/gtsam_examples/ImuFactorExample.py → cython/gtsam/examples/ImuFactorExample.py
View File

@ -1,49 +1,63 @@
"""
A script validating the ImuFactor inference.
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
A script validating and demonstrating the ImuFactor inference.
Author: Frank Dellaert, Varun Agrawal
"""
from __future__ import print_function
import math
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import math
import gtsam
from gtsam_utils import plotPose3
from PreintegrationExample import PreintegrationExample, POSES_FIG
import matplotlib.pyplot as plt
import numpy as np
from gtsam import symbol_shorthand_B as B
from gtsam import symbol_shorthand_V as V
from gtsam import symbol_shorthand_X as X
from gtsam.utils.plot import plot_pose3
from mpl_toolkits.mplot3d import Axes3D
from PreintegrationExample import POSES_FIG, PreintegrationExample
BIAS_KEY = B(0)
# shorthand symbols:
BIAS_KEY = int(gtsam.Symbol('b', 0))
V = lambda j: int(gtsam.Symbol('v', j))
X = lambda i: int(gtsam.Symbol('x', i))
np.set_printoptions(precision=3, suppress=True)
class ImuFactorExample(PreintegrationExample):
def __init__(self):
self.velocity = np.array([2, 0, 0])
self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
self.priorNoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
self.velNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
# Choose one of these twists to change scenario:
zero_twist = (np.zeros(3), np.zeros(3))
forward_twist = (np.zeros(3), self.velocity)
loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity)
sick_twist = (np.array([math.radians(30), -math.radians(30), 0]), self.velocity)
sick_twist = (
np.array([math.radians(30), -math.radians(30), 0]), self.velocity)
accBias = np.array([-0.3, 0.1, 0.2])
gyroBias = np.array([0.1, 0.3, -0.1])
bias = gtsam.ConstantBias(accBias, gyroBias)
bias = gtsam.imuBias_ConstantBias(accBias, gyroBias)
dt = 1e-2
super(ImuFactorExample, self).__init__(sick_twist, bias, dt)
def addPrior(self, i, graph):
state = self.scenario.navState(i)
graph.push_back(gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
graph.push_back(gtsam.PriorFactorVector3(V(i), state.velocity(), self.velNoise))
graph.push_back(gtsam.PriorFactorPose3(
X(i), state.pose(), self.priorNoise))
graph.push_back(gtsam.PriorFactorVector(
V(i), state.velocity(), self.velNoise))
def run(self):
graph = gtsam.NonlinearFactorGraph()
@ -51,16 +65,20 @@ class ImuFactorExample(PreintegrationExample):
# initialize data structure for pre-integrated IMU measurements
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
H9 = gtsam.OptionalJacobian9();
T = 12
num_poses = T + 1 # assumes 1 factor per second
initial = gtsam.Values()
initial.insert(BIAS_KEY, self.actualBias)
for i in range(num_poses):
state_i = self.scenario.navState(float(i))
initial.insert(X(i), state_i.pose())
initial.insert(V(i), state_i.velocity())
poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1)
pose = state_i.pose().compose(poseNoise)
velocity = state_i.velocity() + np.random.randn(3)*0.1
initial.insert(X(i), pose)
initial.insert(V(i), velocity)
# simulate the loop
i = 0 # state index
@ -71,6 +89,12 @@ class ImuFactorExample(PreintegrationExample):
measuredAcc = self.runner.measuredSpecificForce(t)
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1)
actual_state_i = gtsam.NavState(
actual_state_i.pose().compose(poseNoise),
actual_state_i.velocity() + np.random.randn(3)*0.1)
# Plot IMU many times
if k % 10 == 0:
self.plotImu(t, measuredOmega, measuredAcc)
@ -81,12 +105,12 @@ class ImuFactorExample(PreintegrationExample):
# create IMU factor every second
if (k + 1) % int(1 / self.dt) == 0:
factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim)
factor = gtsam.ImuFactor(X(i), V(i), X(
i + 1), V(i + 1), BIAS_KEY, pim)
graph.push_back(factor)
if True:
print(factor)
H2 = gtsam.OptionalJacobian96();
print(pim.predict(actual_state_i, self.actualBias, H9, H2))
print(pim.predict(actual_state_i, self.actualBias))
pim.resetIntegration()
actual_state_i = self.scenario.navState(t + self.dt)
i += 1
@ -105,19 +129,25 @@ class ImuFactorExample(PreintegrationExample):
marginals = gtsam.Marginals(graph, result)
print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY))
for i in range(num_poses):
print("Covariance on pose {}:\n{}\n".format(i, marginals.marginalCovariance(X(i))))
print("Covariance on vel {}:\n{}\n".format(i, marginals.marginalCovariance(V(i))))
print("Covariance on pose {}:\n{}\n".format(
i, marginals.marginalCovariance(X(i))))
print("Covariance on vel {}:\n{}\n".format(
i, marginals.marginalCovariance(V(i))))
# Plot resulting poses
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
plotPose3(POSES_FIG, pose_i, 0.1)
plot_pose3(POSES_FIG, pose_i, 0.1)
i += 1
print(result.atConstantBias(BIAS_KEY))
gtsam.utils.plot.set_axes_equal(POSES_FIG)
print(result.atimuBias_ConstantBias(BIAS_KEY))
plt.ioff()
plt.show()
if __name__ == '__main__':
ImuFactorExample().run()

62
cython/gtsam/examples/ImuFactorExample2.py
View File

@ -8,22 +8,19 @@ from __future__ import print_function
import math
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import gtsam
import gtsam.utils.plot as gtsam_plot
def X(key):
"""Create symbol for pose key."""
return gtsam.symbol(ord('x'), key)
def V(key):
"""Create symbol for velocity key."""
return gtsam.symbol(ord('v'), key)
import matplotlib.pyplot as plt
import numpy as np
from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
PinholeCameraCal3_S2, Point3, Pose3,
PriorFactorConstantBias, PriorFactorPose3,
PriorFactorVector, Rot3, Values)
from gtsam import symbol_shorthand_B as B
from gtsam import symbol_shorthand_V as V
from gtsam import symbol_shorthand_X as X
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
def vector3(x, y, z):
@ -69,8 +66,8 @@ PARAMS.setUse2ndOrderCoriolis(False)
PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0),
gtsam.Point3(0.05, -0.10, 0.20))
DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
Point3(0.05, -0.10, 0.20))
def IMU_example():
@ -78,10 +75,10 @@ def IMU_example():
# Start with a camera on x-axis looking at origin
radius = 30
up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0)
position = gtsam.Point3(radius, 0, 0)
camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2())
up = Point3(0, 0, 1)
target = Point3(0, 0, 0)
position = Point3(radius, 0, 0)
camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
pose_0 = camera.pose()
# Create the set of ground-truth landmarks and poses
@ -90,29 +87,29 @@ def IMU_example():
angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = gtsam.ConstantTwistScenario(
scenario = ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0)
# Create a factor graph
newgraph = gtsam.NonlinearFactorGraph()
newgraph = NonlinearFactorGraph()
# Create (incremental) ISAM2 solver
isam = gtsam.ISAM2()
isam = ISAM2()
# Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initialEstimate = gtsam.Values()
initialEstimate = Values()
# Add a prior on pose x0. This indirectly specifies where the origin is.
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise))
# Add imu priors
biasKey = gtsam.symbol(ord('b'), 0)
biasKey = B(0)
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
biasnoise)
newgraph.push_back(biasprior)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
@ -120,7 +117,7 @@ def IMU_example():
# Calculate with correct initial velocity
n_velocity = vector3(0, angular_velocity * radius, 0)
velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise)
velprior = PriorFactorVector(V(0), n_velocity, velnoise)
newgraph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity)
@ -141,7 +138,7 @@ def IMU_example():
# Add Bias variables periodically
if i % 5 == 0:
biasKey += 1
factor = gtsam.BetweenFactorConstantBias(
factor = BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
newgraph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
@ -154,8 +151,7 @@ def IMU_example():
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
# Add Imu Factor
imufac = gtsam.ImuFactor(
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
newgraph.add(imufac)
# insert new velocity, which is wrong
@ -168,7 +164,7 @@ def IMU_example():
ISAM2_plot(result)
# reset
newgraph = gtsam.NonlinearFactorGraph()
newgraph = NonlinearFactorGraph()
initialEstimate.clear()

17
cython/gtsam/examples/OdometryExample.py
View File

@ -17,6 +17,9 @@ import numpy as np
import gtsam
import matplotlib.pyplot as plt
import gtsam.utils.plot as gtsam_plot
# Create noise models
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
@ -50,3 +53,17 @@ params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))
# 5. Calculate and print marginal covariances for all variables
marginals = gtsam.Marginals(graph, result)
for i in range(1, 4):
print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))
fig = plt.figure(0)
for i in range(1, 4):
gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i))
plt.axis('equal')
plt.show()

334
cython/gtsam/examples/PlanarManipulatorExample.py Normal file
View File

@ -0,0 +1,334 @@
"""
GTSAM Copyright 2010-2018, 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
Kinematics of three-link manipulator with GTSAM poses and product of exponential maps.
Author: Frank Dellaert
"""
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import math
import unittest
from functools import reduce
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import gtsam
import gtsam.utils.plot as gtsam_plot
from gtsam import Pose2
from gtsam.utils.test_case import GtsamTestCase
def vector3(x, y, z):
"""Create 3D double numpy array."""
return np.array([x, y, z], dtype=np.float)
def compose(*poses):
"""Compose all Pose2 transforms given as arguments from left to right."""
return reduce((lambda x, y: x.compose(y)), poses)
def vee(M):
"""Pose2 vee operator."""
return vector3(M[0, 2], M[1, 2], M[1, 0])
def delta(g0, g1):
"""Difference between x,y,,theta components of SE(2) poses."""
return vector3(g1.x() - g0.x(), g1.y() - g0.y(), g1.theta() - g0.theta())
def trajectory(g0, g1, N=20):
""" Create an interpolated trajectory in SE(2), treating x,y, and theta separately.
g0 and g1 are the initial and final pose, respectively.
N is the number of *intervals*
Returns N+1 poses
"""
e = delta(g0, g1)
return [Pose2(g0.x()+e[0]*t, g0.y()+e[1]*t, g0.theta()+e[2]*t) for t in np.linspace(0, 1, N)]
class ThreeLinkArm(object):
"""Three-link arm class."""
def __init__(self):
self.L1 = 3.5
self.L2 = 3.5
self.L3 = 2.5
self.xi1 = vector3(0, 0, 1)
self.xi2 = vector3(self.L1, 0, 1)
self.xi3 = vector3(self.L1+self.L2, 0, 1)
self.sXt0 = Pose2(0, self.L1+self.L2 + self.L3, math.radians(90))
def fk(self, q):
""" Forward kinematics.
Takes numpy array of joint angles, in radians.
"""
sXl1 = Pose2(0, 0, math.radians(90))
l1Zl1 = Pose2(0, 0, q[0])
l1Xl2 = Pose2(self.L1, 0, 0)
l2Zl2 = Pose2(0, 0, q[1])
l2Xl3 = Pose2(self.L2, 0, 0)
l3Zl3 = Pose2(0, 0, q[2])
l3Xt = Pose2(self.L3, 0, 0)
return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt)
def jacobian(self, q):
""" Calculate manipulator Jacobian.
Takes numpy array of joint angles, in radians.
"""
a = q[0]+q[1]
b = a+q[2]
return np.array([[-self.L1*math.cos(q[0]) - self.L2*math.cos(a)-self.L3*math.cos(b),
-self.L1*math.cos(a)-self.L3*math.cos(b),
- self.L3*math.cos(b)],
[-self.L1*math.sin(q[0]) - self.L2*math.sin(a)-self.L3*math.sin(b),
-self.L1*math.sin(a)-self.L3*math.sin(b),
- self.L3*math.sin(b)],
[1, 1, 1]], np.float)
def poe(self, q):
""" Forward kinematics.
Takes numpy array of joint angles, in radians.
"""
l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
def con(self, q):
""" Forward kinematics, conjugation form.
Takes numpy array of joint angles, in radians.
"""
def expmap(x, y, theta):
"""Implement exponential map via conjugation with axis (x,y)."""
return compose(Pose2(x, y, 0), Pose2(0, 0, theta), Pose2(-x, -y, 0))
l1Zl1 = expmap(0.0, 0.0, q[0])
l2Zl2 = expmap(0.0, self.L1, q[1])
l3Zl3 = expmap(0.0, self.L1+self.L2, q[2])
return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
def ik(self, sTt_desired, e=1e-9):
""" Inverse kinematics.
Takes desired Pose2 of tool T with respect to base S.
Optional: mu, gradient descent rate; e: error norm threshold
"""
q = np.radians(vector3(30, -30, 45)) # well within workspace
error = vector3(100, 100, 100)
while np.linalg.norm(error) > e:
error = delta(sTt_desired, self.fk(q))
J = self.jacobian(q)
q -= np.dot(np.linalg.pinv(J), error)
# return result in interval [-pi,pi)
return np.remainder(q+math.pi, 2*math.pi)-math.pi
def manipulator_jacobian(self, q):
""" Calculate manipulator Jacobian.
Takes numpy array of joint angles, in radians.
Returns the manipulator Jacobian of differential twists. When multiplied with
a vector of joint velocities, will yield a single differential twist which is
the spatial velocity d(sTt)/dt * inv(sTt) of the end-effector pose.
Just like always, differential twists can be hatted and multiplied with spatial
coordinates of a point to give the spatial velocity of the point.
"""
l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
# l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
p1 = self.xi1
# p1 = Pose2().Adjoint(self.xi1)
sTl1 = l1Zl1
p2 = sTl1.Adjoint(self.xi2)
sTl2 = compose(l1Zl1, l2Zl2)
p3 = sTl2.Adjoint(self.xi3)
differential_twists = [p1, p2, p3]
return np.stack(differential_twists, axis=1)
def plot(self, fignum, q):
""" Plot arm.
Takes figure number, and numpy array of joint angles, in radians.
"""
fig = plt.figure(fignum)
axes = fig.gca()
sXl1 = Pose2(0, 0, math.radians(90))
t = sXl1.translation()
p1 = np.array([t.x(), t.y()])
gtsam_plot.plot_pose2_on_axes(axes, sXl1)
def plot_line(p, g, color):
t = g.translation()
q = np.array([t.x(), t.y()])
line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
axes.plot(line[:, 0], line[:, 1], color)
return q
l1Zl1 = Pose2(0, 0, q[0])
l1Xl2 = Pose2(self.L1, 0, 0)
sTl2 = compose(sXl1, l1Zl1, l1Xl2)
p2 = plot_line(p1, sTl2, 'r-')
gtsam_plot.plot_pose2_on_axes(axes, sTl2)
l2Zl2 = Pose2(0, 0, q[1])
l2Xl3 = Pose2(self.L2, 0, 0)
sTl3 = compose(sTl2, l2Zl2, l2Xl3)
p3 = plot_line(p2, sTl3, 'g-')
gtsam_plot.plot_pose2_on_axes(axes, sTl3)
l3Zl3 = Pose2(0, 0, q[2])
l3Xt = Pose2(self.L3, 0, 0)
sTt = compose(sTl3, l3Zl3, l3Xt)
plot_line(p3, sTt, 'b-')
gtsam_plot.plot_pose2_on_axes(axes, sTt)
# Create common example configurations.
Q0 = vector3(0, 0, 0)
Q1 = np.radians(vector3(-30, -45, -90))
Q2 = np.radians(vector3(-90, 90, 0))
class TestPose2SLAMExample(GtsamTestCase):
"""Unit tests for functions used below."""
def setUp(self):
self.arm = ThreeLinkArm()
def assertPose2Equals(self, actual, expected, tol=1e-2):
"""Helper function that prints out actual and expected if not equal."""
equal = actual.equals(expected, tol)
if not equal:
raise self.failureException(
"Poses are not equal:\n{}!={}".format(actual, expected))
def test_fk_arm(self):
"""Make sure forward kinematics is correct for some known test configurations."""
# at rest
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
sTt = self.arm.fk(Q0)
self.assertIsInstance(sTt, Pose2)
self.assertPose2Equals(sTt, expected)
# -30, -45, -90
expected = Pose2(5.78, 1.52, math.radians(-75))
sTt = self.arm.fk(Q1)
self.assertPose2Equals(sTt, expected)
def test_jacobian(self):
"""Test Jacobian calculation."""
# at rest
expected = np.array([[-9.5, -6, -2.5], [0, 0, 0], [1, 1, 1]], np.float)
J = self.arm.jacobian(Q0)
np.testing.assert_array_almost_equal(J, expected)
# at -90, 90, 0
expected = np.array([[-6, -6, -2.5], [3.5, 0, 0], [1, 1, 1]], np.float)
J = self.arm.jacobian(Q2)
np.testing.assert_array_almost_equal(J, expected)
def test_con_arm(self):
"""Make sure POE is correct for some known test configurations."""
# at rest
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
sTt = self.arm.con(Q0)
self.assertIsInstance(sTt, Pose2)
self.assertPose2Equals(sTt, expected)
# -30, -45, -90
expected = Pose2(5.78, 1.52, math.radians(-75))
sTt = self.arm.con(Q1)
self.assertPose2Equals(sTt, expected)
def test_poe_arm(self):
"""Make sure POE is correct for some known test configurations."""
# at rest
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
sTt = self.arm.poe(Q0)
self.assertIsInstance(sTt, Pose2)
self.assertPose2Equals(sTt, expected)
# -30, -45, -90
expected = Pose2(5.78, 1.52, math.radians(-75))
sTt = self.arm.poe(Q1)
self.assertPose2Equals(sTt, expected)
def test_ik(self):
"""Check iterative inverse kinematics function."""
# at rest
actual = self.arm.ik(Pose2(0, 2*3.5 + 2.5, math.radians(90)))
np.testing.assert_array_almost_equal(actual, Q0, decimal=2)
# -30, -45, -90
sTt_desired = Pose2(5.78, 1.52, math.radians(-75))
actual = self.arm.ik(sTt_desired)
self.assertPose2Equals(self.arm.fk(actual), sTt_desired)
np.testing.assert_array_almost_equal(actual, Q1, decimal=2)
def test_manipulator_jacobian(self):
"""Test Jacobian calculation."""
# at rest
expected = np.array([[0, 3.5, 7], [0, 0, 0], [1, 1, 1]], np.float)
J = self.arm.manipulator_jacobian(Q0)
np.testing.assert_array_almost_equal(J, expected)
# at -90, 90, 0
expected = np.array(
[[0, 0, 3.5], [0, -3.5, -3.5], [1, 1, 1]], np.float)
J = self.arm.manipulator_jacobian(Q2)
np.testing.assert_array_almost_equal(J, expected)
def run_example():
""" Use trajectory interpolation and then trajectory tracking a la Murray
to move a 3-link arm on a straight line.
"""
# Create arm
arm = ThreeLinkArm()
# Get initial pose using forward kinematics
q = np.radians(vector3(30, -30, 45))
sTt_initial = arm.fk(q)
# Create interpolated trajectory in task space to desired goal pose
sTt_goal = Pose2(2.4, 4.3, math.radians(0))
poses = trajectory(sTt_initial, sTt_goal, 50)
# Setup figure and plot initial pose
fignum = 0
fig = plt.figure(fignum)
axes = fig.gca()
axes.set_xlim(-5, 5)
axes.set_ylim(0, 10)
gtsam_plot.plot_pose2(fignum, arm.fk(q))
# For all poses in interpolated trajectory, calculate dq to move to next pose.
# We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose).
for pose in poses:
sTt = arm.fk(q)
error = delta(sTt, pose)
J = arm.jacobian(q)
q += np.dot(np.linalg.inv(J), error)
arm.plot(fignum, q)
plt.pause(0.01)
plt.pause(10)
if __name__ == "__main__":
run_example()
unittest.main()

15
cython/gtsam/examples/PlanarSLAMExample.py
View File

@ -13,9 +13,10 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
from __future__ import print_function
import numpy as np
import gtsam
import numpy as np
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
# Create noise models
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
@ -26,11 +27,11 @@ MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
graph = gtsam.NonlinearFactorGraph()
# Create the keys corresponding to unknown variables in the factor graph
X1 = gtsam.symbol(ord('x'), 1)
X2 = gtsam.symbol(ord('x'), 2)
X3 = gtsam.symbol(ord('x'), 3)
L1 = gtsam.symbol(ord('l'), 4)
L2 = gtsam.symbol(ord('l'), 5)
X1 = X(1)
X2 = X(2)
X3 = X(3)
L1 = L(4)
L2 = L(5)
# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))

10
cython/gtsam/examples/Pose2SLAMExample.py
View File

@ -19,6 +19,9 @@ import numpy as np
import gtsam
import matplotlib.pyplot as plt
import gtsam.utils.plot as gtsam_plot
def vector3(x, y, z):
"""Create 3d double numpy array."""
@ -85,3 +88,10 @@ print("Final Result:\n{}".format(result))
marginals = gtsam.Marginals(graph, result)
for i in range(1, 6):
print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))
fig = plt.figure(0)
for i in range(1, 6):
gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i))
plt.axis('equal')
plt.show()

88
cython/gtsam/examples/Pose2SLAMExample_g2o.py Normal file
View File

@ -0,0 +1,88 @@
"""
GTSAM Copyright 2010-2018, 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
A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph
and does the optimization. Output is written on a file, in g2o format
"""
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import argparse
import math
import numpy as np
import matplotlib.pyplot as plt
import gtsam
from gtsam.utils import plot
def vector3(x, y, z):
"""Create 3d double numpy array."""
return np.array([x, y, z], dtype=np.float)
parser = argparse.ArgumentParser(
description="A 2D Pose SLAM example that reads input from g2o, "
"converts it to a factor graph and does the optimization. "
"Output is written on a file, in g2o format")
parser.add_argument('-i', '--input', help='input file g2o format')
parser.add_argument('-o', '--output',
help="the path to the output file with optimized graph")
parser.add_argument('-m', '--maxiter', type=int,
help="maximum number of iterations for optimizer")
parser.add_argument('-k', '--kernel', choices=['none', 'huber', 'tukey'],
default="none", help="Type of kernel used")
parser.add_argument("-p", "--plot", action="store_true",
help="Flag to plot results")
args = parser.parse_args()
g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\
else args.input
maxIterations = 100 if args.maxiter is None else args.maxiter
is3D = False
graph, initial = gtsam.readG2o(g2oFile, is3D)
assert args.kernel == "none", "Supplied kernel type is not yet implemented"
# Add prior on the pose having index (key) = 0
priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination")
params.setMaxIterations(maxIterations)
# parameters.setRelativeErrorTol(1e-5)
# Create the optimizer ...
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
# ... and optimize
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
if args.output is None:
print("\nFactor Graph:\n{}".format(graph))
print("\nInitial Estimate:\n{}".format(initial))
print("Final Result:\n{}".format(result))
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
gtsam.writeG2o(graphNoKernel, result, outputFile)
print ("Done!")
if args.plot:
resultPoses = gtsam.utilities_extractPose2(result)
for i in range(resultPoses.shape[0]):
plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
plt.show()

71
cython/gtsam/examples/Pose3SLAMExample_g2o.py Normal file
View File

@ -0,0 +1,71 @@
"""
* @file Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the
* Pose3 using InitializePose3
* @date Jan 17, 2019
* @author Vikrant Shah based on CPP example by Luca Carlone
"""
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import argparse
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import gtsam
from gtsam.utils import plot
def vector6(x, y, z, a, b, c):
"""Create 6d double numpy array."""
return np.array([x, y, z, a, b, c], dtype=np.float)
parser = argparse.ArgumentParser(
description="A 3D Pose SLAM example that reads input from g2o, and "
"initializes Pose3")
parser.add_argument('-i', '--input', help='input file g2o format')
parser.add_argument('-o', '--output',
help="the path to the output file with optimized graph")
parser.add_argument("-p", "--plot", action="store_true",
help="Flag to plot results")
args = parser.parse_args()
g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \
else args.input
is3D = True
graph, initial = gtsam.readG2o(g2oFile, is3D)
# Add Prior on the first key
priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ")
firstKey = initial.keys().at(0)
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
if args.output is None:
print("Final Result:\n{}".format(result))
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
gtsam.writeG2o(graphNoKernel, result, outputFile)
print ("Done!")
if args.plot:
resultPoses = gtsam.utilities_allPose3s(result)
for i in range(resultPoses.size()):
plot.plot_pose3(1, resultPoses.atPose3(i))
plt.show()

35
cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py Normal file
View File

@ -0,0 +1,35 @@
"""
GTSAM Copyright 2010-2018, 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
Initialize PoseSLAM with Chordal init
Author: Luca Carlone, Frank Dellaert (python port)
"""
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import numpy as np
import gtsam
# Read graph from file
g2oFile = gtsam.findExampleDataFile("pose3example.txt")
is3D = True
graph, initial = gtsam.readG2o(g2oFile, is3D)
# Add prior on the first key. TODO: assumes first key ios z
priorModel = gtsam.noiseModel_Diagonal.Variances(
np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
firstKey = initial.keys().at(0)
graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
# Initializing Pose3 - chordal relaxation"
initialization = gtsam.InitializePose3.initialize(graph)
print(initialization)

33
python/gtsam_examples/PreintegrationExample.py → cython/gtsam/examples/PreintegrationExample.py
View File

@ -1,19 +1,26 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
A script validating the Preintegration of IMU measurements
"""
import math
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import gtsam
from gtsam_utils import plotPose3
from gtsam.utils.plot import plot_pose3
IMU_FIG = 1
POSES_FIG = 2
class PreintegrationExample(object):
@staticmethod
@ -22,9 +29,12 @@ class PreintegrationExample(object):
params = gtsam.PreintegrationParams.MakeSharedU(g)
kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW
kAccelSigma = 0.1 / 60 # 10 cm VRW
params.gyroscopeCovariance = kGyroSigma ** 2 * np.identity(3, np.float)
params.accelerometerCovariance = kAccelSigma ** 2 * np.identity(3, np.float)
params.integrationCovariance = 0.0000001 ** 2 * np.identity(3, np.float)
params.setGyroscopeCovariance(
kGyroSigma ** 2 * np.identity(3, np.float))
params.setAccelerometerCovariance(
kAccelSigma ** 2 * np.identity(3, np.float))
params.setIntegrationCovariance(
0.0000001 ** 2 * np.identity(3, np.float))
return params
def __init__(self, twist=None, bias=None, dt=1e-2):
@ -52,16 +62,16 @@ class PreintegrationExample(object):
# Create runner
self.g = 10 # simple gravity constant
self.params = self.defaultParams(self.g)
ptr = gtsam.ScenarioPointer(self.scenario)
if bias is not None:
self.actualBias = bias
else:
accBias = np.array([0, 0.1, 0])
gyroBias = np.array([0, 0, 0])
self.actualBias = gtsam.ConstantBias(accBias, gyroBias)
self.actualBias = gtsam.imuBias_ConstantBias(accBias, gyroBias)
self.runner = gtsam.ScenarioRunner(ptr, self.params, self.dt, self.actualBias)
self.runner = gtsam.ScenarioRunner(
self.scenario, self.params, self.dt, self.actualBias)
def plotImu(self, t, measuredOmega, measuredAcc):
plt.figure(IMU_FIG)
@ -99,9 +109,9 @@ class PreintegrationExample(object):
def plotGroundTruthPose(self, t):
# plot ground truth pose, as well as prediction from integrated IMU measurements
actualPose = self.scenario.pose(t)
plotPose3(POSES_FIG, actualPose, 0.3)
plot_pose3(POSES_FIG, actualPose, 0.3)
t = actualPose.translation()
self.maxDim = max([abs(t[0]), abs(t[1]), abs(t[2]), self.maxDim])
self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim])
ax = plt.gca()
ax.set_xlim3d(-self.maxDim, self.maxDim)
ax.set_ylim3d(-self.maxDim, self.maxDim)
@ -120,10 +130,11 @@ class PreintegrationExample(object):
self.plotGroundTruthPose(t)
pim = self.runner.integrate(t, self.actualBias, True)
predictedNavState = self.runner.predict(pim, self.actualBias)
plotPose3(POSES_FIG, predictedNavState.pose(), 0.1)
plot_pose3(POSES_FIG, predictedNavState.pose(), 0.1)
plt.ioff()
plt.show()
if __name__ == '__main__':
PreintegrationExample().run()

57
cython/gtsam/examples/README.md
View File

@ -1,4 +1,57 @@
These examples are almost identical to the old handwritten python wrapper
examples. However, there are just some slight name changes, for example
noiseModel.Diagonal becomes noiseModel_Diagonal etc...
Also, annoyingly, instead of gtsam.Symbol('b',0) we now need to say gtsam.symbol(ord('b'), 0))
`noiseModel.Diagonal` becomes `noiseModel_Diagonal` etc...
Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthand_B(0)` or `B(0)` if we use python aliasing.
# Porting Progress
| C++ Example Name | Ported |
|-------------------------------------------------------|--------|
| CameraResectioning | |
| CreateSFMExampleData | |
| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through cython |
| easyPoint2KalmanFilter | ExtendedKalmanFilter not exposed through cython |
| elaboratePoint2KalmanFilter | GaussianSequentialSolver not exposed through cython |
| ImuFactorExample2 | X |
| ImuFactorsExample | |
| ISAM2Example_SmartFactor | |
| ISAM2_SmartFactorStereo_IMU | |
| LocalizationExample | impossible? |
| METISOrderingExample | |
| OdometryExample | X |
| PlanarSLAMExample | X |
| Pose2SLAMExample | X |
| Pose2SLAMExampleExpressions | |
| Pose2SLAMExample_g2o | X |
| Pose2SLAMExample_graph | |
| Pose2SLAMExample_graphviz | |
| Pose2SLAMExample_lago | lago not exposed through cython |
| Pose2SLAMStressTest | |
| Pose2SLAMwSPCG | |
| Pose3SLAMExample_changeKeys | |
| Pose3SLAMExampleExpressions_BearingRangeWithTransform | |
| Pose3SLAMExample_g2o | X |
| Pose3SLAMExample_initializePose3Chordal | |
| Pose3SLAMExample_initializePose3Gradient | |
| RangeISAMExample_plaza2 | |
| SelfCalibrationExample | |
| SFMExample_bal_COLAMD_METIS | |
| SFMExample_bal | |
| SFMExample | X |
| SFMExampleExpressions_bal | |
| SFMExampleExpressions | |
| SFMExample_SmartFactor | |
| SFMExample_SmartFactorPCG | |
| SimpleRotation | X |
| SolverComparer | |
| StereoVOExample | |
| StereoVOExample_large | |
| TimeTBB | |
| UGM_chain | discrete functionality not exposed |
| UGM_small | discrete functionality not exposed |
| VisualISAM2Example | X |
| VisualISAMExample | X |
Extra Examples (with no C++ equivalent)
- PlanarManipulatorExample
- SFMData

121
cython/gtsam/examples/SFMExample.py Normal file
View File

@ -0,0 +1,121 @@
"""
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
A structure-from-motion problem on a simulated dataset
"""
from __future__ import print_function
import gtsam
import matplotlib.pyplot as plt
import numpy as np
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
from gtsam.examples import SFMdata
from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
GenericProjectionFactorCal3_S2, Marginals,
NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3,
SimpleCamera, Values)
from gtsam.utils import plot
def main():
"""
Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
Each variable in the system (poses and landmarks) must be identified with a unique key.
We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
Here we will use Symbols
In GTSAM, measurement functions are represented as 'factors'. Several common factors
have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
Here we will use Projection factors to model the camera's landmark observations.
Also, we will initialize the robot at some location using a Prior factor.
When the factors are created, we will add them to a Factor Graph. As the factors we are using
are nonlinear factors, we will need a Nonlinear Factor Graph.
Finally, once all of the factors have been added to our factor graph, we will want to
solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
trust-region method known as Powell's Degleg
The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
nonlinear functions around an initial linearization point, then solve the linear system
to update the linearization point. This happens repeatedly until the solver converges
to a consistent set of variable values. This requires us to specify an initial guess
for each variable, held in a Values container.
"""
# Define the camera calibration parameters
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
points = SFMdata.createPoints()
# Create the set of ground-truth poses
poses = SFMdata.createPoses(K)
# Create a factor graph
graph = NonlinearFactorGraph()
# Add a prior on pose x1. This indirectly specifies where the origin is.
# 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Simulated measurements from each camera pose, adding them to the factor graph
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(
measurement, measurement_noise, X(i), L(j), K)
graph.push_back(factor)
# Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
# Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
# between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
graph.print_('Factor Graph:\n')
# Create the data structure to hold the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initial_estimate = Values()
for i, pose in enumerate(poses):
transformed_pose = pose.retract(0.1*np.random.randn(6,1))
initial_estimate.insert(X(i), transformed_pose)
for j, point in enumerate(points):
transformed_point = Point3(point.vector() + 0.1*np.random.randn(3))
initial_estimate.insert(L(j), transformed_point)
initial_estimate.print_('Initial Estimates:\n')
# Optimize the graph and print results
params = gtsam.DoglegParams()
params.setVerbosity('TERMINATION')
optimizer = DoglegOptimizer(graph, initial_estimate, params)
print('Optimizing:')
result = optimizer.optimize()
result.print_('Final results:\n')
print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result)))
marginals = Marginals(graph, result)
plot.plot_3d_points(1, result, marginals=marginals)
plot.plot_trajectory(1, result, marginals=marginals, scale=8)
plot.set_axes_equal(1)
plt.show()
if __name__ == '__main__':
main()

5
cython/gtsam/examples/SFMdata.py
View File

@ -25,14 +25,15 @@ def createPoints():
def createPoses(K):
# Create the set of ground-truth poses
radius = 30.0
radius = 40.0
height = 10.0
angles = np.linspace(0, 2*np.pi, 8, endpoint=False)
up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0)
poses = []
for theta in angles:
position = gtsam.Point3(radius*np.cos(theta),
radius*np.sin(theta), 0.0)
radius*np.sin(theta), height)
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
poses.append(camera.pose())
return poses

85
cython/gtsam/examples/SimpleRotation.py Normal file
View File

@ -0,0 +1,85 @@
"""
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
This example will perform a relatively trivial optimization on
a single variable with a single factor.
"""
import gtsam
import numpy as np
from gtsam import symbol_shorthand_X as X
def main():
"""
Step 1: Create a factor to express a unary constraint
The "prior" in this case is the measurement from a sensor,
with a model of the noise on the measurement.
The "Key" created here is a label used to associate parts of the
state (stored in "RotValues") with particular factors. They require
an index to allow for lookup, and should be unique.
In general, creating a factor requires:
- A key or set of keys labeling the variables that are acted upon
- A measurement value
- A measurement model with the correct dimensionality for the factor
"""
prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
prior.print_('goal angle')
model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
key = X(1)
factor = gtsam.PriorFactorRot2(key, prior, model)
"""
Step 2: Create a graph container and add the factor to it
Before optimizing, all factors need to be added to a Graph container,
which provides the necessary top-level functionality for defining a
system of constraints.
In this case, there is only one factor, but in a practical scenario,
many more factors would be added.
"""
graph = gtsam.NonlinearFactorGraph()
graph.push_back(factor)
graph.print_('full graph')
"""
Step 3: Create an initial estimate
An initial estimate of the solution for the system is necessary to
start optimization. This system state is the "Values" instance,
which is similar in structure to a dictionary, in that it maps
keys (the label created in step 1) to specific values.
The initial estimate provided to optimization will be used as
a linearization point for optimization, so it is important that
all of the variables in the graph have a corresponding value in
this structure.
"""
initial = gtsam.Values()
initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20)))
initial.print_('initial estimate')
"""
Step 4: Optimize
After formulating the problem with a graph of constraints
and an initial estimate, executing optimization is as simple
as calling a general optimization function with the graph and
initial estimate. This will yield a new RotValues structure
with the final state of the optimization.
"""
result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
result.print_('final result')
if __name__ == '__main__':
main()

21
cython/gtsam/examples/VisualISAM2Example.py
View File

@ -13,23 +13,14 @@ Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
from __future__ import print_function
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import gtsam
import gtsam.utils.plot as gtsam_plot
import matplotlib.pyplot as plt
import numpy as np
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
from gtsam.examples import SFMdata
def X(i):
"""Create key for pose i."""
return int(gtsam.symbol(ord('x'), i))
def L(j):
"""Create key for landmark j."""
return int(gtsam.symbol(ord('l'), j))
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
def visual_ISAM2_plot(result):
@ -120,7 +111,7 @@ def visual_ISAM2_example():
if i == 0:
# Add a prior on pose x0
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array(
[0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
[0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise))
# Add a prior on landmark l0

103
cython/gtsam/examples/VisualISAMExample.py Normal file
View File

@ -0,0 +1,103 @@
"""
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
A visualSLAM example for the structure-from-motion problem on a simulated dataset
This version uses iSAM to solve the problem incrementally
"""
from __future__ import print_function
import numpy as np
import gtsam
from gtsam.examples import SFMdata
from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
NonlinearFactorGraph, NonlinearISAM, Point3, Pose3,
PriorFactorPoint3, PriorFactorPose3, Rot3,
PinholeCameraCal3_S2, Values)
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
def main():
"""
A structure-from-motion example with landmarks
- The landmarks form a 10 meter cube
- The robot rotates around the landmarks, always facing towards the cube
"""
# Define the camera calibration parameters
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
camera_noise = gtsam.noiseModel_Isotropic.Sigma(2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
points = SFMdata.createPoints()
# Create the set of ground-truth poses
poses = SFMdata.createPoses(K)
# Create a NonlinearISAM object which will relinearize and reorder the variables
# every "reorderInterval" updates
isam = NonlinearISAM(reorderInterval=3)
# Create a Factor Graph and Values to hold the new data
graph = NonlinearFactorGraph()
initial_estimate = Values()
# Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
# Add factors for each landmark observation
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, X(i), L(j), K)
graph.push_back(factor)
# Intentionally initialize the variables off from the ground truth
noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), t=Point3(0.05, -0.10, 0.20))
initial_xi = pose.compose(noise)
# Add an initial guess for the current pose
initial_estimate.insert(X(i), initial_xi)
# If this is the first iteration, add a prior on the first pose to set the coordinate frame
# and a prior on the first landmark to set the scale
# Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
# adding it to iSAM.
if i == 0:
# Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Add a prior on landmark l0
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
# Add initial guesses to all observed landmarks
noise = np.array([-0.25, 0.20, 0.15])
for j, point in enumerate(points):
# Intentionally initialize the variables off from the ground truth
initial_lj = points[j].vector() + noise
initial_estimate.insert(L(j), Point3(initial_lj))
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)
current_estimate = isam.estimate()
print('*' * 50)
print('Frame {}:'.format(i))
current_estimate.print_('Current estimate: ')
# Clear the factor graph and values for the next iteration
graph.resize(0)
initial_estimate.clear()
if __name__ == '__main__':
main()

112
cython/gtsam/tests/experiments.py
View File

@ -1,112 +0,0 @@
"""
This file is not a real python unittest. It contains small experiments
to test the wrapper with gtsam_test, a short version of gtsam.h.
Its name convention is different from other tests so it won't be discovered.
"""
import gtsam
import numpy as np
r = gtsam.Rot3()
print(r)
print(r.pitch())
r2 = gtsam.Rot3()
r3 = r.compose(r2)
print("r3 pitch:", r3.pitch())
v = np.array([1, 1, 1])
print("v = ", v)
r4 = r3.retract(v)
print("r4 pitch:", r4.pitch())
r4.print_(b'r4: ')
r3.print_(b"r3: ")
v = r3.localCoordinates(r4)
print("localCoordinates:", v)
Rmat = np.array([
[0.990074, -0.0942928, 0.104218],
[0.104218, 0.990074, -0.0942928],
[-0.0942928, 0.104218, 0.990074]
])
r5 = gtsam.Rot3(Rmat)
r5.print_(b"r5: ")
l = gtsam.Rot3.Logmap(r5)
print("l = ", l)
noise = gtsam.noiseModel_Gaussian.Covariance(Rmat)
noise.print_(b"noise:")
D = np.array([1.,2.,3.])
diag = gtsam.noiseModel_Diagonal.Variances(D)
print("diag:", diag)
diag.print_(b"diag:")
print("diag R:", diag.R())
p = gtsam.Point3()
p.print_("p:")
factor = gtsam.BetweenFactorPoint3(1,2,p, noise)
factor.print_(b"factor:")
vv = gtsam.VectorValues()
vv.print_(b"vv:")
vv.insert(1, np.array([1.,2.,3.]))
vv.insert(2, np.array([3.,4.]))
vv.insert(3, np.array([5.,6.,7.,8.]))
vv.print_(b"vv:")
vv2 = gtsam.VectorValues(vv)
vv2.insert(4, np.array([4.,2.,1]))
vv2.print_(b"vv2:")
vv.print_(b"vv:")
vv.insert(4, np.array([1.,2.,4.]))
vv.print_(b"vv:")
vv3 = vv.add(vv2)
vv3.print_(b"vv3:")
values = gtsam.Values()
values.insert(1, gtsam.Point3())
values.insert(2, gtsam.Rot3())
values.print_(b"values:")
factor = gtsam.PriorFactorVector(1, np.array([1.,2.,3.]), diag)
print "Prior factor vector: ", factor
keys = gtsam.KeyVector()
keys.push_back(1)
keys.push_back(2)
print 'size: ', keys.size()
print keys.at(0)
print keys.at(1)
noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0)
noise.print_('noise:')
print 'noise print:', noise
f = gtsam.JacobianFactor(7, np.ones([2,2]), model=noise, b=np.ones(2))
print 'JacobianFactor(7):\n', f
print "A = ", f.getA()
print "b = ", f.getb()
f = gtsam.JacobianFactor(np.ones(2))
f.print_('jacoboian b_in:')
print "JacobianFactor initalized with b_in:", f
diag = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.]))
fv = gtsam.PriorFactorVector(1, np.array([4.,5.,6.]), diag)
print "priorfactorvector: ", fv
print "base noise: ", fv.get_noiseModel()
print "casted to gaussian2: ", gtsam.dynamic_cast_noiseModel_Diagonal_noiseModel_Base(fv.get_noiseModel())
X = gtsam.symbol(65, 19)
print X
print gtsam.symbolChr(X)
print gtsam.symbolIndex(X)

772
cython/gtsam/tests/gtsam_test.h
View File

@ -1,772 +0,0 @@
namespace gtsam {
#include <gtsam/inference/Key.h>
typedef size_t Key;
#include <gtsam/base/FastVector.h>
template<T> class FastVector {
FastVector();
FastVector(const This& f);
void push_back(const T& e);
//T& operator[](int);
T at(int i);
size_t size() const;
};
typedef gtsam::FastVector<gtsam::Key> KeyVector;
//*************************************************************************
// geometry
//*************************************************************************
#include <gtsam/geometry/Point2.h>
class Point2 {
// Standard Constructors
Point2();
Point2(double x, double y);
Point2(Vector v);
//Point2(const gtsam::Point2& l);
// Testable
void print(string s) const;
bool equals(const gtsam::Point2& pose, double tol) const;
// Group
static gtsam::Point2 identity();
// Standard Interface
double x() const;
double y() const;
Vector vector() const;
double distance(const gtsam::Point2& p2) const;
double norm() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Point3.h>
class Point3 {
// Standard Constructors
Point3();
Point3(double x, double y, double z);
Point3(Vector v);
//Point3(const gtsam::Point3& l);
// Testable
void print(string s) const;
bool equals(const gtsam::Point3& p, double tol) const;
// Group
static gtsam::Point3 identity();
// Standard Interface
Vector vector() const;
double x() const;
double y() const;
double z() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Rot2.h>
class Rot2 {
// Standard Constructors and Named Constructors
Rot2();
Rot2(double theta);
//Rot2(const gtsam::Rot2& l);
static gtsam::Rot2 fromAngle(double theta);
static gtsam::Rot2 fromDegrees(double theta);
static gtsam::Rot2 fromCosSin(double c, double s);
// Testable
void print(string s) const;
bool equals(const gtsam::Rot2& rot, double tol) const;
// Group
static gtsam::Rot2 identity();
gtsam::Rot2 inverse();
gtsam::Rot2 compose(const gtsam::Rot2& p2) const;
gtsam::Rot2 between(const gtsam::Rot2& p2) const;
// Manifold
gtsam::Rot2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot2& p) const;
// Lie Group
static gtsam::Rot2 Expmap(Vector v);
static Vector Logmap(const gtsam::Rot2& p);
// Group Action on Point2
gtsam::Point2 rotate(const gtsam::Point2& point) const;
gtsam::Point2 unrotate(const gtsam::Point2& point) const;
// Standard Interface
static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative
static gtsam::Rot2 atan2(double y, double x);
double theta() const;
double degrees() const;
double c() const;
double s() const;
Matrix matrix() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Rot3.h>
class Rot3 {
// Standard Constructors and Named Constructors
Rot3();
Rot3(Matrix R);
//Rot3(const gtsam::Rot3& l);
static gtsam::Rot3 Rx(double t);
static gtsam::Rot3 Ry(double t);
static gtsam::Rot3 Rz(double t);
static gtsam::Rot3 RzRyRx(double x, double y, double z);
static gtsam::Rot3 RzRyRx(Vector xyz);
static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading)
static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude)
static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft)
static gtsam::Rot3 Ypr(double y, double p, double r);
static gtsam::Rot3 Quaternion(double w, double x, double y, double z);
static gtsam::Rot3 Rodrigues(Vector v);
// Testable
void print(string s) const;
bool equals(const gtsam::Rot3& rot, double tol) const;
// Group
static gtsam::Rot3 identity();
gtsam::Rot3 inverse() const;
gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
gtsam::Rot3 between(const gtsam::Rot3& p2) const;
// Manifold
//gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options
gtsam::Rot3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot3& p) const;
// Group Action on Point3
gtsam::Point3 rotate(const gtsam::Point3& p) const;
gtsam::Point3 unrotate(const gtsam::Point3& p) const;
// Standard Interface
static gtsam::Rot3 Expmap(Vector v);
static Vector Logmap(const gtsam::Rot3& p);
Matrix matrix() const;
Matrix transpose() const;
gtsam::Point3 column(size_t index) const;
Vector xyz() const;
Vector ypr() const;
Vector rpy() const;
double roll() const;
double pitch() const;
double yaw() const;
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly
Vector quaternion() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Pose2.h>
class Pose2 {
// Standard Constructor
Pose2();
//Pose2(const gtsam::Pose2& pose);
Pose2(double x, double y, double theta);
Pose2(double theta, const gtsam::Point2& t);
Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
Pose2(Vector v);
// Testable
void print(string s) const;
bool equals(const gtsam::Pose2& pose, double tol) const;
// Group
static gtsam::Pose2 identity();
gtsam::Pose2 inverse() const;
gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
gtsam::Pose2 between(const gtsam::Pose2& p2) const;
// Manifold
gtsam::Pose2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Pose2& p) const;
// Lie Group
static gtsam::Pose2 Expmap(Vector v);
static Vector Logmap(const gtsam::Pose2& p);
Matrix AdjointMap() const;
Vector Adjoint(const Vector& xi) const;
static Matrix wedge(double vx, double vy, double w);
// Group Actions on Point2
gtsam::Point2 transform_from(const gtsam::Point2& p) const;
gtsam::Point2 transform_to(const gtsam::Point2& p) const;
// Standard Interface
double x() const;
double y() const;
double theta() const;
gtsam::Rot2 bearing(const gtsam::Point2& point) const;
double range(const gtsam::Point2& point) const;
gtsam::Point2 translation() const;
gtsam::Rot2 rotation() const;
Matrix matrix() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Pose3.h>
class Pose3 {
// Standard Constructors
Pose3();
//Pose3(const gtsam::Pose3& pose);
Pose3(const gtsam::Rot3& r, const gtsam::Point3& t);
Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose)
Pose3(Matrix t);
// Testable
void print(string s) const;
bool equals(const gtsam::Pose3& pose, double tol) const;
// Group
static gtsam::Pose3 identity();
gtsam::Pose3 inverse() const;
gtsam::Pose3 compose(const gtsam::Pose3& p2) const;
gtsam::Pose3 between(const gtsam::Pose3& p2) const;
// Manifold
gtsam::Pose3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Pose3& T2) const;
// Lie Group
static gtsam::Pose3 Expmap(Vector v);
static Vector Logmap(const gtsam::Pose3& p);
Matrix AdjointMap() const;
Vector Adjoint(Vector xi) const;
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz);
// Group Action on Point3
gtsam::Point3 transform_from(const gtsam::Point3& p) const;
gtsam::Point3 transform_to(const gtsam::Point3& p) const;
// Standard Interface
gtsam::Rot3 rotation() const;
gtsam::Point3 translation() const;
double x() const;
double y() const;
double z() const;
Matrix matrix() const;
gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to()
double range(const gtsam::Point3& point);
double range(const gtsam::Pose3& pose);
// enabling serialization functionality
void serialize() const;
};
//*************************************************************************
// noise
//*************************************************************************
namespace noiseModel {
#include <gtsam/linear/NoiseModel.h>
virtual class Base {
};
virtual class Gaussian : gtsam::noiseModel::Base {
static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
Matrix R() const;
bool equals(gtsam::noiseModel::Base& expected, double tol);
void print(string s) const;
// enabling serialization functionality
void serializable() const;
};
virtual class Diagonal : gtsam::noiseModel::Gaussian {
static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
static gtsam::noiseModel::Diagonal* Variances(Vector variances);
static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
Matrix R() const;
void print(string s) const;
// enabling serialization functionality
void serializable() const;
};
virtual class Constrained : gtsam::noiseModel::Diagonal {
static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas);
static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas);
static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances);
static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances);
static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions);
static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions);
static gtsam::noiseModel::Constrained* All(size_t dim);
static gtsam::noiseModel::Constrained* All(size_t dim, double mu);
gtsam::noiseModel::Constrained* unit() const;
// enabling serialization functionality
void serializable() const;
};
virtual class Isotropic : gtsam::noiseModel::Diagonal {
static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
void print(string s) const;
// enabling serialization functionality
void serializable() const;
};
virtual class Unit : gtsam::noiseModel::Isotropic {
static gtsam::noiseModel::Unit* Create(size_t dim);
void print(string s) const;
// enabling serialization functionality
void serializable() const;
};
namespace mEstimator {
virtual class Base {
};
virtual class Null: gtsam::noiseModel::mEstimator::Base {
Null();
//Null(const gtsam::noiseModel::mEstimator::Null& other);
void print(string s) const;
static gtsam::noiseModel::mEstimator::Null* Create();
// enabling serialization functionality
void serializable() const;
};
virtual class Fair: gtsam::noiseModel::mEstimator::Base {
Fair(double c);
//Fair(const gtsam::noiseModel::mEstimator::Fair& other);
void print(string s) const;
static gtsam::noiseModel::mEstimator::Fair* Create(double c);
// enabling serialization functionality
void serializable() const;
};
virtual class Huber: gtsam::noiseModel::mEstimator::Base {
Huber(double k);
//Huber(const gtsam::noiseModel::mEstimator::Huber& other);
void print(string s) const;
static gtsam::noiseModel::mEstimator::Huber* Create(double k);
// enabling serialization functionality
void serializable() const;
};
virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
Tukey(double k);
//Tukey(const gtsam::noiseModel::mEstimator::Tukey& other);
void print(string s) const;
static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
// enabling serialization functionality
void serializable() const;
};
}///\namespace mEstimator
virtual class Robust : gtsam::noiseModel::Base {
Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
//Robust(const gtsam::noiseModel::Robust& other);
static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
void print(string s) const;
// enabling serialization functionality
void serializable() const;
};
}///\namespace noiseModel
#include <gtsam/linear/Sampler.h>
class Sampler {
//Constructors
Sampler(gtsam::noiseModel::Diagonal* model, int seed);
Sampler(Vector sigmas, int seed);
Sampler(int seed);
//Sampler(const gtsam::Sampler& other);
//Standard Interface
size_t dim() const;
Vector sigmas() const;
gtsam::noiseModel::Diagonal* model() const;
Vector sample();
Vector sampleNewModel(gtsam::noiseModel::Diagonal* model);
};
#include <gtsam/linear/VectorValues.h>
class VectorValues {
//Constructors
VectorValues();
VectorValues(const gtsam::VectorValues& other);
//Named Constructors
static gtsam::VectorValues Zero(const gtsam::VectorValues& model);
//Standard Interface
size_t size() const;
size_t dim(size_t j) const;
bool exists(size_t j) const;
void print(string s) const;
bool equals(const gtsam::VectorValues& expected, double tol) const;
void insert(size_t j, Vector value);
Vector vector() const;
Vector at(size_t j) const;
void update(const gtsam::VectorValues& values);
//Advanced Interface
void setZero();
gtsam::VectorValues add(const gtsam::VectorValues& c) const;
void addInPlace(const gtsam::VectorValues& c);
gtsam::VectorValues subtract(const gtsam::VectorValues& c) const;
gtsam::VectorValues scale(double a) const;
void scaleInPlace(double a);
bool hasSameStructure(const gtsam::VectorValues& other) const;
double dot(const gtsam::VectorValues& V) const;
double norm() const;
double squaredNorm() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/linear/GaussianFactor.h>
virtual class GaussianFactor {
gtsam::KeyVector keys() const;
void print(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const;
gtsam::GaussianFactor* clone() const;
gtsam::GaussianFactor* negate() const;
Matrix augmentedInformation() const;
Matrix information() const;
Matrix augmentedJacobian() const;
pair<Matrix, Vector> jacobian() const;
size_t size() const;
bool empty() const;
};
#include <gtsam/linear/JacobianFactor.h>
virtual class JacobianFactor : gtsam::GaussianFactor {
//Constructors
JacobianFactor();
JacobianFactor(const gtsam::GaussianFactor& factor);
JacobianFactor(Vector b_in);
JacobianFactor(size_t i1, Matrix A1, Vector b,
const gtsam::noiseModel::Diagonal* model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b,
const gtsam::noiseModel::Diagonal* model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3,
Vector b, const gtsam::noiseModel::Diagonal* model);
//JacobianFactor(const gtsam::GaussianFactorGraph& graph);
//JacobianFactor(const gtsam::JacobianFactor& other);
//Testable
void print(string s) const;
void printKeys(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
size_t size() const;
Vector unweighted_error(const gtsam::VectorValues& c) const;
Vector error_vector(const gtsam::VectorValues& c) const;
double error(const gtsam::VectorValues& c) const;
//Standard Interface
Matrix getA() const;
Vector getb() const;
size_t rows() const;
size_t cols() const;
bool isConstrained() const;
pair<Matrix, Vector> jacobianUnweighted() const;
Matrix augmentedJacobianUnweighted() const;
void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const;
gtsam::JacobianFactor whiten() const;
//pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const;
void setModel(bool anyConstrained, const Vector& sigmas);
gtsam::noiseModel::Diagonal* get_model() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/linear/HessianFactor.h>
virtual class HessianFactor : gtsam::GaussianFactor {
//Constructors
HessianFactor();
HessianFactor(const gtsam::GaussianFactor& factor);
HessianFactor(size_t j, Matrix G, Vector g, double f);
HessianFactor(size_t j, Vector mu, Matrix Sigma);
HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22,
Vector g2, double f);
HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13,
Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3,
double f);
//HessianFactor(const gtsam::GaussianFactorGraph& factors);
//HessianFactor(const gtsam::HessianFactor& other);
//Testable
size_t size() const;
void print(string s) const;
void printKeys(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const;
//Standard Interface
size_t rows() const;
Matrix information() const;
double constantTerm() const;
Vector linearTerm() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/nonlinear/Values.h>
class Values {
Values();
//Values(const gtsam::Values& other);
size_t size() const;
bool empty() const;
void clear();
size_t dim() const;
void print(string s) const;
bool equals(const gtsam::Values& other, double tol) const;
void insert(const gtsam::Values& values);
void update(const gtsam::Values& values);
void erase(size_t j);
void swap(gtsam::Values& values);
bool exists(size_t j) const;
gtsam::KeyVector keys() const;
gtsam::VectorValues zeroVectors() const;
gtsam::Values retract(const gtsam::VectorValues& delta) const;
gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const;
// enabling serialization functionality
void serialize() const;
// New in 4.0, we have to specialize every insert/update/at to generate wrappers
// Instead of the old:
// void insert(size_t j, const gtsam::Value& value);
// void update(size_t j, const gtsam::Value& val);
// gtsam::Value at(size_t j) const;
// template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
// gtsam::Rot3, gtsam::Pose3}>
// void insert(size_t j, const T& t);
// template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
// gtsam::Rot3, gtsam::Pose3}>
// void update(size_t j, const T& t);
void insert(size_t j, const gtsam::Point2& t);
void insert(size_t j, const gtsam::Point3& t);
void insert(size_t j, const gtsam::Rot2& t);
void insert(size_t j, const gtsam::Pose2& t);
void insert(size_t j, const gtsam::Rot3& t);
void insert(size_t j, const gtsam::Pose3& t);
void insert(size_t j, Vector t);
void insert(size_t j, Matrix t);
void update(size_t j, const gtsam::Point2& t);
void update(size_t j, const gtsam::Point3& t);
void update(size_t j, const gtsam::Rot2& t);
void update(size_t j, const gtsam::Pose2& t);
void update(size_t j, const gtsam::Rot3& t);
void update(size_t j, const gtsam::Pose3& t);
void update(size_t j, Vector t);
void update(size_t j, Matrix t);
template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, Vector, Matrix}>
T at(size_t j);
/// version for double
void insertDouble(size_t j, double c);
double atDouble(size_t j) const;
};
#include <gtsam/nonlinear/NonlinearFactor.h>
virtual class NonlinearFactor {
// Factor base class
size_t size() const;
gtsam::KeyVector keys() const;
void print(string s) const;
void printKeys(string s) const;
// NonlinearFactor
bool equals(const gtsam::NonlinearFactor& other, double tol) const;
double error(const gtsam::Values& c) const;
size_t dim() const;
bool active(const gtsam::Values& c) const;
gtsam::GaussianFactor* linearize(const gtsam::Values& c) const;
gtsam::NonlinearFactor* clone() const;
// gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen
};
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
class NonlinearFactorGraph {
NonlinearFactorGraph();
//NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph);
// FactorGraph
void print(string s) const;
bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const;
size_t size() const;
bool empty() const;
void remove(size_t i);
size_t nrFactors() const;
gtsam::NonlinearFactor* at(size_t idx) const;
void push_back(const gtsam::NonlinearFactorGraph& factors);
void push_back(gtsam::NonlinearFactor* factor);
void add(gtsam::NonlinearFactor* factor);
bool exists(size_t idx) const;
// gtsam::KeySet keys() const;
// NonlinearFactorGraph
double error(const gtsam::Values& values) const;
double probPrime(const gtsam::Values& values) const;
//gtsam::Ordering orderingCOLAMD() const;
// Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map<gtsam::Key,int>& constraints) const;
//gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const;
gtsam::NonlinearFactorGraph clone() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/nonlinear/NonlinearFactor.h>
virtual class NoiseModelFactor: gtsam::NonlinearFactor {
void equals(const gtsam::NoiseModelFactor& other, double tol) const;
gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below
gtsam::noiseModel::Base* noiseModel() const;
Vector unwhitenedError(const gtsam::Values& x) const;
Vector whitenedError(const gtsam::Values& x) const;
};
#include <gtsam/slam/PriorFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
//PriorFactor(const This& other);
T prior() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/slam/BetweenFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
virtual class BetweenFactor : gtsam::NoiseModelFactor {
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
//BetweenFactor(const This& other);
T measured() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/inference/Symbol.h>
size_t symbol(char chr, size_t index);
char symbolChr(size_t key);
size_t symbolIndex(size_t key);
#include <gtsam/inference/Key.h>
// Default keyformatter
void PrintKeyVector(const gtsam::KeyVector& keys);
void PrintKeyVector(const gtsam::KeyVector& keys, string s);
#include <gtsam/nonlinear/NonlinearOptimizer.h>
bool checkConvergence(double relativeErrorTreshold,
double absoluteErrorTreshold, double errorThreshold,
double currentError, double newError);
#include <gtsam/slam/dataset.h>
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
gtsam::noiseModel::Base* model);
void save2D(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& estimate, string filename);
//*************************************************************************
// Utilities
//*************************************************************************
namespace utilities {
#include <gtsam/nonlinear/utilities.h>
// gtsam::KeyList createKeyList(Vector I);
// gtsam::KeyList createKeyList(string s, Vector I);
gtsam::KeyVector createKeyVector(Vector I);
gtsam::KeyVector createKeyVector(string s, Vector I);
// gtsam::KeySet createKeySet(Vector I);
// gtsam::KeySet createKeySet(string s, Vector I);
Matrix extractPoint2(const gtsam::Values& values);
Matrix extractPoint3(const gtsam::Values& values);
Matrix extractPose2(const gtsam::Values& values);
gtsam::Values allPose3s(gtsam::Values& values);
Matrix extractPose3(const gtsam::Values& values);
void perturbPoint2(gtsam::Values& values, double sigma, int seed);
void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed);
void perturbPoint3(gtsam::Values& values, double sigma, int seed);
// void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
// void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
// void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base);
gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys);
} //\namespace utilities
#include <gtsam/nonlinear/utilities.h>
class RedirectCout {
RedirectCout();
string str();
};
} //\namespace gtsam

47
cython/gtsam/tests/testScenarioRunner.py Normal file
View File

@ -0,0 +1,47 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
ScenarioRunner unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import math
import unittest
import numpy as np
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestScenarioRunner(GtsamTestCase):
def setUp(self):
self.g = 10 # simple gravity constant
def test_loop_runner(self):
# Forward velocity 2m/s
# Pitch up with angular velocity 6 degree/sec (negative in FLU)
v = 2
w = math.radians(6)
W = np.array([0, -w, 0])
V = np.array([v, 0, 0])
scenario = gtsam.ConstantTwistScenario(W, V)
dt = 0.1
params = gtsam.PreintegrationParams.MakeSharedU(self.g)
bias = gtsam.imuBias_ConstantBias()
runner = gtsam.ScenarioRunner(
scenario, params, dt, bias)
# Test specific force at time 0: a is pointing up
t = 0.0
a = w * v
np.testing.assert_almost_equal(
np.array([0, 0, a + self.g]), runner.actualSpecificForce(t))
if __name__ == '__main__':
unittest.main()

26
cython/gtsam/tests/test_Cal3Unified.py
View File

@ -1,9 +1,22 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Cal3Unified unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import unittest
import gtsam
import numpy as np
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestCal3Unified(unittest.TestCase):
class TestCal3Unified(GtsamTestCase):
def test_Cal3Unified(self):
K = gtsam.Cal3Unified()
@ -11,12 +24,15 @@ class TestCal3Unified(unittest.TestCase):
self.assertEqual(K.fx(), 1.)
def test_retract(self):
expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1)
K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1)
expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1)
K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240,
1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1)
d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F')
actual = K.retract(d)
self.assertTrue(actual.equals(expected, 1e-9))
self.gtsamAssertEquals(actual, expected)
np.testing.assert_allclose(d, K.localCoordinates(actual))
if __name__ == "__main__":
unittest.main()

92
cython/gtsam/tests/test_GaussianFactorGraph.py Normal file
View File

@ -0,0 +1,92 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Unit tests for Linear Factor Graphs.
Author: Frank Dellaert & Gerry Chen
"""
# pylint: disable=invalid-name, no-name-in-module, no-member
from __future__ import print_function
import unittest
import gtsam
import numpy as np
from gtsam import symbol_shorthand_X as X
from gtsam.utils.test_case import GtsamTestCase
def create_graph():
"""Create a basic linear factor graph for testing"""
graph = gtsam.GaussianFactorGraph()
x0 = X(0)
x1 = X(1)
x2 = X(2)
BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE)
graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE)
graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE)
return graph, (x0, x1, x2)
class TestGaussianFactorGraph(GtsamTestCase):
"""Tests for Gaussian Factor Graphs."""
def test_fg(self):
"""Test solving a linear factor graph"""
graph, X = create_graph()
result = graph.optimize()
EXPECTEDX = [0, 1, 3]
# check solutions
self.assertAlmostEqual(EXPECTEDX[0], result.at(X[0]), delta=1e-8)
self.assertAlmostEqual(EXPECTEDX[1], result.at(X[1]), delta=1e-8)
self.assertAlmostEqual(EXPECTEDX[2], result.at(X[2]), delta=1e-8)
def test_convertNonlinear(self):
"""Test converting a linear factor graph to a nonlinear one"""
graph, X = create_graph()
EXPECTEDM = [1, 2, 3]
# create nonlinear factor graph for marginalization
nfg = gtsam.LinearContainerFactor.ConvertLinearGraph(graph)
optimizer = gtsam.LevenbergMarquardtOptimizer(nfg, gtsam.Values())
nlresult = optimizer.optimizeSafely()
# marginalize
marginals = gtsam.Marginals(nfg, nlresult)
m = [marginals.marginalCovariance(x) for x in X]
# check linear marginalizations
self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8)
self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
def test_linearMarginalization(self):
"""Marginalize a linear factor graph"""
graph, X = create_graph()
result = graph.optimize()
EXPECTEDM = [1, 2, 3]
# linear factor graph marginalize
marginals = gtsam.Marginals(graph, result)
m = [marginals.marginalCovariance(x) for x in X]
# check linear marginalizations
self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8)
self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
if __name__ == '__main__':
unittest.main()

22
cython/gtsam/tests/test_JacobianFactor.py
View File

@ -1,8 +1,22 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
JacobianFactor unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import unittest
import gtsam
import numpy as np
class TestJacobianFactor(unittest.TestCase):
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestJacobianFactor(GtsamTestCase):
def test_eliminate(self):
# Recommended way to specify a matrix (see cython/README)
@ -54,7 +68,7 @@ class TestJacobianFactor(unittest.TestCase):
expectedCG = gtsam.GaussianConditional(
x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2))
# check if the result matches
self.assertTrue(actualCG.equals(expectedCG, 1e-4))
self.gtsamAssertEquals(actualCG, expectedCG, 1e-4)
# the expected linear factor
Bl1 = np.array([[4.47214, 0.00],
@ -72,7 +86,7 @@ class TestJacobianFactor(unittest.TestCase):
expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2)
# check if the result matches the combined (reduced) factor
self.assertTrue(lf.equals(expectedLF, 1e-4))
self.gtsamAssertEquals(lf, expectedLF, 1e-4)
if __name__ == "__main__":
unittest.main()

18
cython/gtsam/tests/test_KalmanFilter.py
View File

@ -1,8 +1,22 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
KalmanFilter unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import unittest
import gtsam
import numpy as np
class TestKalmanFilter(unittest.TestCase):
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestKalmanFilter(GtsamTestCase):
def test_KalmanFilter(self):
F = np.eye(2)

80
cython/gtsam/tests/test_KarcherMeanFactor.py Normal file
View File

@ -0,0 +1,80 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
KarcherMeanFactor unit tests.
Author: Frank Dellaert
"""
# pylint: disable=invalid-name, no-name-in-module, no-member
import unittest
import gtsam
import numpy as np
from gtsam.utils.test_case import GtsamTestCase
KEY = 0
MODEL = gtsam.noiseModel_Unit.Create(3)
def find_Karcher_mean_Rot3(rotations):
"""Find the Karcher mean of given values."""
# Cost function C(R) = \sum PriorFactor(R_i)::error(R)
# No closed form solution.
graph = gtsam.NonlinearFactorGraph()
for R in rotations:
graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
initial = gtsam.Values()
initial.insert(KEY, gtsam.Rot3())
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
return result.atRot3(KEY)
# Rot3 version
R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0]))
class TestKarcherMean(GtsamTestCase):
def test_find(self):
# Check that optimizing for Karcher mean (which minimizes Between distance)
# gets correct result.
rotations = {R, R.inverse()}
expected = gtsam.Rot3()
actual = find_Karcher_mean_Rot3(rotations)
self.gtsamAssertEquals(expected, actual)
def test_factor(self):
"""Check that the InnerConstraint factor leaves the mean unchanged."""
# Make a graph with two variables, one between, and one InnerConstraint
# The optimal result should satisfy the between, while moving the other
# variable to make the mean the same as before.
# Mean of R and R' is identity. Let's make a BetweenFactor making R21 =
# R*R*R, i.e. geodesic length is 3 rather than 2.
graph = gtsam.NonlinearFactorGraph()
R12 = R.compose(R.compose(R))
graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
keys = gtsam.KeyVector()
keys.push_back(1)
keys.push_back(2)
graph.add(gtsam.KarcherMeanFactorRot3(keys))
initial = gtsam.Values()
initial.insert(1, R.inverse())
initial.insert(2, R)
expected = find_Karcher_mean_Rot3([R, R.inverse()])
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
actual = find_Karcher_mean_Rot3(
[result.atRot3(1), result.atRot3(2)])
self.gtsamAssertEquals(expected, actual)
self.gtsamAssertEquals(
R12, result.atRot3(1).between(result.atRot3(2)))
if __name__ == "__main__":
unittest.main()

20
cython/gtsam/tests/test_LocalizationExample.py
View File

@ -1,8 +1,22 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Localization unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import unittest
import gtsam
import numpy as np
class TestLocalizationExample(unittest.TestCase):
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestLocalizationExample(GtsamTestCase):
def test_LocalizationExample(self):
# Create the graph (defined in pose2SLAM.h, derived from
@ -43,7 +57,7 @@ class TestLocalizationExample(unittest.TestCase):
P = [None] * result.size()
for i in range(0, result.size()):
pose_i = result.atPose2(i)
self.assertTrue(pose_i.equals(groundTruth.atPose2(i), 1e-4))
self.gtsamAssertEquals(pose_i, groundTruth.atPose2(i), 1e-4)
P[i] = marginals.marginalCovariance(i)
if __name__ == "__main__":

72
cython/gtsam/tests/test_NonlinearOptimizer.py Normal file
View File

@ -0,0 +1,72 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Unit tests for IMU testing scenarios.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
# pylint: disable=invalid-name, no-name-in-module
from __future__ import print_function
import unittest
import gtsam
from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
GaussNewtonParams, LevenbergMarquardtOptimizer,
LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
Point2, PriorFactorPoint2, Values)
from gtsam.utils.test_case import GtsamTestCase
KEY1 = 1
KEY2 = 2
class TestScenario(GtsamTestCase):
def test_optimize(self):
"""Do trivial test with three optimizer variants."""
fg = NonlinearFactorGraph()
model = gtsam.noiseModel_Unit.Create(2)
fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
# test error at minimum
xstar = Point2(0, 0)
optimal_values = Values()
optimal_values.insert(KEY1, xstar)
self.assertEqual(0.0, fg.error(optimal_values), 0.0)
# test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
x0 = Point2(3, 3)
initial_values = Values()
initial_values.insert(KEY1, x0)
self.assertEqual(9.0, fg.error(initial_values), 1e-3)
# optimize parameters
ordering = Ordering()
ordering.push_back(KEY1)
# Gauss-Newton
gnParams = GaussNewtonParams()
gnParams.setOrdering(ordering)
actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize()
self.assertAlmostEqual(0, fg.error(actual1))
# Levenberg-Marquardt
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setOrdering(ordering)
actual2 = LevenbergMarquardtOptimizer(
fg, initial_values, lmParams).optimize()
self.assertAlmostEqual(0, fg.error(actual2))
# Dogleg
dlParams = DoglegParams()
dlParams.setOrdering(ordering)
actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize()
self.assertAlmostEqual(0, fg.error(actual3))
if __name__ == "__main__":
unittest.main()

20
cython/gtsam/tests/test_OdometryExample.py
View File

@ -1,8 +1,22 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Odometry unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import unittest
import gtsam
import numpy as np
class TestOdometryExample(unittest.TestCase):
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestOdometryExample(GtsamTestCase):
def test_OdometryExample(self):
# Create the graph (defined in pose2SLAM.h, derived from
@ -39,7 +53,7 @@ class TestOdometryExample(unittest.TestCase):
# Check first pose equality
pose_1 = result.atPose2(1)
self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4))
self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
if __name__ == "__main__":
unittest.main()

22
cython/gtsam/tests/test_PlanarSLAMExample.py
View File

@ -1,11 +1,25 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
PlanarSLAM unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import unittest
import gtsam
from math import pi
import numpy as np
class TestPose2SLAMExample(unittest.TestCase):
import gtsam
from gtsam.utils.test_case import GtsamTestCase
def test_Pose2SLAMExample(self):
class TestPlanarSLAM(GtsamTestCase):
def test_PlanarSLAM(self):
# Assumptions
# - All values are axis aligned
# - Robot poses are facing along the X axis (horizontal, to the right in images)
@ -56,7 +70,7 @@ class TestPose2SLAMExample(unittest.TestCase):
P = marginals.marginalCovariance(1)
pose_1 = result.atPose2(1)
self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4))
self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)

32
cython/gtsam/tests/test_Pose2.py Normal file
View File

@ -0,0 +1,32 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Pose2 unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import unittest
import numpy as np
import gtsam
from gtsam import Pose2
from gtsam.utils.test_case import GtsamTestCase
class TestPose2(GtsamTestCase):
"""Test selected Pose2 methods."""
def test_adjoint(self):
"""Test adjoint method."""
xi = np.array([1, 2, 3])
expected = np.dot(Pose2.adjointMap_(xi), xi)
actual = Pose2.adjoint_(xi, xi)
np.testing.assert_array_equal(actual, expected)
if __name__ == "__main__":
unittest.main()

20
cython/gtsam/tests/test_Pose2SLAMExample.py
View File

@ -1,9 +1,23 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Pose2SLAM unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import unittest
import gtsam
from math import pi
import numpy as np
class TestPose2SLAMExample(unittest.TestCase):
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestPose2SLAMExample(GtsamTestCase):
def test_Pose2SLAMExample(self):
# Assumptions
@ -56,7 +70,7 @@ class TestPose2SLAMExample(unittest.TestCase):
P = marginals.marginalCovariance(1)
pose_1 = result.atPose2(1)
self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4))
self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
if __name__ == "__main__":
unittest.main()

52
cython/gtsam/tests/test_Pose3.py
View File

@ -1,25 +1,44 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Pose3 unit tests.
Author: Frank Dellaert, Duy Nguyen Ta
"""
# pylint: disable=no-name-in-module
import math
import unittest
from gtsam import Point3, Rot3, Pose3
import numpy as np
import gtsam
from gtsam import Point3, Pose3, Rot3
from gtsam.utils.test_case import GtsamTestCase
class TestPose3(unittest.TestCase):
class TestPose3(GtsamTestCase):
"""Test selected Pose3 methods."""
def test__between(self):
T2 = Pose3(Rot3.Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2))
def test_between(self):
"""Test between method."""
T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2))
T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3))
expected = T2.inverse().compose(T3)
actual = T2.between(T3)
self.assertTrue(actual.equals(expected, 1e-6))
self.gtsamAssertEquals(actual, expected, 1e-6)
def test_transform_to(self):
transform = Pose3(Rot3.Rodrigues(0,0,-1.570796), Point3(2,4, 0))
actual = transform.transform_to(Point3(3,2,10))
expected = Point3 (2,1,10)
self.assertTrue(actual.equals(expected, 1e-6))
"""Test transformTo method."""
transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
actual = transform.transformTo(Point3(3, 2, 10))
expected = Point3(2, 1, 10)
self.gtsamAssertEquals(actual, expected, 1e-6)
def test_range(self):
"""Test range method."""
l1 = Point3(1, 0, 0)
l2 = Point3(1, 1, 0)
x1 = Pose3()
@ -28,16 +47,23 @@ class TestPose3(unittest.TestCase):
xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
# establish range is indeed zero
self.assertEqual(1,x1.range(point=l1))
self.assertEqual(1, x1.range(point=l1))
# establish range is indeed sqrt2
self.assertEqual(math.sqrt(2.0),x1.range(point=l2))
self.assertEqual(math.sqrt(2.0), x1.range(point=l2))
# establish range is indeed zero
self.assertEqual(1,x1.range(pose=xl1))
self.assertEqual(1, x1.range(pose=xl1))
# establish range is indeed sqrt2
self.assertEqual(math.sqrt(2.0),x1.range(pose=xl2))
self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2))
def test_adjoint(self):
"""Test adjoint method."""
xi = np.array([1, 2, 3, 4, 5, 6])
expected = np.dot(Pose3.adjointMap_(xi), xi)
actual = Pose3.adjoint_(xi, xi)
np.testing.assert_array_equal(actual, expected)
if __name__ == "__main__":

22
cython/gtsam/tests/test_Pose3SLAMExample.py
View File

@ -1,10 +1,24 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
PoseSLAM unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import unittest
import numpy as np
import gtsam
from math import pi
import numpy as np
import gtsam
from gtsam.utils.test_case import GtsamTestCase
from gtsam.utils.circlePose3 import *
class TestPose3SLAMExample(unittest.TestCase):
class TestPose3SLAMExample(GtsamTestCase):
def test_Pose3SLAMExample(self):
# Create a hexagon of poses
@ -40,7 +54,7 @@ class TestPose3SLAMExample(unittest.TestCase):
result = optimizer.optimizeSafely()
pose_1 = result.atPose3(1)
self.assertTrue(pose_1.equals(p1, 1e-4))
self.gtsamAssertEquals(pose_1, p1, 1e-4)
if __name__ == "__main__":
unittest.main()

40
cython/gtsam/tests/test_PriorFactor.py
View File

@ -1,8 +1,22 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
PriorFactor unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import unittest
import gtsam
import numpy as np
class TestPriorFactor(unittest.TestCase):
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestPriorFactor(GtsamTestCase):
def test_PriorFactor(self):
values = gtsam.Values()
@ -21,5 +35,27 @@ class TestPriorFactor(unittest.TestCase):
values.insert(key, priorVector)
self.assertEqual(factor.error(values), 0)
def test_AddPrior(self):
"""
Test adding prior factors directly to factor graph via the .addPrior method.
"""
# define factor graph
graph = gtsam.NonlinearFactorGraph()
# define and add Pose3 prior
key = 5
priorPose3 = gtsam.Pose3()
model = gtsam.noiseModel_Unit.Create(6)
graph.addPriorPose3(key, priorPose3, model)
self.assertEqual(graph.size(), 1)
# define and add Vector prior
key = 3
priorVector = np.array([0., 0., 0.])
model = gtsam.noiseModel_Unit.Create(3)
graph.addPriorVector(key, priorVector, model)
self.assertEqual(graph.size(), 2)
if __name__ == "__main__":
unittest.main()

23
cython/gtsam/tests/test_SFMExample.py
View File

@ -1,11 +1,24 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
SFM unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import unittest
import gtsam
from gtsam import symbol
import numpy as np
import gtsam
import gtsam.utils.visual_data_generator as generator
from gtsam import symbol
from gtsam.utils.test_case import GtsamTestCase
class TestSFMExample(unittest.TestCase):
class TestSFMExample(GtsamTestCase):
def test_SFMExample(self):
options = generator.Options()
@ -59,11 +72,11 @@ class TestSFMExample(unittest.TestCase):
# Check optimized results, should be equal to ground truth
for i in range(len(truth.cameras)):
pose_i = result.atPose3(symbol(ord('x'), i))
self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5))
self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
for j in range(len(truth.points)):
point_j = result.atPoint3(symbol(ord('p'), j))
self.assertTrue(point_j.equals(truth.points[j], 1e-5))
self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
if __name__ == "__main__":
unittest.main()

59
cython/gtsam/tests/test_SO4.py Normal file
View File

@ -0,0 +1,59 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
SO4 unit tests.
Author: Frank Dellaert
"""
# pylint: disable=no-name-in-module, import-error
import unittest
import numpy as np
from gtsam import SO4
I4 = SO4()
v1 = np.array([0, 0, 0, .1, 0, 0])
v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
Q1 = SO4.Expmap(v1)
Q2 = SO4.Expmap(v2)
class TestSO4(unittest.TestCase):
"""Test selected SO4 methods."""
def test_constructor(self):
"""Construct from matrix."""
matrix = np.eye(4)
so4 = SO4(matrix)
self.assertIsInstance(so4, SO4)
def test_retract(self):
"""Test retraction to manifold."""
v = np.zeros((6,), np.float)
actual = I4.retract(v)
self.assertTrue(actual.equals(I4, 1e-9))
def test_local(self):
"""Check localCoordinates for trivial case."""
v0 = np.zeros((6,), np.float)
actual = I4.localCoordinates(I4)
np.testing.assert_array_almost_equal(actual, v0)
def test_compose(self):
"""Check compose works in subgroup."""
expected = SO4.Expmap(2*v1)
actual = Q1.compose(Q1)
self.assertTrue(actual.equals(expected, 1e-9))
def test_vec(self):
"""Check wrapping of vec."""
expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1])
actual = I4.vec()
np.testing.assert_array_equal(actual, expected)
if __name__ == "__main__":
unittest.main()

59
cython/gtsam/tests/test_SOn.py Normal file
View File

@ -0,0 +1,59 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Dynamic SO(n) unit tests.
Author: Frank Dellaert
"""
# pylint: disable=no-name-in-module, import-error
import unittest
import numpy as np
from gtsam import SOn
I4 = SOn(4)
v1 = np.array([0, 0, 0, .1, 0, 0])
v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
Q1 = I4.retract(v1)
Q2 = I4.retract(v2)
class TestSO4(unittest.TestCase):
"""Test selected SOn methods."""
def test_constructor(self):
"""Construct from matrix."""
matrix = np.eye(4)
so4 = SOn.FromMatrix(matrix)
self.assertIsInstance(so4, SOn)
def test_retract(self):
"""Test retraction to manifold."""
v = np.zeros((6,), np.float)
actual = I4.retract(v)
self.assertTrue(actual.equals(I4, 1e-9))
def test_local(self):
"""Check localCoordinates for trivial case."""
v0 = np.zeros((6,), np.float)
actual = I4.localCoordinates(I4)
np.testing.assert_array_almost_equal(actual, v0)
def test_compose(self):
"""Check compose works in subgroup."""
expected = I4.retract(2*v1)
actual = Q1.compose(Q1)
self.assertTrue(actual.equals(expected, 1e-3)) # not exmap so only approximate
def test_vec(self):
"""Check wrapping of vec."""
expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1])
actual = I4.vec()
np.testing.assert_array_equal(actual, expected)
if __name__ == "__main__":
unittest.main()

21
cython/gtsam/tests/test_Scenario.py
View File

@ -1,11 +1,27 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Scenario unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
from __future__ import print_function
import math
import unittest
import numpy as np
import gtsam
from gtsam.utils.test_case import GtsamTestCase
# pylint: disable=invalid-name, E1101
class TestScenario(unittest.TestCase):
class TestScenario(GtsamTestCase):
def setUp(self):
pass
@ -29,7 +45,8 @@ class TestScenario(unittest.TestCase):
T30 = scenario.pose(T)
np.testing.assert_almost_equal(
np.array([math.pi, 0, math.pi]), T30.rotation().xyz())
self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9))
self.gtsamAssertEquals(gtsam.Point3(
0, 0, 2.0 * R), T30.translation(), 1e-9)
if __name__ == '__main__':

29
cython/gtsam/tests/test_SimpleCamera.py
View File

@ -1,23 +1,36 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
PinholeCameraCal3_S2 unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import math
import numpy as np
import unittest
from gtsam import Pose2, Point3, Rot3, Pose3, Cal3_S2, SimpleCamera
import numpy as np
import gtsam
from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2
from gtsam.utils.test_case import GtsamTestCase
K = Cal3_S2(625, 625, 0, 0, 0)
class TestSimpleCamera(unittest.TestCase):
class TestSimpleCamera(GtsamTestCase):
def test_constructor(self):
pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5))
camera = SimpleCamera(pose1, K)
self.assertTrue(camera.calibration().equals(K, 1e-9))
self.assertTrue(camera.pose().equals(pose1, 1e-9))
camera = PinholeCameraCal3_S2(pose1, K)
self.gtsamAssertEquals(camera.calibration(), K, 1e-9)
self.gtsamAssertEquals(camera.pose(), pose1, 1e-9)
def test_level2(self):
# Create a level camera, looking in Y-direction
pose2 = Pose2(0.4,0.3,math.pi/2.0)
camera = SimpleCamera.Level(K, pose2, 0.1)
camera = PinholeCameraCal3_S2.Level(K, pose2, 0.1)
# expected
x = Point3(1,0,0)
@ -25,7 +38,7 @@ class TestSimpleCamera(unittest.TestCase):
z = Point3(0,1,0)
wRc = Rot3(x,y,z)
expected = Pose3(wRc,Point3(0.4,0.3,0.1))
self.assertTrue(camera.pose().equals(expected, 1e-9))
self.gtsamAssertEquals(camera.pose(), expected, 1e-9)
if __name__ == "__main__":

23
cython/gtsam/tests/test_StereoVOExample.py
View File

@ -1,10 +1,23 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Stereo VO unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import unittest
import gtsam
from gtsam import symbol
import numpy as np
import gtsam
from gtsam import symbol
from gtsam.utils.test_case import GtsamTestCase
class TestStereoVOExample(unittest.TestCase):
class TestStereoVOExample(GtsamTestCase):
def test_StereoVOExample(self):
## Assumptions
@ -60,10 +73,10 @@ class TestStereoVOExample(unittest.TestCase):
## check equality for the first pose and point
pose_x1 = result.atPose3(x1)
self.assertTrue(pose_x1.equals(first_pose,1e-4))
self.gtsamAssertEquals(pose_x1, first_pose,1e-4)
point_l1 = result.atPoint3(l1)
self.assertTrue(point_l1.equals(expected_l1,1e-4))
self.gtsamAssertEquals(point_l1, expected_l1,1e-4)
if __name__ == "__main__":
unittest.main()

61
cython/gtsam/tests/test_Values.py
View File

@ -1,19 +1,34 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Values unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
# pylint: disable=invalid-name, E1101, E0611
import unittest
import numpy as np
from gtsam import Point2, Point3, Unit3, Rot2, Pose2, Rot3, Pose3
from gtsam import Values, Cal3_S2, Cal3DS2, Cal3Bundler, EssentialMatrix, imuBias_ConstantBias
import gtsam
from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2,
Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values,
imuBias_ConstantBias)
from gtsam.utils.test_case import GtsamTestCase
class TestValues(unittest.TestCase):
class TestValues(GtsamTestCase):
def test_values(self):
values = Values()
E = EssentialMatrix(Rot3(), Unit3())
tol = 1e-9
values.insert(0, Point2(0,0))
values.insert(1, Point3(0,0,0))
values.insert(0, Point2(0, 0))
values.insert(1, Point3(0, 0, 0))
values.insert(2, Rot2())
values.insert(3, Pose2())
values.insert(4, Rot3())
@ -34,36 +49,38 @@ class TestValues(unittest.TestCase):
# The wrapper will automatically fix the type and storage order for you,
# but for performance reasons, it's recommended to specify the correct
# type and storage order.
vec = np.array([1., 2., 3.]) # for vectors, the order is not important, but dtype still is
# for vectors, the order is not important, but dtype still is
vec = np.array([1., 2., 3.])
values.insert(11, vec)
mat = np.array([[1., 2.], [3., 4.]], order='F')
values.insert(12, mat)
# Test with dtype int and the default order='C'
# This still works as the wrapper converts to the correct type and order for you
# but is nornally not recommended!
mat2 = np.array([[1,2,],[3,5]])
mat2 = np.array([[1, 2, ], [3, 5]])
values.insert(13, mat2)
self.assertTrue(values.atPoint2(0).equals(Point2(), tol))
self.assertTrue(values.atPoint3(1).equals(Point3(), tol))
self.assertTrue(values.atRot2(2).equals(Rot2(), tol))
self.assertTrue(values.atPose2(3).equals(Pose2(), tol))
self.assertTrue(values.atRot3(4).equals(Rot3(), tol))
self.assertTrue(values.atPose3(5).equals(Pose3(), tol))
self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol))
self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol))
self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol))
self.assertTrue(values.atEssentialMatrix(9).equals(E, tol))
self.assertTrue(values.atimuBias_ConstantBias(
10).equals(imuBias_ConstantBias(), tol))
self.gtsamAssertEquals(values.atPoint2(0), Point2(0,0), tol)
self.gtsamAssertEquals(values.atPoint3(1), Point3(0,0,0), tol)
self.gtsamAssertEquals(values.atRot2(2), Rot2(), tol)
self.gtsamAssertEquals(values.atPose2(3), Pose2(), tol)
self.gtsamAssertEquals(values.atRot3(4), Rot3(), tol)
self.gtsamAssertEquals(values.atPose3(5), Pose3(), tol)
self.gtsamAssertEquals(values.atCal3_S2(6), Cal3_S2(), tol)
self.gtsamAssertEquals(values.atCal3DS2(7), Cal3DS2(), tol)
self.gtsamAssertEquals(values.atCal3Bundler(8), Cal3Bundler(), tol)
self.gtsamAssertEquals(values.atEssentialMatrix(9), E, tol)
self.gtsamAssertEquals(values.atimuBias_ConstantBias(
10), imuBias_ConstantBias(), tol)
# special cases for Vector and Matrix:
actualVector = values.atVector(11)
self.assertTrue(np.allclose(vec, actualVector, tol))
np.testing.assert_allclose(vec, actualVector, tol)
actualMatrix = values.atMatrix(12)
self.assertTrue(np.allclose(mat, actualMatrix, tol))
np.testing.assert_allclose(mat, actualMatrix, tol)
actualMatrix2 = values.atMatrix(13)
self.assertTrue(np.allclose(mat2, actualMatrix2, tol))
np.testing.assert_allclose(mat2, actualMatrix2, tol)
if __name__ == "__main__":
unittest.main()

23
cython/gtsam/tests/test_VisualISAMExample.py
View File

@ -1,10 +1,25 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
visual_isam unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import unittest
import numpy as np
from gtsam import symbol
import gtsam
import gtsam.utils.visual_data_generator as generator
import gtsam.utils.visual_isam as visual_isam
from gtsam import symbol
from gtsam.utils.test_case import GtsamTestCase
class TestVisualISAMExample(unittest.TestCase):
class TestVisualISAMExample(GtsamTestCase):
def test_VisualISAMExample(self):
# Data Options
@ -32,11 +47,11 @@ class TestVisualISAMExample(unittest.TestCase):
for i in range(len(truth.cameras)):
pose_i = result.atPose3(symbol(ord('x'), i))
self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5))
self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
for j in range(len(truth.points)):
point_j = result.atPoint3(symbol(ord('l'), j))
self.assertTrue(point_j.equals(truth.points[j], 1e-5))
self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
if __name__ == "__main__":
unittest.main()

45
cython/gtsam/tests/test_dataset.py Normal file
View File

@ -0,0 +1,45 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Unit tests for testing dataset access.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
# pylint: disable=invalid-name, no-name-in-module, no-member
from __future__ import print_function
import unittest
import gtsam
from gtsam import BetweenFactorPose3, BetweenFactorPose3s
from gtsam.utils.test_case import GtsamTestCase
class TestDataset(GtsamTestCase):
"""Tests for datasets.h wrapper."""
def setUp(self):
"""Get some common paths."""
self.pose3_example_g2o_file = gtsam.findExampleDataFile(
"pose3example.txt")
def test_readG2o3D(self):
"""Test reading directly into factor graph."""
is3D = True
graph, initial = gtsam.readG2o(self.pose3_example_g2o_file, is3D)
self.assertEqual(graph.size(), 6)
self.assertEqual(initial.size(), 5)
def test_parse3Dfactors(self):
"""Test parsing into data structure."""
factors = gtsam.parse3DFactors(self.pose3_example_g2o_file)
self.assertEqual(factors.size(), 6)
self.assertIsInstance(factors.at(0), BetweenFactorPose3)
if __name__ == '__main__':
unittest.main()

40
cython/gtsam/tests/test_dsf_map.py Normal file
View File

@ -0,0 +1,40 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Unit tests for Disjoint Set Forest.
Author: Frank Dellaert & Varun Agrawal
"""
# pylint: disable=invalid-name, no-name-in-module, no-member
from __future__ import print_function
import unittest
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestDSFMap(GtsamTestCase):
"""Tests for DSFMap."""
def test_all(self):
"""Test everything in DFSMap."""
def key(index_pair):
return index_pair.i(), index_pair.j()
dsf = gtsam.DSFMapIndexPair()
pair1 = gtsam.IndexPair(1, 18)
self.assertEqual(key(dsf.find(pair1)), key(pair1))
pair2 = gtsam.IndexPair(2, 2)
# testing the merge feature of dsf
dsf.merge(pair1, pair2)
self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2)))
if __name__ == '__main__':
unittest.main()

89
cython/gtsam/tests/test_initialize_pose3.py Normal file
View File

@ -0,0 +1,89 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Unit tests for 3D SLAM initialization, using rotation relaxation.
Author: Luca Carlone and Frank Dellaert (Python)
"""
# pylint: disable=invalid-name, E1101, E0611
import unittest
import numpy as np
import gtsam
from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values
from gtsam.utils.test_case import GtsamTestCase
x0, x1, x2, x3 = 0, 1, 2, 3
class TestValues(GtsamTestCase):
def setUp(self):
model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
# We consider a small graph:
# symbolic FG
# x2 0 1
# / | \ 1 2
# / | \ 2 3
# x3 | x1 2 0
# \ | / 0 3
# \ | /
# x0
#
p0 = Point3(0, 0, 0)
self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0]))
p1 = Point3(1, 2, 0)
self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796]))
p2 = Point3(0, 2, 0)
self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593]))
p3 = Point3(-1, 1, 0)
self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389]))
pose0 = Pose3(self.R0, p0)
pose1 = Pose3(self.R1, p1)
pose2 = Pose3(self.R2, p2)
pose3 = Pose3(self.R3, p3)
g = NonlinearFactorGraph()
g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model))
g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model))
g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model))
g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model))
g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model))
g.add(gtsam.PriorFactorPose3(x0, pose0, model))
self.graph = g
def test_buildPose3graph(self):
pose3graph = gtsam.InitializePose3.buildPose3graph(self.graph)
def test_orientations(self):
pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph)
initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph)
# comparison is up to M_PI, that's why we add some multiples of 2*M_PI
self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6)
self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6)
self.gtsamAssertEquals(initial.atRot3(x2), self.R2, 1e-6)
self.gtsamAssertEquals(initial.atRot3(x3), self.R3, 1e-6)
def test_initializePoses(self):
g2oFile = gtsam.findExampleDataFile("pose3example-grid")
is3D = True
inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D)
priorModel = gtsam.noiseModel_Unit.Create(6)
inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel))
initial = gtsam.InitializePose3.initialize(inputGraph)
# TODO(frank): very loose !!
self.gtsamAssertEquals(initial, expectedValues, 0.1)
if __name__ == "__main__":
unittest.main()

93
cython/gtsam/tests/test_logging_optimizer.py Normal file
View File

@ -0,0 +1,93 @@
"""
Unit tests for optimization that logs to comet.ml.
Author: Jing Wu and Frank Dellaert
"""
# pylint: disable=invalid-name
import sys
if sys.version_info.major >= 3:
from io import StringIO
else:
from cStringIO import StringIO
import unittest
from datetime import datetime
import gtsam
import numpy as np
from gtsam import Rot3
from gtsam.utils.test_case import GtsamTestCase
from gtsam.utils.logging_optimizer import gtsam_optimize
KEY = 0
MODEL = gtsam.noiseModel_Unit.Create(3)
class TestOptimizeComet(GtsamTestCase):
"""Check correct logging to comet.ml."""
def setUp(self):
"""Set up a small Karcher mean optimization example."""
# Grabbed from KarcherMeanFactor unit tests.
R = Rot3.Expmap(np.array([0.1, 0, 0]))
rotations = {R, R.inverse()} # mean is the identity
self.expected = Rot3()
graph = gtsam.NonlinearFactorGraph()
for R in rotations:
graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
initial = gtsam.Values()
initial.insert(KEY, R)
self.params = gtsam.GaussNewtonParams()
self.optimizer = gtsam.GaussNewtonOptimizer(
graph, initial, self.params)
# setup output capture
self.capturedOutput = StringIO()
sys.stdout = self.capturedOutput
def tearDown(self):
"""Reset print capture."""
sys.stdout = sys.__stdout__
def test_simple_printing(self):
"""Test with a simple hook."""
# Provide a hook that just prints
def hook(_, error):
print(error)
# Only thing we require from optimizer is an iterate method
gtsam_optimize(self.optimizer, self.params, hook)
# Check that optimizing yields the identity.
actual = self.optimizer.values()
self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
@unittest.skip("Not a test we want run every time, as needs comet.ml account")
def test_comet(self):
"""Test with a comet hook."""
from comet_ml import Experiment
comet = Experiment(project_name="Testing",
auto_output_logging="native")
comet.log_dataset_info(name="Karcher", path="shonan")
comet.add_tag("GaussNewton")
comet.log_parameter("method", "GaussNewton")
time = datetime.now()
comet.set_name("GaussNewton-" + str(time.month) + "/" + str(time.day) + " "
+ str(time.hour)+":"+str(time.minute)+":"+str(time.second))
# I want to do some comet thing here
def hook(optimizer, error):
comet.log_metric("Karcher error",
error, optimizer.iterations())
gtsam_optimize(self.optimizer, self.params, hook)
comet.end()
actual = self.optimizer.values()
self.gtsamAssertEquals(actual.atRot3(KEY), self.expected)
if __name__ == "__main__":
unittest.main()

50
cython/gtsam/utils/logging_optimizer.py Normal file
View File

@ -0,0 +1,50 @@
"""
Optimization with logging via a hook.
Author: Jing Wu and Frank Dellaert
"""
# pylint: disable=invalid-name
from gtsam import NonlinearOptimizer, NonlinearOptimizerParams
import gtsam
def optimize(optimizer, check_convergence, hook):
""" Given an optimizer and a convergence check, iterate until convergence.
After each iteration, hook(optimizer, error) is called.
After the function, use values and errors to get the result.
Arguments:
optimizer (T): needs an iterate and an error function.
check_convergence: T * float * float -> bool
hook -- hook function to record the error
"""
# the optimizer is created with default values which incur the error below
current_error = optimizer.error()
hook(optimizer, current_error)
# Iterative loop
while True:
# Do next iteration
optimizer.iterate()
new_error = optimizer.error()
hook(optimizer, new_error)
if check_convergence(optimizer, current_error, new_error):
return
current_error = new_error
def gtsam_optimize(optimizer,
params,
hook):
""" Given an optimizer and params, iterate until convergence.
After each iteration, hook(optimizer) is called.
After the function, use values and errors to get the result.
Arguments:
optimizer {NonlinearOptimizer} -- Nonlinear optimizer
params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters
hook -- hook function to record the error
"""
def check_convergence(optimizer, current_error, new_error):
return (optimizer.iterations() >= params.getMaxIterations()) or (
gtsam.checkConvergence(params.getRelativeErrorTol(), params.getAbsoluteErrorTol(), params.getErrorTol(),
current_error, new_error))
optimize(optimizer, check_convergence, hook)

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