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

Merge branch 'develop' into feature/RobustShonan

release/4.3a0
Varun Agrawal 2020-11-09 13:58:31 -05:00
parent dd45797813 ed971210c7
commit 48699f917e
89 changed files with 1990 additions and 1284 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -43,11 +43,6 @@ if [ -z ${PYTHON_VERSION+x} ]; then
exit 127
fi
if [ -z ${WRAPPER+x} ]; then
echo "Please provide the wrapper to build!"
exit 126
fi
PYTHON="python${PYTHON_VERSION}"
if [[ $(uname) == "Darwin" ]]; then
@ -61,25 +56,11 @@ PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
[ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
case $WRAPPER in
"cython")
BUILD_CYTHON="ON"
BUILD_PYBIND="OFF"
TYPEDEF_POINTS_TO_VECTORS="OFF"
sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/cython/requirements.txt
;;
"pybind")
BUILD_CYTHON="OFF"
BUILD_PYBIND="ON"
TYPEDEF_POINTS_TO_VECTORS="ON"
BUILD_PYBIND="ON"
TYPEDEF_POINTS_TO_VECTORS="ON"
sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
;;
*)
exit 126
;;
esac
sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
mkdir $GITHUB_WORKSPACE/build
cd $GITHUB_WORKSPACE/build
@ -90,7 +71,6 @@ cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=Release \
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DGTSAM_INSTALL_CYTHON_TOOLBOX=${BUILD_CYTHON} \
-DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \
-DGTSAM_TYPEDEF_POINTS_TO_VECTORS=${TYPEDEF_POINTS_TO_VECTORS} \
-DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
@ -98,30 +78,10 @@ cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=Release \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V41=OFF \
-DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install
make -j$(nproc) install &
make -j$(nproc) install
while ps -p $! > /dev/null
do
sleep 60
now=$(date +%s)
printf "%d seconds have elapsed\n" $(( (now - start) ))
done
case $WRAPPER in
"cython")
cd $GITHUB_WORKSPACE/build/cython
$PYTHON setup.py install --user --prefix=
cd $GITHUB_WORKSPACE/build/cython/gtsam/tests
$PYTHON -m unittest discover
;;
"pybind")
cd $GITHUB_WORKSPACE/build/python
$PYTHON setup.py install --user --prefix=
cd $GITHUB_WORKSPACE/python/gtsam/tests
$PYTHON -m unittest discover
;;
*)
echo "THIS SHOULD NEVER HAPPEN!"
exit 125
;;
esac
cd $GITHUB_WORKSPACE/build/python
$PYTHON setup.py install --user --prefix=
cd $GITHUB_WORKSPACE/python/gtsam/tests
$PYTHON -m unittest discover

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

@ -48,8 +48,15 @@ jobs:
- name: Install (Linux)
if: runner.os == 'Linux'
run: |
# LLVM 9 is not in Bionic's repositories so we add the official LLVM repository.
# LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository.
if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
# (ipv4|ha).pool.sks-keyservers.net is the SKS GPG global keyserver pool
# ipv4 avoids potential timeouts because of crappy IPv6 infrastructure
# 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository
# This key is not in the keystore by default for Ubuntu so we need to add it.
LLVM_KEY=15CF4D18AF4F7421
gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY
gpg -a --export $LLVM_KEY | sudo apt-key add -
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
fi
sudo apt-get -y update

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

@ -35,7 +35,9 @@ jobs:
- name: Install (macOS)
if: runner.os == 'macOS'
run: |
brew install cmake ninja boost
brew tap ProfFan/robotics
brew install cmake ninja
brew install ProfFan/robotics/boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "::set-env name=CC::gcc-${{ matrix.version }}"
@ -48,4 +50,4 @@ jobs:
- name: Build and Test (macOS)
if: runner.os == 'macOS'
run: |
bash .github/scripts/unix.sh -t
bash .github/scripts/unix.sh -t

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

@ -12,7 +12,6 @@ jobs:
CTEST_PARALLEL_LEVEL: 2
CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
PYTHON_VERSION: ${{ matrix.python_version }}
WRAPPER: ${{ matrix.wrapper }}
strategy:
fail-fast: false
matrix:
@ -28,7 +27,6 @@ jobs:
build_type: [Debug, Release]
python_version: [3]
wrapper: [pybind]
include:
- name: ubuntu-18.04-gcc-5
os: ubuntu-18.04
@ -63,8 +61,14 @@ jobs:
- name: Install (Linux)
if: runner.os == 'Linux'
run: |
# LLVM 9 is not in Bionic's repositories so we add the official LLVM repository.
if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
# (ipv4|ha).pool.sks-keyservers.net is the SKS GPG global keyserver pool
# ipv4 avoids potential timeouts because of crappy IPv6 infrastructure
# 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository
# This key is not in the keystore by default for Ubuntu so we need to add it.
LLVM_KEY=15CF4D18AF4F7421
gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY
gpg -a --export $LLVM_KEY | sudo apt-key add -
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
fi
sudo apt-get -y update
@ -83,7 +87,9 @@ jobs:
- name: Install (macOS)
if: runner.os == 'macOS'
run: |
brew install cmake ninja boost
brew tap ProfFan/robotics
brew install cmake ninja
brew install ProfFan/robotics/boost
if [ "${{ matrix.compiler }}" = "gcc" ]; then
brew install gcc@${{ matrix.version }}
echo "::set-env name=CC::gcc-${{ matrix.version }}"
@ -105,4 +111,4 @@ jobs:
- name: Build (macOS)
if: runner.os == 'macOS'
run: |
bash .github/scripts/python.sh
bash .github/scripts/python.sh

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

@ -56,6 +56,8 @@ jobs:
run: |
# LLVM 9 is not in Bionic's repositories so we add the official LLVM repository.
if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421
gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add -
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
fi
sudo apt-get -y update

6
.github/workflows/trigger-python.yml vendored
View File

@ -1,11 +1,11 @@
# This triggers Cython builds on `gtsam-manylinux-build`
# This triggers Python builds on `gtsam-manylinux-build`
name: Trigger Python Builds
on:
push:
branches:
- develop
jobs:
triggerCython:
triggerPython:
runs-on: ubuntu-latest
steps:
- name: Repository Dispatch
@ -13,5 +13,5 @@ jobs:
with:
token: ${{ secrets.PYTHON_CI_REPO_ACCESS_TOKEN }}
repository: borglab/gtsam-manylinux-build
event-type: cython-wrapper
event-type: python-wrapper
client-payload: '{"ref": "${{ github.ref }}", "sha": "${{ github.sha }}"}'

6
.gitignore vendored
View File

@ -9,12 +9,6 @@
*.txt.user
*.txt.user.6d59f0c
*.pydevproject
cython/venv
cython/gtsam.cpp
cython/gtsam.cpython-35m-darwin.so
cython/gtsam.pyx
cython/gtsam.so
cython/gtsam_wrapper.pxd
.vscode
.env
/.vs/

551
CMakeLists.txt
View File

@ -22,17 +22,10 @@ set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH})
###############################################################################
# Gather information, perform checks, set defaults
# Set the default install path to home
#set (CMAKE_INSTALL_PREFIX ${HOME} CACHE PATH "Install prefix for library")
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})
message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]")
# Load build type flags and default to Debug mode
include(GtsamBuildTypes)
@ -45,399 +38,21 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ")
endif()
# See whether gtsam_unstable is available (it will be present only if we're using a git checkout)
if(EXISTS "${PROJECT_SOURCE_DIR}/gtsam_unstable" AND IS_DIRECTORY "${PROJECT_SOURCE_DIR}/gtsam_unstable")
set(GTSAM_UNSTABLE_AVAILABLE 1)
else()
set(GTSAM_UNSTABLE_AVAILABLE 0)
endif()
include(cmake/HandleBoost.cmake) # Boost
include(cmake/HandleCCache.cmake) # ccache
include(cmake/HandleCPack.cmake) # CPack
include(cmake/HandleEigen.cmake) # Eigen3
include(cmake/HandleGeneralOptions.cmake) # CMake build options
include(cmake/HandleMKL.cmake) # MKL
include(cmake/HandleOpenMP.cmake) # OpenMP
include(cmake/HandlePerfTools.cmake) # Google perftools
include(cmake/HandlePython.cmake) # Python options and commands
include(cmake/HandleTBB.cmake) # TBB
include(cmake/HandleUninstall.cmake) # for "make uninstall"
# ----------------------------------------------------------------------------
# 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)
include(cmake/HandleAllocators.cmake) # Must be after tbb, pertools
add_custom_target(uninstall
"${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
###############################################################################
# Set up options
# Configurable Options
if(GTSAM_UNSTABLE_AVAILABLE)
option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON)
option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON)
option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF)
endif()
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." ON)
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." ON)
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" 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 with pybind11" OFF)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
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)
set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.")
# Check / set dependent variables for MATLAB wrapper
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES)
set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
endif()
if(GTSAM_BUILD_PYTHON)
# Get info about the Python3 interpreter
# https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3
find_package(Python3 COMPONENTS Interpreter Development)
if(NOT ${Python3_FOUND})
message(FATAL_ERROR "Cannot find Python3 interpreter. Please install Python >= 3.6.")
endif()
if(${GTSAM_PYTHON_VERSION} STREQUAL "Default")
set(GTSAM_PYTHON_VERSION "${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}"
CACHE
STRING
"The version of Python to build the wrappers against."
FORCE)
endif()
if(GTSAM_UNSTABLE_BUILD_PYTHON)
if (NOT GTSAM_BUILD_UNSTABLE)
message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.")
set(GTSAM_UNSTABLE_BUILD_PYTHON OFF)
endif()
endif()
set(GTSAM_PY_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/python")
endif()
# Flags for choosing default packaging tools
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator")
if (CMAKE_GENERATOR STREQUAL "Ninja" AND
((CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.9) OR
(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 3.5)))
# Force colored warnings in Ninja's output, if the compiler has -fdiagnostics-color support.
# Rationale in https://github.com/ninja-build/ninja/issues/814
add_compile_options(-fdiagnostics-color=always)
endif()
###############################################################################
# Find boost
# To change the path for boost, you will need to set:
# 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(MSVC)
# 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)
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()
# 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.58)
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
NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY)
message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.")
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
Boost::system
Boost::filesystem
Boost::thread
Boost::date_time
Boost::regex
)
if (GTSAM_DISABLE_NEW_TIMERS)
message("WARNING: GTSAM timing instrumentation manually disabled")
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS)
else()
if(Boost_TIMER_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()
###############################################################################
# Find 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
if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020))
set(TBB_GREATER_EQUAL_2020 1)
else()
set(TBB_GREATER_EQUAL_2020 0)
endif()
# 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()
###############################################################################
# Prohibit Timing build mode in combination with TBB
if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing"))
message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.")
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
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
list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES})
# --no-as-needed is required with gcc according to the MKL link advisor
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed")
endif()
else()
set(GTSAM_USE_EIGEN_MKL 0)
set(EIGEN_USE_MKL_ALL 0)
endif()
###############################################################################
# Find OpenMP (if we're also using MKL)
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
list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC ${OpenMP_CXX_FLAGS})
endif()
endif()
###############################################################################
# Option for using system Eigen or GTSAM-bundled Eigen
### These patches only affect usage of MKL. If you want to enable MKL, you *must*
### use our patched version of Eigen
### 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)
# Use generic Eigen include paths e.g. <Eigen/Core>
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()
# 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_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()
###############################################################################
# Global compile options
# Build list of possible allocators
set(possible_allocators "")
if(GTSAM_USE_TBB)
list(APPEND possible_allocators TBB)
set(preferred_allocator TBB)
else()
list(APPEND possible_allocators BoostPool STL)
set(preferred_allocator STL)
endif()
if(GOOGLE_PERFTOOLS_FOUND)
list(APPEND possible_allocators tcmalloc)
endif()
# Check if current allocator choice is valid and set cache option
list(FIND possible_allocators "${GTSAM_DEFAULT_ALLOCATOR}" allocator_valid)
if(allocator_valid EQUAL -1)
set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator" FORCE)
else()
set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator")
endif()
set_property(CACHE GTSAM_DEFAULT_ALLOCATOR PROPERTY STRINGS ${possible_allocators})
mark_as_advanced(GTSAM_DEFAULT_ALLOCATOR)
# Define compile flags depending on allocator
if("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "BoostPool")
set(GTSAM_ALLOCATOR_BOOSTPOOL 1)
elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "STL")
set(GTSAM_ALLOCATOR_STL 1)
elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "TBB")
set(GTSAM_ALLOCATOR_TBB 1)
elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc")
set(GTSAM_ALLOCATOR_STL 1) # tcmalloc replaces malloc, so to use it we use the STL allocator
list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc")
endif()
if(MSVC)
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)
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)
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs)
endif()
endif()
if(GTSAM_ENABLE_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()
include(cmake/HandleGlobalBuildFlags.cmake) # Build flags
###############################################################################
# Add components
@ -477,7 +92,6 @@ endif()
GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in")
export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake)
# Check for doxygen availability - optional dependency
find_package(Doxygen)
@ -489,146 +103,11 @@ endif()
# CMake Tools
add_subdirectory(cmake)
###############################################################################
# Set up CPack
set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM")
set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology")
set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu")
set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.md")
set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE")
set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR})
set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR})
set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH})
set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}")
#set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory
#set(CPACK_INSTALLED_DIRECTORIES ".") # FIXME: throws error
set(CPACK_SOURCE_IGNORE_FILES "/build*;/\\\\.;/makestats.sh$")
set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/")
set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/package_scripts/")
set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
#set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs
# Deb-package specific cpack
set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev")
set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")
###############################################################################
# Print configuration variables
message(STATUS "===============================================================")
message(STATUS "================ Configuration Options ======================")
print_config("CMAKE_CXX_COMPILER_ID type" "${CMAKE_CXX_COMPILER_ID}")
print_config("CMAKE_CXX_COMPILER_VERSION" "${CMAKE_CXX_COMPILER_VERSION}")
print_config("CMake version" "${CMAKE_VERSION}")
print_config("CMake generator" "${CMAKE_GENERATOR}")
print_config("CMake build tool" "${CMAKE_BUILD_TOOL}")
message(STATUS "Build flags ")
print_enabled_config(${GTSAM_BUILD_TESTS} "Build Tests")
print_enabled_config(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all'")
print_enabled_config(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts with 'make all'")
if (DOXYGEN_FOUND)
print_enabled_config(${GTSAM_BUILD_DOCS} "Build Docs")
endif()
print_enabled_config(${BUILD_SHARED_LIBS} "Build shared GTSAM libraries")
print_enabled_config(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name")
if(GTSAM_UNSTABLE_AVAILABLE)
print_enabled_config(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ")
print_enabled_config(${GTSAM_UNSTABLE_BUILD_PYTHON} "Build GTSAM unstable Python ")
print_enabled_config(${GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX} "Build MATLAB Toolbox for unstable")
endif()
if(NOT MSVC AND NOT XCODE_VERSION)
print_enabled_config(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ")
print_config("Build type" "${CMAKE_BUILD_TYPE}")
print_config("C compilation flags" "${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
print_config("C++ compilation flags" "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
endif()
print_build_options_for_target(gtsam)
print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
if(GTSAM_USE_TBB)
print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})")
elseif(TBB_FOUND)
print_config("Use Intel TBB" "TBB (Version: ${TBB_VERSION}) found but GTSAM_WITH_TBB is disabled")
else()
print_config("Use Intel TBB" "TBB not found")
endif()
if(GTSAM_USE_EIGEN_MKL)
print_config("Eigen will use MKL" "Yes")
elseif(MKL_FOUND)
print_config("Eigen will use MKL" "MKL found but GTSAM_WITH_EIGEN_MKL is disabled")
else()
print_config("Eigen will use MKL" "MKL not found")
endif()
if(GTSAM_USE_EIGEN_MKL_OPENMP)
print_config("Eigen will use MKL and OpenMP" "Yes")
elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL)
print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled")
elseif(OPENMP_FOUND AND NOT MKL_FOUND)
print_config("Eigen will use MKL and OpenMP" "OpenMP found but MKL not found")
elseif(OPENMP_FOUND)
print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled")
else()
print_config("Eigen will use MKL and OpenMP" "OpenMP not found")
endif()
print_config("Default allocator" "${GTSAM_DEFAULT_ALLOCATOR}")
if(GTSAM_THROW_CHEIRALITY_EXCEPTION)
print_config("Cheirality exceptions enabled" "YES")
else()
print_config("Cheirality exceptions enabled" "NO")
endif()
if(NOT MSVC AND NOT XCODE_VERSION)
if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
print_config("Build with ccache" "Yes")
elseif(CCACHE_FOUND)
print_config("Build with ccache" "ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
else()
print_config("Build with ccache" "No")
endif()
endif()
message(STATUS "Packaging flags")
print_config("CPack Source Generator" "${CPACK_SOURCE_GENERATOR}")
print_config("CPack Generator" "${CPACK_GENERATOR}")
message(STATUS "GTSAM flags ")
print_enabled_config(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V41} "Allow features deprecated in GTSAM 4.1")
print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")
message(STATUS "MATLAB toolbox flags")
print_enabled_config(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ")
if (${GTSAM_INSTALL_MATLAB_TOOLBOX})
print_config("MATLAB root" "${MATLAB_ROOT}")
print_config("MEX binary" "${MEX_COMMAND}")
endif()
message(STATUS "Python toolbox flags ")
print_enabled_config(${GTSAM_BUILD_PYTHON} "Build Python module with pybind ")
if(GTSAM_BUILD_PYTHON)
print_config("Python version" ${GTSAM_PYTHON_VERSION})
endif()
message(STATUS "===============================================================")
include(cmake/HandlePrintConfiguration.cmake)
# Print warnings at the end
if(GTSAM_WITH_TBB AND NOT TBB_FOUND)
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 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. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.")
endif()
include(cmake/HandleFinalChecks.cmake)
# Include CPack *after* all flags
include(CPack)

4
INSTALL.md
View File

@ -173,7 +173,7 @@ NOTE: If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against g
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.
`LD_PRELOAD` need only be set if you are building the python 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"
@ -190,6 +190,6 @@ 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.
when importing GTSAM using the python wrapper.

1
cmake/CMakeLists.txt
View File

@ -19,7 +19,6 @@ install(FILES
GtsamMatlabWrap.cmake
GtsamTesting.cmake
GtsamPrinting.cmake
FindCython.cmake
FindNumPy.cmake
README.html
DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools")

81
cmake/FindCython.cmake
View File

@ -1,81 +0,0 @@
# Modifed from: https://github.com/nest/nest-simulator/blob/master/cmake/FindCython.cmake
#
# Find the Cython compiler.
#
# This code sets the following variables:
#
# CYTHON_FOUND
# CYTHON_PATH
# CYTHON_EXECUTABLE
# CYTHON_VERSION
#
# See also UseCython.cmake
#=============================================================================
# Copyright 2011 Kitware, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#=============================================================================
# Use the Cython executable that lives next to the Python executable
# if it is a local installation.
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__[0])"
RESULT_VARIABLE RESULT
OUTPUT_VARIABLE CYTHON_PATH
OUTPUT_STRIP_TRAILING_WHITESPACE
)
endif ()
# RESULT=0 means ok
if ( NOT RESULT )
get_filename_component( _python_path ${PYTHON_EXECUTABLE} PATH )
find_program( CYTHON_EXECUTABLE
NAMES cython cython.bat cython3
HINTS ${_python_path}
)
endif ()
# RESULT=0 means ok
if ( NOT RESULT )
execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c"
"import Cython; print(Cython.__version__)"
RESULT_VARIABLE RESULT
OUTPUT_VARIABLE CYTHON_VAR_OUTPUT
ERROR_VARIABLE CYTHON_VAR_OUTPUT
OUTPUT_STRIP_TRAILING_WHITESPACE
)
if ( RESULT EQUAL 0 )
string( REGEX REPLACE ".* ([0-9]+\\.[0-9]+(\\.[0-9]+)?).*" "\\1"
CYTHON_VERSION "${CYTHON_VAR_OUTPUT}" )
endif ()
endif ()
include( FindPackageHandleStandardArgs )
find_package_handle_standard_args( Cython
FOUND_VAR
CYTHON_FOUND
REQUIRED_VARS
CYTHON_PATH
CYTHON_EXECUTABLE
VERSION_VAR
CYTHON_VERSION
)

29
cmake/GtsamBuildTypes.cmake
View File

@ -1,3 +1,5 @@
include(CheckCXXCompilerFlag) # for check_cxx_compiler_flag()
# Set cmake policy to recognize the AppleClang compiler
# independently from the Clang compiler.
if(POLICY CMP0025)
@ -105,11 +107,14 @@ if(MSVC)
else()
# Common to all configurations, next for each configuration:
if (
((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR
(CMAKE_CXX_COMPILER_ID MATCHES "GNU")
)
set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday
if (NOT MSVC)
check_cxx_compiler_flag(-Wsuggest-override COMPILER_HAS_WSUGGEST_OVERRIDE)
check_cxx_compiler_flag(-Wmissing COMPILER_HAS_WMISSING_OVERRIDE)
if (COMPILER_HAS_WSUGGEST_OVERRIDE)
set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday
elseif(COMPILER_HAS_WMISSING_OVERRIDE)
set(flag_override_ -Wmissing-override) # -Werror=missing-override: Add again someday
endif()
endif()
set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON
@ -263,3 +268,17 @@ function(gtsam_apply_build_flags target_name_)
target_compile_options(${target_name_} PRIVATE ${GTSAM_COMPILE_OPTIONS_PRIVATE})
endfunction(gtsam_apply_build_flags)
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()

34
cmake/HandleAllocators.cmake Normal file
View File

@ -0,0 +1,34 @@
# Build list of possible allocators
set(possible_allocators "")
if(GTSAM_USE_TBB)
list(APPEND possible_allocators TBB)
set(preferred_allocator TBB)
else()
list(APPEND possible_allocators BoostPool STL)
set(preferred_allocator STL)
endif()
if(GOOGLE_PERFTOOLS_FOUND)
list(APPEND possible_allocators tcmalloc)
endif()
# Check if current allocator choice is valid and set cache option
list(FIND possible_allocators "${GTSAM_DEFAULT_ALLOCATOR}" allocator_valid)
if(allocator_valid EQUAL -1)
set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator" FORCE)
else()
set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator")
endif()
set_property(CACHE GTSAM_DEFAULT_ALLOCATOR PROPERTY STRINGS ${possible_allocators})
mark_as_advanced(GTSAM_DEFAULT_ALLOCATOR)
# Define compile flags depending on allocator
if("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "BoostPool")
set(GTSAM_ALLOCATOR_BOOSTPOOL 1)
elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "STL")
set(GTSAM_ALLOCATOR_STL 1)
elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "TBB")
set(GTSAM_ALLOCATOR_TBB 1)
elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc")
set(GTSAM_ALLOCATOR_STL 1) # tcmalloc replaces malloc, so to use it we use the STL allocator
list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc")
endif()

56
cmake/HandleBoost.cmake Normal file
View File

@ -0,0 +1,56 @@
###############################################################################
# Find boost
# To change the path for boost, you will need to set:
# 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(MSVC)
# 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)
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()
# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
set(BOOST_FIND_MINIMUM_VERSION 1.58)
set(BOOST_FIND_MINIMUM_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
NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY)
message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.")
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
Boost::system
Boost::filesystem
Boost::thread
Boost::date_time
Boost::regex
)
if (GTSAM_DISABLE_NEW_TIMERS)
message("WARNING: GTSAM timing instrumentation manually disabled")
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS)
else()
if(Boost_TIMER_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()

14
cmake/HandleCCache.cmake Normal file
View File

@ -0,0 +1,14 @@
###############################################################################
# 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()

28
cmake/HandleCPack.cmake Normal file
View File

@ -0,0 +1,28 @@
#JLBC: is all this actually used by someone? could it be removed?
# Flags for choosing default packaging tools
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator")
###############################################################################
# Set up CPack
set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM")
set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology")
set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu")
set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.md")
set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE")
set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR})
set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR})
set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH})
set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}")
#set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory
#set(CPACK_INSTALLED_DIRECTORIES ".") # FIXME: throws error
set(CPACK_SOURCE_IGNORE_FILES "/build*;/\\\\.;/makestats.sh$")
set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/")
set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/package_scripts/")
set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
#set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs
# Deb-package specific cpack
set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev")
set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")

77
cmake/HandleEigen.cmake Normal file
View File

@ -0,0 +1,77 @@
###############################################################################
# Option for using system Eigen or GTSAM-bundled Eigen
option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
if(NOT GTSAM_USE_SYSTEM_EIGEN)
# This option only makes sense if using the embedded copy of Eigen, it is
# used to decide whether to *install* the "unsupported" module:
option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" OFF)
endif()
# Switch for using system Eigen or GTSAM-bundled Eigen
if(GTSAM_USE_SYSTEM_EIGEN)
find_package(Eigen3 REQUIRED)
# Use generic Eigen include paths e.g. <Eigen/Core>
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()
# 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_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()

10
cmake/HandleFinalChecks.cmake Normal file
View File

@ -0,0 +1,10 @@
# Print warnings at the end
if(GTSAM_WITH_TBB AND NOT TBB_FOUND)
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 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. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.")
endif()

46
cmake/HandleGeneralOptions.cmake Normal file
View File

@ -0,0 +1,46 @@
###############################################################################
# Set up options
# See whether gtsam_unstable is available (it will be present only if we're using a git checkout)
if(EXISTS "${PROJECT_SOURCE_DIR}/gtsam_unstable" AND IS_DIRECTORY "${PROJECT_SOURCE_DIR}/gtsam_unstable")
set(GTSAM_UNSTABLE_AVAILABLE 1)
else()
set(GTSAM_UNSTABLE_AVAILABLE 0)
endif()
# Configurable Options
if(GTSAM_UNSTABLE_AVAILABLE)
option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON)
option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON)
option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF)
endif()
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." ON)
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." ON)
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" 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 with pybind11" OFF)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
if(NOT MSVC AND NOT XCODE_VERSION)
option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON)
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)
set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.")
# Check / set dependent variables for MATLAB wrapper
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES)
set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
endif()

52
cmake/HandleGlobalBuildFlags.cmake Normal file
View File

@ -0,0 +1,52 @@
# JLBC: These should ideally be ported to "modern cmake" via target properties.
#
if (CMAKE_GENERATOR STREQUAL "Ninja" AND
((CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.9) OR
(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 3.5)))
# Force colored warnings in Ninja's output, if the compiler has -fdiagnostics-color support.
# Rationale in https://github.com/ninja-build/ninja/issues/814
add_compile_options(-fdiagnostics-color=always)
endif()
# 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()
if (APPLE AND BUILD_SHARED_LIBS)
# Set the default install directory on macOS
set(CMAKE_INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib")
endif()
###############################################################################
# Global compile options
if(MSVC)
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)
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)
list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs)
endif()
endif()
if(GTSAM_ENABLE_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()

17
cmake/HandleMKL.cmake Normal file
View File

@ -0,0 +1,17 @@
###############################################################################
# Find MKL
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
list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES})
# --no-as-needed is required with gcc according to the MKL link advisor
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed")
endif()
else()
set(GTSAM_USE_EIGEN_MKL 0)
set(EIGEN_USE_MKL_ALL 0)
endif()

11
cmake/HandleOpenMP.cmake Normal file
View File

@ -0,0 +1,11 @@
###############################################################################
# Find OpenMP (if we're also using MKL)
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
list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC ${OpenMP_CXX_FLAGS})
endif()
endif()

4
cmake/HandlePerfTools.cmake Normal file
View File

@ -0,0 +1,4 @@
###############################################################################
# Find Google perftools
find_package(GooglePerfTools)

104
cmake/HandlePrintConfiguration.cmake Normal file
View File

@ -0,0 +1,104 @@
###############################################################################
# Print configuration variables
message(STATUS "===============================================================")
message(STATUS "================ Configuration Options ======================")
print_config("CMAKE_CXX_COMPILER_ID type" "${CMAKE_CXX_COMPILER_ID}")
print_config("CMAKE_CXX_COMPILER_VERSION" "${CMAKE_CXX_COMPILER_VERSION}")
print_config("CMake version" "${CMAKE_VERSION}")
print_config("CMake generator" "${CMAKE_GENERATOR}")
print_config("CMake build tool" "${CMAKE_BUILD_TOOL}")
message(STATUS "Build flags ")
print_enabled_config(${GTSAM_BUILD_TESTS} "Build Tests")
print_enabled_config(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all'")
print_enabled_config(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts with 'make all'")
if (DOXYGEN_FOUND)
print_enabled_config(${GTSAM_BUILD_DOCS} "Build Docs")
endif()
print_enabled_config(${BUILD_SHARED_LIBS} "Build shared GTSAM libraries")
print_enabled_config(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name")
if(GTSAM_UNSTABLE_AVAILABLE)
print_enabled_config(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ")
print_enabled_config(${GTSAM_UNSTABLE_BUILD_PYTHON} "Build GTSAM unstable Python ")
print_enabled_config(${GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX} "Build MATLAB Toolbox for unstable")
endif()
if(NOT MSVC AND NOT XCODE_VERSION)
print_enabled_config(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ")
print_config("Build type" "${CMAKE_BUILD_TYPE}")
print_config("C compilation flags" "${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
print_config("C++ compilation flags" "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
endif()
print_build_options_for_target(gtsam)
print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
if(GTSAM_USE_TBB)
print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})")
elseif(TBB_FOUND)
print_config("Use Intel TBB" "TBB (Version: ${TBB_VERSION}) found but GTSAM_WITH_TBB is disabled")
else()
print_config("Use Intel TBB" "TBB not found")
endif()
if(GTSAM_USE_EIGEN_MKL)
print_config("Eigen will use MKL" "Yes")
elseif(MKL_FOUND)
print_config("Eigen will use MKL" "MKL found but GTSAM_WITH_EIGEN_MKL is disabled")
else()
print_config("Eigen will use MKL" "MKL not found")
endif()
if(GTSAM_USE_EIGEN_MKL_OPENMP)
print_config("Eigen will use MKL and OpenMP" "Yes")
elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL)
print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled")
elseif(OPENMP_FOUND AND NOT MKL_FOUND)
print_config("Eigen will use MKL and OpenMP" "OpenMP found but MKL not found")
elseif(OPENMP_FOUND)
print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled")
else()
print_config("Eigen will use MKL and OpenMP" "OpenMP not found")
endif()
print_config("Default allocator" "${GTSAM_DEFAULT_ALLOCATOR}")
if(GTSAM_THROW_CHEIRALITY_EXCEPTION)
print_config("Cheirality exceptions enabled" "YES")
else()
print_config("Cheirality exceptions enabled" "NO")
endif()
if(NOT MSVC AND NOT XCODE_VERSION)
if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
print_config("Build with ccache" "Yes")
elseif(CCACHE_FOUND)
print_config("Build with ccache" "ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
else()
print_config("Build with ccache" "No")
endif()
endif()
message(STATUS "Packaging flags")
print_config("CPack Source Generator" "${CPACK_SOURCE_GENERATOR}")
print_config("CPack Generator" "${CPACK_GENERATOR}")
message(STATUS "GTSAM flags ")
print_enabled_config(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V41} "Allow features deprecated in GTSAM 4.1")
print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")
message(STATUS "MATLAB toolbox flags")
print_enabled_config(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ")
if (${GTSAM_INSTALL_MATLAB_TOOLBOX})
print_config("MATLAB root" "${MATLAB_ROOT}")
print_config("MEX binary" "${MEX_COMMAND}")
endif()
message(STATUS "Python toolbox flags ")
print_enabled_config(${GTSAM_BUILD_PYTHON} "Build Python module with pybind ")
if(GTSAM_BUILD_PYTHON)
print_config("Python version" ${GTSAM_PYTHON_VERSION})
endif()
message(STATUS "===============================================================")

26
cmake/HandlePython.cmake Normal file
View File

@ -0,0 +1,26 @@
if(GTSAM_BUILD_PYTHON)
if(${GTSAM_PYTHON_VERSION} STREQUAL "Default")
# Get info about the Python3 interpreter
# https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3
find_package(Python3 COMPONENTS Interpreter Development)
if(NOT ${Python3_FOUND})
message(FATAL_ERROR "Cannot find Python3 interpreter. Please install Python >= 3.6.")
endif()
set(GTSAM_PYTHON_VERSION "${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}"
CACHE
STRING
"The version of Python to build the wrappers against."
FORCE)
endif()
if(GTSAM_UNSTABLE_BUILD_PYTHON)
if (NOT GTSAM_BUILD_UNSTABLE)
message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.")
set(GTSAM_UNSTABLE_BUILD_PYTHON OFF)
endif()
endif()
set(GTSAM_PY_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/python")
endif()

24
cmake/HandleTBB.cmake Normal file
View File

@ -0,0 +1,24 @@
###############################################################################
# Find 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
if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020))
set(TBB_GREATER_EQUAL_2020 1)
else()
set(TBB_GREATER_EQUAL_2020 0)
endif()
# 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()
###############################################################################
# Prohibit Timing build mode in combination with TBB
if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing"))
message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.")
endif()

10
cmake/HandleUninstall.cmake Normal file
View File

@ -0,0 +1,10 @@
# ----------------------------------------------------------------------------
# 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")

8
docker/ubuntu-gtsam-python/Dockerfile
View File

@ -7,9 +7,9 @@ FROM dellaert/ubuntu-gtsam:bionic
RUN apt-get install -y python3-pip python3-dev
# Install python wrapper requirements
RUN python3 -m pip install -U -r /usr/src/gtsam/cython/requirements.txt
RUN python3 -m pip install -U -r /usr/src/gtsam/python/requirements.txt
# Run cmake again, now with cython toolbox on
# Run cmake again, now with python toolbox on
WORKDIR /usr/src/gtsam/build
RUN cmake \
-DCMAKE_BUILD_TYPE=Release \
@ -17,7 +17,7 @@ RUN cmake \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-DGTSAM_BUILD_TIMING_ALWAYS=OFF \
-DGTSAM_BUILD_TESTS=OFF \
-DGTSAM_INSTALL_CYTHON_TOOLBOX=ON \
-DGTSAM_BUILD_PYTHON=ON \
-DGTSAM_PYTHON_VERSION=3\
..
@ -25,7 +25,7 @@ RUN cmake \
RUN make -j4 install && make clean
# Needed to run python wrapper:
RUN echo 'export PYTHONPATH=/usr/local/cython/:$PYTHONPATH' >> /root/.bashrc
RUN echo 'export PYTHONPATH=/usr/local/python/:$PYTHONPATH' >> /root/.bashrc
# Run bash
CMD ["bash"]

1
docker/ubuntu-gtsam/Dockerfile
View File

@ -23,7 +23,6 @@ RUN cmake \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-DGTSAM_BUILD_TIMING_ALWAYS=OFF \
-DGTSAM_BUILD_TESTS=OFF \
-DGTSAM_INSTALL_CYTHON_TOOLBOX=OFF \
..
# Build

104
gtsam/base/numericalDerivative.h
View File

@ -109,7 +109,7 @@ typename Eigen::Matrix<double, N, 1> numericalGradient(
* @param delta increment for numerical derivative
* Class Y is the output argument
* Class X is the input argument
* int N is the dimension of the X input value if variable dimension type but known at test time
* @tparam int N is the dimension of the X input value if variable dimension type but known at test time
* @return m*n Jacobian computed via central differencing
*/
@ -167,15 +167,16 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const
* @param x2 second argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2>
template<class Y, class X1, class X2, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
const X1& x1, const X2& x2, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2)), x1, delta);
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2)), x1, delta);
}
/** use a raw C++ function pointer */
@ -192,15 +193,16 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(cons
* @param x2 n-dimensional second argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2>
template<class Y, class X1, class X2, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta = 1e-5) {
// BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
// "Template argument X1 must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1), x2, delta);
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1), x2, delta);
}
/** use a raw C++ function pointer */
@ -219,8 +221,9 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(cons
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* All classes Y,X1,X2,X3 need dim, expmap, logmap
* @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3>
template<class Y, class X1, class X2, class X3, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
@ -228,7 +231,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta);
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta);
}
template<class Y, class X1, class X2, class X3>
@ -247,8 +250,9 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(cons
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* All classes Y,X1,X2,X3 need dim, expmap, logmap
* @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3>
template<class Y, class X1, class X2, class X3, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
@ -256,7 +260,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta);
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta);
}
template<class Y, class X1, class X2, class X3>
@ -275,8 +279,9 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* All classes Y,X1,X2,X3 need dim, expmap, logmap
* @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3>
template<class Y, class X1, class X2, class X3, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
@ -284,7 +289,7 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
"Template argument X3 must be a manifold type.");
return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta);
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta);
}
template<class Y, class X1, class X2, class X3>
@ -303,8 +308,9 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*
* @param x4 fourth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4>
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
@ -312,7 +318,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta);
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
@ -330,8 +336,9 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*
* @param x4 fourth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4>
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
@ -339,7 +346,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta);
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
@ -357,8 +364,9 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*
* @param x4 fourth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4>
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
@ -366,7 +374,7 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
"Template argument X3 must be a manifold type.");
return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta);
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
@ -384,8 +392,9 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*
* @param x4 n-dimensional fourth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4>
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
@ -393,7 +402,7 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
"Template argument X4 must be a manifold type.");
return numericalDerivative11<Y, X4>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta);
return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
@ -412,8 +421,9 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
@ -421,7 +431,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta);
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
@ -440,8 +450,9 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
@ -449,7 +460,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta);
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
@ -468,8 +479,9 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
@ -477,7 +489,7 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta);
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
@ -496,8 +508,9 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
@ -505,7 +518,7 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X4>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta);
return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
@ -524,8 +537,9 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X5 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X5>::dimension>
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
@ -533,7 +547,7 @@ typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X5>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta);
return numericalDerivative11<Y, X5, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
@ -553,8 +567,9 @@ inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*
* @param x6 sixth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
@ -562,7 +577,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta);
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
@ -582,8 +597,9 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*
* @param x6 sixth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
@ -591,7 +607,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta);
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
@ -608,11 +624,12 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*
* @param x3 third argument value
* @param x4 fourth argument value
* @param x5 fifth argument value
* @param x6 sixth argument value
* @param x6 sixth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
@ -620,7 +637,7 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta);
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
@ -640,8 +657,9 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*
* @param x6 sixth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
@ -649,7 +667,7 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X4>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta);
return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
@ -669,8 +687,9 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*
* @param x6 sixth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X5 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X5>::dimension>
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
@ -678,7 +697,7 @@ typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X5>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta);
return numericalDerivative11<Y, X5, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
@ -698,8 +717,9 @@ inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*
* @param x6 sixth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X6 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X6>::dimension>
typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
@ -708,7 +728,7 @@ typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X6>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta);
return numericalDerivative11<Y, X6, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>

54
gtsam/base/tests/testNumericalDerivative.cpp
View File

@ -143,6 +143,13 @@ Vector6 f6(const double x1, const double x2, const double x3, const double x4,
return result;
}
Vector g6(const double x1, const double x2, const double x3, const double x4,
const double x5, const double x6) {
Vector result(6);
result << sin(x1), cos(x2), x3 * x3, x4 * x4 * x4, sqrt(x5), sin(x6) - cos(x6);
return result;
}
/* ************************************************************************* */
//
TEST(testNumericalDerivative, numeriDerivative61) {
@ -153,6 +160,14 @@ TEST(testNumericalDerivative, numeriDerivative61) {
double, double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected61, actual61, 1e-5));
Matrix expected61Dynamic = Matrix::Zero(6, 1);
expected61Dynamic(0, 0) = cos(x1);
Matrix actual61Dynamic =
numericalDerivative61<Vector, double, double, double, double, double,
double, 1>(g6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected61Dynamic, actual61Dynamic, 1e-5));
}
/* ************************************************************************* */
@ -165,6 +180,13 @@ TEST(testNumericalDerivative, numeriDerivative62) {
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected62, actual62, 1e-5));
Matrix expected62Dynamic = Matrix::Zero(6, 1);
expected62Dynamic(1, 0) = -sin(x2);
Matrix61 actual62Dynamic = numericalDerivative62<Vector, double, double,
double, double, double, double, 1>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected62Dynamic, actual62Dynamic, 1e-5));
}
/* ************************************************************************* */
@ -177,6 +199,14 @@ TEST(testNumericalDerivative, numeriDerivative63) {
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected63, actual63, 1e-5));
Matrix expected63Dynamic = Matrix::Zero(6, 1);
expected63Dynamic(2, 0) = 2 * x3;
Matrix61 actual63Dynamic =
numericalDerivative63<Vector, double, double, double, double, double,
double, 1>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected63Dynamic, actual63Dynamic, 1e-5));
}
/* ************************************************************************* */
@ -189,6 +219,14 @@ TEST(testNumericalDerivative, numeriDerivative64) {
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected64, actual64, 1e-5));
Matrix expected64Dynamic = Matrix::Zero(6, 1);
expected64Dynamic(3, 0) = 3 * x4 * x4;
Matrix61 actual64Dynamic =
numericalDerivative64<Vector, double, double, double, double, double,
double, 1>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected64Dynamic, actual64Dynamic, 1e-5));
}
/* ************************************************************************* */
@ -201,6 +239,14 @@ TEST(testNumericalDerivative, numeriDerivative65) {
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected65, actual65, 1e-5));
Matrix expected65Dynamic = Matrix::Zero(6, 1);
expected65Dynamic(4, 0) = 0.5 / sqrt(x5);
Matrix61 actual65Dynamic =
numericalDerivative65<Vector, double, double, double, double, double,
double, 1>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected65Dynamic, actual65Dynamic, 1e-5));
}
/* ************************************************************************* */
@ -213,6 +259,14 @@ TEST(testNumericalDerivative, numeriDerivative66) {
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected66, actual66, 1e-5));
Matrix expected66Dynamic = Matrix::Zero(6, 1);
expected66Dynamic(5, 0) = cos(x6) + sin(x6);
Matrix61 actual66Dynamic =
numericalDerivative66<Vector, double, double, double, double, double,
double, 1>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected66Dynamic, actual66Dynamic, 1e-5));
}
/* ************************************************************************* */

40
gtsam/geometry/Cal3Bundler.cpp
View File

@ -25,13 +25,13 @@ namespace gtsam {
/* ************************************************************************* */
Cal3Bundler::Cal3Bundler() :
f_(1), k1_(0), k2_(0), u0_(0), v0_(0) {
f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) {
}
/* ************************************************************************* */
Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) :
f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) {
}
Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0,
double tol)
: f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {}
/* ************************************************************************* */
Matrix3 Cal3Bundler::K() const {
@ -94,21 +94,24 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
}
/* ************************************************************************* */
Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
Point2 Cal3Bundler::calibrate(const Point2& pi,
OptionalJacobian<2, 3> Dcal,
OptionalJacobian<2, 2> Dp) const {
// Copied from Cal3DS2 :-(
// but specialized with k1,k2 non-zero only and fx=fy and s=0
const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_);
double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_;
const Point2 invKPi(x, y);
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
Point2 pn = invKPi;
Point2 pn(x, y);
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (distance2(uncalibrate(pn), pi) <= tol)
if (distance2(uncalibrate(pn), pi) <= tol_)
break;
const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y;
const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
pn = invKPi / g;
@ -118,6 +121,25 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
throw std::runtime_error(
"Cal3Bundler::calibrate fails to converge. need a better initialization");
// We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
// Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians
// df/pi = -I (pn and pi are independent args)
// Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
// Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
Matrix23 H_uncal_K;
Matrix22 H_uncal_pn, H_uncal_pn_inv;
if (Dcal || Dp) {
// Compute uncalibrate Jacobians
uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
H_uncal_pn_inv = H_uncal_pn.inverse();
if (Dp) *Dp = H_uncal_pn_inv;
if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
}
return pn;
}

31
gtsam/geometry/Cal3Bundler.h
View File

@ -33,6 +33,7 @@ private:
double f_; ///< focal length
double k1_, k2_; ///< radial distortion
double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
double tol_; ///< tolerance value when calibrating
public:
@ -51,8 +52,10 @@ public:
* @param k2 second radial distortion coefficient (quartic)
* @param u0 optional image center (default 0), considered a constant
* @param v0 optional image center (default 0), considered a constant
* @param tol optional calibration tolerance value
*/
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0);
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
double tol = 1e-5);
virtual ~Cal3Bundler() {}
@ -95,6 +98,17 @@ public:
return k2_;
}
/// image center in x
inline double px() const {
return u0_;
}
/// image center in y
inline double py() const {
return v0_;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/// get parameter u0
inline double u0() const {
return u0_;
@ -104,6 +118,7 @@ public:
inline double v0() const {
return v0_;
}
#endif
/**
@ -117,8 +132,17 @@ public:
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Convert a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
/**
* Convert a pixel coordinate to ideal coordinate xy
* @param p point in image coordinates
* @param tol optional tolerance threshold value for iterative minimization
* @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& pi,
OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// @deprecated might be removed in next release, use uncalibrate
Matrix2 D2d_intrinsic(const Point2& p) const;
@ -164,6 +188,7 @@ private:
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(tol_);
}
/// @}

12
gtsam/geometry/Point3.cpp
View File

@ -75,6 +75,18 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
return p.x() * q.x() + p.y() * q.y() + p.z() * q.z();
}
Point3Pair means(const std::vector<Point3Pair> &abPointPairs) {
const size_t n = abPointPairs.size();
if (n == 0) throw std::invalid_argument("Point3::mean input Point3Pair vector is empty");
Point3 aSum(0, 0, 0), bSum(0, 0, 0);
for (const Point3Pair &abPair : abPointPairs) {
aSum += abPair.first;
bSum += abPair.second;
}
const double f = 1.0 / n;
return {aSum * f, bSum * f};
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const gtsam::Point3Pair &p) {
os << p.first << " <-> " << p.second;

13
gtsam/geometry/Point3.h
View File

@ -26,6 +26,7 @@
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <numeric>
namespace gtsam {
@ -58,6 +59,18 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H_p = boost::none,
OptionalJacobian<1, 3> H_q = boost::none);
/// mean
template <class CONTAINER>
GTSAM_EXPORT Point3 mean(const CONTAINER& points) {
if (points.size() == 0) throw std::invalid_argument("Point3::mean input container is empty");
Point3 sum(0, 0, 0);
sum = std::accumulate(points.begin(), points.end(), sum);
return sum / points.size();
}
/// Calculate the two means of a set of Point3 pairs
GTSAM_EXPORT Point3Pair means(const std::vector<Point3Pair> &abPointPairs);
template <typename A1, typename A2>
struct Range;

79
gtsam/geometry/Pose3.cpp
View File

@ -24,10 +24,11 @@
#include <limits>
#include <string>
using namespace std;
namespace gtsam {
using std::vector;
using Point3Pairs = vector<Point3Pair>;
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3);
@ -190,15 +191,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
}
/* ************************************************************************* */
/**
* Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix
* J(xi) = [J_(w) Z_3x3;
* Q J_(w)]
* where J_(w) is the SO3 Expmap derivative.
* (see Chirikjian11book2, pg 44, eq 10.95.
* The closed-form formula is similar to formula 102 in Barfoot14tro)
*/
static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) {
const auto w = xi.head<3>();
const auto v = xi.tail<3>();
const Matrix3 V = skewSymmetric(v);
@ -220,18 +213,20 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
#else
// The closed-form formula in Barfoot14tro eq. (102)
double phi = w.norm();
if (std::abs(phi)>1e-5) {
const double sinPhi = sin(phi), cosPhi = cos(phi);
const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
const Matrix3 WVW = W * V * W;
if (std::abs(phi) > nearZeroThreshold) {
const double s = sin(phi), c = cos(phi);
const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi,
phi5 = phi4 * phi;
// Invert the sign of odd-order terms to have the right Jacobian
Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W)
+ (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W)
- 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W);
}
else {
Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W)
+ 1./24.*(W*W*V + V*W*W - 3*W*V*W)
- 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W);
Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) +
(1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) -
0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) *
(WVW * W + W * WVW);
} else {
Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) -
1. / 24. * (W * W * V + V * W * W - 3 * WVW) +
1. / 120. * (WVW * W + W * WVW);
}
#endif
@ -242,7 +237,7 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
const Vector3 w = xi.head<3>();
const Matrix3 Jw = Rot3::ExpmapDerivative(w);
const Matrix3 Q = computeQforExpmapDerivative(xi);
const Matrix3 Q = ComputeQforExpmapDerivative(xi);
Matrix6 J;
J << Jw, Z_3x3, Q, Jw;
return J;
@ -253,7 +248,7 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
const Vector6 xi = Logmap(pose);
const Vector3 w = xi.head<3>();
const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Q = computeQforExpmapDerivative(xi);
const Matrix3 Q = ComputeQforExpmapDerivative(xi);
const Matrix3 Q2 = -Jw*Q*Jw;
Matrix6 J;
J << Jw, Z_3x3, Q2, Jw;
@ -389,39 +384,33 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself,
}
/* ************************************************************************* */
boost::optional<Pose3> Pose3::Align(const std::vector<Point3Pair>& abPointPairs) {
boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
const size_t n = abPointPairs.size();
if (n < 3)
return boost::none; // we need at least three pairs
if (n < 3) {
return boost::none; // we need at least three pairs
}
// calculate centroids
Point3 aCentroid(0,0,0), bCentroid(0,0,0);
for(const Point3Pair& abPair: abPointPairs) {
aCentroid += abPair.first;
bCentroid += abPair.second;
}
double f = 1.0 / n;
aCentroid *= f;
bCentroid *= f;
const auto centroids = means(abPointPairs);
// Add to form H matrix
Matrix3 H = Z_3x3;
for(const Point3Pair& abPair: abPointPairs) {
Point3 da = abPair.first - aCentroid;
Point3 db = abPair.second - bCentroid;
for (const Point3Pair &abPair : abPointPairs) {
const Point3 da = abPair.first - centroids.first;
const Point3 db = abPair.second - centroids.second;
H += da * db.transpose();
}
}
// ClosestTo finds rotation matrix closest to H in Frobenius sense
Rot3 aRb = Rot3::ClosestTo(H);
Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid);
const Rot3 aRb = Rot3::ClosestTo(H);
const Point3 aTb = centroids.first - aRb * centroids.second;
return Pose3(aRb, aTb);
}
boost::optional<Pose3> align(const vector<Point3Pair>& baPointPairs) {
vector<Point3Pair> abPointPairs;
for (const Point3Pair& baPair: baPointPairs) {
abPointPairs.push_back(make_pair(baPair.second, baPair.first));
boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
Point3Pairs abPointPairs;
for (const Point3Pair &baPair : baPointPairs) {
abPointPairs.emplace_back(baPair.second, baPair.first);
}
return Pose3::Align(abPointPairs);
}

15
gtsam/geometry/Pose3.h
View File

@ -181,6 +181,18 @@ public:
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
};
/**
* Compute the 3x3 bottom-left block Q of SE3 Expmap right derivative matrix
* J_r(xi) = [J_(w) Z_3x3;
* Q_r J_(w)]
* where J_(w) is the SO3 Expmap right derivative.
* (see Chirikjian11book2, pg 44, eq 10.95.
* The closed-form formula is identical to formula 102 in Barfoot14tro where
* Q_l of the SE3 Expmap left derivative matrix is given.
*/
static Matrix3 ComputeQforExpmapDerivative(
const Vector6& xi, double nearZeroThreshold = 1e-5);
using LieGroup<Pose3, 6>::inverse; // version with derivative
/**
@ -356,6 +368,9 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
}
// Convenience typedef
using Pose3Pair = std::pair<Pose3, Pose3>;
// For MATLAB wrapper
typedef std::vector<Pose3> Pose3Vector;

24
gtsam/geometry/Rot3.h
View File

@ -262,9 +262,29 @@ namespace gtsam {
static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, //
const Unit3& a_q, const Unit3& b_q);
/// Static, named constructor that finds Rot3 element closest to M in Frobenius norm.
/**
* Static, named constructor that finds Rot3 element closest to M in Frobenius norm.
*
* Uses Full SVD to compute the orthogonal matrix, thus is highly accurate and robust.
*
* N. J. Higham. Matrix nearness problems and applications.
* In M. J. C. Gover and S. Barnett, editors, Applications of Matrix Theory, pages 1–27.
* Oxford University Press, 1989.
*/
static Rot3 ClosestTo(const Matrix3& M) { return Rot3(SO3::ClosestTo(M)); }
/**
* Normalize rotation so that its determinant is 1.
* This means either re-orthogonalizing the Matrix representation or
* normalizing the quaternion representation.
*
* This method is akin to `ClosestTo` but uses a computationally cheaper
* algorithm.
*
* Ref: https://drive.google.com/file/d/0B9rLLz1XQKmaZTlQdV81QjNoZTA/view
*/
Rot3 normalized() const;
/// @}
/// @name Testable
/// @{
@ -506,7 +526,7 @@ namespace gtsam {
/**
* @brief Spherical Linear intERPolation between *this and other
* @param s a value between 0 and 1
* @param t a value between 0 and 1
* @param other final point of iterpolation geodesic on manifold
*/
Rot3 slerp(double t, const Rot3& other) const;

27
gtsam/geometry/Rot3M.cpp
View File

@ -108,6 +108,33 @@ Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx,
);
}
/* ************************************************************************* */
Rot3 Rot3::normalized() const {
/// Implementation from here: https://stackoverflow.com/a/23082112/1236990
/// Essentially, this computes the orthogonalization error, distributes the
/// error to the x and y rows, and then performs a Taylor expansion to
/// orthogonalize.
Matrix3 rot = rot_.matrix(), rot_orth;
// Check if determinant is already 1.
// If yes, then return the current Rot3.
if (std::fabs(rot.determinant()-1) < 1e-12) return Rot3(rot_);
Vector3 x = rot.block<1, 3>(0, 0), y = rot.block<1, 3>(1, 0);
double error = x.dot(y);
Vector3 x_ort = x - (error / 2) * y, y_ort = y - (error / 2) * x;
Vector3 z_ort = x_ort.cross(y_ort);
rot_orth.block<1, 3>(0, 0) = 0.5 * (3 - x_ort.dot(x_ort)) * x_ort;
rot_orth.block<1, 3>(1, 0) = 0.5 * (3 - y_ort.dot(y_ort)) * y_ort;
rot_orth.block<1, 3>(2, 0) = 0.5 * (3 - z_ort.dot(z_ort)) * z_ort;
return Rot3(rot_orth);
}
/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(rot_*R2.rot_);

4
gtsam/geometry/Rot3Q.cpp
View File

@ -86,6 +86,10 @@ namespace gtsam {
gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
}
/* ************************************************************************* */
Rot3 Rot3::normalized() const {
return Rot3(quaternion_.normalized());
}
/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(quaternion_ * R2.quaternion_);

23
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -52,12 +52,14 @@ TEST( Cal3Bundler, calibrate )
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 pn_hat = K.calibrate(pi);
CHECK( traits<Point2>::Equals(pn, pn_hat, 1e-5));
CHECK(traits<Point2>::Equals(pn, pn_hat, 1e-5));
}
/* ************************************************************************* */
Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
/* ************************************************************************* */
TEST( Cal3Bundler, Duncalibrate)
{
@ -69,11 +71,20 @@ TEST( Cal3Bundler, Duncalibrate)
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical1,Dcal,1e-7));
CHECK(assert_equal(numerical2,Dp,1e-7));
CHECK(assert_equal(numerical1,K.D2d_calibration(p),1e-7));
CHECK(assert_equal(numerical2,K.D2d_intrinsic(p),1e-7));
Matrix Dcombined(2,5);
Dcombined << Dp, Dcal;
CHECK(assert_equal(Dcombined,K.D2d_intrinsic_calibration(p),1e-7));
}
/* ************************************************************************* */
TEST( Cal3Bundler, Dcalibrate)
{
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 actual = K.calibrate(pi, Dcal, Dp);
CHECK(assert_equal(pn, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical1,Dcal,1e-5));
CHECK(assert_equal(numerical2,Dp,1e-5));
}
/* ************************************************************************* */

9
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -336,6 +336,15 @@ TEST( PinholeCamera, range3) {
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
TEST( PinholeCamera, Cal3Bundler) {
Cal3Bundler calibration;
Pose3 wTc;
PinholeCamera<Cal3Bundler> camera(wTc, calibration);
Point2 p(50, 100);
camera.backproject(p, 10);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

20
gtsam/geometry/tests/testPoint3.cpp
View File

@ -164,6 +164,26 @@ TEST (Point3, normalize) {
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
//*************************************************************************
TEST(Point3, mean) {
Point3 expected(2, 2, 2);
Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
std::vector<Point3> a_points{a1, a2, a3};
Point3 actual = mean(a_points);
EXPECT(assert_equal(expected, actual));
}
TEST(Point3, mean_pair) {
Point3 a_mean(2, 2, 2), b_mean(-1, 1, 0);
Point3Pair expected = std::make_pair(a_mean, b_mean);
Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0);
std::vector<Point3Pair> point_pairs{{a1, b1}, {a2, b2}, {a3, b3}};
Point3Pair actual = means(point_pairs);
EXPECT(assert_equal(expected.first, actual.first));
EXPECT(assert_equal(expected.second, actual.second));
}
//*************************************************************************
double norm_proxy(const Point3& point) {
return double(point.norm());

11
gtsam/geometry/tests/testPose3.cpp
View File

@ -807,6 +807,17 @@ TEST(Pose3, ExpmapDerivative2) {
}
}
TEST( Pose3, ExpmapDerivativeQr) {
Vector6 w = Vector6::Random();
w.head<3>().normalize();
w.head<3>() = w.head<3>() * 0.9e-2;
Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none);
Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
}
/* ************************************************************************* */
TEST( Pose3, LogmapDerivative) {
Matrix6 actualH;

20
gtsam/geometry/tests/testRot3.cpp
View File

@ -910,6 +910,26 @@ TEST(Rot3, yaw_derivative) {
CHECK(assert_equal(num, calc));
}
/* ************************************************************************* */
TEST(Rot3, determinant) {
size_t degree = 1;
Rot3 R_w0; // Zero rotation
Rot3 R_w1 = Rot3::Ry(degree * M_PI / 180);
Rot3 R_01, R_w2;
double actual, expected = 1.0;
for (size_t i = 2; i < 360; ++i) {
R_01 = R_w0.between(R_w1);
R_w2 = R_w1 * R_01;
R_w0 = R_w1;
R_w1 = R_w2.normalized();
actual = R_w2.matrix().determinant();
EXPECT_DOUBLES_EQUAL(expected, actual, 1e-7);
}
}
/* ************************************************************************* */
int main() {
TestResult tr;

6
gtsam/geometry/tests/testSerializationGeometry.cpp
View File

@ -57,7 +57,7 @@ static StereoCamera cam2(pose3, cal4ptr);
static StereoPoint2 spt(1.0, 2.0, 3.0);
/* ************************************************************************* */
TEST_DISABLED (Serialization, text_geometry) {
TEST (Serialization, text_geometry) {
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
@ -82,7 +82,7 @@ TEST_DISABLED (Serialization, text_geometry) {
}
/* ************************************************************************* */
TEST_DISABLED (Serialization, xml_geometry) {
TEST (Serialization, xml_geometry) {
EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
@ -106,7 +106,7 @@ TEST_DISABLED (Serialization, xml_geometry) {
}
/* ************************************************************************* */
TEST_DISABLED (Serialization, binary_geometry) {
TEST (Serialization, binary_geometry) {
EXPECT(equalsBinary<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsBinary<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsBinary<gtsam::Rot2>(Rot2::fromDegrees(30.0)));

179
gtsam/gtsam.i
View File

@ -93,9 +93,9 @@
* - Add "void serializable()" to a class if you only want the class to be serialized as a
* part of a container (such as noisemodel). This version does not require a publicly
* accessible default constructor.
* Forward declarations and class definitions for Cython:
* - Need to specify the base class (both this forward class and base class are declared in an external cython header)
* This is so Cython can generate proper inheritance.
* Forward declarations and class definitions for Pybind:
* - Need to specify the base class (both this forward class and base class are declared in an external Pybind header)
* This is so Pybind can generate proper inheritance.
* Example when wrapping a gtsam-based project:
* // forward declarations
* virtual class gtsam::NonlinearFactor
@ -104,7 +104,7 @@
* #include <MyFactor.h>
* virtual class MyFactor : gtsam::NoiseModelFactor {...};
* - *DO NOT* re-define overriden function already declared in the external (forward-declared) base class
* - This will cause an ambiguity problem in Cython pxd header file
* - This will cause an ambiguity problem in Pybind header file
*/
/**
@ -334,99 +334,6 @@ virtual class GenericValue : gtsam::Value {
void serializable() const;
};
#include <gtsam/base/deprecated/LieScalar.h>
class LieScalar {
// Standard constructors
LieScalar();
LieScalar(double d);
// Standard interface
double value() const;
// Testable
void print(string s) const;
bool equals(const gtsam::LieScalar& expected, double tol) const;
// Group
static gtsam::LieScalar identity();
gtsam::LieScalar inverse() const;
gtsam::LieScalar compose(const gtsam::LieScalar& p) const;
gtsam::LieScalar between(const gtsam::LieScalar& l2) const;
// Manifold
size_t dim() const;
gtsam::LieScalar retract(Vector v) const;
Vector localCoordinates(const gtsam::LieScalar& t2) const;
// Lie group
static gtsam::LieScalar Expmap(Vector v);
static Vector Logmap(const gtsam::LieScalar& p);
};
#include <gtsam/base/deprecated/LieVector.h>
class LieVector {
// Standard constructors
LieVector();
LieVector(Vector v);
// Standard interface
Vector vector() const;
// Testable
void print(string s) const;
bool equals(const gtsam::LieVector& expected, double tol) const;
// Group
static gtsam::LieVector identity();
gtsam::LieVector inverse() const;
gtsam::LieVector compose(const gtsam::LieVector& p) const;
gtsam::LieVector between(const gtsam::LieVector& l2) const;
// Manifold
size_t dim() const;
gtsam::LieVector retract(Vector v) const;
Vector localCoordinates(const gtsam::LieVector& t2) const;
// Lie group
static gtsam::LieVector Expmap(Vector v);
static Vector Logmap(const gtsam::LieVector& p);
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/base/deprecated/LieMatrix.h>
class LieMatrix {
// Standard constructors
LieMatrix();
LieMatrix(Matrix v);
// Standard interface
Matrix matrix() const;
// Testable
void print(string s) const;
bool equals(const gtsam::LieMatrix& expected, double tol) const;
// Group
static gtsam::LieMatrix identity();
gtsam::LieMatrix inverse() const;
gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const;
gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const;
// Manifold
size_t dim() const;
gtsam::LieMatrix retract(Vector v) const;
Vector localCoordinates(const gtsam::LieMatrix & t2) const;
// Lie group
static gtsam::LieMatrix Expmap(Vector v);
static Vector Logmap(const gtsam::LieMatrix& p);
// enabling serialization functionality
void serialize() const;
};
//*************************************************************************
// geometry
//*************************************************************************
@ -690,6 +597,7 @@ class Rot3 {
Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33);
Rot3(double w, double x, double y, double z);
static gtsam::Rot3 Rx(double t);
static gtsam::Rot3 Ry(double t);
@ -1052,6 +960,7 @@ class Cal3Bundler {
// Standard Constructors
Cal3Bundler();
Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol);
// Testable
void print(string s) const;
@ -1064,7 +973,7 @@ class Cal3Bundler {
Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
// Standard Interface
@ -1072,8 +981,8 @@ class Cal3Bundler {
double fy() const;
double k1() const;
double k2() const;
double u0() const;
double v0() const;
double px() const;
double py() const;
Vector vector() const;
Vector k() const;
Matrix K() const;
@ -1198,7 +1107,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
//typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
#include <gtsam/geometry/StereoCamera.h>
class StereoCamera {
@ -2160,7 +2069,7 @@ class NonlinearFactorGraph {
gtsam::KeySet keys() const;
gtsam::KeyVector keyVector() const;
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}>
void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
// NonlinearFactorGraph
@ -2249,12 +2158,13 @@ class Values {
void insert(size_t j, const gtsam::SOn& P);
void insert(size_t j, const gtsam::Rot3& rot3);
void insert(size_t j, const gtsam::Pose3& pose3);
void insert(size_t j, const gtsam::Unit3& unit3);
void insert(size_t j, const gtsam::Cal3_S2& cal3_s2);
void insert(size_t j, const gtsam::Cal3DS2& cal3ds2);
void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
// void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void insert(size_t j, const gtsam::NavState& nav_state);
@ -2267,18 +2177,19 @@ class Values {
void update(size_t j, const gtsam::SOn& P);
void update(size_t j, const gtsam::Rot3& rot3);
void update(size_t j, const gtsam::Pose3& pose3);
void update(size_t j, const gtsam::Unit3& unit3);
void update(size_t j, const gtsam::Cal3_S2& cal3_s2);
void update(size_t j, const gtsam::Cal3DS2& cal3ds2);
void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
// void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void update(size_t j, const gtsam::NavState& nav_state);
void update(size_t j, Vector vector);
void update(size_t j, Matrix matrix);
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
T at(size_t j);
/// version for double
@ -2555,6 +2466,8 @@ class ISAM2Result {
size_t getVariablesRelinearized() const;
size_t getVariablesReeliminated() const;
size_t getCliques() const;
double getErrorBefore() const;
double getErrorAfter() const;
};
class ISAM2 {
@ -2571,16 +2484,17 @@ class ISAM2 {
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys);
// TODO: wrap the full version of update
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys);
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys, bool force_relinearize);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, bool force_relinearize);
gtsam::Values getLinearizationPoint() const;
gtsam::Values calculateEstimate() const;
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, Vector, Matrix}>
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
Vector, Matrix}>
VALUE calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const;
Matrix marginalCovariance(size_t key) const;
@ -2618,7 +2532,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/nonlinear/PriorFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
@ -2705,8 +2619,7 @@ class BearingRange {
BearingRange(const BEARING& b, const RANGE& r);
BEARING bearing() const;
RANGE range() const;
// TODO(frank): can't class instance itself?
// static gtsam::BearingRange Measure(const POSE& pose, const POINT& point);
static This Measure(const POSE& pose, const POINT& point);
static BEARING MeasureBearing(const POSE& pose, const POINT& point);
static RANGE MeasureRange(const POSE& pose, const POINT& point);
void print(string s) const;
@ -2764,6 +2677,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
template<CALIBRATION = {gtsam::Cal3_S2}>
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
@ -2850,22 +2764,36 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
};
#include <gtsam/slam/dataset.h>
class SfmTrack {
Point3 point3() const;
SfmTrack();
SfmTrack(const gtsam::Point3& pt);
const Point3& point3() const;
double r;
double g;
double b;
// TODO Need to close wrap#10 to allow this to work.
// std::vector<pair<size_t, gtsam::Point2>> measurements;
size_t number_measurements() const;
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
pair<size_t, size_t> siftIndex(size_t idx) const;
void add_measurement(size_t idx, const gtsam::Point2& m);
};
class SfmData {
SfmData();
size_t number_cameras() const;
size_t number_tracks() const;
//TODO(Varun) Need to fix issue #237 first before this can work
// gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::SfmTrack track(size_t idx) const;
void add_track(const gtsam::SfmTrack& t) ;
void add_camera(const gtsam::SfmCamera& cam);
};
gtsam::SfmData readBal(string filename);
bool writeBAL(string filename, gtsam::SfmData& data);
gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
@ -2977,6 +2905,7 @@ class BinaryMeasurement {
size_t key1() const;
size_t key2() const;
T measured() const;
gtsam::noiseModel::Base* noiseModel() const;
};
typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
@ -3110,6 +3039,26 @@ class ShonanAveraging3 {
pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
};
#include <gtsam/sfm/MFAS.h>
class KeyPairDoubleMap {
KeyPairDoubleMap();
KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other);
size_t size() const;
bool empty() const;
void clear();
size_t at(const pair<size_t, size_t>& keypair) const;
};
class MFAS {
MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
const gtsam::Unit3& projectionDirection);
gtsam::KeyPairDoubleMap computeOutlierWeights() const;
gtsam::KeyVector computeOrdering() const;
};
#include <gtsam/sfm/TranslationRecovery.h>
class TranslationRecovery {
TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations,

10
gtsam/inference/Symbol.h
View File

@ -164,8 +164,16 @@ inline Key Y(std::uint64_t j) { return Symbol('y', j); }
inline Key Z(std::uint64_t j) { return Symbol('z', j); }
}
/** Generates symbol shorthands with alternative names different than the
* one-letter predefined ones. */
class SymbolGenerator {
const char c_;
public:
SymbolGenerator(const char c) : c_(c) {}
Symbol operator()(const std::uint64_t j) const { return Symbol(c_, j); }
};
/// traits
template<> struct traits<Symbol> : public Testable<Symbol> {};
} // \ namespace gtsam

20
gtsam/inference/tests/testKey.cpp
View File

@ -40,6 +40,25 @@ TEST(Key, KeySymbolConversion) {
EXPECT(assert_equal(original, actual))
}
/* ************************************************************************* */
TEST(Key, SymbolGenerator) {
const auto x1 = gtsam::symbol_shorthand::X(1);
const auto v1 = gtsam::symbol_shorthand::V(1);
const auto a1 = gtsam::symbol_shorthand::A(1);
const auto Z = gtsam::SymbolGenerator('x');
const auto DZ = gtsam::SymbolGenerator('v');
const auto DDZ = gtsam::SymbolGenerator('a');
const auto z1 = Z(1);
const auto dz1 = DZ(1);
const auto ddz1 = DDZ(1);
EXPECT(assert_equal(x1, z1));
EXPECT(assert_equal(v1, dz1));
EXPECT(assert_equal(a1, ddz1));
}
/* ************************************************************************* */
template<int KeySize>
Key KeyTestValue();
@ -106,4 +125,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

2
gtsam/navigation/AHRSFactor.h
View File

@ -42,7 +42,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
public:
/// Default constructor, only for serialization and Cython wrapper
/// Default constructor, only for serialization and wrappers
PreintegratedAhrsMeasurements() {}
/**

2
gtsam/navigation/CombinedImuFactor.h
View File

@ -145,7 +145,7 @@ public:
/// @name Constructors
/// @{
/// Default constructor only for serialization and Cython wrapper
/// Default constructor only for serialization and wrappers
PreintegratedCombinedMeasurements() {
preintMeasCov_.setZero();
}

2
gtsam/navigation/ImuFactor.h
View File

@ -80,7 +80,7 @@ protected:
public:
/// Default constructor for serialization and Cython wrapper
/// Default constructor for serialization and wrappers
PreintegratedImuMeasurements() {
preintMeasCov_.setZero();
}

2
gtsam/nonlinear/ISAM2Result.h
View File

@ -176,6 +176,8 @@ struct ISAM2Result {
size_t getVariablesRelinearized() const { return variablesRelinearized; }
size_t getVariablesReeliminated() const { return variablesReeliminated; }
size_t getCliques() const { return cliques; }
double getErrorBefore() const { return errorBefore ? *errorBefore : std::nan(""); }
double getErrorAfter() const { return errorAfter ? *errorAfter : std::nan(""); }
};
} // namespace gtsam

4
gtsam/nonlinear/Marginals.h
View File

@ -48,7 +48,7 @@ protected:
public:
/// Default constructor only for Cython wrapper
/// Default constructor only for wrappers
Marginals(){}
/** Construct a marginals class from a nonlinear factor graph.
@ -156,7 +156,7 @@ protected:
FastMap<Key, size_t> indices_;
public:
/// Default constructor only for Cython wrapper
/// Default constructor only for wrappers
JointMarginal() {}
/** Access a block, corresponding to a pair of variables, of the joint

10
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -88,20 +88,24 @@ void NonlinearOptimizer::defaultOptimize() {
}
// Iterative loop
double newError = currentError; // used to avoid repeated calls to error()
do {
// Do next iteration
currentError = error(); // TODO(frank): don't do this twice at first !? Computed above!
currentError = newError;
iterate();
tictoc_finishedIteration();
// Update newError for either printouts or conditional-end checks:
newError = error();
// Maybe show output
if (params.verbosity >= NonlinearOptimizerParams::VALUES)
values().print("newValues");
if (params.verbosity >= NonlinearOptimizerParams::ERROR)
cout << "newError: " << error() << endl;
cout << "newError: " << newError << endl;
} while (iterations() < params.maxIterations &&
!checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol,
currentError, error(), params.verbosity) && std::isfinite(currentError));
currentError, newError, params.verbosity) && std::isfinite(currentError));
// Printing if verbose
if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) {

2
gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -41,7 +41,7 @@ struct Cal3Bundler0 : public Cal3Bundler {
double v0 = 0)
: Cal3Bundler(f, k1, k2, u0, v0) {}
Cal3Bundler0 retract(const Vector& d) const {
return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0());
return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), px(), py());
}
Vector3 localCoordinates(const Cal3Bundler0& T2) const {
return T2.vector() - vector();

6
gtsam/sfm/MFAS.cpp
View File

@ -111,10 +111,8 @@ void removeNodeFromGraph(const Key node,
graph.erase(node);
}
MFAS::MFAS(const std::shared_ptr<vector<Key>>& nodes,
const TranslationEdges& relativeTranslations,
const Unit3& projectionDirection)
: nodes_(nodes) {
MFAS::MFAS(const TranslationEdges& relativeTranslations,
const Unit3& projectionDirection) {
// Iterate over edges, obtain weights by projecting
// their relativeTranslations along the projection direction
for (const auto& measurement : relativeTranslations) {

24
gtsam/sfm/MFAS.h
View File

@ -56,40 +56,28 @@ class MFAS {
using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
private:
// pointer to nodes in the graph
const std::shared_ptr<std::vector<Key>> nodes_;
// edges with a direction such that all weights are positive
// i.e, edges that originally had negative weights are flipped
std::map<KeyPair, double> edgeWeights_;
public:
/**
* @brief Construct from the nodes in a graph and weighted directed edges
* @brief Construct from the weighted directed edges
* between the nodes. Each node is identified by a Key.
* A shared pointer to the nodes is used as input parameter
* because, MFAS ordering is usually used to compute the ordering of a
* large graph that is already stored in memory. It is unnecessary to make a
* copy of the nodes in this class.
* @param nodes: Nodes in the graph
* @param edgeWeights: weights of edges in the graph
*/
MFAS(const std::shared_ptr<std::vector<Key>> &nodes,
const std::map<KeyPair, double> &edgeWeights)
: nodes_(nodes), edgeWeights_(edgeWeights) {}
MFAS(const std::map<KeyPair, double> &edgeWeights)
: edgeWeights_(edgeWeights) {}
/**
* @brief Constructor to be used in the context of translation averaging.
* Here, the nodes of the graph are cameras in 3D and the edges have a unit
* translation direction between them. The weights of the edges is computed by
* projecting them along a projection direction.
* @param nodes cameras in the epipolar graph (each camera is identified by a
* Key)
* @param relativeTranslations translation directions between the cameras
* @param projectionDirection direction in which edges are to be projected
*/
MFAS(const std::shared_ptr<std::vector<Key>> &nodes,
const TranslationEdges &relativeTranslations,
MFAS(const TranslationEdges &relativeTranslations,
const Unit3 &projectionDirection);
/**
@ -99,7 +87,7 @@ class MFAS {
std::vector<Key> computeOrdering() const;
/**
* @brief Computes the "outlier weights" of the graph. We define the outlier
* @brief Computes the outlier weights of the graph. We define the outlier
* weight of a edge to be zero if the edge is an inlier and the magnitude of
* its edgeWeight if it is an outlier. This function internally calls
* computeOrdering and uses the obtained ordering to identify outlier edges.
@ -108,4 +96,6 @@ class MFAS {
std::map<KeyPair, double> computeOutlierWeights() const;
};
typedef std::map<std::pair<Key, Key>, double> KeyPairDoubleMap;
} // namespace gtsam

7
gtsam/sfm/tests/testMFAS.cpp
View File

@ -6,7 +6,7 @@
*/
#include <gtsam/sfm/MFAS.h>
#include <iostream>
#include <CppUnitLite/TestHarness.h>
using namespace std;
@ -39,14 +39,13 @@ map<MFAS::KeyPair, double> getEdgeWeights(const vector<MFAS::KeyPair> &edges,
for (size_t i = 0; i < edges.size(); i++) {
edgeWeights[edges[i]] = weights[i];
}
cout << "returning edge weights " << edgeWeights.size() << endl;
return edgeWeights;
}
// test the ordering and the outlierWeights function using weights2 - outlier
// edge is rejected when projected in a direction that gives weights2
TEST(MFAS, OrderingWeights2) {
MFAS mfas_obj(make_shared<vector<Key>>(nodes), getEdgeWeights(edges, weights2));
MFAS mfas_obj(getEdgeWeights(edges, weights2));
vector<Key> ordered_nodes = mfas_obj.computeOrdering();
@ -76,7 +75,7 @@ TEST(MFAS, OrderingWeights2) {
// weights1 (outlier edge is accepted when projected in a direction that
// produces weights1)
TEST(MFAS, OrderingWeights1) {
MFAS mfas_obj(make_shared<vector<Key>>(nodes), getEdgeWeights(edges, weights1));
MFAS mfas_obj(getEdgeWeights(edges, weights1));
vector<Key> ordered_nodes = mfas_obj.computeOrdering();

4
gtsam/slam/dataset.cpp
View File

@ -1164,8 +1164,8 @@ bool writeBAL(const string &filename, SfmData &data) {
for (size_t k = 0; k < track.number_measurements();
k++) { // for each observation of the 3D point j
size_t i = track.measurements[k].first; // camera id
double u0 = data.cameras[i].calibration().u0();
double v0 = data.cameras[i].calibration().v0();
double u0 = data.cameras[i].calibration().px();
double v0 = data.cameras[i].calibration().py();
if (u0 != 0 || v0 != 0) {
cout << "writeBAL has not been tested for calibration with nonzero "

20
gtsam/slam/dataset.h
View File

@ -211,16 +211,18 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
/// A measurement with its camera index
typedef std::pair<size_t, Point2> SfmMeasurement;
/// SfmTrack
/// Sift index for SfmTrack
typedef std::pair<size_t, size_t> SiftIndex;
/// Define the structure for the 3D points
struct SfmTrack {
SfmTrack(): p(0,0,0) {}
SfmTrack(const gtsam::Point3& pt) : p(pt) {}
Point3 p; ///< 3D position of the point
float r, g, b; ///< RGB color of the 3D point
std::vector<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
std::vector<SiftIndex> siftIndices;
/// Total number of measurements in this track
size_t number_measurements() const {
return measurements.size();
@ -233,11 +235,17 @@ struct SfmTrack {
SiftIndex siftIndex(size_t idx) const {
return siftIndices[idx];
}
Point3 point3() const {
/// Get 3D point
const Point3& point3() const {
return p;
}
/// Add measurement (camera_idx, Point2) to track
void add_measurement(size_t idx, const gtsam::Point2& m) {
measurements.emplace_back(idx, m);
}
};
/// Define the structure for the camera poses
typedef PinholeCamera<Cal3Bundler> SfmCamera;
@ -260,6 +268,14 @@ struct SfmData {
SfmTrack track(size_t idx) const {
return tracks[idx];
}
/// Add a track to SfmData
void add_track(const SfmTrack& t) {
tracks.push_back(t);
}
/// Add a camera to SfmData
void add_camera(const SfmCamera& cam){
cameras.push_back(cam);
}
};
/**

5
gtsam_extra.cmake.in
View File

@ -7,8 +7,3 @@ set (GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@")
set (GTSAM_USE_TBB @GTSAM_USE_TBB@)
set (GTSAM_DEFAULT_ALLOCATOR @GTSAM_DEFAULT_ALLOCATOR@)
if("@GTSAM_INSTALL_CYTHON_TOOLBOX@")
list(APPEND GTSAM_CYTHON_INSTALL_PATH "@GTSAM_CYTHON_INSTALL_PATH@")
list(APPEND GTSAM_EIGENCY_INSTALL_PATH "@GTSAM_EIGENCY_INSTALL_PATH@")
endif()

116
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -19,9 +19,68 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/slam/KarcherMeanFactor-inl.h>
namespace gtsam {
using std::vector;
using PointPairs = vector<Point3Pair>;
namespace {
/// Subtract centroids from point pairs.
static PointPairs subtractCentroids(const PointPairs &abPointPairs,
const Point3Pair &centroids) {
PointPairs d_abPointPairs;
for (const Point3Pair& abPair : abPointPairs) {
Point3 da = abPair.first - centroids.first;
Point3 db = abPair.second - centroids.second;
d_abPointPairs.emplace_back(da, db);
}
return d_abPointPairs;
}
/// Form inner products x and y and calculate scale.
static const double calculateScale(const PointPairs &d_abPointPairs,
const Rot3 &aRb) {
double x = 0, y = 0;
Point3 da, db;
for (const Point3Pair& d_abPair : d_abPointPairs) {
std::tie(da, db) = d_abPair;
const Vector3 da_prime = aRb * db;
y += da.transpose() * da_prime;
x += da_prime.transpose() * da_prime;
}
const double s = y / x;
return s;
}
/// Form outer product H.
static Matrix3 calculateH(const PointPairs &d_abPointPairs) {
Matrix3 H = Z_3x3;
for (const Point3Pair& d_abPair : d_abPointPairs) {
H += d_abPair.first * d_abPair.second.transpose();
}
return H;
}
/// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids.
static Similarity3 align(const PointPairs &d_abPointPairs, const Rot3 &aRb,
const Point3Pair &centroids) {
const double s = calculateScale(d_abPointPairs, aRb);
const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
return Similarity3(aRb, aTb, s);
}
/// This method estimates the similarity transform from point pairs, given a known or estimated rotation.
// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
static Similarity3 alignGivenR(const PointPairs &abPointPairs,
const Rot3 &aRb) {
auto centroids = means(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
return align(d_abPointPairs, aRb, centroids);
}
} // namespace
Similarity3::Similarity3() :
t_(0,0,0), s_(1) {
}
@ -54,15 +113,15 @@ bool Similarity3::operator==(const Similarity3& other) const {
void Similarity3::print(const std::string& s) const {
std::cout << std::endl;
std::cout << s;
rotation().print("R:\n");
std::cout << "t: " << translation().transpose() << "s: " << scale() << std::endl;
rotation().print("\nR:\n");
std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl;
}
Similarity3 Similarity3::identity() {
return Similarity3();
}
Similarity3 Similarity3::operator*(const Similarity3& T) const {
return Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_);
Similarity3 Similarity3::operator*(const Similarity3& S) const {
return Similarity3(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
}
Similarity3 Similarity3::inverse() const {
@ -85,11 +144,51 @@ Point3 Similarity3::transformFrom(const Point3& p, //
return s_ * q;
}
Pose3 Similarity3::transformFrom(const Pose3& T) const {
Rot3 R = R_.compose(T.rotation());
Point3 t = Point3(s_ * (R_ * T.translation() + t_));
return Pose3(R, t);
}
Point3 Similarity3::operator*(const Point3& p) const {
return transformFrom(p);
}
Matrix4 Similarity3::wedge(const Vector7& xi) {
Similarity3 Similarity3::Align(const PointPairs &abPointPairs) {
// Refer to Chapter 3 of
// http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
if (abPointPairs.size() < 3)
throw std::runtime_error("input should have at least 3 pairs of points");
auto centroids = means(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
Matrix3 H = calculateH(d_abPointPairs);
// ClosestTo finds rotation matrix closest to H in Frobenius sense
Rot3 aRb = Rot3::ClosestTo(H);
return align(d_abPointPairs, aRb, centroids);
}
Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
const size_t n = abPosePairs.size();
if (n < 2)
throw std::runtime_error("input should have at least 2 pairs of poses");
// calculate rotation
vector<Rot3> rotations;
PointPairs abPointPairs;
rotations.reserve(n);
abPointPairs.reserve(n);
Pose3 wTa, wTb;
for (const Pose3Pair &abPair : abPosePairs) {
std::tie(wTa, wTb) = abPair;
rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse()));
abPointPairs.emplace_back(wTa.translation(), wTb.translation());
}
const Rot3 aRb = FindKarcherMean<Rot3>(rotations);
return alignGivenR(abPointPairs, aRb);
}
Matrix4 Similarity3::wedge(const Vector7 &xi) {
// http://www.ethaneade.org/latex2html/lie/node29.html
const auto w = xi.head<3>();
const auto u = xi.segment<3>(3);
@ -128,12 +227,13 @@ Matrix3 Similarity3::GetV(Vector3 w, double lambda) {
W = 1.0 / 24.0 - theta2 / 720.0;
}
const double lambda2 = lambda * lambda, lambda3 = lambda2 * lambda;
const double expMinLambda = exp(-lambda);
double A, alpha = 0.0, beta, mu;
if (lambda2 > 1e-9) {
A = (1.0 - exp(-lambda)) / lambda;
A = (1.0 - expMinLambda) / lambda;
alpha = 1.0 / (1.0 + theta2 / lambda2);
beta = (exp(-lambda) - 1 + lambda) / lambda2;
mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda)) / lambda3;
beta = (expMinLambda - 1 + lambda) / lambda2;
mu = (1 - lambda + (0.5 * lambda2) - expMinLambda) / lambda3;
} else {
A = 1.0 - lambda / 2.0 + lambda2 / 6.0;
beta = 0.5 - lambda / 6.0 + lambda2 / 24.0 - lambda3 / 120.0;

29
gtsam_unstable/geometry/Similarity3.h
View File

@ -19,10 +19,12 @@
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
#include <gtsam_unstable/dllexport.h>
namespace gtsam {
// Forward declarations
@ -87,7 +89,7 @@ public:
GTSAM_UNSTABLE_EXPORT static Similarity3 identity();
/// Composition
GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& T) const;
GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& S) const;
/// Return the inverse
GTSAM_UNSTABLE_EXPORT Similarity3 inverse() const;
@ -101,9 +103,32 @@ public:
OptionalJacobian<3, 7> H1 = boost::none, //
OptionalJacobian<3, 3> H2 = boost::none) const;
/**
* Action on a pose T.
* |Rs ts| |R t| |Rs*R Rs*t+ts|
* |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim3 object.
* To retrieve a Pose3, we normalized the scale value into 1.
* |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)|
* | 0 1/s | = | 0 1 |
*
* This group action satisfies the compatibility condition.
* For more details, refer to: https://en.wikipedia.org/wiki/Group_action
*/
GTSAM_UNSTABLE_EXPORT Pose3 transformFrom(const Pose3& T) const;
/** syntactic sugar for transformFrom */
GTSAM_UNSTABLE_EXPORT Point3 operator*(const Point3& p) const;
/**
* Create Similarity3 by aligning at least three point pairs
*/
GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
/**
* Create Similarity3 by aligning at least two pose pairs
*/
GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
/// @}
/// @name Lie Group
/// @{
@ -182,8 +207,8 @@ public:
/// @name Helper functions
/// @{
/// Calculate expmap and logmap coefficients.
private:
/// Calculate expmap and logmap coefficients.
static Matrix3 GetV(Vector3 w, double lambda);
/// @}

110
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -51,6 +51,8 @@ static const Similarity3 T4(R, P, s);
static const Similarity3 T5(R, P, 10);
static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform
const double degree = M_PI / 180;
//******************************************************************************
TEST(Similarity3, Concepts) {
BOOST_CONCEPT_ASSERT((IsGroup<Similarity3 >));
@ -255,6 +257,114 @@ TEST(Similarity3, GroupAction) {
}
}
//******************************************************************************
// Group action on Pose3
TEST(Similarity3, GroupActionPose3) {
Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
// Create source poses
Pose3 Ta1(Rot3(), Point3(0, 0, 0));
Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
// Create destination poses
Pose3 expected_Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 expected_Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
EXPECT(assert_equal(expected_Tb1, bSa.transformFrom(Ta1)));
EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2)));
}
// Test left group action compatibility.
// cSa*Ta = cSb*bSa*Ta
TEST(Similarity3, GroupActionPose3_Compatibility) {
Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
Similarity3 cSb(Rot3::Ry(90 * degree), Point3(-10, -4, 0), 3.0);
Similarity3 cSa(Rot3::Ry(270 * degree), Point3(0, 1, -2), 6.0);
// Create poses
Pose3 Ta1(Rot3(), Point3(0, 0, 0));
Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
Pose3 Tc1(Rot3(0, 0, -1, 0, 1, 0, 1, 0, 0), Point3(0, 6, -12));
Pose3 Tc2(Rot3(0, 0, -1, 0, -1, 0, -1, 0, 0), Point3(0, 6, 12));
EXPECT(assert_equal(Tc1, cSb.transformFrom(Tb1)));
EXPECT(assert_equal(Tc2, cSb.transformFrom(Tb2)));
EXPECT(assert_equal(cSa.transformFrom(Ta1), cSb.transformFrom(Tb1)));
EXPECT(assert_equal(cSa.transformFrom(Ta2), cSb.transformFrom(Tb2)));
}
//******************************************************************************
// Align with Point3 Pairs
TEST(Similarity3, AlignPoint3_1) {
Similarity3 expected_aSb(Rot3::Rz(-90 * degree), Point3(3, 4, 5), 2.0);
Point3 b1(0, 0, 0), b2(3, 0, 0), b3(3, 0, 4);
Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));
vector<Point3Pair> correspondences{ab1, ab2, ab3};
Similarity3 actual_aSb = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_aSb, actual_aSb));
}
TEST(Similarity3, AlignPoint3_2) {
Similarity3 expected_aSb(Rot3(), Point3(10, 10, 0), 1.0);
Point3 b1(0, 0, 0), b2(20, 10, 0), b3(10, 20, 0);
Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));
vector<Point3Pair> correspondences{ab1, ab2, ab3};
Similarity3 actual_aSb = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_aSb, actual_aSb));
}
TEST(Similarity3, AlignPoint3_3) {
Similarity3 expected_aSb(Rot3::RzRyRx(0.3, 0.2, 0.1), Point3(20, 10, 5), 1.0);
Point3 b1(0, 0, 1), b2(10, 0, 2), b3(20, -10, 30);
Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));
vector<Point3Pair> correspondences{ab1, ab2, ab3};
Similarity3 actual_aSb = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_aSb, actual_aSb));
}
//******************************************************************************
// Align with Pose3 Pairs
TEST(Similarity3, AlignPose3) {
Similarity3 expected_aSb(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
// Create source poses
Pose3 Ta1(Rot3(), Point3(0, 0, 0));
Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
// Create destination poses
Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
Pose3Pair bTa1(make_pair(Tb1, Ta1));
Pose3Pair bTa2(make_pair(Tb2, Ta2));
vector<Pose3Pair> correspondences{bTa1, bTa2};
Similarity3 actual_aSb = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_aSb, actual_aSb));
}
//******************************************************************************
// Test very simple prior optimization example
TEST(Similarity3, Optimization) {

14
gtsam_unstable/gtsam_unstable.i
View File

@ -5,8 +5,6 @@
// specify the classes from gtsam we are using
virtual class gtsam::Value;
class gtsam::Vector6;
class gtsam::LieScalar;
class gtsam::LieVector;
class gtsam::Point2;
class gtsam::Point2Vector;
class gtsam::Rot2;
@ -476,14 +474,12 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model);
};
#include <gtsam/base/deprecated/LieScalar.h>
#include <gtsam_unstable/dynamics/VelocityConstraint3.h>
virtual class VelocityConstraint3 : gtsam::NonlinearFactor {
/** Standard constructor */
VelocityConstraint3(size_t key1, size_t key2, size_t velKey, double dt);
Vector evaluateError(const gtsam::LieScalar& x1, const gtsam::LieScalar& x2, const gtsam::LieScalar& v) const;
Vector evaluateError(const double& x1, const double& x2, const double& v) const;
};
#include <gtsam_unstable/dynamics/Pendulum.h>
@ -491,7 +487,7 @@ virtual class PendulumFactor1 : gtsam::NonlinearFactor {
/** Standard constructor */
PendulumFactor1(size_t k1, size_t k, size_t velKey, double dt);
Vector evaluateError(const gtsam::LieScalar& qk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& v) const;
Vector evaluateError(const double& qk1, const double& qk, const double& v) const;
};
#include <gtsam_unstable/dynamics/Pendulum.h>
@ -499,21 +495,21 @@ virtual class PendulumFactor2 : gtsam::NonlinearFactor {
/** Standard constructor */
PendulumFactor2(size_t vk1, size_t vk, size_t qKey, double dt, double L, double g);
Vector evaluateError(const gtsam::LieScalar& vk1, const gtsam::LieScalar& vk, const gtsam::LieScalar& q) const;
Vector evaluateError(const double& vk1, const double& vk, const double& q) const;
};
virtual class PendulumFactorPk : gtsam::NonlinearFactor {
/** Standard constructor */
PendulumFactorPk(size_t pk, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha);
Vector evaluateError(const gtsam::LieScalar& pk, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const;
Vector evaluateError(const double& pk, const double& qk, const double& qk1) const;
};
virtual class PendulumFactorPk1 : gtsam::NonlinearFactor {
/** Standard constructor */
PendulumFactorPk1(size_t pk1, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha);
Vector evaluateError(const gtsam::LieScalar& pk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const;
Vector evaluateError(const double& pk1, const double& qk, const double& qk1) const;
};
#include <gtsam_unstable/dynamics/SimpleHelicopter.h>

22
gtsam_unstable/linear/ActiveSetSolver-inl.h
View File

@ -149,7 +149,7 @@ Template JacobianFactor::shared_ptr This::createDualFactor(
// to compute the least-square approximation of dual variables
return boost::make_shared<JacobianFactor>(Aterms, b);
} else {
return boost::make_shared<JacobianFactor>();
return nullptr;
}
}
@ -165,14 +165,13 @@ Template JacobianFactor::shared_ptr This::createDualFactor(
* if lambda = 0 you are on the constraint
* if lambda > 0 you are violating the constraint.
*/
Template GaussianFactorGraph::shared_ptr This::buildDualGraph(
Template GaussianFactorGraph This::buildDualGraph(
const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
GaussianFactorGraph dualGraph;
for (Key key : constrainedKeys_) {
// Each constrained key becomes a factor in the dual graph
JacobianFactor::shared_ptr dualFactor =
createDualFactor(key, workingSet, delta);
if (!dualFactor->empty()) dualGraph->push_back(dualFactor);
auto dualFactor = createDualFactor(key, workingSet, delta);
if (dualFactor) dualGraph.push_back(dualFactor);
}
return dualGraph;
}
@ -193,19 +192,16 @@ This::buildWorkingGraph(const InequalityFactorGraph& workingSet,
Template typename This::State This::iterate(
const typename This::State& state) const {
// Algorithm 16.3 from Nocedal06book.
// Solve with the current working set eqn 16.39, but instead of solving for p
// solve for x
GaussianFactorGraph workingGraph =
buildWorkingGraph(state.workingSet, state.values);
// Solve with the current working set eqn 16.39, but solve for x not p
auto workingGraph = buildWorkingGraph(state.workingSet, state.values);
VectorValues newValues = workingGraph.optimize();
// If we CAN'T move further
// if p_k = 0 is the original condition, modified by Duy to say that the state
// update is zero.
if (newValues.equals(state.values, 1e-7)) {
// Compute lambda from the dual graph
GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet,
newValues);
VectorValues duals = dualGraph->optimize();
auto dualGraph = buildDualGraph(state.workingSet, newValues);
VectorValues duals = dualGraph.optimize();
int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
// If all inequality constraints are satisfied: We have the solution!!
if (leavingFactor < 0) {

4
gtsam_unstable/linear/ActiveSetSolver.h
View File

@ -154,8 +154,8 @@ protected:
public: /// Just for testing...
/// Builds a dual graph from the current working set.
GaussianFactorGraph::shared_ptr buildDualGraph(
const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
GaussianFactorGraph buildDualGraph(const InequalityFactorGraph &workingSet,
const VectorValues &delta) const;
/**
* Build a working graph of cost, equality and active inequality constraints

5
gtsam_unstable/linear/EqualityFactorGraph.h
View File

@ -31,6 +31,11 @@ class EqualityFactorGraph: public FactorGraph<LinearEquality> {
public:
typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr;
/// Add a linear inequality, forwards arguments to LinearInequality.
template <class... Args> void add(Args &&... args) {
emplace_shared<LinearEquality>(std::forward<Args>(args)...);
}
/// Compute error of a guess.
double error(const VectorValues& x) const {
double total_error = 0.;

5
gtsam_unstable/linear/InequalityFactorGraph.h
View File

@ -47,6 +47,11 @@ public:
return Base::equals(other, tol);
}
/// Add a linear inequality, forwards arguments to LinearInequality.
template <class... Args> void add(Args &&... args) {
emplace_shared<LinearInequality>(std::forward<Args>(args)...);
}
/**
* Compute error of a guess.
* Infinity error if it violates an inequality; zero otherwise. */

3
gtsam_unstable/linear/LPInitSolver.h
View File

@ -21,7 +21,6 @@
#include <gtsam_unstable/linear/LP.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <CppUnitLite/Test.h>
namespace gtsam {
/**
@ -83,7 +82,7 @@ private:
const InequalityFactorGraph& inequalities) const;
// friend class for unit-testing private methods
FRIEND_TEST(LPInitSolver, initialization);
friend class LPInitSolverInitializationTest;
};
}

132
gtsam_unstable/linear/tests/testLPSolver.cpp
View File

@ -16,21 +16,23 @@
* @author Duy-Nguyen Ta
*/
#include <gtsam_unstable/linear/LPInitSolver.h>
#include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam_unstable/linear/EqualityFactorGraph.h>
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/foreach.hpp>
#include <boost/range/adaptor/map.hpp>
#include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam_unstable/linear/LPInitSolver.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
@ -47,37 +49,27 @@ static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1);
*/
LP simpleLP1() {
LP lp;
lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2)
lp.inequalities.push_back(
LinearInequality(1, Vector2(-1, 0), 0, 1)); // x1 >= 0
lp.inequalities.push_back(
LinearInequality(1, Vector2(0, -1), 0, 2)); // x2 >= 0
lp.inequalities.push_back(
LinearInequality(1, Vector2(1, 2), 4, 3)); // x1 + 2*x2 <= 4
lp.inequalities.push_back(
LinearInequality(1, Vector2(4, 2), 12, 4)); // 4x1 + 2x2 <= 12
lp.inequalities.push_back(
LinearInequality(1, Vector2(-1, 1), 1, 5)); // -x1 + x2 <= 1
lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2)
lp.inequalities.add(1, Vector2(-1, 0), 0, 1); // x1 >= 0
lp.inequalities.add(1, Vector2(0, -1), 0, 2); // x2 >= 0
lp.inequalities.add(1, Vector2(1, 2), 4, 3); // x1 + 2*x2 <= 4
lp.inequalities.add(1, Vector2(4, 2), 12, 4); // 4x1 + 2x2 <= 12
lp.inequalities.add(1, Vector2(-1, 1), 1, 5); // -x1 + x2 <= 1
return lp;
}
/* ************************************************************************* */
namespace gtsam {
TEST(LPInitSolver, infinite_loop_single_var) {
LP initchecker;
initchecker.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha
initchecker.inequalities.push_back(
LinearInequality(1, Vector3(-2, -1, -1), -2, 1)); //-2x-y-alpha <= -2
initchecker.inequalities.push_back(
LinearInequality(1, Vector3(-1, 2, -1), 6, 2)); // -x+2y-alpha <= 6
initchecker.inequalities.push_back(
LinearInequality(1, Vector3(-1, 0, -1), 0, 3)); // -x - alpha <= 0
initchecker.inequalities.push_back(
LinearInequality(1, Vector3(1, 0, -1), 20, 4)); // x - alpha <= 20
initchecker.inequalities.push_back(
LinearInequality(1, Vector3(0, -1, -1), 0, 5)); // -y - alpha <= 0
LPSolver solver(initchecker);
TEST(LPInitSolver, InfiniteLoopSingleVar) {
LP lp;
lp.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha
lp.inequalities.add(1, Vector3(-2, -1, -1), -2, 1); //-2x-y-a <= -2
lp.inequalities.add(1, Vector3(-1, 2, -1), 6, 2); // -x+2y-a <= 6
lp.inequalities.add(1, Vector3(-1, 0, -1), 0, 3); // -x - a <= 0
lp.inequalities.add(1, Vector3(1, 0, -1), 20, 4); // x - a <= 20
lp.inequalities.add(1, Vector3(0, -1, -1), 0, 5); // -y - a <= 0
LPSolver solver(lp);
VectorValues starter;
starter.insert(1, Vector3(0, 0, 2));
VectorValues results, duals;
@ -87,25 +79,23 @@ TEST(LPInitSolver, infinite_loop_single_var) {
CHECK(assert_equal(results, expected, 1e-7));
}
TEST(LPInitSolver, infinite_loop_multi_var) {
LP initchecker;
TEST(LPInitSolver, InfiniteLoopMultiVar) {
LP lp;
Key X = symbol('X', 1);
Key Y = symbol('Y', 1);
Key Z = symbol('Z', 1);
initchecker.cost = LinearCost(Z, kOne); // min alpha
initchecker.inequalities.push_back(
LinearInequality(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2,
1)); //-2x-y-alpha <= -2
initchecker.inequalities.push_back(
LinearInequality(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6,
2)); // -x+2y-alpha <= 6
initchecker.inequalities.push_back(LinearInequality(
X, -1.0 * kOne, Z, -1.0 * kOne, 0, 3)); // -x - alpha <= 0
initchecker.inequalities.push_back(LinearInequality(
X, 1.0 * kOne, Z, -1.0 * kOne, 20, 4)); // x - alpha <= 20
initchecker.inequalities.push_back(LinearInequality(
Y, -1.0 * kOne, Z, -1.0 * kOne, 0, 5)); // -y - alpha <= 0
LPSolver solver(initchecker);
lp.cost = LinearCost(Z, kOne); // min alpha
lp.inequalities.add(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2,
1); //-2x-y-alpha <= -2
lp.inequalities.add(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6,
2); // -x+2y-alpha <= 6
lp.inequalities.add(X, -1.0 * kOne, Z, -1.0 * kOne, 0,
3); // -x - alpha <= 0
lp.inequalities.add(X, 1.0 * kOne, Z, -1.0 * kOne, 20,
4); // x - alpha <= 20
lp.inequalities.add(Y, -1.0 * kOne, Z, -1.0 * kOne, 0,
5); // -y - alpha <= 0
LPSolver solver(lp);
VectorValues starter;
starter.insert(X, kZero);
starter.insert(Y, kZero);
@ -119,7 +109,7 @@ TEST(LPInitSolver, infinite_loop_multi_var) {
CHECK(assert_equal(results, expected, 1e-7));
}
TEST(LPInitSolver, initialization) {
TEST(LPInitSolver, Initialization) {
LP lp = simpleLP1();
LPInitSolver initSolver(lp);
@ -138,19 +128,19 @@ TEST(LPInitSolver, initialization) {
LP::shared_ptr initLP = initSolver.buildInitialLP(yKey);
LP expectedInitLP;
expectedInitLP.cost = LinearCost(yKey, kOne);
expectedInitLP.inequalities.push_back(LinearInequality(
1, Vector2(-1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0
expectedInitLP.inequalities.push_back(LinearInequality(
1, Vector2(0, -1), 2, Vector::Constant(1, -1), 0, 2)); // -x2 - y <= 0
expectedInitLP.inequalities.push_back(
LinearInequality(1, Vector2(1, 2), 2, Vector::Constant(1, -1), 4,
3)); // x1 + 2*x2 - y <= 4
expectedInitLP.inequalities.push_back(
LinearInequality(1, Vector2(4, 2), 2, Vector::Constant(1, -1), 12,
4)); // 4x1 + 2x2 - y <= 12
expectedInitLP.inequalities.push_back(
LinearInequality(1, Vector2(-1, 1), 2, Vector::Constant(1, -1), 1,
5)); // -x1 + x2 - y <= 1
expectedInitLP.inequalities.add(1, Vector2(-1, 0), 2, Vector::Constant(1, -1),
0, 1); // -x1 - y <= 0
expectedInitLP.inequalities.add(1, Vector2(0, -1), 2, Vector::Constant(1, -1),
0, 2); // -x2 - y <= 0
expectedInitLP.inequalities.add(1, Vector2(1, 2), 2, Vector::Constant(1, -1),
4,
3); // x1 + 2*x2 - y <= 4
expectedInitLP.inequalities.add(1, Vector2(4, 2), 2, Vector::Constant(1, -1),
12,
4); // 4x1 + 2x2 - y <= 12
expectedInitLP.inequalities.add(1, Vector2(-1, 1), 2, Vector::Constant(1, -1),
1,
5); // -x1 + x2 - y <= 1
CHECK(assert_equal(expectedInitLP, *initLP, 1e-10));
LPSolver lpSolveInit(*initLP);
VectorValues xy0(x0);
@ -164,7 +154,7 @@ TEST(LPInitSolver, initialization) {
VectorValues x = initSolver.solve();
CHECK(lp.isFeasible(x));
}
}
} // namespace gtsam
/* ************************************************************************* */
/**
@ -173,28 +163,24 @@ TEST(LPInitSolver, initialization) {
* x - y = 5
* x + 2y = 6
*/
TEST(LPSolver, overConstrainedLinearSystem) {
TEST(LPSolver, OverConstrainedLinearSystem) {
GaussianFactorGraph graph;
Matrix A1 = Vector3(1, 1, 1);
Matrix A2 = Vector3(1, -1, 2);
Vector b = Vector3(1, 5, 6);
JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3));
graph.push_back(factor);
graph.add(1, A1, 2, A2, b, noiseModel::Constrained::All(3));
VectorValues x = graph.optimize();
// This check confirms that gtsam linear constraint solver can't handle
// over-constrained system
CHECK(factor.error(x) != 0.0);
CHECK(graph[0]->error(x) != 0.0);
}
TEST(LPSolver, overConstrainedLinearSystem2) {
GaussianFactorGraph graph;
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, I_1x1, kOne,
noiseModel::Constrained::All(1));
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, -I_1x1, 5 * kOne,
noiseModel::Constrained::All(1));
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, 2 * I_1x1, 6 * kOne,
noiseModel::Constrained::All(1));
graph.add(1, I_1x1, 2, I_1x1, kOne, noiseModel::Constrained::All(1));
graph.add(1, I_1x1, 2, -I_1x1, 5 * kOne, noiseModel::Constrained::All(1));
graph.add(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, noiseModel::Constrained::All(1));
VectorValues x = graph.optimize();
// This check confirms that gtsam linear constraint solver can't handle
// over-constrained system
@ -202,7 +188,7 @@ TEST(LPSolver, overConstrainedLinearSystem2) {
}
/* ************************************************************************* */
TEST(LPSolver, simpleTest1) {
TEST(LPSolver, SimpleTest1) {
LP lp = simpleLP1();
LPSolver lpSolver(lp);
VectorValues init;
@ -222,7 +208,7 @@ TEST(LPSolver, simpleTest1) {
}
/* ************************************************************************* */
TEST(LPSolver, testWithoutInitialValues) {
TEST(LPSolver, TestWithoutInitialValues) {
LP lp = simpleLP1();
LPSolver lpSolver(lp);
VectorValues result, duals, expectedResult;

309
gtsam_unstable/linear/tests/testQPSolver.cpp
View File

@ -17,10 +17,12 @@
* @author Ivan Dario Jimenez
*/
#include <gtsam_unstable/linear/QPSParser.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam_unstable/linear/QPSParser.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
@ -40,15 +42,15 @@ QP createTestCase() {
// Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10
//TODO: THIS TEST MIGHT BE WRONG : the last parameter might be 5 instead of 10 because the form of the equation
// Should be 0.5x'Gx + gx + f : Nocedal 449
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, 2.0 * I_1x1,
Z_1x1, 10.0));
qp.cost.push_back(HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1,
2.0 * I_1x1, Z_1x1, 10.0));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2
qp.inequalities.add(X(1), I_1x1, X(2), I_1x1, 2,
0); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
qp.inequalities.add(X(1), -I_1x1, 0, 1); // -x1 <= 0
qp.inequalities.add(X(2), -I_1x1, 0, 2); // -x2 <= 0
qp.inequalities.add(X(1), I_1x1, 1.5, 3); // x1 <= 3/2
return qp;
}
@ -94,16 +96,15 @@ QP createEqualityConstrainedTest() {
// Note the Hessian encodes:
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, 2.0 * I_1x1, Z_1x1,
0.0));
qp.cost.push_back(HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1,
2.0 * I_1x1, Z_1x1, 0.0));
// Equality constraints
// x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector
Matrix A1 = I_1x1;
Matrix A2 = I_1x1;
Vector b = -kOne;
qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0));
qp.equalities.add(X(1), A1, X(2), A2, b, 0);
return qp;
}
@ -118,9 +119,8 @@ TEST(QPSolver, dual) {
QPSolver solver(qp);
GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph(
qp.inequalities, initialValues);
VectorValues dual = dualGraph->optimize();
auto dualGraph = solver.buildDualGraph(qp.inequalities, initialValues);
VectorValues dual = dualGraph.optimize();
VectorValues expectedDual;
expectedDual.insert(0, (Vector(1) << 2.0).finished());
CHECK(assert_equal(expectedDual, dual, 1e-10));
@ -135,19 +135,19 @@ TEST(QPSolver, indentifyActiveConstraints) {
currentSolution.insert(X(1), Z_1x1);
currentSolution.insert(X(2), Z_1x1);
InequalityFactorGraph workingSet = solver.identifyActiveConstraints(
qp.inequalities, currentSolution);
auto workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
CHECK(!workingSet.at(0)->active()); // inactive
CHECK(workingSet.at(1)->active());// active
CHECK(workingSet.at(2)->active());// active
CHECK(!workingSet.at(3)->active());// inactive
CHECK(workingSet.at(1)->active()); // active
CHECK(workingSet.at(2)->active()); // active
CHECK(!workingSet.at(3)->active()); // inactive
VectorValues solution = solver.buildWorkingGraph(workingSet).optimize();
VectorValues expectedSolution;
expectedSolution.insert(X(1), kZero);
expectedSolution.insert(X(2), kZero);
CHECK(assert_equal(expectedSolution, solution, 1e-100));
VectorValues expected;
expected.insert(X(1), kZero);
expected.insert(X(2), kZero);
CHECK(assert_equal(expected, solution, 1e-100));
}
/* ************************************************************************* */
@ -159,24 +159,24 @@ TEST(QPSolver, iterate) {
currentSolution.insert(X(1), Z_1x1);
currentSolution.insert(X(2), Z_1x1);
std::vector<VectorValues> expectedSolutions(4), expectedDuals(4);
expectedSolutions[0].insert(X(1), kZero);
expectedSolutions[0].insert(X(2), kZero);
std::vector<VectorValues> expected(4), expectedDuals(4);
expected[0].insert(X(1), kZero);
expected[0].insert(X(2), kZero);
expectedDuals[0].insert(1, (Vector(1) << 3).finished());
expectedDuals[0].insert(2, kZero);
expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[1].insert(X(2), kZero);
expected[1].insert(X(1), (Vector(1) << 1.5).finished());
expected[1].insert(X(2), kZero);
expectedDuals[1].insert(3, (Vector(1) << 1.5).finished());
expectedSolutions[2].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[2].insert(X(2), (Vector(1) << 0.75).finished());
expected[2].insert(X(1), (Vector(1) << 1.5).finished());
expected[2].insert(X(2), (Vector(1) << 0.75).finished());
expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished());
expected[3].insert(X(1), (Vector(1) << 1.5).finished());
expected[3].insert(X(2), (Vector(1) << 0.5).finished());
InequalityFactorGraph workingSet = solver.identifyActiveConstraints(
qp.inequalities, currentSolution);
auto workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
QPSolver::State state(currentSolution, VectorValues(), workingSet, false,
100);
@ -188,12 +188,12 @@ TEST(QPSolver, iterate) {
// Forst10book do not follow exactly what we implemented from Nocedal06book.
// Specifically, we do not re-identify active constraints and
// do not recompute dual variables after every step!!!
// CHECK(assert_equal(expectedSolutions[it], state.values, 1e-10));
// CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10));
// CHECK(assert_equal(expected[it], state.values, 1e-10));
// CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10));
it++;
}
CHECK(assert_equal(expectedSolutions[3], state.values, 1e-10));
CHECK(assert_equal(expected[3], state.values, 1e-10));
}
/* ************************************************************************* */
@ -204,182 +204,161 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
VectorValues initialValues;
initialValues.insert(X(1), Z_1x1);
initialValues.insert(X(2), Z_1x1);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 1.5).finished());
expectedSolution.insert(X(2), (Vector(1) << 0.5).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-100));
VectorValues solution = solver.optimize(initialValues).first;
VectorValues expected;
expected.insert(X(1), (Vector(1) << 1.5).finished());
expected.insert(X(2), (Vector(1) << 0.5).finished());
CHECK(assert_equal(expected, solution, 1e-100));
}
pair<QP, QP> testParser(QPSParser parser) {
QP exampleqp = parser.Parse();
QP expectedqp;
QP expected;
Key X1(Symbol('X', 1)), X2(Symbol('X', 2));
// min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
expectedqp.cost.push_back(
HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1,
2.0 * kOne, 8.0));
// 2x + y >= 2
// -x + 2y <= 6
expectedqp.inequalities.push_back(
LinearInequality(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0));
expectedqp.inequalities.push_back(
LinearInequality(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1));
// x<= 20
expectedqp.inequalities.push_back(LinearInequality(X1, I_1x1, 20, 4));
//x >= 0
expectedqp.inequalities.push_back(LinearInequality(X1, -I_1x1, 0, 2));
// y > = 0
expectedqp.inequalities.push_back(LinearInequality(X2, -I_1x1, 0, 3));
return std::make_pair(expectedqp, exampleqp);
}
;
expected.cost.push_back(HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1,
-1.5 * kOne, 10.0 * I_1x1, 2.0 * kOne,
8.0));
expected.inequalities.add(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0); // 2x + y >= 2
expected.inequalities.add(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1); // -x + 2y <= 6
expected.inequalities.add(X1, I_1x1, 20, 4); // x<= 20
expected.inequalities.add(X1, -I_1x1, 0, 2); // x >= 0
expected.inequalities.add(X2, -I_1x1, 0, 3); // y > = 0
return {expected, exampleqp};
};
TEST(QPSolver, ParserSyntaticTest) {
auto expectedActual = testParser(QPSParser("QPExample.QPS"));
CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost,
auto result = testParser(QPSParser("QPExample.QPS"));
CHECK(assert_equal(result.first.cost, result.second.cost, 1e-7));
CHECK(assert_equal(result.first.inequalities, result.second.inequalities,
1e-7));
CHECK(assert_equal(expectedActual.first.inequalities,
expectedActual.second.inequalities, 1e-7));
CHECK(assert_equal(expectedActual.first.equalities,
expectedActual.second.equalities, 1e-7));
CHECK(assert_equal(result.first.equalities, result.second.equalities, 1e-7));
}
TEST(QPSolver, ParserSemanticTest) {
auto expected_actual = testParser(QPSParser("QPExample.QPS"));
VectorValues actualSolution, expectedSolution;
boost::tie(expectedSolution, boost::tuples::ignore) =
QPSolver(expected_actual.first).optimize();
boost::tie(actualSolution, boost::tuples::ignore) =
QPSolver(expected_actual.second).optimize();
CHECK(assert_equal(actualSolution, expectedSolution, 1e-7));
auto result = testParser(QPSParser("QPExample.QPS"));
VectorValues expected = QPSolver(result.first).optimize().first;
VectorValues actual = QPSolver(result.second).optimize().first;
CHECK(assert_equal(actual, expected, 1e-7));
}
TEST(QPSolver, QPExampleTest){
TEST(QPSolver, QPExampleTest) {
QP problem = QPSParser("QPExample.QPS").Parse();
VectorValues actualSolution;
auto solver = QPSolver(problem);
boost::tie(actualSolution, boost::tuples::ignore) = solver.optimize();
VectorValues expectedSolution;
expectedSolution.insert(Symbol('X',1),0.7625*I_1x1);
expectedSolution.insert(Symbol('X',2),0.4750*I_1x1);
double error_expected = problem.cost.error(expectedSolution);
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(expectedSolution, actualSolution, 1e-7))
VectorValues actual = solver.optimize().first;
VectorValues expected;
expected.insert(Symbol('X', 1), 0.7625 * I_1x1);
expected.insert(Symbol('X', 2), 0.4750 * I_1x1);
double error_expected = problem.cost.error(expected);
double error_actual = problem.cost.error(actual);
CHECK(assert_equal(expected, actual, 1e-7))
CHECK(assert_equal(error_expected, error_actual))
}
TEST(QPSolver, HS21) {
QP problem = QPSParser("HS21.QPS").Parse();
VectorValues actualSolution;
VectorValues expectedSolution;
expectedSolution.insert(Symbol('X',1), 2.0*I_1x1);
expectedSolution.insert(Symbol('X',2), 0.0*I_1x1);
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution);
VectorValues expected;
expected.insert(Symbol('X', 1), 2.0 * I_1x1);
expected.insert(Symbol('X', 2), 0.0 * I_1x1);
VectorValues actual = QPSolver(problem).optimize().first;
double error_actual = problem.cost.error(actual);
CHECK(assert_equal(-99.9599999, error_actual, 1e-7))
CHECK(assert_equal(expectedSolution, actualSolution))
CHECK(assert_equal(expected, actual))
}
TEST(QPSolver, HS35) {
QP problem = QPSParser("HS35.QPS").Parse();
VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(1.11111111e-01,error_actual, 1e-7))
VectorValues actual = QPSolver(problem).optimize().first;
double error_actual = problem.cost.error(actual);
CHECK(assert_equal(1.11111111e-01, error_actual, 1e-7))
}
TEST(QPSolver, HS35MOD) {
QP problem = QPSParser("HS35MOD.QPS").Parse();
VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(2.50000001e-01,error_actual, 1e-7))
VectorValues actual = QPSolver(problem).optimize().first;
double error_actual = problem.cost.error(actual);
CHECK(assert_equal(2.50000001e-01, error_actual, 1e-7))
}
TEST(QPSolver, HS51) {
QP problem = QPSParser("HS51.QPS").Parse();
VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(8.88178420e-16,error_actual, 1e-7))
VectorValues actual = QPSolver(problem).optimize().first;
double error_actual = problem.cost.error(actual);
CHECK(assert_equal(8.88178420e-16, error_actual, 1e-7))
}
TEST(QPSolver, HS52) {
QP problem = QPSParser("HS52.QPS").Parse();
VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(5.32664756,error_actual, 1e-7))
VectorValues actual = QPSolver(problem).optimize().first;
double error_actual = problem.cost.error(actual);
CHECK(assert_equal(5.32664756, error_actual, 1e-7))
}
TEST(QPSolver, HS268) { // This test needs an extra order of magnitude of tolerance than the rest
TEST(QPSolver, HS268) { // This test needs an extra order of magnitude of
// tolerance than the rest
QP problem = QPSParser("HS268.QPS").Parse();
VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(5.73107049e-07,error_actual, 1e-6))
VectorValues actual = QPSolver(problem).optimize().first;
double error_actual = problem.cost.error(actual);
CHECK(assert_equal(5.73107049e-07, error_actual, 1e-6))
}
TEST(QPSolver, QPTEST) { // REQUIRES Jacobian Fix
QP problem = QPSParser("QPTEST.QPS").Parse();
VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(0.437187500e01,error_actual, 1e-7))
VectorValues actual = QPSolver(problem).optimize().first;
double error_actual = problem.cost.error(actual);
CHECK(assert_equal(0.437187500e01, error_actual, 1e-7))
}
/* ************************************************************************* */
// Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html
// Create Matlab's test graph as in
// http://www.mathworks.com/help/optim/ug/quadprog.html
QP createTestMatlabQPEx() {
QP qp;
// Objective functions 0.5*x1^2 + x2^2 - x1*x2 - 2*x1 -6*x2
// Note the Hessian encodes:
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 +
// 0.5*f
// Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0
qp.cost.push_back(
HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, 2.0 * I_1x1,
6 * I_1x1, 1000.0));
qp.cost.push_back(HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1,
2.0 * I_1x1, 6 * I_1x1, 1000.0));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2
qp.inequalities.push_back(
LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2
qp.inequalities.push_back(
LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3
qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 3)); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 4)); // -x2 <= 0
qp.inequalities.add(X(1), I_1x1, X(2), I_1x1, 2, 0); // x1 + x2 <= 2
qp.inequalities.add(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1); //-x1 + 2*x2 <=2
qp.inequalities.add(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2); // 2*x1 + x2 <=3
qp.inequalities.add(X(1), -I_1x1, 0, 3); // -x1 <= 0
qp.inequalities.add(X(2), -I_1x1, 0, 4); // -x2 <= 0
return qp;
}
///* ************************************************************************* */
///* *************************************************************************
///*/
TEST(QPSolver, optimizeMatlabEx) {
QP qp = createTestMatlabQPEx();
QPSolver solver(qp);
VectorValues initialValues;
initialValues.insert(X(1), Z_1x1);
initialValues.insert(X(2), Z_1x1);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-7));
VectorValues solution = solver.optimize(initialValues).first;
VectorValues expected;
expected.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
expected.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
CHECK(assert_equal(expected, solution, 1e-7));
}
///* ************************************************************************* */
///* *************************************************************************
///*/
TEST(QPSolver, optimizeMatlabExNoinitials) {
QP qp = createTestMatlabQPEx();
QPSolver solver(qp);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize();
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-7));
VectorValues solution = solver.optimize().first;
VectorValues expected;
expected.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
expected.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
CHECK(assert_equal(expected, solution, 1e-7));
}
/* ************************************************************************* */
@ -387,18 +366,15 @@ TEST(QPSolver, optimizeMatlabExNoinitials) {
QP createTestNocedal06bookEx16_4() {
QP qp;
qp.cost.push_back(JacobianFactor(X(1), I_1x1, I_1x1));
qp.cost.push_back(JacobianFactor(X(2), I_1x1, 2.5 * I_1x1));
qp.cost.add(X(1), I_1x1, I_1x1);
qp.cost.add(X(2), I_1x1, 2.5 * I_1x1);
// Inequality constraints
qp.inequalities.push_back(
LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0));
qp.inequalities.push_back(
LinearInequality(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1));
qp.inequalities.push_back(
LinearInequality(X(1), I_1x1, X(2), -2 * I_1x1, 2, 2));
qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0.0, 3));
qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0.0, 4));
qp.inequalities.add(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0);
qp.inequalities.add(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1);
qp.inequalities.add(X(1), I_1x1, X(2), -2 * I_1x1, 2, 2);
qp.inequalities.add(X(1), -I_1x1, 0.0, 3);
qp.inequalities.add(X(2), -I_1x1, 0.0, 4);
return qp;
}
@ -410,21 +386,19 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) {
initialValues.insert(X(1), (Vector(1) << 2.0).finished());
initialValues.insert(X(2), Z_1x1);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 1.4).finished());
expectedSolution.insert(X(2), (Vector(1) << 1.7).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-7));
VectorValues solution = solver.optimize(initialValues).first;
VectorValues expected;
expected.insert(X(1), (Vector(1) << 1.4).finished());
expected.insert(X(2), (Vector(1) << 1.7).finished());
CHECK(assert_equal(expected, solution, 1e-7));
}
/* ************************************************************************* */
TEST(QPSolver, failedSubproblem) {
QP qp;
qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1));
qp.cost.add(X(1), I_2x2, Z_2x1);
qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0));
qp.inequalities.push_back(
LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0));
qp.inequalities.add(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0);
VectorValues expected;
expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished());
@ -433,8 +407,7 @@ TEST(QPSolver, failedSubproblem) {
initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished());
QPSolver solver(qp);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues solution = solver.optimize(initialValues).first;
CHECK(assert_equal(expected, solution, 1e-7));
}
@ -442,10 +415,9 @@ TEST(QPSolver, failedSubproblem) {
/* ************************************************************************* */
TEST(QPSolver, infeasibleInitial) {
QP qp;
qp.cost.push_back(JacobianFactor(X(1), I_2x2, Vector::Zero(2)));
qp.cost.add(X(1), I_2x2, Vector::Zero(2));
qp.cost.push_back(HessianFactor(X(1), Z_2x2, Vector::Zero(2), 100.0));
qp.inequalities.push_back(
LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0));
qp.inequalities.add(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0);
VectorValues expected;
expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished());
@ -464,4 +436,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

7
matlab/CMakeLists.txt
View File

@ -2,6 +2,10 @@
include(GtsamMatlabWrap)
# Record the root dir for gtsam - needed during external builds, e.g., ROS
set(GTSAM_SOURCE_ROOT_DIR ${GTSAM_SOURCE_DIR})
message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]")
# Tests
#message(STATUS "Installing Matlab Toolbox")
install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig")
@ -21,7 +25,7 @@ install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "README-gtsam-toolbox.
file(GLOB matlab_examples_data_graph "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph")
file(GLOB matlab_examples_data_mat "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.mat")
file(GLOB matlab_examples_data_txt "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt")
set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_mat} ${matlab_examples_data_txt})
set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_mat} ${matlab_examples_data_txt})
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
@ -38,4 +42,3 @@ if(GTSAM_BUILD_TYPE_POSTFIXES)
else()
install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data)
endif()

27
matlab/README.md
View File

@ -4,16 +4,16 @@ http://borg.cc.gatech.edu/projects/gtsam
This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ library. To build it, enable `GTSAM_INSTALL_MATLAB_TOOLBOX=ON` in CMake.
The interface to the toolbox is generated automatically by the wrap
tool which directly parses C++ header files. The tool generates matlab proxy objects together with all the native functions for wrapping and unwrapping scalar and non scalar types and objects. The interface generated by the wrap tool also redirects the standard output stream (cout) to matlab's console.
The interface to the toolbox is generated automatically by the wrap tool which directly parses C++ header files.
The tool generates matlab proxy objects together with all the native functions for wrapping and unwrapping scalar and non scalar types and objects.
The interface generated by the wrap tool also redirects the standard output stream (cout) to matlab's console.
## Ubuntu
If you have a newer Ubuntu system (later than 10.04), you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library. Delete or rename all files starting with `libstdc++` in your MATLAB installation directory, in paths:
/usr/local/MATLAB/[version]/sys/os/[system]/
/usr/local/MATLAB/[version]/bin/[system]/
If you have a newer Ubuntu system (later than 10.04), you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library. Delete or rename all files starting with `libstdc++` in your MATLAB installation directory, in paths:
/usr/local/MATLAB/[version]/sys/os/[system]/
/usr/local/MATLAB/[version]/bin/[system]/
## Adding the toolbox to your MATLAB path
@ -37,25 +37,28 @@ export LD_LIBRARY_PATH=<install-path>/gtsam:$LD_LIBRARY_PATH
### Linker issues
If you compile the MATLAB toolbox and everything compiles smoothly, but when you run any MATLAB script, you get following error messages in MATLAB
If you compile the MATLAB toolbox and everything compiles smoothly, but when you run any MATLAB script, you get any of the following error messages in MATLAB
```
Invalid MEX-file '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64':
Missing symbol 'mexAtExit' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64'
Missing symbol 'mexCallMATLABWithObject' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64'
...
```
run following shell line
```sh
export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6
```
before you run MATLAB from the same shell.
before you run MATLAB from the same shell.
This mainly happens if you have GCC >= 5 and newer version MATLAB like R2017a.
## Trying out the examples
The examples are located in the 'gtsam_examples' subfolder. You may either run them individually at the MATLAB command line, or open the GTSAM example GUI by running 'gtsamExamples'. Example:
The examples are located in the 'gtsam_examples' subfolder. You may either run them individually at the MATLAB command line, or open the GTSAM example GUI by running 'gtsamExamples'. Example:
```matlab
>> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox
@ -65,7 +68,7 @@ The examples are located in the 'gtsam_examples' subfolder. You may either run
## Running the unit tests
The GTSAM MATLAB toolbox also has a small set of unit tests located in the gtsam_tests directory. Example:
The GTSAM MATLAB toolbox also has a small set of unit tests located in the gtsam_tests directory. Example:
```matlab
>> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox
@ -86,4 +89,4 @@ Tests complete!
## Writing your own code
Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you understand a few basic concepts! Please see the manual to get started.
Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you understand a few basic concepts! Please see the manual to get started.

4
matlab/gtsam_tests/testSerialization.m
View File

@ -28,10 +28,6 @@ serialized_pose1 = pose1.string_serialize();
pose1ds = Pose2.string_deserialize(serialized_pose1);
CHECK('pose1ds.equals(pose1, 1e-9)', pose1ds.equals(pose1, 1e-9));
serialized_landmark1 = landmark1.string_serialize();
landmark1ds = Point2.string_deserialize(serialized_landmark1);
CHECK('landmark1ds.equals(landmark1, 1e-9)', landmark1ds.equals(landmark1, 1e-9));
%% Create and serialize Values
values = Values;
values.insert(i1, pose1);

7
python/CMakeLists.txt
View File

@ -37,7 +37,8 @@ set(ignore
gtsam::Point2Vector
gtsam::Pose3Vector
gtsam::KeyVector
gtsam::BinaryMeasurementsUnit3)
gtsam::BinaryMeasurementsUnit3
gtsam::KeyPairDoubleMap)
pybind_wrap(gtsam_py # target
${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header
@ -80,7 +81,9 @@ set(ignore
gtsam::Pose3Vector
gtsam::KeyVector
gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::BinaryMeasurementsUnit3)
gtsam::BinaryMeasurementsUnit3
gtsam::KeyPairDoubleMap)
pybind_wrap(gtsam_unstable_py # target
${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header
"gtsam_unstable.cpp" # generated_cpp

4
python/gtsam/examples/ImuFactorExample.py
View File

@ -104,7 +104,7 @@ class ImuFactorExample(PreintegrationExample):
if verbose:
print(factor)
print(pim.predict(actual_state_i, self.actualBias))
print(pim.predict(initial_state_i, self.actualBias))
pim.resetIntegration()
@ -125,7 +125,7 @@ class ImuFactorExample(PreintegrationExample):
i += 1
# add priors on end
# self.addPrior(num_poses - 1, graph)
self.addPrior(num_poses - 1, graph)
initial.print_("Initial values:")

2
python/gtsam/examples/README.md
View File

@ -39,7 +39,7 @@
| SelfCalibrationExample | |
| SFMdata | |
| SFMExample_bal_COLAMD_METIS | |
| SFMExample_bal | |
| SFMExample_bal | :heavy_check_mark: |
| SFMExample | :heavy_check_mark: |
| SFMExampleExpressions_bal | |
| SFMExampleExpressions | |

118
python/gtsam/examples/SFMExample_bal.py Normal file
View File

@ -0,0 +1,118 @@
"""
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
Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file
Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert)
"""
import argparse
import logging
import sys
import matplotlib.pyplot as plt
import numpy as np
import gtsam
from gtsam import (
GeneralSFMFactorCal3Bundler,
PinholeCameraCal3Bundler,
PriorFactorPinholeCameraCal3Bundler,
readBal,
symbol_shorthand
)
C = symbol_shorthand.C
P = symbol_shorthand.P
logging.basicConfig(stream=sys.stdout, level=logging.DEBUG)
def run(args):
""" Run LM optimization with BAL input data and report resulting error """
input_file = gtsam.findExampleDataFile(args.input_file)
# Load the SfM data from file
scene_data = readBal(input_file)
logging.info(f"read {scene_data.number_tracks()} tracks on {scene_data.number_cameras()} cameras\n")
# Create a factor graph
graph = gtsam.NonlinearFactorGraph()
# We share *one* noiseModel between all projection factors
noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
# Add measurements to the factor graph
j = 0
for t_idx in range(scene_data.number_tracks()):
track = scene_data.track(t_idx) # SfmTrack
# retrieve the SfmMeasurement objects
for m_idx in range(track.number_measurements()):
# i represents the camera index, and uv is the 2d measurement
i, uv = track.measurement(m_idx)
# note use of shorthand symbols C and P
graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j)))
j += 1
# Add a prior on pose x1. This indirectly specifies where the origin is.
graph.push_back(
gtsam.PriorFactorPinholeCameraCal3Bundler(
C(0), scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1)
)
)
# Also add a prior on the position of the first landmark to fix the scale
graph.push_back(
gtsam.PriorFactorPoint3(
P(0), scene_data.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
)
)
# Create initial estimate
initial = gtsam.Values()
i = 0
# add each PinholeCameraCal3Bundler
for cam_idx in range(scene_data.number_cameras()):
camera = scene_data.camera(cam_idx)
initial.insert(C(i), camera)
i += 1
j = 0
# add each SfmTrack
for t_idx in range(scene_data.number_tracks()):
track = scene_data.track(t_idx)
initial.insert(P(j), track.point3())
j += 1
# Optimize the graph and print results
try:
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("ERROR")
lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = lm.optimize()
except Exception as e:
logging.exception("LM Optimization failed")
return
# Error drops from ~2764.22 to ~0.046
logging.info(f"final error: {graph.error(result)}")
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument(
'-i',
'--input_file',
type=str,
default="dubrovnik-3-7-pre",
help='Read SFM data from the specified BAL file'
'The data format is described here: https://grail.cs.washington.edu/projects/bal/.'
'BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, '
'then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector'
'and (x,y,z) 3d point initializations.'
)
run(parser.parse_args())

144
python/gtsam/examples/TranslationAveragingExample.py Normal file
View File

@ -0,0 +1,144 @@
"""
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
This example shows how 1dsfm uses outlier rejection (MFAS) and optimization (translation recovery)
together for estimating global translations from relative translation directions and global rotations.
The purpose of this example is to illustrate the connection between these two classes using a small SfM dataset.
Author: Akshay Krishnan
Date: September 2020
"""
from collections import defaultdict
from typing import List, Tuple
import numpy as np
import gtsam
from gtsam.examples import SFMdata
# Hyperparameters for 1dsfm, values used from Kyle Wilson's code.
MAX_1DSFM_PROJECTION_DIRECTIONS = 48
OUTLIER_WEIGHT_THRESHOLD = 0.1
def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]:
""""Returns global rotations and unit translation directions between 8 cameras
that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata
and the unit translations directions between some camera pairs are computed from their
global translations. """
fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0
wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(fx, fy, s, u0, v0))
# Rotations of the cameras in the world frame.
wRc_values = gtsam.Values()
# Normalized translation directions from camera i to camera j
# in the coordinate frame of camera i.
i_iZj_list = []
for i in range(0, len(wTc_list) - 2):
# Add the rotation.
wRi = wTc_list[i].rotation()
wRc_values.insert(i, wRi)
# Create unit translation measurements with next two poses.
for j in range(i + 1, i + 3):
# Compute the translation from pose i to pose j, in the world reference frame.
w_itj = wTc_list[j].translation() - wTc_list[i].translation()
# Obtain the translation in the camera i's reference frame.
i_itj = wRi.unrotate(w_itj)
# Compute the normalized unit translation direction.
i_iZj = gtsam.Unit3(i_itj)
i_iZj_list.append(gtsam.BinaryMeasurementUnit3(
i, j, i_iZj, gtsam.noiseModel.Isotropic.Sigma(3, 0.01)))
# Add the last two rotations.
wRc_values.insert(len(wTc_list) - 1, wTc_list[-1].rotation())
wRc_values.insert(len(wTc_list) - 2, wTc_list[-2].rotation())
return wRc_values, i_iZj_list
def filter_outliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMeasurementsUnit3:
"""Removes outliers from a list of Unit3 measurements that are the
translation directions from camera i to camera j in the world frame."""
# Indices of measurements that are to be used as projection directions.
# These are randomly chosen. All sampled directions must be unique.
num_directions_to_sample = min(
MAX_1DSFM_PROJECTION_DIRECTIONS, len(w_iZj_list))
sampled_indices = np.random.choice(
len(w_iZj_list), num_directions_to_sample, replace=False)
# Sample projection directions from the measurements.
projection_directions = [w_iZj_list[idx].measured()
for idx in sampled_indices]
outlier_weights = []
# Find the outlier weights for each direction using MFAS.
for direction in projection_directions:
algorithm = gtsam.MFAS(w_iZj_list, direction)
outlier_weights.append(algorithm.computeOutlierWeights())
# Compute average of outlier weights. Each outlier weight is a map from a pair of Keys
# (camera IDs) to a weight, where weights are proportional to the probability of the edge
# being an outlier.
avg_outlier_weights = defaultdict(float)
for outlier_weight_dict in outlier_weights:
for keypair, weight in outlier_weight_dict.items():
avg_outlier_weights[keypair] += weight / len(outlier_weights)
# Remove w_iZj that have weight greater than threshold, these are outliers.
w_iZj_inliers = gtsam.BinaryMeasurementsUnit3()
[w_iZj_inliers.append(w_iZj) for w_iZj in w_iZj_list if avg_outlier_weights[(
w_iZj.key1(), w_iZj.key2())] < OUTLIER_WEIGHT_THRESHOLD]
return w_iZj_inliers
def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3,
wRc_values: gtsam.Values) -> gtsam.Values:
"""Estimate poses given rotations and normalized translation directions between cameras.
Args:
i_iZj_list: List of normalized translation direction measurements between camera pairs,
Z here refers to measurements. The measurements are of camera j with reference
to camera i (iZj), in camera i's coordinate frame (i_). iZj represents a unit
vector to j in i's frame and is not a transformation.
wRc_values: Rotations of the cameras in the world frame.
Returns:
gtsam.Values: Estimated poses.
"""
# Convert the translation direction measurements to world frame using the rotations.
w_iZj_list = gtsam.BinaryMeasurementsUnit3()
for i_iZj in i_iZj_list:
w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1())
.rotate(i_iZj.measured().point3()))
w_iZj_list.append(gtsam.BinaryMeasurementUnit3(
i_iZj.key1(), i_iZj.key2(), w_iZj, i_iZj.noiseModel()))
# Remove the outliers in the unit translation directions.
w_iZj_inliers = filter_outliers(w_iZj_list)
# Run the optimizer to obtain translations for normalized directions.
wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run()
wTc_values = gtsam.Values()
for key in wRc_values.keys():
wTc_values.insert(key, gtsam.Pose3(
wRc_values.atRot3(key), wtc_values.atPoint3(key)))
return wTc_values
def main():
wRc_values, i_iZj_list = get_data()
wTc_values = estimate_poses(i_iZj_list, wRc_values)
print("**** Translation averaging output ****")
print(wTc_values)
print("**************************************")
if __name__ == '__main__':
main()

1
python/gtsam/specializations.h
View File

@ -12,3 +12,4 @@ py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(m_, "BinaryMeasurementsUnit3");
py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");

2
python/gtsam/tests/test_JacobianFactor.py
View File

@ -19,7 +19,7 @@ from gtsam.utils.test_case import GtsamTestCase
class TestJacobianFactor(GtsamTestCase):
def test_eliminate(self):
# Recommended way to specify a matrix (see cython/README)
# Recommended way to specify a matrix (see python/README)
Ax2 = np.array(
[[-5., 0.],
[+0., -5.],

8
python/gtsam/tests/test_Pose3.py
View File

@ -65,6 +65,14 @@ class TestPose3(GtsamTestCase):
actual = Pose3.adjoint_(xi, xi)
np.testing.assert_array_equal(actual, expected)
def test_serialization(self):
"""Test if serialization is working normally"""
expected = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
actual = Pose3()
serialized = expected.serialize()
actual.deserialize(serialized)
self.gtsamAssertEquals(expected, actual, 1e-10)
if __name__ == "__main__":
unittest.main()

79
python/gtsam/tests/test_SfmData.py Normal file
View File

@ -0,0 +1,79 @@
"""
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 (Python: Sushmita Warrier)
"""
# pylint: disable=invalid-name, no-name-in-module, no-member
from __future__ import print_function
import unittest
import numpy as np
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestSfmData(GtsamTestCase):
"""Tests for SfmData and SfmTrack modules."""
def setUp(self):
"""Initialize SfmData and SfmTrack"""
self.data = gtsam.SfmData()
# initialize SfmTrack with 3D point
self.tracks = gtsam.SfmTrack()
def test_tracks(self):
"""Test functions in SfmTrack"""
# measurement is of format (camera_idx, imgPoint)
# create arbitrary camera indices for two cameras
i1, i2 = 4,5
# create arbitrary image measurements for cameras i1 and i2
uv_i1 = gtsam.Point2(12.6, 82)
# translating point uv_i1 along X-axis
uv_i2 = gtsam.Point2(24.88, 82)
# add measurements to the track
self.tracks.add_measurement(i1, uv_i1)
self.tracks.add_measurement(i2, uv_i2)
# Number of measurements in the track is 2
self.assertEqual(self.tracks.number_measurements(), 2)
# camera_idx in the first measurement of the track corresponds to i1
cam_idx, img_measurement = self.tracks.measurement(0)
self.assertEqual(cam_idx, i1)
np.testing.assert_array_almost_equal(
gtsam.Point3(0.,0.,0.),
self.tracks.point3()
)
def test_data(self):
"""Test functions in SfmData"""
# Create new track with 3 measurements
i1, i2, i3 = 3,5,6
uv_i1 = gtsam.Point2(21.23, 45.64)
# translating along X-axis
uv_i2 = gtsam.Point2(45.7, 45.64)
uv_i3 = gtsam.Point2(68.35, 45.64)
# add measurements and arbitrary point to the track
measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
pt = gtsam.Point3(1.0, 6.0, 2.0)
track2 = gtsam.SfmTrack(pt)
track2.add_measurement(i1, uv_i1)
track2.add_measurement(i2, uv_i2)
track2.add_measurement(i3, uv_i3)
self.data.add_track(self.tracks)
self.data.add_track(track2)
# Number of tracks in SfmData is 2
self.assertEqual(self.data.number_tracks(), 2)
# camera idx of first measurement of second track corresponds to i1
cam_idx, img_measurement = self.data.track(1).measurement(0)
self.assertEqual(cam_idx, i1)
if __name__ == '__main__':
unittest.main()

2
wrap/cmake/PybindWrap.cmake
View File

@ -146,7 +146,7 @@ function(install_python_scripts
else()
set(build_type_tag "")
endif()
# Split up filename to strip trailing '/' in WRAP_CYTHON_INSTALL_PATH if
# Split up filename to strip trailing '/' in GTSAM_PY_INSTALL_PATH if
# there is one
get_filename_component(location "${dest_directory}" PATH)
get_filename_component(name "${dest_directory}" NAME)

22
wrap/matlab_wrapper.py
View File

@ -210,18 +210,26 @@ class MatlabWrapper(object):
else:
formatted_type_name += name
if len(type_name.instantiations) == 1:
if separator == "::": # C++
formatted_type_name += '<{}>'.format(cls._format_type_name(type_name.instantiations[0],
include_namespace=include_namespace,
constructor=constructor, method=method))
else:
if separator == "::": # C++
templates = []
for idx in range(len(type_name.instantiations)):
template = '{}'.format(cls._format_type_name(type_name.instantiations[idx],
include_namespace=include_namespace,
constructor=constructor, method=method))
templates.append(template)
if len(templates) > 0: # If there are no templates
formatted_type_name += '<{}>'.format(','.join(templates))
else:
for idx in range(len(type_name.instantiations)):
formatted_type_name += '{}'.format(cls._format_type_name(
type_name.instantiations[0],
type_name.instantiations[idx],
separator=separator,
include_namespace=False,
constructor=constructor, method=method
))
return formatted_type_name
@classmethod

6
wrap/pybind_wrapper.py
View File

@ -69,13 +69,13 @@ class PybindWrapper(object):
return textwrap.dedent('''
.def("serialize",
[]({class_inst} self){{
return gtsam::serialize(self);
return gtsam::serialize(*self);
}}
)
.def("deserialize",
[]({class_inst} self, string serialized){{
return gtsam::deserialize(serialized, self);
}})
gtsam::deserialize(serialized, *self);
}}, py::arg("serialized"))
'''.format(class_inst=cpp_class + '*'))
is_method = isinstance(method, instantiator.InstantiatedMethod)

12
wrap/tests/expected-python/geometry_pybind.cpp
View File

@ -40,13 +40,13 @@ PYBIND11_MODULE(geometry_py, m_) {
.def("vectorConfusion",[](gtsam::Point2* self){return self->vectorConfusion();})
.def("serialize",
[](gtsam::Point2* self){
return gtsam::serialize(self);
return gtsam::serialize(*self);
}
)
.def("deserialize",
[](gtsam::Point2* self, string serialized){
return gtsam::deserialize(serialized, self);
})
gtsam::deserialize(serialized, *self);
}, py::arg("serialized"))
;
py::class_<gtsam::Point3, std::shared_ptr<gtsam::Point3>>(m_gtsam, "Point3")
@ -54,13 +54,13 @@ PYBIND11_MODULE(geometry_py, m_) {
.def("norm",[](gtsam::Point3* self){return self->norm();})
.def("serialize",
[](gtsam::Point3* self){
return gtsam::serialize(self);
return gtsam::serialize(*self);
}
)
.def("deserialize",
[](gtsam::Point3* self, string serialized){
return gtsam::deserialize(serialized, self);
})
gtsam::deserialize(serialized, *self);
}, py::arg("serialized"))
.def_static("staticFunction",[](){return gtsam::Point3::staticFunction();})
.def_static("StaticFunctionRet",[]( double z){return gtsam::Point3::StaticFunctionRet(z);}, py::arg("z"));