diff --git a/.github/ISSUE_TEMPLATE/bug-report.md b/.github/ISSUE_TEMPLATE/bug-report.md new file mode 100644 index 0000000000..9f15b2b7c2 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug-report.md @@ -0,0 +1,35 @@ +--- +name: "Bug Report" +about: Submit a bug report to help us improve GTSAM +--- + + + + + + + +## Description + + + +## Steps to reproduce + +1. +2. + + + +## Expected behavior + + + +## Environment + + + + + +## Additional information + + \ No newline at end of file diff --git a/.github/ISSUE_TEMPLATE/feature-request.md b/.github/ISSUE_TEMPLATE/feature-request.md new file mode 100644 index 0000000000..e1e13650a2 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature-request.md @@ -0,0 +1,24 @@ +--- +name: "Feature Request" +about: Submit a proposal/request for a new GTSAM feature +--- + +## Feature + + + +## Motivation + + + +## Pitch + + + +## Alternatives + + + +## Additional context + + \ No newline at end of file diff --git a/.github/ISSUE_TEMPLATE/questions-help-support.md b/.github/ISSUE_TEMPLATE/questions-help-support.md new file mode 100644 index 0000000000..49dcefbd01 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/questions-help-support.md @@ -0,0 +1,5 @@ +--- +name: "Questions/Help/Support" +--- + +Please post questions and support requests in the [GTSAM Google group](https://groups.google.com/forum/#!forum/gtsam-users) and not on Github. diff --git a/.travis.sh b/.travis.sh new file mode 100755 index 0000000000..3cec20f539 --- /dev/null +++ b/.travis.sh @@ -0,0 +1,85 @@ +#!/bin/bash + +# common tasks before either build or test +function prepare () +{ + set -e # Make sure any error makes the script to return an error code + set -x # echo + + SOURCE_DIR=`pwd` + BUILD_DIR=build + + #env + git clean -fd || true + rm -fr $BUILD_DIR || true + mkdir $BUILD_DIR && cd $BUILD_DIR + + if [ -z "$CMAKE_BUILD_TYPE" ]; then + CMAKE_BUILD_TYPE=Debug + fi + + if [ -z "$GTSAM_ALLOW_DEPRECATED_SINCE_V4" ]; then + GTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF + fi + + if [ ! -z "$GCC_VERSION" ]; then + sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-$GCC_VERSION 60 \ + --slave /usr/bin/g++ g++ /usr/bin/g++-$GCC_VERSION + sudo update-alternatives --set gcc /usr/bin/gcc-$GCC_VERSION + fi +} + +# common tasks after either build or test +function finish () +{ + # Print ccache stats + ccache -s + + cd $SOURCE_DIR +} + +# compile the code with the intent of populating the cache +function build () +{ + prepare + + cmake $SOURCE_DIR \ + -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \ + -DGTSAM_BUILD_TESTS=OFF \ + -DGTSAM_BUILD_UNSTABLE=$GTSAM_BUILD_UNSTABLE \ + -DGTSAM_BUILD_EXAMPLES_ALWAYS=ON \ + -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=$GTSAM_ALLOW_DEPRECATED_SINCE_V4 + + # Actual build: + VERBOSE=1 make -j2 + + finish +} + +# run the tests +function test () +{ + prepare + + cmake $SOURCE_DIR \ + -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \ + -DGTSAM_BUILD_TESTS=ON \ + -DGTSAM_BUILD_UNSTABLE=$GTSAM_BUILD_UNSTABLE \ + -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ + -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF + + # Actual build: + make -j2 check + + finish +} + +# select between build or test +case $1 in + -b) + build + ;; + -t) + test + ;; +esac diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000000..1e2d6760a1 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,113 @@ +language: cpp +cache: ccache +sudo: required +dist: xenial + +addons: + apt: + sources: + - ubuntu-toolchain-r-test + packages: + - g++-8 + - clang-3.8 + - build-essential + - pkg-config + - cmake + - libpython-dev python-numpy + - libboost-all-dev + +# before_install: +# - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update ; fi + +install: + - if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache ; fi + - if [ "$TRAVIS_OS_NAME" == "osx" ]; then export PATH="/usr/local/opt/ccache/libexec:$PATH" ; fi + +# We first do the compile stage specified below, then the matrix expansion specified after. +stages: + - compile + - test + +# Compile stage without building examples/tests to populate the caches. +jobs: + include: +# on Mac, GCC + - stage: compile + os: osx + compiler: gcc + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + script: bash .travis.sh -b + - stage: compile + os: osx + compiler: gcc + env: CMAKE_BUILD_TYPE=Release + script: bash .travis.sh -b +# on Mac, CLANG + - stage: compile + os: osx + compiler: clang + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + script: bash .travis.sh -b + - stage: compile + os: osx + compiler: clang + env: CMAKE_BUILD_TYPE=Release + script: bash .travis.sh -b +# on Linux, GCC + - stage: compile + os: linux + compiler: gcc + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + script: bash .travis.sh -b + - stage: compile + os: linux + compiler: gcc + env: CMAKE_BUILD_TYPE=Release + script: bash .travis.sh -b +# on Linux, CLANG + - stage: compile + os: linux + compiler: clang + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + script: bash .travis.sh -b + - stage: compile + os: linux + compiler: clang + env: CMAKE_BUILD_TYPE=Release + script: bash .travis.sh -b +# on Linux, with deprecated ON to make sure that path still compiles + - stage: compile + os: linux + compiler: clang + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON + script: bash .travis.sh -b + +# Matrix configuration: +os: + - osx + - linux +compiler: + - gcc + - clang +env: + global: + - MAKEFLAGS="-j2" + - CCACHE_SLOPPINESS=pch_defines,time_macros + - GTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF + - GTSAM_BUILD_UNSTABLE=ON + matrix: + - CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + - CMAKE_BUILD_TYPE=Release +script: + - bash .travis.sh -t + +matrix: + exclude: + # Exclude g++ debug on Linux as it consistently times out + - os: linux + compiler: gcc + env : CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + # Exclude clang on Linux/clang in release until issue #57 is solved + - os: linux + compiler: clang + env : CMAKE_BUILD_TYPE=Release diff --git a/CMakeLists.txt b/CMakeLists.txt index e9498ac218..b0457cb1c2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,3 @@ - project(GTSAM CXX C) cmake_minimum_required(VERSION 3.0) @@ -47,6 +46,17 @@ else() set(GTSAM_UNSTABLE_AVAILABLE 0) endif() +# ---------------------------------------------------------------------------- +# Uninstall target, for "make uninstall" +# ---------------------------------------------------------------------------- +configure_file( + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in" + "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" + IMMEDIATE @ONLY) + +add_custom_target(uninstall + "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake") + ############################################################################### # Set up options @@ -65,13 +75,26 @@ option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" 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_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) -option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF) +option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typedef Point2 and Point3 to Eigen::Vector equivalents" OFF) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) if(NOT MSVC AND NOT XCODE_VERSION) option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON) endif() +if(NOT MSVC AND NOT XCODE_VERSION) + # Set the build type to upper case for downstream use + string(TOUPPER "${CMAKE_BUILD_TYPE}" CMAKE_BUILD_TYPE_UPPER) + + # Set the GTSAM_BUILD_TAG variable. + # If build type is Release, set to blank (""), else set to the build type. + if("${CMAKE_BUILD_TYPE_UPPER}" STREQUAL "RELEASE") + set(GTSAM_BUILD_TAG "") # Don't create release mode tag on installed directory + else() + set(GTSAM_BUILD_TAG "${CMAKE_BUILD_TYPE}") + endif() +endif() + # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) @@ -84,10 +107,7 @@ if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND NOT GTSAM_ message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX or GTSAM_INSTALL_CYTHON_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP") endif() if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND GTSAM_BUILD_TYPE_POSTFIXES) - set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE}_POSTFIX}) - if(NOT "${CURRENT_POSTFIX}" STREQUAL "") - message(FATAL_ERROR "Cannot use executable postfixes with the matlab or cython wrappers. Please disable GTSAM_BUILD_TYPE_POSTFIXES") - endif() + set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX}) endif() if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP) message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP") @@ -123,20 +143,20 @@ if(MSVC) # 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 GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK) endif() # Virtual memory range for PCH exceeded on VS2015 if(MSVC_VERSION LESS 1910) # older than VS2017 - list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295) + 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. +# 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 GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT) + 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. @@ -159,34 +179,30 @@ option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) # so we downgraded this to classic filenames-based variables, and manually adding # the target_include_directories(xxx ${Boost_INCLUDE_DIR}) set(GTSAM_BOOST_LIBRARIES - optimized - ${Boost_SERIALIZATION_LIBRARY_RELEASE} - ${Boost_SYSTEM_LIBRARY_RELEASE} - ${Boost_FILESYSTEM_LIBRARY_RELEASE} - ${Boost_THREAD_LIBRARY_RELEASE} - ${Boost_DATE_TIME_LIBRARY_RELEASE} - ${Boost_REGEX_LIBRARY_RELEASE} - debug - ${Boost_SERIALIZATION_LIBRARY_DEBUG} - ${Boost_SYSTEM_LIBRARY_DEBUG} - ${Boost_FILESYSTEM_LIBRARY_DEBUG} - ${Boost_THREAD_LIBRARY_DEBUG} - ${Boost_DATE_TIME_LIBRARY_DEBUG} - ${Boost_REGEX_LIBRARY_DEBUG} + optimized ${Boost_SERIALIZATION_LIBRARY_RELEASE} + optimized ${Boost_SYSTEM_LIBRARY_RELEASE} + optimized ${Boost_FILESYSTEM_LIBRARY_RELEASE} + optimized ${Boost_THREAD_LIBRARY_RELEASE} + optimized ${Boost_DATE_TIME_LIBRARY_RELEASE} + optimized ${Boost_REGEX_LIBRARY_RELEASE} + debug ${Boost_SERIALIZATION_LIBRARY_DEBUG} + debug ${Boost_SYSTEM_LIBRARY_DEBUG} + debug ${Boost_FILESYSTEM_LIBRARY_DEBUG} + debug ${Boost_THREAD_LIBRARY_DEBUG} + debug ${Boost_DATE_TIME_LIBRARY_DEBUG} + debug ${Boost_REGEX_LIBRARY_DEBUG} ) message(STATUS "GTSAM_BOOST_LIBRARIES: ${GTSAM_BOOST_LIBRARIES}") if (GTSAM_DISABLE_NEW_TIMERS) message("WARNING: GTSAM timing instrumentation manually disabled") - list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) else() if(Boost_TIMER_LIBRARY) list(APPEND GTSAM_BOOST_LIBRARIES - optimized - ${Boost_TIMER_LIBRARY_RELEASE} - ${Boost_CHRONO_LIBRARY_RELEASE} - debug - ${Boost_TIMER_LIBRARY_DEBUG} - ${Boost_CHRONO_LIBRARY_DEBUG} + optimized ${Boost_TIMER_LIBRARY_RELEASE} + optimized ${Boost_CHRONO_LIBRARY_RELEASE} + debug ${Boost_TIMER_LIBRARY_DEBUG} + debug ${Boost_CHRONO_LIBRARY_DEBUG} ) else() list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt @@ -197,7 +213,7 @@ endif() if(NOT (${Boost_VERSION} LESS 105600)) message("Ignoring Boost restriction on optional lvalue assignment from rvalues") - list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) endif() ############################################################################### @@ -265,7 +281,7 @@ find_package(OpenMP) # do this here to generate correct message if disabled if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL) if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP) set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC ${OpenMP_CXX_FLAGS}) endif() endif() @@ -277,6 +293,7 @@ endif() ### See: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection) ### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code) option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) +option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" OFF) # Switch for using system Eigen or GTSAM-bundled Eigen if(GTSAM_USE_SYSTEM_EIGEN) @@ -314,12 +331,35 @@ else() 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 GTSAM_COMPILE_DEFINITIONS_PUBLIC DEIGEN_NO_STATIC_ASSERT) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT) endif() - list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen + list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen endif() ############################################################################### @@ -361,28 +401,28 @@ elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc") endif() if(MSVC) - list(APPEND GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS) - list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings - list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE /bigobj) # Allow large object files for template-based code + 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 GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs) + list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs) endif() endif() # As of XCode 7, clang also complains about this if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7.0) - list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs) + 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 GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS) endif() ############################################################################### @@ -480,6 +520,9 @@ message(STATUS "===============================================================" message(STATUS "================ Configuration Options ======================") message(STATUS " CMAKE_CXX_COMPILER_ID type : ${CMAKE_CXX_COMPILER_ID}") message(STATUS " CMAKE_CXX_COMPILER_VERSION : ${CMAKE_CXX_COMPILER_VERSION}") +message(STATUS " CMake version : ${CMAKE_VERSION}") +message(STATUS " CMake generator : ${CMAKE_GENERATOR}") +message(STATUS " CMake build tool : ${CMAKE_BUILD_TOOL}") message(STATUS "Build flags ") print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ") print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all' ") @@ -492,18 +535,18 @@ print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in librar if(GTSAM_UNSTABLE_AVAILABLE) print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ") endif() -string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper) -print_config_flag(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ") + if(NOT MSVC AND NOT XCODE_VERSION) + print_config_flag(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ") message(STATUS " Build type : ${CMAKE_BUILD_TYPE}") - message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}") - message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}") -endif() -if(GTSAM_USE_SYSTEM_EIGEN) - message(STATUS " Use System Eigen : Yes") -else() - message(STATUS " Use System Eigen : No") + message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}") + message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}") endif() + +print_build_options_for_target(gtsam) + +message(STATUS " Use System Eigen : ${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})") + if(GTSAM_USE_TBB) message(STATUS " Use Intel TBB : Yes") elseif(TBB_FOUND) diff --git a/DEVELOP b/DEVELOP deleted file mode 100644 index 483197bc8c..0000000000 --- a/DEVELOP +++ /dev/null @@ -1,19 +0,0 @@ -Information for developers - -Coding Conventions: - -* Classes are Uppercase, methods and functions lowerMixedCase -* We use a modified K&R Style, with 2-space tabs, inserting spaces for tabs -* Use meaningful variable names, e.g., measurement not msm - - -Windows: - -On Windows it is necessary to explicitly export all functions from the library -which should be externally accessible. To do this, include the macro -GTSAM_EXPORT in your class or function definition. - -For example: -class GTSAM_EXPORT MyClass { ... }; - -GTSAM_EXPORT myFunction(); \ No newline at end of file diff --git a/DEVELOP.md b/DEVELOP.md new file mode 100644 index 0000000000..133f3ea110 --- /dev/null +++ b/DEVELOP.md @@ -0,0 +1,19 @@ +# Information for Developers + +### Coding Conventions + +* Classes are Uppercase, methods and functions lowerMixedCase. +* We use a modified K&R Style, with 2-space tabs, inserting spaces for tabs. +* Use meaningful variable names, e.g. `measurement` not `msm`. + + +### Windows + +On Windows it is necessary to explicitly export all functions from the library which should be externally accessible. To do this, include the macro `GTSAM_EXPORT` in your class or function definition. + +For example: +```cpp +class GTSAM_EXPORT MyClass { ... }; + +GTSAM_EXPORT myFunction(); +``` diff --git a/README.md b/README.md index 720517669d..381b56ba3d 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,8 @@ -README - Georgia Tech Smoothing and Mapping library -=================================================== +[![Build Status](https://travis-ci.com/borglab/gtsam.svg?branch=develop)](https://travis-ci.com/borglab/gtsam/) -What is GTSAM? --------------- +# README - Georgia Tech Smoothing and Mapping library + +## What is GTSAM? GTSAM is a library of C++ classes that implement smoothing and mapping (SAM) in robotics and vision, using factor graphs and Bayes @@ -13,12 +13,11 @@ On top of the C++ library, GTSAM includes a MATLAB interface (enable GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface is under development. -Quickstart ----------- +## Quickstart In the root library folder execute: -``` +```sh #!bash $ mkdir build $ cd build @@ -40,32 +39,33 @@ Optional prerequisites - used automatically if findable by CMake: - See [INSTALL.md](INSTALL.md) for more installation information - Note that MKL may not provide a speedup in all cases. Make sure to benchmark your problem with and without MKL. -GTSAM 4 Compatibility ---------------------- +## GTSAM 4 Compatibility GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag. Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect. -The Preintegrated IMU Factor ----------------------------- +## Wrappers + +We provide support for [MATLAB](matlab/README.md) and [Python](cython/README.md) wrappers for GTSAM. Please refer to the linked documents for more details. + +## The Preintegrated IMU Factor GTSAM includes a state of the art IMU handling scheme based on -- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. +- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. [[link]](https://ieeexplore.ieee.org/document/6092505) Our implementation improves on this using integration on the manifold, as detailed in -- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. -- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. +- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483) +- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf) If you are using the factor in academic work, please cite the publications above. In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF. -Additional Information ----------------------- +## Additional Information There is a [`GTSAM users Google group`](https://groups.google.com/forum/#!forum/gtsam-users) for general discussion. diff --git a/THANKS b/THANKS.md similarity index 100% rename from THANKS rename to THANKS.md diff --git a/USAGE.md b/USAGE.md index 0493db6809..f8e4625502 100644 --- a/USAGE.md +++ b/USAGE.md @@ -1,48 +1,42 @@ -USAGE - Georgia Tech Smoothing and Mapping library -=================================== -What is this file? +# GTSAM USAGE - This file explains how to make use of the library for common SLAM tasks, - using a visual SLAM implementation as an example. - +This file explains how to make use of the library for common SLAM tasks, using a visual SLAM implementation as an example. + +## Getting Started + +### Install -Getting Started ---------------------------------------------------- -Install: - Follow the installation instructions in the README file to build and - install gtsam, as well as running tests to ensure the library is working - properly. - -Compiling/Linking with gtsam: - The installation creates a binary "libgtsam" at the installation prefix, - and an include folder "gtsam". These are the only required includes, but - the library has also been designed to make use of XML serialization through - the Boost.serialization library, which requires the the Boost.serialization - headers and binaries to be linked. - - If you use CMake for your project, you can use the CMake scripts in the - cmake folder for finding GTSAM, CppUnitLite, and Wrap. - -Examples: - To see how the library works, examine the unit tests provided. +Follow the installation instructions in the README file to build and install gtsam, as well as running tests to ensure the library is working properly. + +### Compiling/Linking with GTSAM + +The installation creates a binary `libgtsam` at the installation prefix, and an include folder `gtsam`. These are the only required includes, but the library has also been designed to make use of XML serialization through the `Boost.serialization` library, which requires the the Boost.serialization headers and binaries to be linked. + +If you use CMake for your project, you can use the CMake scripts in the cmake folder for finding `GTSAM`, `CppUnitLite`, and `Wrap`. + +### Examples + +To see how the library works, examine the unit tests provided. +## Overview + +The GTSAM library has three primary components necessary for the construction of factor graph representation and optimization which users will need to adapt to their particular problem. + +* FactorGraph + + A factor graph contains a set of variables to solve for (i.e., robot poses, landmark poses, etc.) and a set of constraints between these variables, which make up factors. + +* Values: + + Values is a single object containing labeled values for all of the variables. Currently, all variables are labeled with strings, but the type or organization of the variables can change. + +* Factors -Overview ---------------------------------------------------- -The GTSAM library has three primary components necessary for the construction -of factor graph representation and optimization which users will need to -adapt to their particular problem. - -* FactorGraph: - A factor graph contains a set of variables to solve for (i.e., robot poses, landmark poses, etc.) and a set of constraints between these variables, which make up factors. -* Values: - Values is a single object containing labeled values for all of the variables. Currently, all variables are labeled with strings, but the type or organization of the variables can change -* Factors: A nonlinear factor expresses a constraint between variables, which in the SLAM example, is a measurement such as a visual reading on a landmark or odometry. The library is organized according to the following directory structure: - 3rdparty local copies of third party libraries - Eigen3 and CCOLAMD + 3rdparty local copies of third party libraries e.g. Eigen3 and CCOLAMD base provides some base Math and data structures, as well as test-related utilities geometry points, poses, tensors, etc inference core graphical model inference such as factor graphs, junction trees, Bayes nets, Bayes trees diff --git a/Using-GTSAM-EXPORT.md b/Using-GTSAM-EXPORT.md new file mode 100644 index 0000000000..41eccc178d --- /dev/null +++ b/Using-GTSAM-EXPORT.md @@ -0,0 +1,37 @@ +# Using GTSAM_EXPORT + +To create a DLL in windows, the `GTSAM_EXPORT` keyword has been created and needs to be applied to different methods and classes in the code to expose this code outside of the DLL. However, there are several intricacies that make this more difficult than it sounds. In general, if you follow the following three rules, GTSAM_EXPORT should work properly. The rest of the document also describes (1) the common error message encountered when you are not following these rules and (2) the reasoning behind these usage rules. + +## Usage Rules +1. Put `GTSAM_EXPORT` in front of any function that you want exported in the DLL _if and only if_ that function is declared in a .cpp file, not just a .h file. +2. Use `GTSAM_EXPORT` in a class definition (i.e. `class GSTAM_EXPORT MyClass {...}`) only if: + * At least one of the functions inside that class is declared in a .cpp file and not just the .h file. + * You can `GTSAM_EXPORT` any class it inherits from as well. (Note that this implictly requires the class does not derive from a "header-only" class. Note that Eigen is a "header-only" library, so if your class derives from Eigen, _do not_ use `GTSAM_EXPORT` in the class definition!) +3. If you have defined a class using `GTSAM_EXPORT`, do not use `GTSAM_EXPORT` in any of its individual function declarations. (Note that you _can_ put `GTSAM_EXPORT` in the definition of individual functions within a class as long as you don't put `GTSAM_EXPORT` in the class definition.) + +## When is GTSAM_EXPORT being used incorrectly +Unfortunately, using `GTSAM_EXPORT` incorrectly often does not cause a compiler or linker error in the library that is being compiled, but only when you try to use that DLL in a different library. For example, an error in gtsam/base will often show up when compiling the check_base_program or the MATLAB wrapper, but not when compiling/linking gtsam itself. The most common errors will say something like: + +``` +Error LNK2019 unresolved external symbol "public: void __cdecl gtsam::SO3::print(class std::basic_string,class std::allocator > const &)const " (?print@SO3@gtsam@@QEBAXAEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@@Z) referenced in function "public: static void __cdecl gtsam::Testable::Print(class gtsam::SO3 const &,class std::basic_string,class std::allocator > const &)" (?Print@?$Testable@VSO3@gtsam@@@gtsam@@SAXAEBVSO3@2@AEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@@Z) check_geometry_program C:\AFIT\lib\gtsam\build\gtsam\geometry\tests\testSO3.obj +``` + +Let's analyze this error statement. First, there is an unresolved symbol `gtsam::SO3::print`. This can occur because _either_ `GTSAM_EXPORT` was not added to the print function definition when it should have been, _OR_ because `GTSAM_EXPORT` was added to the print function definition when it is fully declared in the header. This error was detected while compiling `check_geometry_program` and pulling in `...\testSO3.obj`. Specifically, within the function call `gtsam::Testable::Print (...)`. Note that this error did _not_ occur when compiling the library that actually has SO3 inside of it. + +## But Why? +I believe that how the compiler/linker interacts with GTSAM_EXPORT can be explained by understanding the rules that MSVC operates under. + +But first, we need to understand exactly what `GTSAM_EXPORT` is. `GTSAM_EXPORT` is a `#define` macro that is created by CMAKE when GTSAM is being compiled on a Windows machine. Inside the GTSAM project, GTSAM export will be set to a "dllexport" command. A "dllexport" command tells the compiler that this function should go into the DLL and be visible externally. In any other library, `GTSAM_EXPORT` will be set to a "dllimport" command, telling the linker it should find this function in a DLL somewhere. This leads to the first rule the compiler uses. (Note that I say "compiler rules" when the rules may actually be in the linker, but I am conflating the two terms here when I speak of the "compiler rules".) + +***Compiler Rule #1*** If a `dllimport` command is used in defining a function or class, that function or class _must_ be found in a DLL. + +Rule #1 doesn't seem very bad, until you combine it with rule #2 + +***Compiler Rule #2*** Anything declared in a header file is not included in a DLL. + +When these two rules are combined, you get some very confusing results. For example, a class which is completely defined in a header (e.g. LieMatrix) cannot use `GTSAM_EXPORT` in its definition. If LieMatrix is defined with `GTSAM_EXPORT`, then the compiler _must_ find LieMatrix in a DLL. Because LieMatrix is a header-only class, however, it can't find it, leading to a very confusing "I can't find this symbol" type of error. Note that the linker says it can't find the symbol even though the compiler found the header file that completely defines the class. + +Also note that when a class that you want to export inherits from another class that is not exportable, this can cause significant issues. According to this [MSVC Warning page](https://docs.microsoft.com/en-us/cpp/error-messages/compiler-warnings/compiler-warning-level-2-c4275?view=vs-2019), it may not strictly be a rule, but we have seen several linker errors when a class that is defined with `GTSAM_EXPORT` extended an Eigen class. In general, it appears that any inheritance of non-exportable class by an exportable class is a bad idea. + +## Conclusion +Hopefully, this little document clarifies when `GTSAM_EXPORT` should and should not be used whenever future GTSAM code is being written. Following the usage rules above, we have been able to get all of the libraries, together with their test and wrapping libraries, to compile/link successfully. \ No newline at end of file diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml deleted file mode 100644 index 9a94aa86c8..0000000000 --- a/bitbucket-pipelines.yml +++ /dev/null @@ -1,15 +0,0 @@ -# Built from sample configuration for C++ – Make. -# Check https://confluence.atlassian.com/x/5Q4SMw for more examples. -# ----- -# Our custom docker image from Docker Hub as the build environment. -image: dellaert/ubuntu-boost-tbb-eigen3:bionic - -pipelines: - default: - - step: - script: # Modify the commands below to build your repository. - - mkdir build - - cd build - - cmake -DGTSAM_USE_SYSTEM_EIGEN=OFF -DGTSAM_USE_EIGEN_MKL=OFF .. - - make -j2 - - make -j2 check \ No newline at end of file diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index b807652980..aa35fea05f 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -1,7 +1,45 @@ +# function: list_append_cache(var [new_values ...]) +# Like "list(APPEND ...)" but working for CACHE variables. +# ----------------------------------------------------------- +function(list_append_cache var) + set(cur_value ${${var}}) + list(APPEND cur_value ${ARGN}) + get_property(MYVAR_DOCSTRING CACHE ${var} PROPERTY HELPSTRING) + set(${var} "${cur_value}" CACHE STRING "${MYVAR_DOCSTRING}" FORCE) +endfunction() + +# function: append_config_if_not_empty(TARGET_VARIABLE build_type) +# Auxiliary function used to merge configuration-specific flags into the +# global variables that will actually be send to cmake targets. +# ----------------------------------------------------------- +function(append_config_if_not_empty TARGET_VARIABLE_ build_type) + string(TOUPPER "${build_type}" build_type_toupper) + set(flags_variable_name "${TARGET_VARIABLE_}_${build_type_toupper}") + set(flags_ ${${flags_variable_name}}) + if (NOT "${flags_}" STREQUAL "") + if (${build_type_toupper} STREQUAL "COMMON") + # Special "COMMON" configuration type, just append without CMake expression: + list_append_cache(${TARGET_VARIABLE_} "${flags_}") + else() + # Regular configuration type: + list_append_cache(${TARGET_VARIABLE_} "$<$:${flags_}>") + endif() + endif() +endfunction() + + # Add install prefix to search path list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}") + +# Set up build types for MSVC and XCode +set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel + CACHE STRING "Build types available to MSVC and XCode") +mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES) +set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES} CACHE STRING "Build configurations" FORCE) + + # Default to Release mode if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION) set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING @@ -13,39 +51,79 @@ endif() # Add option for using build type postfixes to allow installing multiple build modes option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON) -# Set custom compilation flags. -# NOTE: We set all the CACHE variables with a GTSAM prefix, and then set a normal local variable below -# so that we don't "pollute" the global variable namespace in the cmake cache. - # Set all CMAKE_BUILD_TYPE flags: - # (see https://cmake.org/Wiki/CMake_Useful_Variables#Compilers_and_Tools) +# Define all cache variables, to be populated below depending on the OS/compiler: +set(GTSAM_COMPILE_OPTIONS_PRIVATE "" CACHE STRING "(Do not edit) Private compiler flags for all build configurations." FORCE) +set(GTSAM_COMPILE_OPTIONS_PUBLIC "" CACHE STRING "(Do not edit) Public compiler flags (exported to user projects) for all build configurations." FORCE) +set(GTSAM_COMPILE_DEFINITIONS_PRIVATE "" CACHE STRING "(Do not edit) Private preprocessor macros for all build configurations." FORCE) +set(GTSAM_COMPILE_DEFINITIONS_PUBLIC "" CACHE STRING "(Do not edit) Public preprocessor macros for all build configurations." FORCE) +mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE) +mark_as_advanced(GTSAM_COMPILE_OPTIONS_PUBLIC) +mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE) +mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PUBLIC) + +foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES}) + string(TOUPPER "${build_type}" build_type_toupper) + + # Define empty cache variables for "public". "private" are creaed below. + set(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper} "" CACHE STRING "(User editable) Public compiler flags (exported to user projects) for `${build_type_toupper}` configuration.") + set(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper} "" CACHE STRING "(User editable) Public preprocessor macros for `${build_type_toupper}` configuration.") +endforeach() + +# Common preprocessor macros for each configuration: +set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_DEBUG "_DEBUG;EIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "(User editable) Private preprocessor macros for Debug configuration.") +set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELWITHDEBINFO "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for RelWithDebInfo configuration.") +set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELEASE "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for Release configuration.") +set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_PROFILING "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for Profiling configuration.") +set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_TIMING "NDEBUG;ENABLE_TIMING" CACHE STRING "(User editable) Private preprocessor macros for Timing configuration.") +if(MSVC) + # Common to all configurations: + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE + WINDOWS_LEAN_AND_MEAN + NOMINMAX + ) +endif() + +# Other (non-preprocessor macros) compiler flags: if(MSVC) - set(GTSAM_CMAKE_C_FLAGS "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.") - set(GTSAM_CMAKE_CXX_FLAGS "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.") - set(GTSAM_CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.") - set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.") - set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.") - set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.") - set(GTSAM_CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.") - set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.") - set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.") - set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.") - set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.") - set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.") + # Common to all configurations, next for each configuration: + set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON /W3 /GR /EHsc /MP CACHE STRING "(User editable) Private compiler flags for all configurations.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG /MDd /Zi /Ob0 /Od /RTC1 CACHE STRING "(User editable) Private compiler flags for Debug configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO /MD /O2 /D /Zi /d2Zi+ CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE /MD /O2 CACHE STRING "(User editable) Private compiler flags for Release configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING /MD /O2 /Zi CACHE STRING "(User editable) Private compiler flags for Profiling configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING /MD /O2 CACHE STRING "(User editable) Private compiler flags for Timing configuration.") else() - set(GTSAM_CMAKE_C_FLAGS "-std=c11 -Wall" CACHE STRING "Flags used by the compiler for all builds.") - set(GTSAM_CMAKE_CXX_FLAGS "-std=c++11 -Wall" CACHE STRING "Flags used by the compiler for all builds.") - set(GTSAM_CMAKE_C_FLAGS_DEBUG "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.") - set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.") - set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.") - set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.") - set(GTSAM_CMAKE_C_FLAGS_RELEASE " -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.") - set(GTSAM_CMAKE_CXX_FLAGS_RELEASE " -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.") - set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.") - set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.") - set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.") - set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.") + # Common to all configurations, next for each configuration: + set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON -Wall CACHE STRING "(User editable) Private compiler flags for all configurations.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG -g -fno-inline CACHE STRING "(User editable) Private compiler flags for Debug configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO -g -O3 CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE -O3 CACHE STRING "(User editable) Private compiler flags for Release configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING -O3 CACHE STRING "(User editable) Private compiler flags for Profiling configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING -g -O3 CACHE STRING "(User editable) Private compiler flags for Timing configuration.") endif() +# Enable C++11: +if (NOT CMAKE_VERSION VERSION_LESS 3.8) + set(GTSAM_COMPILE_FEATURES_PUBLIC "cxx_std_11" CACHE STRING "CMake compile features property for all gtsam targets.") + # See: https://cmake.org/cmake/help/latest/prop_tgt/CXX_EXTENSIONS.html + # This is to enable -std=c++11 instead of -std=g++11 + set(CMAKE_CXX_EXTENSIONS OFF) +else() + # Old cmake versions: + if (NOT MSVC) + list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC $<$:-std=c++11>) + endif() +endif() + +# Merge all user-defined flags into the variables that are to be actually used by CMake: +foreach(build_type "common" ${GTSAM_CMAKE_CONFIGURATION_TYPES}) + append_config_if_not_empty(GTSAM_COMPILE_OPTIONS_PRIVATE ${build_type}) + append_config_if_not_empty(GTSAM_COMPILE_OPTIONS_PUBLIC ${build_type}) + append_config_if_not_empty(GTSAM_COMPILE_DEFINITIONS_PRIVATE ${build_type}) + append_config_if_not_empty(GTSAM_COMPILE_DEFINITIONS_PUBLIC ${build_type}) +endforeach() + +# Linker flags: set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") set(GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") @@ -54,26 +132,11 @@ set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEA set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.") set(GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.") -mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING +mark_as_advanced(GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING - GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_CMAKE_CXX_FLAGS_PROFILING GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING + GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_ GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING) -# Apply the gtsam specific build flags as normal variables. This makes it so that they only -# apply to the gtsam part of the build if gtsam is built as a subproject -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${GTSAM_CMAKE_C_FLAGS}") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GTSAM_CMAKE_CXX_FLAGS}") -set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} ${GTSAM_CMAKE_C_FLAGS_DEBUG}") -set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${GTSAM_CMAKE_CXX_FLAGS_DEBUG}") -set(CMAKE_C_FLAGS_RELWITHDEBINFO "${CMAKE_C_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}") -set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO}") -set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} ${GTSAM_CMAKE_C_FLAGS_RELEASE}") -set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${GTSAM_CMAKE_CXX_FLAGS_RELEASE}") -set(CMAKE_C_FLAGS_PROFILING "${CMAKE_C_FLAGS_PROFILING} ${GTSAM_CMAKE_C_FLAGS_PROFILING}") -set(CMAKE_CXX_FLAGS_PROFILING "${CMAKE_CXX_FLAGS_PROFILING} ${GTSAM_CMAKE_CXX_FLAGS_PROFILING}") -set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_TIMING} ${GTSAM_CMAKE_C_FLAGS_TIMING}") -set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_TIMING} ${GTSAM_CMAKE_CXX_FLAGS_TIMING}") - set(CMAKE_SHARED_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING}) set(CMAKE_MODULE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING}) set(CMAKE_EXE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING}) @@ -86,14 +149,17 @@ set(CMAKE_EXE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING}) if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") # Apple Clang before 5.0 does not support -ftemplate-depth. if(NOT (APPLE AND "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") + list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-ftemplate-depth=1024") endif() endif() -option(GTSAM_BUILD_WITH_MARCH_NATIVE "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" ON) -if(GTSAM_BUILD_WITH_MARCH_NATIVE) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=native") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") +if (NOT MSVC) + option(GTSAM_BUILD_WITH_MARCH_NATIVE "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" ON) + if(GTSAM_BUILD_WITH_MARCH_NATIVE) + # Add as public flag so all dependant projects also use it, as required + # by Eigen to avid crashes due to SIMD vectorization: + list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-march=native") + endif() endif() # Set up build type library postfixes @@ -118,12 +184,6 @@ if(NOT "${CMAKE_BUILD_TYPE}" STREQUAL "") endif() endif() -# Set up build types for MSVC and XCode -set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel - CACHE STRING "Build types available to MSVC and XCode") -mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES) -set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES}) - # Check build types string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower) if( NOT cmake_build_type_tolower STREQUAL "" @@ -159,3 +219,20 @@ function(gtsam_assign_all_source_folders) gtsam_assign_source_folders("${all_c_srcs};${all_cpp_srcs};${all_headers}") endfunction() +# Applies the per-config build flags to the given target (e.g. gtsam, wrap_lib) +function(gtsam_apply_build_flags target_name_) + # To enable C++11: the use of target_compile_features() is preferred since + # it will be not in conflict with a more modern C++ standard, if used in a + # client program. + if (NOT "${GTSAM_COMPILE_FEATURES_PUBLIC}" STREQUAL "") + target_compile_features(${target_name_} PUBLIC ${GTSAM_COMPILE_FEATURES_PUBLIC}) + endif() + + target_compile_definitions(${target_name_} PRIVATE ${GTSAM_COMPILE_DEFINITIONS_PRIVATE}) + target_compile_definitions(${target_name_} PUBLIC ${GTSAM_COMPILE_DEFINITIONS_PUBLIC}) + if (NOT "${GTSAM_COMPILE_OPTIONS_PUBLIC}" STREQUAL "") + target_compile_options(${target_name_} PUBLIC ${GTSAM_COMPILE_OPTIONS_PUBLIC}) + endif() + target_compile_options(${target_name_} PRIVATE ${GTSAM_COMPILE_OPTIONS_PRIVATE}) + +endfunction(gtsam_apply_build_flags) diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index 6366c15083..c3cad9d83c 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -86,8 +86,13 @@ function(build_cythonized_cpp target cpp_file output_lib_we output_dir) if(APPLE) set(link_flags "-undefined dynamic_lookup") endif() - set_target_properties(${target} PROPERTIES COMPILE_FLAGS "-w" LINK_FLAGS "${link_flags}" - OUTPUT_NAME ${output_lib_we} PREFIX "" LIBRARY_OUTPUT_DIRECTORY ${output_dir}) + set_target_properties(${target} + PROPERTIES COMPILE_FLAGS "-w" + LINK_FLAGS "${link_flags}" + OUTPUT_NAME ${output_lib_we} + PREFIX "" + ${CMAKE_BUILD_TYPE_UPPER}_POSTFIX "" + LIBRARY_OUTPUT_DIRECTORY ${output_dir}) endfunction() # Cythonize a pyx from the command line as described at @@ -161,9 +166,13 @@ endfunction() function(install_cython_wrapped_library interface_header generated_files_path install_path) get_filename_component(module_name "${interface_header}" NAME_WE) - # NOTE: only installs .pxd and .pyx and binary files (not .cpp) - the trailing slash on the directory name + # NOTE: only installs .pxd and .pyx and binary files (not .cpp) - the trailing slash on the directory name # here prevents creating the top-level module name directory in the destination. - message(STATUS "Installing Cython Toolbox to ${install_path}") #${GTSAM_CYTHON_INSTALL_PATH}") + # Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH/subdirectory if there is one + get_filename_component(location "${install_path}" PATH) + get_filename_component(name "${install_path}" NAME) + message(STATUS "Installing Cython Toolbox to ${location}${GTSAM_BUILD_TAG}/${name}") #${GTSAM_CYTHON_INSTALL_PATH}" + if(GTSAM_BUILD_TYPE_POSTFIXES) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) @@ -172,10 +181,8 @@ function(install_cython_wrapped_library interface_header generated_files_path in else() set(build_type_tag "${build_type}") endif() - # Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH if there is one - get_filename_component(location "${install_path}" PATH) - get_filename_component(name "${install_path}" NAME) - install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}/${name}${build_type_tag}" + + install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}${build_type_tag}/${name}" CONFIGURATIONS "${build_type}" PATTERN "build" EXCLUDE PATTERN "CMakeFiles" EXCLUDE diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index bdd868665f..5fc829bf21 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -35,7 +35,11 @@ mark_as_advanced(FORCE MEX_COMMAND) # Now that we have mex, trace back to find the Matlab installation root get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH) get_filename_component(mex_path "${MEX_COMMAND}" PATH) -get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE) +if(mex_path MATCHES ".*/win64$") + get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE) +else() + get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE) +endif() set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)") @@ -78,28 +82,29 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex set(mexModuleExt mexw32) endif() endif() - + # Wrap codegen interface #usage: wrap interfacePath moduleName toolboxPath headerPath # interfacePath : *absolute* path to directory of module interface file # moduleName : the name of the module, interface file must be called moduleName.h # toolboxPath : the directory in which to generate the wrappers - # headerPath : path to matlab.h - + # headerPath : path to matlab.h + # Extract module name from interface header file name get_filename_component(interfaceHeader "${interfaceHeader}" ABSOLUTE) get_filename_component(modulePath "${interfaceHeader}" PATH) get_filename_component(moduleName "${interfaceHeader}" NAME_WE) - + # Paths for generated files set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}") set(generated_cpp_file "${generated_files_path}/${moduleName}_wrapper.cpp") set(compiled_mex_modules_root "${PROJECT_BINARY_DIR}/wrap/${moduleName}_mex") - + message(STATUS "Building wrap module ${moduleName}") - + # Find matlab.h in GTSAM - if("${PROJECT_NAME}" STREQUAL "GTSAM") + if(("${PROJECT_NAME}" STREQUAL "gtsam") OR + ("${PROJECT_NAME}" STREQUAL "gtsam_unstable")) set(matlab_h_path "${PROJECT_SOURCE_DIR}") else() if(NOT GTSAM_INCLUDE_DIR) @@ -221,6 +226,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex string(REPLACE ";" " " mexFlagsSpaced "${GTSAM_BUILD_MEX_BINARY_FLAGS}") add_library(${moduleName}_matlab_wrapper MODULE ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects}) target_link_libraries(${moduleName}_matlab_wrapper ${correctedOtherLibraries}) + target_link_libraries(${moduleName}_matlab_wrapper ${moduleName}) set_target_properties(${moduleName}_matlab_wrapper PROPERTIES OUTPUT_NAME "${moduleName}_wrapper" PREFIX "" diff --git a/cmake/GtsamPrinting.cmake b/cmake/GtsamPrinting.cmake index 674fd40865..e53f9c54fe 100644 --- a/cmake/GtsamPrinting.cmake +++ b/cmake/GtsamPrinting.cmake @@ -7,4 +7,45 @@ function(print_config_flag flag msg) else () message(STATUS " ${msg}: Disabled") endif () -endfunction(print_config_flag) +endfunction() + +# Based on https://github.com/jimbraun/XCDF/blob/master/cmake/CMakePadString.cmake +function(string_pad RESULT_NAME DESIRED_LENGTH VALUE) + string(LENGTH "${VALUE}" VALUE_LENGTH) + math(EXPR REQUIRED_PADS "${DESIRED_LENGTH} - ${VALUE_LENGTH}") + set(PAD ${VALUE}) + if(REQUIRED_PADS GREATER 0) + math(EXPR REQUIRED_MINUS_ONE "${REQUIRED_PADS} - 1") + foreach(FOO RANGE ${REQUIRED_MINUS_ONE}) + set(PAD "${PAD} ") + endforeach() + endif() + set(${RESULT_NAME} "${PAD}" PARENT_SCOPE) +endfunction() + +set(GTSAM_PRINT_SUMMARY_PADDING_LENGTH 50 CACHE STRING "Padding of cmake summary report lines after configuring.") +mark_as_advanced(GTSAM_PRINT_SUMMARY_PADDING_LENGTH) + +# Print " var: ${var}" padding with spaces as needed +function(print_padded variable_name) + string_pad(padded_prop ${GTSAM_PRINT_SUMMARY_PADDING_LENGTH} " ${variable_name}") + message(STATUS "${padded_prop}: ${${variable_name}}") +endfunction() + + +# Prints all the relevant CMake build options for a given target: +function(print_build_options_for_target target_name_) + print_padded(GTSAM_COMPILE_FEATURES_PUBLIC) + print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE) + print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC) + print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE) + print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC) + + foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES}) + string(TOUPPER "${build_type}" build_type_toupper) + print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE_${build_type_toupper}) + print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper}) + print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE_${build_type_toupper}) + print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper}) + endforeach() +endfunction() diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index 15d4219e69..3b42ffa215 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -8,7 +8,7 @@ # Macro: # # gtsamAddTestsGlob(groupName globPatterns excludedFiles linkLibraries) -# +# # Add a group of unit tests. A list of unit test .cpp files or glob patterns specifies the # tests to create. Tests are assigned into a group name so they can easily by run # independently with a make target. Running 'make check' builds and runs all tests. @@ -35,7 +35,7 @@ endmacro() # Macro: # # gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries) -# +# # Add scripts that will serve as examples of how to use the library. A list of files or # glob patterns is specified, and one executable will be created for each matching .cpp # file. These executables will not be installed. They are built with 'make all' if @@ -60,7 +60,7 @@ endmacro() # Macro: # # gtsamAddTimingGlob(globPatterns excludedFiles linkLibraries) -# +# # Add scripts that time aspects of the library. A list of files or # glob patterns is specified, and one executable will be created for each matching .cpp # file. These executables will not be installed. They are not built with 'make all', @@ -88,29 +88,36 @@ enable_testing() option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON) option(GTSAM_BUILD_EXAMPLES_ALWAYS "Build examples with 'make all' (build with 'make examples' if not)" ON) -option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF) + option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF) -# Add option for combining unit tests -if(MSVC OR XCODE_VERSION) - option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" ON) -else() - option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" OFF) -endif() -mark_as_advanced(GTSAM_SINGLE_TEST_EXE) + # Add option for combining unit tests + if(MSVC OR XCODE_VERSION) + option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" ON) + else() + option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" OFF) + endif() + mark_as_advanced(GTSAM_SINGLE_TEST_EXE) + + # Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck) + if(GTSAM_BUILD_TESTS) + add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $ --output-on-failure) + # Also add alternative checks using valgrind. + # We don't look for valgrind being installed in the system, since these + # targets are not invoked unless directly instructed by the user. + if (UNIX) + # Run all tests using valgrind: + add_custom_target(check_valgrind) + endif() -# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck) -if(GTSAM_BUILD_TESTS) - add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $ --output-on-failure) - - # Add target to build tests without running - add_custom_target(all.tests) -endif() + # Add target to build tests without running + add_custom_target(all.tests) + endif() -# Add examples target -add_custom_target(examples) + # Add examples target + add_custom_target(examples) -# Add timing target -add_custom_target(timing) + # Add timing target + add_custom_target(timing) # Implementations of this file's macros: @@ -118,23 +125,24 @@ add_custom_target(timing) macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) if(GTSAM_BUILD_TESTS) # Add group target if it doesn't already exist - if(NOT TARGET check.${groupName}) + if(NOT TARGET check.${groupName}) add_custom_target(check.${groupName} COMMAND ${CMAKE_CTEST_COMMAND} -C $ --output-on-failure) + set_property(TARGET check.${groupName} PROPERTY FOLDER "Unit tests") endif() - - # Get all script files - file(GLOB script_files ${globPatterns}) - # Remove excluded scripts from the list - if(NOT "${excludedFiles}" STREQUAL "") + # Get all script files + file(GLOB script_files ${globPatterns}) + + # Remove excluded scripts from the list + if(NOT "${excludedFiles}" STREQUAL "") file(GLOB excludedFilePaths ${excludedFiles}) if("${excludedFilePaths}" STREQUAL "") message(WARNING "The pattern '${excludedFiles}' for excluding tests from group ${groupName} did not match any files") else() - list(REMOVE_ITEM script_files ${excludedFilePaths}) + list(REMOVE_ITEM script_files ${excludedFilePaths}) endif() - endif() - + endif() + # Separate into source files and headers (allows for adding headers to show up in # MSVC and Xcode projects). set(script_srcs "") @@ -147,7 +155,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) list(APPEND script_srcs ${script_file}) endif() endforeach() - + # Don't put test files in folders in MSVC and Xcode because they're already grouped source_group("" FILES ${script_srcs} ${script_headers}) @@ -156,26 +164,41 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) foreach(script_src IN ITEMS ${script_srcs}) # Get test base name get_filename_component(script_name ${script_src} NAME_WE) - + # Add executable add_executable(${script_name} ${script_src} ${script_headers}) target_link_libraries(${script_name} CppUnitLite ${linkLibraries}) - + + # Apply user build flags from CMake cache variables: + gtsam_apply_build_flags(${script_name}) + # Add target dependencies add_test(NAME ${script_name} COMMAND ${script_name}) add_dependencies(check.${groupName} ${script_name}) add_dependencies(check ${script_name}) - add_dependencies(all.tests ${script_name}) + add_dependencies(all.tests ${script_name}) if(NOT MSVC AND NOT XCODE_VERSION) - add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name}) + # Regular test run: + add_custom_target(${script_name}.run + COMMAND ${EXECUTABLE_OUTPUT_PATH}${script_name} + DEPENDS ${script_name} + ) + + # Run with valgrind: + set(GENERATED_EXE "$") + add_custom_target(${script_name}.valgrind + COMMAND "valgrind" "--error-exitcode=1" ${GENERATED_EXE} + DEPENDS ${script_name} + ) + add_dependencies(check_valgrind ${script_name}.valgrind) endif() - + # Add TOPSRCDIR - set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") - + set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"") + # Exclude from 'make all' and 'make install' set_target_properties(${script_name} PROPERTIES EXCLUDE_FROM_ALL ON) - + # Configure target folder (for MSVC and Xcode) set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests/${groupName}") endforeach() @@ -188,16 +211,21 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) # Default on MSVC and XCode - combine test group into a single exectuable set(target_name check_${groupName}_program) - + # Add executable add_executable(${target_name} "${script_srcs}" ${script_headers}) target_link_libraries(${target_name} CppUnitLite ${linkLibraries}) - + + # Apply user build flags from CMake cache variables: + gtsam_apply_build_flags(${target_name}) + + set_property(TARGET check_${groupName}_program PROPERTY FOLDER "Unit tests") + # Only have a main function in one script - use preprocessor set(rest_script_srcs ${script_srcs}) list(REMOVE_AT rest_script_srcs 0) set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=inline no_main") - + # Add target dependencies add_test(NAME ${target_name} COMMAND ${target_name}) add_dependencies(check.${groupName} ${target_name}) @@ -205,10 +233,10 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) if(NOT XCODE_VERSION) add_dependencies(all.tests ${target_name}) endif() - + # Add TOPSRCDIR - set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") - + set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"") + # Exclude from 'make all' and 'make install' set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON) @@ -220,18 +248,18 @@ endmacro() macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName buildWithAll) - # Get all script files - file(GLOB script_files ${globPatterns}) + # Get all script files + file(GLOB script_files ${globPatterns}) - # Remove excluded scripts from the list - if(NOT "${excludedFiles}" STREQUAL "") + # Remove excluded scripts from the list + if(NOT "${excludedFiles}" STREQUAL "") file(GLOB excludedFilePaths ${excludedFiles}) if("${excludedFilePaths}" STREQUAL "") message(WARNING "The script exclusion pattern '${excludedFiles}' did not match any files") else() - list(REMOVE_ITEM script_files ${excludedFilePaths}) + list(REMOVE_ITEM script_files ${excludedFilePaths}) endif() - endif() + endif() # Separate into source files and headers (allows for adding headers to show up in # MSVC and Xcode projects). @@ -257,18 +285,21 @@ macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName b # Add executable add_executable(${script_name} ${script_src} ${script_headers}) target_link_libraries(${script_name} ${linkLibraries}) - + + # Apply user build flags from CMake cache variables: + gtsam_apply_build_flags(${script_name}) + # Add target dependencies add_dependencies(${groupName} ${script_name}) if(NOT MSVC AND NOT XCODE_VERSION) - add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name}) + add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name}) endif() # Add TOPSRCDIR - set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") + set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"") - # Exclude from all or not - note weird variable assignment because we're in a macro - set(buildWithAll_on ${buildWithAll}) + # Exclude from all or not - note weird variable assignment because we're in a macro + set(buildWithAll_on ${buildWithAll}) if(NOT buildWithAll_on) # Exclude from 'make all' and 'make install' set_target_properties("${script_name}" PROPERTIES EXCLUDE_FROM_ALL ON) diff --git a/cmake/README.md b/cmake/README.md index 34d1ffb525..7f38bbcf23 100644 --- a/cmake/README.md +++ b/cmake/README.md @@ -1,5 +1,4 @@ -GTSAMCMakeTools -=============== +# GTSAMCMakeTools This is the collection of GTSAM CMake tools that may be useful in external projects. The way to use this collection is by first making a find_package call: @@ -7,8 +6,7 @@ This is the collection of GTSAM CMake tools that may be useful in external proje which will add a directory containing the GTSAM CMake tools to the CMAKE_MODULE_PATH variable. After that, you may include the files you would like to use. These files and the functions they define are explained below. -GtsamBuildTypes ---------------- +## GtsamBuildTypes include(GtsamBuildTypes) @@ -17,8 +15,8 @@ Including this file immediately sets up the following build types and a drop-dow * `Debug` * `Release` * `RelWithDebInfo` -* `Profiling`: All optimizations enabled and minimal debug symbols -* `Timing`: Defines the symbol GTSAM_ENABLE_TIMING for using GTSAM timing instrumentation +* `Profiling`: All optimizations enabled and minimal debug symbols +* `Timing`: Defines the symbol GTSAM_ENABLE_TIMING for using GTSAM timing instrumentation It also configures several minor details, as follows: @@ -30,8 +28,7 @@ It defines the following functions: * `gtsam_assign_source_folders( [files] )` Organizes files in the IDE into folders to reflect the actual directory structure of those files. Folders will be determined relative to the current source folder when this function is called. * `gtsam_assign_all_source_folders()` Calls `gtsam_assign_source_folders` on all cpp, c, and h files recursively in the current source folder. -GtsamTesting ------------- +## GtsamTesting include(GtsamTesting) @@ -70,8 +67,7 @@ Defines two useful functions for creating CTest unit tests. Also immediately cr an empty string "" if nothing needs to be excluded. linkLibraries: The list of libraries to link to. -GtsamMatlabWrap ---------------- +## GtsamMatlabWrap include(GtsamMatlabWrap) @@ -97,8 +93,7 @@ Defines functions for generating MATLAB wrappers. Also immediately creates seve extraMexFlags: Any *additional* flags to pass to the compiler when building the wrap code. Normally, leave this empty. -GtsamMakeConfigFile -------------------- +## GtsamMakeConfigFile include(GtsamMakeConfigFile) diff --git a/cmake/cmake_uninstall.cmake.in b/cmake/cmake_uninstall.cmake.in new file mode 100644 index 0000000000..aa20a69a62 --- /dev/null +++ b/cmake/cmake_uninstall.cmake.in @@ -0,0 +1,27 @@ +# ----------------------------------------------- +# File that provides "make uninstall" target +# We use the file 'install_manifest.txt' +# ----------------------------------------------- +if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") + message(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"") +endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") + +file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) +string(REGEX REPLACE "\n" ";" files "${files}") +foreach(file ${files}) + message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"") + if(EXISTS "$ENV{DESTDIR}${file}") + exec_program( + "@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\"" + OUTPUT_VARIABLE rm_out + RETURN_VALUE rm_retval + ) + if(NOT "${rm_retval}" STREQUAL 0) + message(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"") + endif(NOT "${rm_retval}" STREQUAL 0) + else(EXISTS "$ENV{DESTDIR}${file}") + message(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.") + endif(EXISTS "$ENV{DESTDIR}${file}") +endforeach(file) + + diff --git a/cmake/example_cmake_find_gtsam/CMakeLists.txt b/cmake/example_cmake_find_gtsam/CMakeLists.txt new file mode 100644 index 0000000000..9a4be4d70c --- /dev/null +++ b/cmake/example_cmake_find_gtsam/CMakeLists.txt @@ -0,0 +1,17 @@ +# This file shows how to build and link a user project against GTSAM using CMake +################################################################################### +# To create your own project, replace "example" with the actual name of your project +cmake_minimum_required(VERSION 3.0) +project(example CXX) + +# Find GTSAM, either from a local build, or from a Debian/Ubuntu package. +find_package(GTSAM REQUIRED) + +add_executable(example + main.cpp +) + +# By using CMake exported targets, a simple "link" dependency introduces the +# include directories (-I) flags, links against Boost, and add any other +# required build flags (e.g. C++11, etc.) +target_link_libraries(example PRIVATE gtsam) diff --git a/cmake/example_cmake_find_gtsam/main.cpp b/cmake/example_cmake_find_gtsam/main.cpp new file mode 100644 index 0000000000..4d93e1b19f --- /dev/null +++ b/cmake/example_cmake_find_gtsam/main.cpp @@ -0,0 +1,127 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file Pose2SLAMExample.cpp + * @brief A 2D Pose SLAM example + * @date Oct 21, 2010 + * @author Yong Dian Jian + */ + +/** + * A simple 2D pose slam example + * - The robot moves in a 2 meter square + * - The robot moves 2 meters each step, turning 90 degrees after each step + * - The robot initially faces along the X axis (horizontal, to the right in 2D) + * - We have full odometry between pose + * - We have a loop closure constraint when the robot returns to the first position + */ + +// In planar SLAM example we use Pose2 variables (x, y, theta) to represent the robot poses +#include + +// We will use simple integer Keys to refer to the robot poses. +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Between factors for the relative motion described by odometry measurements. +// We will also use a Between Factor to encode the loop closure constraint +// Also, we will initialize the robot at the origin using a Prior factor. +#include +#include + +// When the factors are created, we will add them to a Factor Graph. As the factors we are using +// are nonlinear factors, we will need a Nonlinear Factor Graph. +#include + +// Finally, once all of the factors have been added to our factor graph, we will want to +// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. +// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the +// a Gauss-Newton solver +#include + +// Once the optimized values have been calculated, we can also calculate the marginal covariance +// of desired variables +#include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + + +using namespace std; +using namespace gtsam; + +int main(int argc, char** argv) { + + // 1. Create a factor graph container and add factors to it + NonlinearFactorGraph graph; + + // 2a. Add a prior on the first pose, setting it to the origin + // A prior factor consists of a mean and a noise model (covariance matrix) + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + graph.emplace_shared >(1, Pose2(0, 0, 0), priorNoise); + + // For simplicity, we will use the same noise model for odometry and loop closures + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + + // 2b. Add odometry factors + // Create odometry (Between) factors between consecutive poses + graph.emplace_shared >(1, 2, Pose2(2, 0, 0 ), model); + graph.emplace_shared >(2, 3, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(3, 4, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(4, 5, Pose2(2, 0, M_PI_2), model); + + // 2c. Add the loop closure constraint + // This factor encodes the fact that we have returned to the same pose. In real systems, + // these constraints may be identified in many ways, such as appearance-based techniques + // with camera images. We will use another Between Factor to enforce this constraint: + graph.emplace_shared >(5, 2, Pose2(2, 0, M_PI_2), model); + graph.print("\nFactor Graph:\n"); // print + + // 3. Create the data structure to hold the initialEstimate estimate to the solution + // For illustrative purposes, these have been deliberately set to incorrect values + Values initialEstimate; + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); + initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 )); + initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); + initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI )); + initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2)); + initialEstimate.print("\nInitial Estimate:\n"); // print + + // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer + // The optimizer accepts an optional set of configuration parameters, + // controlling things like convergence criteria, the type of linear + // system solver to use, and the amount of information displayed during + // optimization. We will set a few parameters as a demonstration. + GaussNewtonParams parameters; + // Stop iterating once the change in error between steps is less than this value + parameters.relativeErrorTol = 1e-5; + // Do not perform more than N iteration steps + parameters.maxIterations = 100; + // Create the optimizer ... + GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters); + // ... and optimize + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + + // 5. Calculate and print marginal covariances for all variables + cout.precision(3); + Marginals marginals(graph, result); + cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl; + cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl; + cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl; + cout << "x4 covariance:\n" << marginals.marginalCovariance(4) << endl; + cout << "x5 covariance:\n" << marginals.marginalCovariance(5) << endl; + + return 0; +} diff --git a/cmake/obsolete/GtsamTestingObsolete.cmake b/cmake/obsolete/GtsamTestingObsolete.cmake index f56d138e67..c90abfa6c4 100644 --- a/cmake/obsolete/GtsamTestingObsolete.cmake +++ b/cmake/obsolete/GtsamTestingObsolete.cmake @@ -1,22 +1,22 @@ -# Macro for adding categorized tests in a "tests" folder, with +# Macro for adding categorized tests in a "tests" folder, with # optional exclusion of tests and convenience library linking options -# +# # By default, all tests are linked with CppUnitLite and boost -# Arguments: +# Arguments: # - subdir The name of the category for this test # - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true) # - full_libs The main library to link against if not using convenience libraries -# - excluded_tests A list of test files that should not be compiled - use for debugging +# - excluded_tests A list of test files that should not be compiled - use for debugging function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests) # Subdirectory target for tests add_custom_target(check.${subdir} COMMAND ${CMAKE_CTEST_COMMAND} -C $ --output-on-failure) set(is_test TRUE) # Put check target in Visual Studio solution folder - file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") + file(RELATIVE_PATH relative_path "${GTSAM_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") set_property(TARGET check.${subdir} PROPERTY FOLDER "${relative_path}") - + # Link with CppUnitLite - pulled from gtsam installation list(APPEND local_libs CppUnitLite) list(APPEND full_libs CppUnitLite) @@ -29,15 +29,15 @@ function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests) ${is_test}) # Set all as tests endfunction() -# Macro for adding categorized timing scripts in a "tests" folder, with +# Macro for adding categorized timing scripts in a "tests" folder, with # optional exclusion of tests and convenience library linking options -# +# # By default, all tests are linked with boost -# Arguments: +# Arguments: # - subdir The name of the category for this timing script # - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true) # - full_libs The main library to link against if not using convenience libraries -# - excluded_srcs A list of timing files that should not be compiled - use for debugging +# - excluded_srcs A list of timing files that should not be compiled - use for debugging macro(gtsam_add_subdir_timing subdir local_libs full_libs excluded_srcs) # Subdirectory target for timing - does not actually execute the scripts add_custom_target(timing.${subdir}) @@ -60,11 +60,11 @@ endmacro() # - excluded_srcs A list of timing files that should not be compiled - use for debugging function(gtsam_add_executables pattern local_libs full_libs excluded_srcs) set(is_test FALSE) - + if(NOT excluded_srcs) set(excluded_srcs "") endif() - + # Build executables gtsam_add_grouped_scripts("" "${pattern}" "" "Executable" "${local_libs}" "${full_libs}" "${excluded_srcs}" ${is_test}) endfunction() @@ -73,7 +73,7 @@ endfunction() macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name local_libs full_libs excluded_srcs is_test) # Print warning about using this obsolete function message(AUTHOR_WARNING "Warning: Please see GtsamTesting.cmake - obsolete cmake cmake macro for creating unit tests, examples, and scripts was called. This will be removed in the future. The new macros are much easier anyway!!") - + # Get all script files set(script_files "") foreach(one_pattern ${pattern}) @@ -102,20 +102,20 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l list(APPEND script_srcs ${script_file}) endif() endforeach() - - + + # Add targets and dependencies for each script if(NOT "${group}" STREQUAL "") message(STATUS "Adding ${pretty_prefix_name}s in ${group}") endif() - + # Create exe's for each script, unless we're in SINGLE_TEST_EXE mode if(NOT is_test OR NOT GTSAM_SINGLE_TEST_EXE) foreach(script_src ${script_srcs}) get_filename_component(script_base ${script_src} NAME_WE) if (script_base) # Check for null filenames and headers set( script_bin ${script_base} ) - message(STATUS "Adding ${pretty_prefix_name} ${script_bin}") + message(STATUS "Adding ${pretty_prefix_name} ${script_bin}") add_executable(${script_bin} ${script_src} ${script_headers}) if(NOT "${target_prefix}" STREQUAL "") if(NOT "${group}" STREQUAL "") @@ -123,37 +123,37 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l endif() add_dependencies(${target_prefix} ${script_bin}) endif() - + # Add TOPSRCDIR - set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") + set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"") # Disable building during make all/install if (GTSAM_DISABLE_TESTS_ON_INSTALL) set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON) endif() - + if (is_test) add_test(NAME ${script_base} COMMAND ${script_bin}) endif() - + # Linking and dependendencies if (GTSAM_BUILD_CONVENIENCE_LIBRARIES) target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES}) else() target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES}) endif() - + # Add .run target if(NOT MSVC AND NOT XCODE_VERSION) add_custom_target(${script_bin}.run ${EXECUTABLE_OUTPUT_PATH}${script_bin} ${ARGN}) endif() - + # Set up Visual Studio folders - file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") + file(RELATIVE_PATH relative_path "${GTSAM_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}") endif() endforeach(script_src) - + if(MSVC) source_group("" FILES ${script_srcs} ${script_headers}) endif() @@ -166,28 +166,28 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l else() target_link_libraries(${script_bin} ${Boost_LIBRARIES} ${full_libs}) endif() - + # Only have a main function in one script set(rest_script_srcs ${script_srcs}) list(REMOVE_AT rest_script_srcs 0) set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=static no_main") - + # Add TOPSRCDIR - set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") - + set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"") + # Add test add_dependencies(${target_prefix}.${group} ${script_bin}) add_dependencies(${target_prefix} ${script_bin}) add_test(NAME ${target_prefix}.${group} COMMAND ${script_bin}) - + # Disable building during make all/install if (GTSAM_DISABLE_TESTS_ON_INSTALL) set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON) endif() - + # Set up Visual Studio folders if(MSVC) - file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") + file(RELATIVE_PATH relative_path "${GTSAM_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}") source_group("" FILES ${script_srcs} ${script_headers}) endif() diff --git a/cython/README.md b/cython/README.md index cb23b0d4a9..bc6e346d9d 100644 --- a/cython/README.md +++ b/cython/README.md @@ -1,7 +1,9 @@ +# Python Wrapper + This is the Cython/Python wrapper around the GTSAM C++ library. -INSTALL -======= +## Install + - if you want to build the gtsam python library for a specific python version (eg 2.7), use the `-DGTSAM_PYTHON_VERSION=2.7` option when running `cmake` otherwise the default interpreter will be used. - If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment) then the environment should be active while building gtsam. - This wrapper needs Cython(>=0.25.2), backports_abc>=0.5, and numpy. These can be installed as follows: @@ -27,8 +29,8 @@ export PYTHONPATH=$PYTHONPATH: - if you run `setup.py` from the build directory rather than the installation directory, the script will warn you with the message: `setup.py is being run from an unexpected location`. Before `make install` is run, not all the components of the package have been copied across, so running `setup.py` from the build directory would result in an incomplete package. -UNIT TESTS -========== +## Unit Tests + The Cython toolbox also has a small set of unit tests located in the test directory. To run them: @@ -37,11 +39,11 @@ test directory. To run them: python -m unittest discover ``` -WRITING YOUR OWN SCRIPTS -======================== +## Writing Your Own Scripts + See the tests for examples. -## Some important notes: +### Some Important Notes: - Vector/Matrix: + GTSAM expects double-precision floating point vectors and matrices. @@ -66,8 +68,8 @@ Examples: noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase) ``` -WRAPPING YOUR OWN PROJECT THAT USES GTSAM -========================================= +## Wrapping Your Own Project That Uses GTSAM + - Set PYTHONPATH to include ${GTSAM_CYTHON_INSTALL_PATH} + so that it can find gtsam Cython header: gtsam/gtsam.pxd @@ -88,8 +90,8 @@ wrap_and_install_library_cython("your_project_interface.h" #Optional: install_cython_scripts and install_cython_files. See GtsamCythonWrap.cmake. ``` -KNOWN ISSUES -============ +## KNOWN ISSUES + - Doesn't work with python3 installed from homebrew - size-related issue: can only wrap up to a certain number of classes: up to mEstimator! - Guess: 64 vs 32b? disutils Compiler flags? @@ -99,63 +101,55 @@ KNOWN ISSUES - support these constructors by default and declare "delete" for special classes? -TODO -===== -☐ allow duplication of parent' functions in child classes. Not allowed for now due to conflicts in Cython. -☐ a common header for boost shared_ptr? (Or wait until everything is switched to std::shared_ptr in gtsam?) -☐ inner namespaces ==> inner packages? -☐ Wrap fixed-size Matrices/Vectors? - - -Completed/Cancelled: -===== -✔ Fix Python tests: don't use " import * ": Bad style!!! @done (18-03-17 19:50) -✔ Unit tests for cython wrappers @done (18-03-17 18:45) -- simply compare generated files -✔ Wrap unstable @done (18-03-17 15:30) -✔ Unify cython/gtsam.h and the original gtsam.h @done (18-03-17 15:30) - ✔ 18-03-17: manage to unify the two versions by removing std container stubs from the matlab version,and keeping KeyList/KeyVector/KeySet as in the matlab version. Probably Cython 0.25 fixes the casting problem. - ✔ 06-03-17: manage to remove the requirements for default and copy constructors - ✘ 25-11-16: - Try to unify but failed. Main reasons are: Key/size_t, std containers, KeyVector/KeyList/KeySet. - Matlab doesn't need to know about Key, but I can't make Cython to ignore Key as it couldn't cast KeyVector, i.e. FastVector, to FastVector. -✘ Marginal and JointMarginal: revert changes @failed (17-03-17 11:00) -- Cython does need a default constructor! It produces cpp code like this: ```gtsam::JointMarginal __pyx_t_1;``` Users don't have to wrap this constructor, however. -✔ Convert input numpy Matrix/Vector to float dtype and storage order 'F' automatically, cannot crash! @done (15-03-17 13:00) -✔ Remove requirements.txt - Frank: don't bother with only 2 packages and a special case for eigency! @done (08-03-17 10:30) -✔ CMake install script @done (25-11-16 02:30) -✘ [REFACTOR] better name for uninstantiateClass: very vague!! @cancelled (25-11-16 02:30) -- lazy -✘ forward declaration? @cancelled (23-11-16 13:00) - nothing to do, seem to work? -✔ wrap VariableIndex: why is it in inference? If need to, shouldn't have constructors to specific FactorGraphs @done (23-11-16 13:00) -✔ Global functions @done (22-11-16 21:00) -✔ [REFACTOR] typesEqual --> isSameSignature @done (22-11-16 21:00) -✔ Proper overloads (constructors, static methods, methods) @done (20-11-16 21:00) -✔ Allow overloading methods. The current solution is annoying!!! @done (20-11-16 21:00) -✔ Casting from parent and grandparents @done (16-11-16 17:00) -✔ Allow overloading constructors. The current solution is annoying!!! @done (16-11-16 17:00) -✔ Support "print obj" @done (16-11-16 17:00) -✔ methods for FastVector: at, [], ... @done (16-11-16 17:00) -✔ Cython: Key and size_t: traits doesn't exist @done (16-09-12 18:34) -✔ KeyVector, KeyList, KeySet... @done (16-09-13 17:19) -✔ [Nice to have] parse typedef @done (16-09-13 17:19) -✔ ctypedef at correct places @done (16-09-12 18:34) -✔ expand template variable type in constructor/static methods? @done (16-09-12 18:34) -✔ NonlinearOptimizer: copy constructor deleted!!! @done (16-09-13 17:20) -✔ Value: no default constructor @done (16-09-13 17:20) -✔ ctypedef PriorFactor[Vector] PriorFactorVector @done (16-09-19 12:25) -✔ Delete duplicate methods in derived class @done (16-09-12 13:38) -✔ Fix return properly @done (16-09-11 17:14) - ✔ handle pair @done (16-09-11 17:14) -✔ Eigency: ambiguous call: A(const T&) A(const Vector& v) and Eigency A(Map[Vector]& v) @done (16-09-11 07:59) -✔ Eigency: Constructor: ambiguous construct from Vector/Matrix @done (16-09-11 07:59) -✔ Eigency: Fix method template of Vector/Matrix: template argument is [Vector] while arugment is Map[Vector] @done (16-09-11 08:22) -✔ Robust noise: copy assignment operator is deleted because of shared_ptr of the abstract Base class @done (16-09-10 09:05) -✘ Cython: Constructor: generate default constructor? (hack: if it's serializable?) @cancelled (16-09-13 17:20) -✘ Eigency: Map[] to Block @created(16-09-10 07:59) @cancelled (16-09-11 08:28) +### TODO + +- [ ] allow duplication of parent' functions in child classes. Not allowed for now due to conflicts in Cython. +- [ ] a common header for boost shared_ptr? (Or wait until everything is switched to std::shared_ptr in gtsam?) +- [ ] inner namespaces ==> inner packages? +- [ ] Wrap fixed-size Matrices/Vectors? + + +### Completed/Cancelled: + +- [x] Fix Python tests: don't use " import * ": Bad style!!! (18-03-17 19:50) +- [x] Unit tests for cython wrappers @done (18-03-17 18:45) -- simply compare generated files +- [x] Wrap unstable @done (18-03-17 15:30) +- [x] Unify cython/gtsam.h and the original gtsam.h @done (18-03-17 15:30) +- [x] 18-03-17: manage to unify the two versions by removing std container stubs from the matlab version,and keeping KeyList/KeyVector/KeySet as in the matlab version. Probably Cython 0.25 fixes the casting problem. +- [x] 06-03-17: manage to remove the requirements for default and copy constructors +- [ ] 25-11-16: Try to unify but failed. Main reasons are: Key/size_t, std containers, KeyVector/KeyList/KeySet. Matlab doesn't need to know about Key, but I can't make Cython to ignore Key as it couldn't cast KeyVector, i.e. FastVector, to FastVector. +- [ ] Marginal and JointMarginal: revert changes @failed (17-03-17 11:00) -- Cython does need a default constructor! It produces cpp code like this: ```gtsam::JointMarginal __pyx_t_1;``` Users don't have to wrap this constructor, however. +- [x] Convert input numpy Matrix/Vector to float dtype and storage order 'F' automatically, cannot crash! @done (15-03-17 13:00) +- [x] Remove requirements.txt - Frank: don't bother with only 2 packages and a special case for eigency! @done (08-03-17 10:30) +- [x] CMake install script @done (25-11-16 02:30) +- [ ] [REFACTOR] better name for uninstantiateClass: very vague!! @cancelled (25-11-16 02:30) -- lazy +- [ ] forward declaration? @cancelled (23-11-16 13:00) - nothing to do, seem to work? +- [x] wrap VariableIndex: why is it in inference? If need to, shouldn't have constructors to specific FactorGraphs @done (23-11-16 13:00) +- [x] Global functions @done (22-11-16 21:00) +- [x] [REFACTOR] typesEqual --> isSameSignature @done (22-11-16 21:00) +- [x] Proper overloads (constructors, static methods, methods) @done (20-11-16 21:00) +- [x] Allow overloading methods. The current solution is annoying!!! @done (20-11-16 21:00) +- [x] Casting from parent and grandparents @done (16-11-16 17:00) +- [x] Allow overloading constructors. The current solution is annoying!!! @done (16-11-16 17:00) +- [x] Support "print obj" @done (16-11-16 17:00) +- [x] methods for FastVector: at, [], ... @done (16-11-16 17:00) +- [x] Cython: Key and size_t: traits doesn't exist @done (16-09-12 18:34) +- [x] KeyVector, KeyList, KeySet... @done (16-09-13 17:19) +- [x] [Nice to have] parse typedef @done (16-09-13 17:19) +- [x] ctypedef at correct places @done (16-09-12 18:34) +- [x] expand template variable type in constructor/static methods? @done (16-09-12 18:34) +- [x] NonlinearOptimizer: copy constructor deleted!!! @done (16-09-13 17:20) +- [x] Value: no default constructor @done (16-09-13 17:20) +- [x] ctypedef PriorFactor[Vector] PriorFactorVector @done (16-09-19 12:25) +- [x] Delete duplicate methods in derived class @done (16-09-12 13:38) +- [x] Fix return properly @done (16-09-11 17:14) +- [x] handle pair @done (16-09-11 17:14) +- [x] Eigency: ambiguous call: A(const T&) A(const Vector& v) and Eigency A(Map[Vector]& v) @done (16-09-11 07:59) +- [x] Eigency: Constructor: ambiguous construct from Vector/Matrix @done (16-09-11 07:59) +- [x] Eigency: Fix method template of Vector/Matrix: template argument is [Vector] while arugment is Map[Vector] @done (16-09-11 08:22) +- [x] Robust noise: copy assignment operator is deleted because of shared_ptr of the abstract Base class @done (16-09-10 09:05) +- [ ] Cython: Constructor: generate default constructor? (hack: if it's serializable?) @cancelled (16-09-13 17:20) +- [ ] Eigency: Map[] to Block @created(16-09-10 07:59) @cancelled (16-09-11 08:28) - inference before symbolic/linear - what's the purpose of "virtual" ?? - -Installation: - ☐ Prerequisite: - - Users create venv and pip install requirements before compiling - - Wrap cython script in gtsam/cython folder - ☐ Install built module into venv? diff --git a/cython/gtsam_eigency/CMakeLists.txt b/cython/gtsam_eigency/CMakeLists.txt index 4bff567ebe..77bead834d 100644 --- a/cython/gtsam_eigency/CMakeLists.txt +++ b/cython/gtsam_eigency/CMakeLists.txt @@ -39,11 +39,11 @@ add_dependencies(cythonize_eigency cythonize_eigency_conversions cythonize_eigen # install install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} - DESTINATION ${GTSAM_CYTHON_INSTALL_PATH} + DESTINATION "${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}" PATTERN "CMakeLists.txt" EXCLUDE PATTERN "__init__.py.in" EXCLUDE) install(TARGETS cythonize_eigency_core cythonize_eigency_conversions - DESTINATION "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency") -install(FILES ${OUTPUT_DIR}/conversions_api.h DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency) + DESTINATION "${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}/gtsam_eigency") +install(FILES ${OUTPUT_DIR}/conversions_api.h DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}/gtsam_eigency) configure_file(__init__.py.in ${OUTPUT_DIR}/__init__.py) -install(FILES ${OUTPUT_DIR}/__init__.py DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency) +install(FILES ${OUTPUT_DIR}/__init__.py DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}/gtsam_eigency) diff --git a/cython/setup.py.in b/cython/setup.py.in index ed02372d80..c35e54079d 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -7,7 +7,7 @@ except ImportError: if 'SETUP_PY_NO_CHECK' not in os.environ: script_path = os.path.abspath(os.path.realpath(__file__)) - install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py'))) + install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}', 'setup.py'))) if script_path != install_path: print('setup.py is being run from an unexpected location: "{}"'.format(script_path)) print('please run `make install` and run the script from there') diff --git a/debian/README.md b/debian/README.md new file mode 100644 index 0000000000..74eb351cd8 --- /dev/null +++ b/debian/README.md @@ -0,0 +1,12 @@ +# How to build a GTSAM debian package + +To use the ``debuild`` command, install the ``devscripts`` package + + sudo apt install devscripts + +Change into the gtsam directory, then run: + + debuild -us -uc -j4 + +Adjust the ``-j4`` depending on how many CPUs you want to build on in +parallel. diff --git a/debian/changelog b/debian/changelog new file mode 100644 index 0000000000..ef5d5ab972 --- /dev/null +++ b/debian/changelog @@ -0,0 +1,5 @@ +gtsam (4.0.0-1berndpfrommer) bionic; urgency=medium + + * initial release + + -- Bernd Pfrommer Wed, 18 Jul 2018 20:36:44 -0400 diff --git a/debian/compat b/debian/compat new file mode 100644 index 0000000000..ec635144f6 --- /dev/null +++ b/debian/compat @@ -0,0 +1 @@ +9 diff --git a/debian/control b/debian/control new file mode 100644 index 0000000000..9a37e4377d --- /dev/null +++ b/debian/control @@ -0,0 +1,15 @@ +Source: gtsam +Section: libs +Priority: optional +Maintainer: Frank Dellaert +Uploaders: Jose Luis Blanco Claraco , Bernd Pfrommer +Build-Depends: cmake, libboost-all-dev (>= 1.58), libeigen3-dev, libtbb-dev, debhelper (>=9) +Standards-Version: 3.9.7 +Homepage: https://github.com/borglab/gtsam +Vcs-Browser: https://github.com/borglab/gtsam + +Package: libgtsam-dev +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends} +Description: Georgia Tech Smoothing and Mapping Library + gtsam: Georgia Tech Smoothing and Mapping library for SLAM type applications diff --git a/debian/copyright b/debian/copyright new file mode 100644 index 0000000000..c2f41d83d5 --- /dev/null +++ b/debian/copyright @@ -0,0 +1,15 @@ +Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ +Upstream-Name: gtsam +Source: https://bitbucket.org/gtborg/gtsam.git + +Files: * +Copyright: 2017, Frank Dellaert +License: BSD + +Files: gtsam/3rdparty/CCOLAMD/* +Copyright: 2005-2011, Univ. of Florida. Authors: Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert. http://www.cise.ufl.edu/research/sparse +License: GNU LESSER GENERAL PUBLIC LICENSE + +Files: gtsam/3rdparty/Eigen/* +Copyright: 2017, Multiple Authors +License: MPL2 diff --git a/debian/gtsam-docs.docs b/debian/gtsam-docs.docs new file mode 100644 index 0000000000..e69de29bb2 diff --git a/debian/rules b/debian/rules new file mode 100755 index 0000000000..6a8d21c00f --- /dev/null +++ b/debian/rules @@ -0,0 +1,25 @@ +#!/usr/bin/make -f +# See debhelper(7) (uncomment to enable) +# output every command that modifies files on the build system. +export DH_VERBOSE = 1 + + +# see FEATURE AREAS in dpkg-buildflags(1) +#export DEB_BUILD_MAINT_OPTIONS = hardening=+all + +# see ENVIRONMENT in dpkg-buildflags(1) +# package maintainers to append CFLAGS +#export DEB_CFLAGS_MAINT_APPEND = -Wall -pedantic +# package maintainers to append LDFLAGS +#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed + + +%: + dh $@ --parallel + + +# dh_make generated override targets +# This is example for Cmake (See https://bugs.debian.org/641051 ) +override_dh_auto_configure: + dh_auto_configure -- -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=/usr -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_WRAP=OFF -DGTSAM_BUILD_DOCS=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DGTSAM_INSTALL_GEOGRAPHICLIB=OFF -DGTSAM_BUILD_TYPE_POSTFIXES=OFF + diff --git a/debian/source/format b/debian/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/debian/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/doc/ImuFactor.lyx b/doc/ImuFactor.lyx index d9cd35584b..0922a3e9c7 100644 --- a/doc/ImuFactor.lyx +++ b/doc/ImuFactor.lyx @@ -398,7 +398,7 @@ The local coordinates and note that \begin_inset Formula \[ -\frac{d\Phi_{R_{0}}\left(\omega t\right)}{dt}\biggr\vert_{t=0}=\frac{dR_{0}\exp\left(\Skew{\omega t}\right)}{dt}\biggr\vert_{t=0}=R_{0}\Skew{\omega} +\frac{d\Phi_{R_{0}}\left(\omega t\right)}{dt}\biggr\vert_{t=0}=\frac{dR_{0}\exp\left(\Skew{\omega t}\right)}{dt}\biggr\vert_{t=0}=R_{0}\Skew{\omega t} \] \end_inset diff --git a/doc/ImuFactor.pdf b/doc/ImuFactor.pdf index f5a25f54f7..0b13c1f594 100644 Binary files a/doc/ImuFactor.pdf and b/doc/ImuFactor.pdf differ diff --git a/doc/trustregion.bib b/doc/trustregion.bib index 7fcd509f4f..7c87eef9b6 100644 --- a/doc/trustregion.bib +++ b/doc/trustregion.bib @@ -8,7 +8,6 @@ %% Saved with string encoding Unicode (UTF-8) - @webpage{Hauser06lecture, Author = {Raphael Hauser}, Date-Added = {2011-10-10 15:21:22 +0000}, @@ -16,11 +15,5 @@ @webpage{Hauser06lecture Title = {Lecture Notes on Unconstrained Optimization}, Url = {http://www.numerical.rl.ac.uk/nimg/oupartc/lectures/raphael/}, Year = {2006}, - Bdsk-Url-1 = {http://www.numerical.rl.ac.uk/nimg/oupartc/lectures/raphael/}, - Bdsk-File-1 = {YnBsaXN0MDDUAQIDBAUIJidUJHRvcFgkb2JqZWN0c1gkdmVyc2lvblkkYXJjaGl2ZXLRBgdUcm9vdIABqAkKFRYXGyIjVSRudWxs0wsMDQ4RFFpOUy5vYmplY3RzV05TLmtleXNWJGNsYXNzog8QgASABqISE4ACgAOAB1lhbGlhc0RhdGFccmVsYXRpdmVQYXRo0hgNGRpXTlMuZGF0YU8RAYwAAAAAAYwAAgAADE1hY2ludG9zaCBIRAAAAAAAAAAAAAAAAAAAAMpAsaxIKwAAAD/T8xBIYXVzZXIwNmxlY3R1cmUxAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQn+Sv+qSlgAAAAAAAAAAAAMAAgAACSAAAAAAAAAAAAAAAAAAAAAKTGl0ZXJhdHVyZQAQAAgAAMpA6ewAAAARAAgAAL/q2OYAAAABAAwAP9PzAAUCJwAAvuwAAgA5TWFjaW50b3NoIEhEOlVzZXJzOgByaWNoYXJkOgBMaXRlcmF0dXJlOgBIYXVzZXIwNmxlY3R1cmUxAAAOACIAEABIAGEAdQBzAGUAcgAwADYAbABlAGMAdAB1AHIAZQAxAA8AGgAMAE0AYQBjAGkAbgB0AG8AcwBoACAASABEABIAKVVzZXJzL3JpY2hhcmQvTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmUxAAATAAEvAAAVAAIADv//AACABdIcHR4fWCRjbGFzc2VzWiRjbGFzc25hbWWjHyAhXU5TTXV0YWJsZURhdGFWTlNEYXRhWE5TT2JqZWN0XxAkLi4vLi4vLi4vTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmUx0hwdJCWiJSFcTlNEaWN0aW9uYXJ5EgABhqBfEA9OU0tleWVkQXJjaGl2ZXIACAARABYAHwAoADIANQA6ADwARQBLAFIAXQBlAGwAbwBxAHMAdgB4AHoAfACGAJMAmACgAjACMgI3AkACSwJPAl0CZAJtApQCmQKcAqkCrgAAAAAAAAIBAAAAAAAAACgAAAAAAAAAAAAAAAAAAALA}, - Bdsk-File-2 = {YnBsaXN0MDDUAQIDBAUIJidUJHRvcFgkb2JqZWN0c1gkdmVyc2lvblkkYXJjaGl2ZXLRBgdUcm9vdIABqAkKFRYXGyIjVSRudWxs0wsMDQ4RFFpOUy5vYmplY3RzV05TLmtleXNWJGNsYXNzog8QgASABqISE4ACgAOAB1lhbGlhc0RhdGFccmVsYXRpdmVQYXRo0hgNGRpXTlMuZGF0YU8RAYwAAAAAAYwAAgAADE1hY2ludG9zaCBIRAAAAAAAAAAAAAAAAAAAAMpAsaxIKwAAAD/T8xBIYXVzZXIwNmxlY3R1cmUyAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQn+Xv+qSowAAAAAAAAAAAAMAAgAACSAAAAAAAAAAAAAAAAAAAAAKTGl0ZXJhdHVyZQAQAAgAAMpA6ewAAAARAAgAAL/q2PMAAAABAAwAP9PzAAUCJwAAvuwAAgA5TWFjaW50b3NoIEhEOlVzZXJzOgByaWNoYXJkOgBMaXRlcmF0dXJlOgBIYXVzZXIwNmxlY3R1cmUyAAAOACIAEABIAGEAdQBzAGUAcgAwADYAbABlAGMAdAB1AHIAZQAyAA8AGgAMAE0AYQBjAGkAbgB0AG8AcwBoACAASABEABIAKVVzZXJzL3JpY2hhcmQvTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmUyAAATAAEvAAAVAAIADv//AACABdIcHR4fWCRjbGFzc2VzWiRjbGFzc25hbWWjHyAhXU5TTXV0YWJsZURhdGFWTlNEYXRhWE5TT2JqZWN0XxAkLi4vLi4vLi4vTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmUy0hwdJCWiJSFcTlNEaWN0aW9uYXJ5EgABhqBfEA9OU0tleWVkQXJjaGl2ZXIACAARABYAHwAoADIANQA6ADwARQBLAFIAXQBlAGwAbwBxAHMAdgB4AHoAfACGAJMAmACgAjACMgI3AkACSwJPAl0CZAJtApQCmQKcAqkCrgAAAAAAAAIBAAAAAAAAACgAAAAAAAAAAAAAAAAAAALA}, - Bdsk-File-3 = {YnBsaXN0MDDUAQIDBAUIJidUJHRvcFgkb2JqZWN0c1gkdmVyc2lvblkkYXJjaGl2ZXLRBgdUcm9vdIABqAkKFRYXGyIjVSRudWxs0wsMDQ4RFFpOUy5vYmplY3RzV05TLmtleXNWJGNsYXNzog8QgASABqISE4ACgAOAB1lhbGlhc0RhdGFccmVsYXRpdmVQYXRo0hgNGRpXTlMuZGF0YU8RAYwAAAAAAYwAAgAADE1hY2ludG9zaCBIRAAAAAAAAAAAAAAAAAAAAMpAsaxIKwAAAD/T8xBIYXVzZXIwNmxlY3R1cmUzAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQn+mv+qSpQAAAAAAAAAAAAMAAgAACSAAAAAAAAAAAAAAAAAAAAAKTGl0ZXJhdHVyZQAQAAgAAMpA6ewAAAARAAgAAL/q2PUAAAABAAwAP9PzAAUCJwAAvuwAAgA5TWFjaW50b3NoIEhEOlVzZXJzOgByaWNoYXJkOgBMaXRlcmF0dXJlOgBIYXVzZXIwNmxlY3R1cmUzAAAOACIAEABIAGEAdQBzAGUAcgAwADYAbABlAGMAdAB1AHIAZQAzAA8AGgAMAE0AYQBjAGkAbgB0AG8AcwBoACAASABEABIAKVVzZXJzL3JpY2hhcmQvTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmUzAAATAAEvAAAVAAIADv//AACABdIcHR4fWCRjbGFzc2VzWiRjbGFzc25hbWWjHyAhXU5TTXV0YWJsZURhdGFWTlNEYXRhWE5TT2JqZWN0XxAkLi4vLi4vLi4vTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmUz0hwdJCWiJSFcTlNEaWN0aW9uYXJ5EgABhqBfEA9OU0tleWVkQXJjaGl2ZXIACAARABYAHwAoADIANQA6ADwARQBLAFIAXQBlAGwAbwBxAHMAdgB4AHoAfACGAJMAmACgAjACMgI3AkACSwJPAl0CZAJtApQCmQKcAqkCrgAAAAAAAAIBAAAAAAAAACgAAAAAAAAAAAAAAAAAAALA}, - Bdsk-File-4 = {YnBsaXN0MDDUAQIDBAUIJidUJHRvcFgkb2JqZWN0c1gkdmVyc2lvblkkYXJjaGl2ZXLRBgdUcm9vdIABqAkKFRYXGyIjVSRudWxs0wsMDQ4RFFpOUy5vYmplY3RzV05TLmtleXNWJGNsYXNzog8QgASABqISE4ACgAOAB1lhbGlhc0RhdGFccmVsYXRpdmVQYXRo0hgNGRpXTlMuZGF0YU8RAYwAAAAAAYwAAgAADE1hY2ludG9zaCBIRAAAAAAAAAAAAAAAAAAAAMpAsaxIKwAAAD/T8xBIYXVzZXIwNmxlY3R1cmU0AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQn+vv+qSpwAAAAAAAAAAAAMAAgAACSAAAAAAAAAAAAAAAAAAAAAKTGl0ZXJhdHVyZQAQAAgAAMpA6ewAAAARAAgAAL/q2PcAAAABAAwAP9PzAAUCJwAAvuwAAgA5TWFjaW50b3NoIEhEOlVzZXJzOgByaWNoYXJkOgBMaXRlcmF0dXJlOgBIYXVzZXIwNmxlY3R1cmU0AAAOACIAEABIAGEAdQBzAGUAcgAwADYAbABlAGMAdAB1AHIAZQA0AA8AGgAMAE0AYQBjAGkAbgB0AG8AcwBoACAASABEABIAKVVzZXJzL3JpY2hhcmQvTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU0AAATAAEvAAAVAAIADv//AACABdIcHR4fWCRjbGFzc2VzWiRjbGFzc25hbWWjHyAhXU5TTXV0YWJsZURhdGFWTlNEYXRhWE5TT2JqZWN0XxAkLi4vLi4vLi4vTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU00hwdJCWiJSFcTlNEaWN0aW9uYXJ5EgABhqBfEA9OU0tleWVkQXJjaGl2ZXIACAARABYAHwAoADIANQA6ADwARQBLAFIAXQBlAGwAbwBxAHMAdgB4AHoAfACGAJMAmACgAjACMgI3AkACSwJPAl0CZAJtApQCmQKcAqkCrgAAAAAAAAIBAAAAAAAAACgAAAAAAAAAAAAAAAAAAALA}, - Bdsk-File-5 = {YnBsaXN0MDDUAQIDBAUIJidUJHRvcFgkb2JqZWN0c1gkdmVyc2lvblkkYXJjaGl2ZXLRBgdUcm9vdIABqAkKFRYXGyIjVSRudWxs0wsMDQ4RFFpOUy5vYmplY3RzV05TLmtleXNWJGNsYXNzog8QgASABqISE4ACgAOAB1lhbGlhc0RhdGFccmVsYXRpdmVQYXRo0hgNGRpXTlMuZGF0YU8RAYwAAAAAAYwAAgAADE1hY2ludG9zaCBIRAAAAAAAAAAAAAAAAAAAAMpAsaxIKwAAAD/T8xBIYXVzZXIwNmxlY3R1cmU1AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQn+5v+qSqAAAAAAAAAAAAAMAAgAACSAAAAAAAAAAAAAAAAAAAAAKTGl0ZXJhdHVyZQAQAAgAAMpA6ewAAAARAAgAAL/q2PgAAAABAAwAP9PzAAUCJwAAvuwAAgA5TWFjaW50b3NoIEhEOlVzZXJzOgByaWNoYXJkOgBMaXRlcmF0dXJlOgBIYXVzZXIwNmxlY3R1cmU1AAAOACIAEABIAGEAdQBzAGUAcgAwADYAbABlAGMAdAB1AHIAZQA1AA8AGgAMAE0AYQBjAGkAbgB0AG8AcwBoACAASABEABIAKVVzZXJzL3JpY2hhcmQvTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU1AAATAAEvAAAVAAIADv//AACABdIcHR4fWCRjbGFzc2VzWiRjbGFzc25hbWWjHyAhXU5TTXV0YWJsZURhdGFWTlNEYXRhWE5TT2JqZWN0XxAkLi4vLi4vLi4vTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU10hwdJCWiJSFcTlNEaWN0aW9uYXJ5EgABhqBfEA9OU0tleWVkQXJjaGl2ZXIACAARABYAHwAoADIANQA6ADwARQBLAFIAXQBlAGwAbwBxAHMAdgB4AHoAfACGAJMAmACgAjACMgI3AkACSwJPAl0CZAJtApQCmQKcAqkCrgAAAAAAAAIBAAAAAAAAACgAAAAAAAAAAAAAAAAAAALA}, - Bdsk-File-6 = {YnBsaXN0MDDUAQIDBAUIJidUJHRvcFgkb2JqZWN0c1gkdmVyc2lvblkkYXJjaGl2ZXLRBgdUcm9vdIABqAkKFRYXGyIjVSRudWxs0wsMDQ4RFFpOUy5vYmplY3RzV05TLmtleXNWJGNsYXNzog8QgASABqISE4ACgAOAB1lhbGlhc0RhdGFccmVsYXRpdmVQYXRo0hgNGRpXTlMuZGF0YU8RAYwAAAAAAYwAAgAADE1hY2ludG9zaCBIRAAAAAAAAAAAAAAAAAAAAMpAsaxIKwAAAD/T8xBIYXVzZXIwNmxlY3R1cmU2AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQn/Fv+qSqgAAAAAAAAAAAAMAAgAACSAAAAAAAAAAAAAAAAAAAAAKTGl0ZXJhdHVyZQAQAAgAAMpA6ewAAAARAAgAAL/q2PoAAAABAAwAP9PzAAUCJwAAvuwAAgA5TWFjaW50b3NoIEhEOlVzZXJzOgByaWNoYXJkOgBMaXRlcmF0dXJlOgBIYXVzZXIwNmxlY3R1cmU2AAAOACIAEABIAGEAdQBzAGUAcgAwADYAbABlAGMAdAB1AHIAZQA2AA8AGgAMAE0AYQBjAGkAbgB0AG8AcwBoACAASABEABIAKVVzZXJzL3JpY2hhcmQvTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU2AAATAAEvAAAVAAIADv//AACABdIcHR4fWCRjbGFzc2VzWiRjbGFzc25hbWWjHyAhXU5TTXV0YWJsZURhdGFWTlNEYXRhWE5TT2JqZWN0XxAkLi4vLi4vLi4vTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU20hwdJCWiJSFcTlNEaWN0aW9uYXJ5EgABhqBfEA9OU0tleWVkQXJjaGl2ZXIACAARABYAHwAoADIANQA6ADwARQBLAFIAXQBlAGwAbwBxAHMAdgB4AHoAfACGAJMAmACgAjACMgI3AkACSwJPAl0CZAJtApQCmQKcAqkCrgAAAAAAAAIBAAAAAAAAACgAAAAAAAAAAAAAAAAAAALA}, - Bdsk-File-7 = {YnBsaXN0MDDUAQIDBAUIJidUJHRvcFgkb2JqZWN0c1gkdmVyc2lvblkkYXJjaGl2ZXLRBgdUcm9vdIABqAkKFRYXGyIjVSRudWxs0wsMDQ4RFFpOUy5vYmplY3RzV05TLmtleXNWJGNsYXNzog8QgASABqISE4ACgAOAB1lhbGlhc0RhdGFccmVsYXRpdmVQYXRo0hgNGRpXTlMuZGF0YU8RAYwAAAAAAYwAAgAADE1hY2ludG9zaCBIRAAAAAAAAAAAAAAAAAAAAMpAsaxIKwAAAD/T8xBIYXVzZXIwNmxlY3R1cmU3AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQn/Pv+qSrAAAAAAAAAAAAAMAAgAACSAAAAAAAAAAAAAAAAAAAAAKTGl0ZXJhdHVyZQAQAAgAAMpA6ewAAAARAAgAAL/q2PwAAAABAAwAP9PzAAUCJwAAvuwAAgA5TWFjaW50b3NoIEhEOlVzZXJzOgByaWNoYXJkOgBMaXRlcmF0dXJlOgBIYXVzZXIwNmxlY3R1cmU3AAAOACIAEABIAGEAdQBzAGUAcgAwADYAbABlAGMAdAB1AHIAZQA3AA8AGgAMAE0AYQBjAGkAbgB0AG8AcwBoACAASABEABIAKVVzZXJzL3JpY2hhcmQvTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU3AAATAAEvAAAVAAIADv//AACABdIcHR4fWCRjbGFzc2VzWiRjbGFzc25hbWWjHyAhXU5TTXV0YWJsZURhdGFWTlNEYXRhWE5TT2JqZWN0XxAkLi4vLi4vLi4vTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU30hwdJCWiJSFcTlNEaWN0aW9uYXJ5EgABhqBfEA9OU0tleWVkQXJjaGl2ZXIACAARABYAHwAoADIANQA6ADwARQBLAFIAXQBlAGwAbwBxAHMAdgB4AHoAfACGAJMAmACgAjACMgI3AkACSwJPAl0CZAJtApQCmQKcAqkCrgAAAAAAAAIBAAAAAAAAACgAAAAAAAAAAAAAAAAAAALA}} + howpublished = {\href{http://www.numerical.rl.ac.uk/nimg/oupartc/lectures/raphael/}{link}}, +} diff --git a/doc/trustregion.lyx b/doc/trustregion.lyx index bc4393fdf7..97bc6ec74f 100644 --- a/doc/trustregion.lyx +++ b/doc/trustregion.lyx @@ -1,10 +1,11 @@ -#LyX 2.0 created this file. For more info see http://www.lyx.org/ -\lyxformat 413 +#LyX 2.1 created this file. For more info see http://www.lyx.org/ +\lyxformat 474 \begin_document \begin_header \textclass article \begin_preamble -\usepackage{amssymb} +\usepackage{url} +\usepackage{hyperref} \end_preamble \use_default_options true \maintain_unincluded_children false @@ -15,13 +16,13 @@ \font_roman default \font_sans default \font_typewriter default +\font_math auto \font_default_family default \use_non_tex_fonts false \font_sc false \font_osf false \font_sf_scale 100 \font_tt_scale 100 - \graphics default \default_output_format default \output_sync 0 @@ -32,15 +33,24 @@ \use_hyperref false \papersize default \use_geometry false -\use_amsmath 1 -\use_esint 1 -\use_mhchem 1 -\use_mathdots 1 +\use_package amsmath 1 +\use_package amssymb 2 +\use_package cancel 1 +\use_package esint 1 +\use_package mathdots 1 +\use_package mathtools 1 +\use_package mhchem 1 +\use_package stackrel 0 +\use_package stmaryrd 1 +\use_package undertilde 1 \cite_engine basic +\cite_engine_type default +\biblio_style plain \use_bibtopic false \use_indices false \paperorientation portrait \suppress_date false +\justification true \use_refstyle 1 \index Index \shortcut idx @@ -231,7 +241,7 @@ key "Hauser06lecture" \end_inset - (in our /net/hp223/borg/Literature folder). +. \end_layout \begin_layout Standard @@ -465,22 +475,39 @@ where \end_inset . - A typical update rule [ -\color blue -see where this came from in paper -\color inherit -] is + A typical update rule, as per Lec. + 7-1.2 of +\begin_inset CommandInset citation +LatexCommand cite +key "Hauser06lecture" + +\end_inset + + is: \begin_inset Formula \[ -\Delta\leftarrow\begin{cases} -\max\left(\Delta,3\norm{\delta x_{d}}\right)\text{,} & \rho>0.75\\ -\Delta & 0.75>\rho>0.25\\ -\Delta/2 & 0.25>\rho +\Delta_{k+1}\leftarrow\begin{cases} +\Delta_{k}/4 & \rho<0.25\\ +\min\left(2\Delta_{k},\Delta_{max}\right)\text{,} & \rho>0.75\\ +\Delta_{k} & 0.75>\rho>0.25 \end{cases} \] \end_inset +where +\begin_inset Formula $\Delta_{k}\triangleq\norm{\delta x_{d}}$ +\end_inset + +. + Note that the rule is designed to ensure that +\begin_inset Formula $\Delta_{k}$ +\end_inset + +never exceeds the maximum trust region size +\begin_inset Formula $\Delta_{max}.$ +\end_inset + \end_layout @@ -593,9 +620,9 @@ To find the intersection of the line between \begin{align*} \Delta & =\norm{\left(1-\tau\right)\delta x_{u}+\tau\delta x_{n}}\\ \Delta^{2} & =\left(1-\tau\right)^{2}\delta x_{u}^{\t}\delta x_{u}+2\tau\left(1-\tau\right)\delta x_{u}^{\t}\delta x_{n}+\tau^{2}\delta x_{n}^{\t}\delta x_{n}\\ -0 & =uu-2\tau uu+\tau^{2}uu+2\tau un-2\tau^{2}un+\tau^{2}nn-\Delta^{2}\\ -0 & =\left(uu-2un+nn\right)\tau^{2}+\left(2un-2uu\right)\tau-\Delta^{2}+uu\\ -\tau & =\frac{-\left(2un-2uu\right)\pm\sqrt{\left(2un-2uu\right)^{2}-4\left(uu-2un+nn\right)\left(uu-\Delta^{2}\right)}}{2\left(uu-un+nn\right)} +0 & =\delta x_{u}^{\t}\delta x_{u}-2\tau\delta x_{u}^{\t}\delta x_{u}+\tau^{2}\delta x_{u}^{\t}\delta x_{u}+2\tau\delta x_{u}^{\t}\delta x_{n}-2\tau^{2}\delta x_{u}^{\t}\delta x_{n}+\tau^{2}\delta x_{n}^{\t}\delta x_{n}-\Delta^{2}\\ +0 & =\left(\delta x_{u}^{\t}\delta x_{u}-2\delta x_{u}^{\t}\delta x_{n}+\delta x_{n}^{\t}\delta x_{n}\right)\tau^{2}+\left(2\delta x_{u}^{\t}\delta x_{n}-2\delta x_{u}^{\t}\delta x_{u}\right)\tau-\Delta^{2}+\delta x_{u}^{\t}\delta x_{u}\\ +\tau & =\frac{-\left(2\delta x_{u}^{\t}\delta x_{n}-2\delta x_{u}^{\t}\delta x_{u}\right)\pm\sqrt{\left(2\delta x_{u}^{\t}\delta x_{n}-2\delta x_{u}^{\t}\delta x_{u}\right)^{2}-4\left(\delta x_{u}^{\t}\delta x_{u}-2\delta x_{u}^{\t}\delta x_{n}+\delta x_{n}^{\t}\delta x_{n}\right)\left(\delta x_{u}^{\t}\delta x_{u}-\Delta^{2}\right)}}{2\left(\delta x_{u}^{\t}\delta x_{u}-\delta x_{u}^{\t}\delta x_{n}+\delta x_{n}^{\t}\delta x_{n}\right)} \end{align*} \end_inset @@ -641,7 +668,7 @@ Thus, mathematically, we can write the dogleg update \begin_inset Formula \[ \delta x_{d}^{\left(k\right)}=\begin{cases} --\frac{\Delta}{\norm{g^{\left(k\right)}}}g^{\left(k\right)}\text{,} & \Delta<\norm{\delta x_{u}^{\left(k\right)}}\\ +-\frac{\Delta}{\norm{\delta x_{u}^{\left(k\right)}}}\delta x_{u}^{\left(k\right)}\text{,} & \Delta<\norm{\delta x_{u}^{\left(k\right)}}\\ \left(1-\tau^{\left(k\right)}\right)\delta x_{u}^{\left(k\right)}+\tau^{\left(k\right)}\delta x_{n}^{\left(k\right)}\text{,} & \norm{\delta x_{u}^{\left(k\right)}}<\Delta<\norm{\delta x_{n}^{\left(k\right)}}\\ \delta x_{n}^{\left(k\right)}\text{,} & \norm{\delta x_{n}^{\left(k\right)}}<\Delta \end{cases} diff --git a/doc/trustregion.pdf b/doc/trustregion.pdf index 31ca0c5b10..22dcac035f 100644 Binary files a/doc/trustregion.pdf and b/doc/trustregion.pdf differ diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 032d61a4af..7b1667e122 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -108,7 +108,7 @@ int main (int argc, char** argv) { // Set Noise parameters Vector priorSigmas = Vector3(1,1,M_PI); - Vector odoSigmas = Vector3(0.05, 0.01, 0.2); + Vector odoSigmas = Vector3(0.05, 0.01, 0.1); double sigmaR = 100; // range standard deviation const NM::Base::shared_ptr // all same type priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior @@ -157,7 +157,7 @@ int main (int argc, char** argv) { boost::tie(t, odometry) = timedOdometry; // add odometry factor - newFactors.push_back(BetweenFactor(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas))); + newFactors.push_back(BetweenFactor(i-1, i, odometry, odoNoise)); // predict pose and add as initial estimate Pose2 predictedPose = lastPose.compose(odometry); diff --git a/gtsam.h b/gtsam.h index d40a109b39..bf35755809 100644 --- a/gtsam.h +++ b/gtsam.h @@ -248,10 +248,13 @@ class FactorIndices { /** gtsam namespace functions */ +#include +bool isDebugVersion(); + #include -class IndexPair { - IndexPair(); - IndexPair(size_t i, size_t j); +class IndexPair { + IndexPair(); + IndexPair(size_t i, size_t j); size_t i() const; size_t j() const; }; @@ -1385,6 +1388,15 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { void serializable() const; }; +virtual class Welsch: gtsam::noiseModel::mEstimator::Base { + Welsch(double k); + static gtsam::noiseModel::mEstimator::Welsch* Create(double k); + + // enabling serialization functionality + void serializable() const; +}; + + }///\namespace mEstimator virtual class Robust : gtsam::noiseModel::Base { @@ -1886,6 +1898,7 @@ class NonlinearFactorGraph { gtsam::KeyVector keyVector() const; // NonlinearFactorGraph + void printErrors(const gtsam::Values& values) const; double error(const gtsam::Values& values) const; double probPrime(const gtsam::Values& values) const; gtsam::Ordering orderingCOLAMD() const; @@ -2495,6 +2508,9 @@ virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, const CALIBRATION* K, const gtsam::Pose3& body_P_sensor); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::SmartProjectionParams& params); SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, const CALIBRATION* K, const gtsam::Pose3& body_P_sensor, diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 9a99bdba85..ed18b7aad2 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -16,6 +16,21 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endif() endforeach(eigen_dir) + if(GTSAM_WITH_EIGEN_UNSUPPORTED) + message("-- Installing Eigen Unsupported modules") + # do the same for the unsupported eigen folder + file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h") + + file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*") + foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all}) + get_filename_component(filename ${unsupported_eigen_dir} NAME) + if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES "CXX11") OR (${filename} MATCHES ".svn"))) + list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}") + install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/unsupported/Eigen) + endif() + endforeach(unsupported_eigen_dir) + endif() + # Add to project source set(eigen_headers ${eigen_headers} PARENT_SCOPE) # set(unsupported_eigen_headers ${unsupported_eigen_headers} PARENT_SCOPE) @@ -24,6 +39,13 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) install(DIRECTORY Eigen/Eigen DESTINATION include/gtsam/3rdparty/Eigen FILES_MATCHING PATTERN "*.h") + + if(GTSAM_WITH_EIGEN_UNSUPPORTED) + install(DIRECTORY Eigen/unsupported/Eigen + DESTINATION include/gtsam/3rdparty/Eigen/unsupported/ + FILES_MATCHING PATTERN "*.h") + endif() + endif() option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 60915eeadf..b4a33943e5 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -1,3 +1,5 @@ +project(gtsam LANGUAGES CXX) + # We split the library in to separate subfolders, each containing # tests, timing, and an optional convenience library. # The following variable is the master list of subdirs to add @@ -82,9 +84,9 @@ ENDIF(MSVC) # Generate and install config and dllexport files configure_file(config.h.in config.h) set(library_name GTSAM) # For substitution in dllexport.h.in -configure_file("${PROJECT_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") -list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h") -install(FILES "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h" DESTINATION include/gtsam) +configure_file("${GTSAM_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") +list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h") +install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION include/gtsam) if(GTSAM_SUPPORT_NESTED_DISSECTION) list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis) @@ -101,17 +103,16 @@ message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}") # BUILD_SHARED_LIBS automatically defines static/shared libs: add_library(gtsam ${gtsam_srcs}) + # Boost: target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES}) target_include_directories(gtsam PUBLIC ${Boost_INCLUDE_DIR}) # Other deps: target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES}) -target_compile_definitions(gtsam PRIVATE ${GTSAM_COMPILE_DEFINITIONS_PRIVATE}) -target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS_PUBLIC}) -if (NOT "${GTSAM_COMPILE_OPTIONS_PUBLIC}" STREQUAL "") - target_compile_options(gtsam PUBLIC ${GTSAM_COMPILE_OPTIONS_PUBLIC}) -endif() -target_compile_options(gtsam PRIVATE ${GTSAM_COMPILE_OPTIONS_PRIVATE}) + +# Apply build flags: +gtsam_apply_build_flags(gtsam) + set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 @@ -171,7 +172,7 @@ if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with set_target_properties(gtsam PROPERTIES PREFIX "" DEFINE_SYMBOL GTSAM_EXPORTS - RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin") + RUNTIME_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/bin") endif() endif() diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 235cd30f35..2910ce74c9 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -90,7 +90,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBas for(size_t j=0; j tol) + else if(std::abs(A(i,j) - B(i,j)) > tol) return false; } return true; @@ -299,7 +299,7 @@ GTSAM_EXPORT std::pair qr(const Matrix& A); * @param A is the input matrix, and is the output * @param clear_below_diagonal enables zeroing out below diagonal */ -void inplace_QR(Matrix& A); +GTSAM_EXPORT void inplace_QR(Matrix& A); /** * Imperative algorithm for in-place full elimination with diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 856b559c37..ac03c0f53f 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -84,7 +84,7 @@ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) { for(size_t i=0; i tol) + if(std::abs(vec1[i] - vec2[i]) > tol) return false; } return true; @@ -97,7 +97,7 @@ bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol for(size_t i=0; i tol) + if(std::abs(vec1[i] - vec2[i]) > tol) return false; } return true; @@ -145,14 +145,14 @@ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) { bool flag = false; double scale = 1.0; size_t m = vec1.size(); for(size_t i=0; itol&&fabs(vec2[i])tol)) + if((std::abs(vec1[i])>tol && std::abs(vec2[i])tol)) return false; if(vec1[i] == 0 && vec2[i] == 0) continue; if (!flag) { scale = vec1[i] / vec2[i]; flag = true ; } - else if (fabs(vec1[i] - vec2[i]*scale) > tol) return false; + else if (std::abs(vec1[i] - vec2[i]*scale) > tol) return false; } return flag; } @@ -213,7 +213,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights, // Check once for zero entries of a. TODO: really needed ? vector isZero; - for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9); + for (size_t i = 0; i < m; ++i) isZero.push_back(std::abs(a[i]) < 1e-9); // If there is a valid (a!=0) constraint (sigma==0) return the first one for (size_t i = 0; i < m; ++i) { diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 91aec85b88..99c5a4d425 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -63,6 +63,18 @@ GTSAM_MAKE_VECTOR_DEFS(12); typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; +/** + * Ensure we are not including a different version of Eigen in user code than + * while compiling gtsam, since it can lead to hard-to-understand runtime + * crashes. + */ +#if defined(GTSAM_EIGEN_VERSION_WORLD) +static_assert( + GTSAM_EIGEN_VERSION_WORLD==EIGEN_WORLD_VERSION && + GTSAM_EIGEN_VERSION_MAJOR==EIGEN_MAJOR_VERSION, + "Error: GTSAM was built against a different version of Eigen"); +#endif + /** * print without optional string, must specify cout yourself */ diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 31e4b82442..17cb291f02 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -111,7 +111,7 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) { return true; assert(ABC.cols() == ABC.rows()); - assert(ABC.rows() >= topleft); + assert(size_t(ABC.rows()) >= topleft); const size_t n = static_cast(ABC.rows() - topleft); assert(nFrontal <= size_t(n)); diff --git a/gtsam/base/debug.cpp b/gtsam/base/debug.cpp index 1c4d08dcdc..d6d2a4fe02 100644 --- a/gtsam/base/debug.cpp +++ b/gtsam/base/debug.cpp @@ -47,4 +47,15 @@ void guardedSetDebug(const std::string& s, const bool v) { gtsam::debugFlags[s] = v; } +bool isDebugVersion() { +#ifdef NDEBUG + // nondebug + return false; +#else + // debug + return true; +#endif + +} + } diff --git a/gtsam/base/debug.h b/gtsam/base/debug.h index e21efcc831..5f42580b1f 100644 --- a/gtsam/base/debug.h +++ b/gtsam/base/debug.h @@ -47,6 +47,9 @@ namespace gtsam { // Non-guarded use led to crashes, and solved in commit cd35db2 bool GTSAM_EXPORT guardedIsDebug(const std::string& s); void GTSAM_EXPORT guardedSetDebug(const std::string& s, const bool v); + + // function to check if compiled version has debug information + bool GTSAM_EXPORT isDebugVersion(); } #undef ISDEBUG diff --git a/gtsam/base/deprecated/LieMatrix.h b/gtsam/base/deprecated/LieMatrix.h index 953537bf7d..a3d0a43289 100644 --- a/gtsam/base/deprecated/LieMatrix.h +++ b/gtsam/base/deprecated/LieMatrix.h @@ -29,7 +29,7 @@ namespace gtsam { * we can directly add double, Vector, and Matrix into values now, because of * gtsam::traits. */ -struct GTSAM_EXPORT LieMatrix : public Matrix { +struct LieMatrix : public Matrix { /// @name Constructors /// @{ diff --git a/gtsam/base/deprecated/LieScalar.h b/gtsam/base/deprecated/LieScalar.h index 50ea9d6957..6c9a5f766d 100644 --- a/gtsam/base/deprecated/LieScalar.h +++ b/gtsam/base/deprecated/LieScalar.h @@ -28,7 +28,7 @@ namespace gtsam { * we can directly add double, Vector, and Matrix into values now, because of * gtsam::traits. */ - struct GTSAM_EXPORT LieScalar { + struct LieScalar { enum { dimension = 1 }; @@ -53,7 +53,7 @@ namespace gtsam { std::cout << name << ": " << d_ << std::endl; } bool equals(const LieScalar& expected, double tol = 1e-5) const { - return fabs(expected.d_ - d_) <= tol; + return std::abs(expected.d_ - d_) <= tol; } /// @} diff --git a/gtsam/base/deprecated/LieVector.h b/gtsam/base/deprecated/LieVector.h index 60c8103e28..745189c3de 100644 --- a/gtsam/base/deprecated/LieVector.h +++ b/gtsam/base/deprecated/LieVector.h @@ -27,7 +27,7 @@ namespace gtsam { * we can directly add double, Vector, and Matrix into values now, because of * gtsam::traits. */ -struct GTSAM_EXPORT LieVector : public Vector { +struct LieVector : public Vector { enum { dimension = Eigen::Dynamic }; diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index c315b9c273..bd715e3cb2 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -165,7 +165,7 @@ TEST(Vector, weightedPseudoinverse ) // verify EXPECT(assert_equal(expected,actual)); - EXPECT(fabs(expPrecision-precision) < 1e-5); + EXPECT(std::abs(expPrecision-precision) < 1e-5); } /* ************************************************************************* */ diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index ec80baad37..557500e73c 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -142,7 +142,7 @@ namespace gtsam { /** * Timing Entry, arranged in a tree */ - class GTSAM_EXPORT TimingOutline { + class TimingOutline { protected: size_t id_; size_t t_; @@ -174,21 +174,21 @@ namespace gtsam { public: /// Constructor - TimingOutline(const std::string& label, size_t myId); - size_t time() const; ///< time taken, including children + GTSAM_EXPORT TimingOutline(const std::string& label, size_t myId); + GTSAM_EXPORT size_t time() const; ///< time taken, including children double secs() const { return double(time()) / 1000000.0;} ///< time taken, in seconds, including children double self() const { return double(t_) / 1000000.0;} ///< self time only, in seconds double wall() const { return double(tWall_) / 1000000.0;} ///< wall time, in seconds double min() const { return double(tMin_) / 1000000.0;} ///< min time, in seconds double max() const { return double(tMax_) / 1000000.0;} ///< max time, in seconds double mean() const { return self() / double(n_); } ///< mean self time, in seconds - void print(const std::string& outline = "") const; - void print2(const std::string& outline = "", const double parentTotal = -1.0) const; - const boost::shared_ptr& + GTSAM_EXPORT void print(const std::string& outline = "") const; + GTSAM_EXPORT void print2(const std::string& outline = "", const double parentTotal = -1.0) const; + GTSAM_EXPORT const boost::shared_ptr& child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr); - void tic(); - void toc(); - void finishedIteration(); + GTSAM_EXPORT void tic(); + GTSAM_EXPORT void toc(); + GTSAM_EXPORT void finishedIteration(); GTSAM_EXPORT friend void toc(size_t id, const char *label); }; // \TimingOutline diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 8433f19b0d..4b8bd180d3 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -25,7 +25,7 @@ #define GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@" // Paths to example datasets distributed with GTSAM -#define GTSAM_SOURCE_TREE_DATASET_DIR "@PROJECT_SOURCE_DIR@/examples/Data" +#define GTSAM_SOURCE_TREE_DATASET_DIR "@GTSAM_SOURCE_DIR@/examples/Data" #define GTSAM_INSTALLED_DATASET_DIR "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data" // Whether GTSAM is compiled to use quaternions for Rot3 (otherwise uses rotation matrices) @@ -52,6 +52,12 @@ // Whether Eigen with MKL will use OpenMP (if OpenMP was found, Eigen uses MKL, and GTSAM_WITH_EIGEN_MKL_OPENMP is enabled in CMake) #cmakedefine GTSAM_USE_EIGEN_MKL_OPENMP +// Eigen library version (needed to avoid mixing versions, which often leads +// to segfaults) +#cmakedefine GTSAM_EIGEN_VERSION_WORLD @GTSAM_EIGEN_VERSION_WORLD@ +#cmakedefine GTSAM_EIGEN_VERSION_MAJOR @GTSAM_EIGEN_VERSION_MAJOR@ +#cmakedefine GTSAM_EIGEN_VERSION_MINOR @GTSAM_EIGEN_VERSION_MINOR@ + // The default allocator to use #cmakedefine GTSAM_ALLOCATOR_BOOSTPOOL #cmakedefine GTSAM_ALLOCATOR_TBB diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 2ad9fc1e32..2efd069cc0 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -79,7 +79,7 @@ namespace gtsam { bool equals(const Node& q, double tol) const { const Leaf* other = dynamic_cast (&q); if (!other) return false; - return fabs(double(this->constant_ - other->constant_)) < tol; + return std::abs(double(this->constant_ - other->constant_)) < tol; } /** print */ diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 4ad1dffa2b..223bcc2421 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -59,9 +59,9 @@ void Cal3Bundler::print(const std::string& s) const { /* ************************************************************************* */ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { - if (fabs(f_ - K.f_) > tol || fabs(k1_ - K.k1_) > tol - || fabs(k2_ - K.k2_) > tol || fabs(u0_ - K.u0_) > tol - || fabs(v0_ - K.v0_) > tol) + if (std::abs(f_ - K.f_) > tol || std::abs(k1_ - K.k1_) > tol + || std::abs(k2_ - K.k2_) > tol || std::abs(u0_ - K.u0_) > tol + || std::abs(v0_ - K.v0_) > tol) return false; return true; } diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index 044d47de1c..070d16c6cc 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -30,9 +30,9 @@ void Cal3DS2::print(const std::string& s_) const { /* ************************************************************************* */ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { - if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || - fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || - fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol) + if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol || + std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol || + std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol) return false; return true; } diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 2071b87921..6c03883cef 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -49,9 +49,9 @@ void Cal3DS2_Base::print(const std::string& s_) const { /* ************************************************************************* */ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const { - if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || - fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || - fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol) + if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol || + std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol || + std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol) return false; return true; } diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 8b7c1abf46..b1b9c37221 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -43,10 +43,10 @@ void Cal3Unified::print(const std::string& s) const { /* ************************************************************************* */ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { - if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || - fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || - fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol || - fabs(xi_ - K.xi_) > tol) + if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol || + std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol || + std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol || + std::abs(xi_ - K.xi_) > tol) return false; return true; } diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index ca4e88eb6a..b3d1be4b68 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -64,15 +64,15 @@ void Cal3_S2::print(const std::string& s) const { /* ************************************************************************* */ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { - if (fabs(fx_ - K.fx_) > tol) + if (std::abs(fx_ - K.fx_) > tol) return false; - if (fabs(fy_ - K.fy_) > tol) + if (std::abs(fy_ - K.fy_) > tol) return false; - if (fabs(s_ - K.s_) > tol) + if (std::abs(s_ - K.s_) > tol) return false; - if (fabs(u0_ - K.u0_) > tol) + if (std::abs(u0_ - K.u0_) > tol) return false; - if (fabs(v0_ - K.v0_) > tol) + if (std::abs(v0_ - K.v0_) > tol) return false; return true; } diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp index 414fe6711c..9b5aea4eda 100644 --- a/gtsam/geometry/Cal3_S2Stereo.cpp +++ b/gtsam/geometry/Cal3_S2Stereo.cpp @@ -30,7 +30,7 @@ void Cal3_S2Stereo::print(const std::string& s) const { /* ************************************************************************* */ bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const { - if (fabs(b_ - other.b_) > tol) return false; + if (std::abs(b_ - other.b_) > tol) return false; return K_.equals(other.K_,tol); } diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 1f791935d6..9d9b37d7a9 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -29,8 +29,7 @@ namespace gtsam { -class GTSAM_EXPORT CheiralityException: public ThreadsafeException< - CheiralityException> { +class GTSAM_EXPORT CheiralityException: public ThreadsafeException { public: CheiralityException() : CheiralityException(std::numeric_limits::max()) {} diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 891902da7a..3235fdedd1 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -23,7 +23,7 @@ namespace gtsam { * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ -class GTSAM_EXPORT EssentialMatrix { +class EssentialMatrix { private: Rot3 R_; ///< Rotation Unit3 t_; ///< Translation @@ -48,12 +48,12 @@ class GTSAM_EXPORT EssentialMatrix { } /// Named constructor with derivatives - static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb, + GTSAM_EXPORT static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb, OptionalJacobian<5, 3> H1 = boost::none, OptionalJacobian<5, 2> H2 = boost::none); /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) - static EssentialMatrix FromPose3(const Pose3& _1P2_, + GTSAM_EXPORT static EssentialMatrix FromPose3(const Pose3& _1P2_, OptionalJacobian<5, 6> H = boost::none); /// Random, using Rot3::Random and Unit3::Random @@ -70,7 +70,7 @@ class GTSAM_EXPORT EssentialMatrix { /// @{ /// print with optional string - void print(const std::string& s = "") const; + GTSAM_EXPORT void print(const std::string& s = "") const; /// assert equality up to a tolerance bool equals(const EssentialMatrix& other, double tol = 1e-8) const { @@ -138,7 +138,7 @@ class GTSAM_EXPORT EssentialMatrix { * @param Dpoint optional 3*3 Jacobian wrpt point * @return point in pose coordinates */ - Point3 transformTo(const Point3& p, + GTSAM_EXPORT Point3 transformTo(const Point3& p, OptionalJacobian<3, 5> DE = boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; @@ -147,7 +147,7 @@ class GTSAM_EXPORT EssentialMatrix { * @param cRb rotation from body frame to camera frame * @param E essential matrix E in camera frame C */ - EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE = + GTSAM_EXPORT EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE = boost::none, OptionalJacobian<5, 3> HR = boost::none) const; /** @@ -160,7 +160,7 @@ class GTSAM_EXPORT EssentialMatrix { } /// epipolar error, algebraic - double error(const Vector3& vA, const Vector3& vB, + GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, OptionalJacobian<1, 5> H = boost::none) const; /// @} diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 596118385f..61d8a30d2e 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -78,7 +78,7 @@ class GTSAM_EXPORT OrientedPlane3 { /// The equals function with tolerance bool equals(const OrientedPlane3& s, double tol = 1e-9) const { - return (n_.equals(s.n_, tol) && (fabs(d_ - s.d_) < tol)); + return (n_.equals(s.n_, tol) && (std::abs(d_ - s.d_) < tol)); } /// @} diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 74b9a2bec8..3d4bb753ea 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -27,7 +27,7 @@ namespace gtsam { double norm2(const Point2& p, OptionalJacobian<1,2> H) { double r = std::sqrt(p.x() * p.x() + p.y() * p.y()); if (H) { - if (fabs(r) > 1e-10) + if (std::abs(r) > 1e-10) *H << p.x() / r, p.y() / r; else *H << 1, 1; // really infinity, why 1 ? @@ -59,7 +59,7 @@ void Point2::print(const string& s) const { /* ************************************************************************* */ bool Point2::equals(const Point2& q, double tol) const { - return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol); + return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 30e902c2b9..718fb2992e 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -37,7 +37,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Point2 : public Vector2 { +class Point2 : public Vector2 { private: public: @@ -66,10 +66,10 @@ class GTSAM_EXPORT Point2 : public Vector2 { /// @{ /// print with optional string - void print(const std::string& s = "") const; + GTSAM_EXPORT void print(const std::string& s = "") const; /// equals with an tolerance, prints out message if unequal - bool equals(const Point2& q, double tol = 1e-9) const; + GTSAM_EXPORT bool equals(const Point2& q, double tol = 1e-9) const; /// @} /// @name Group @@ -86,10 +86,10 @@ class GTSAM_EXPORT Point2 : public Vector2 { Point2 unit() const { return *this/norm(); } /** norm of point, with derivative */ - double norm(OptionalJacobian<1,2> H = boost::none) const; + GTSAM_EXPORT double norm(OptionalJacobian<1,2> H = boost::none) const; /** distance between two points */ - double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none, + GTSAM_EXPORT double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none, OptionalJacobian<1,2> H2 = boost::none) const; /// @} @@ -124,9 +124,9 @@ class GTSAM_EXPORT Point2 : public Vector2 { static Vector2 Logmap(const Point2& p) { return p;} static Point2 Expmap(const Vector2& v) { return Point2(v);} inline double dist(const Point2& p2) const {return distance(p2);} - static boost::optional CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9); - static std::list CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh); - static std::list CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9); + GTSAM_EXPORT static boost::optional CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9); + GTSAM_EXPORT static std::list CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh); + GTSAM_EXPORT static std::list CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9); /// @} #endif diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 8df5f56074..8aa339a898 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -24,8 +24,8 @@ namespace gtsam { #ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS bool Point3::equals(const Point3 &q, double tol) const { - return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && - fabs(z() - q.z()) < tol); + return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol && + std::abs(z() - q.z()) < tol); } void Point3::print(const string& s) const { @@ -98,7 +98,7 @@ double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, double norm3(const Point3 &p, OptionalJacobian<1, 3> H) { double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z()); if (H) { - if (fabs(r) > 1e-10) + if (std::abs(r) > 1e-10) *H << p.x() / r, p.y() / r, p.z() / r; else *H << 1, 1, 1; // really infinity, why 1 ? diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 215161b3a4..3b2330403c 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -42,7 +42,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Point3 : public Vector3 { +class Point3 : public Vector3 { public: @@ -63,10 +63,10 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// @{ /** print with optional string */ - void print(const std::string& s = "") const; + GTSAM_EXPORT void print(const std::string& s = "") const; /** equals with an tolerance */ - bool equals(const Point3& p, double tol = 1e-9) const; + GTSAM_EXPORT bool equals(const Point3& p, double tol = 1e-9) const; /// @} /// @name Group @@ -80,21 +80,21 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// @{ /** distance between two points */ - double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none, + GTSAM_EXPORT double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H2 = boost::none) const; /** Distance of the point from the origin, with Jacobian */ - double norm(OptionalJacobian<1,3> H = boost::none) const; + GTSAM_EXPORT double norm(OptionalJacobian<1,3> H = boost::none) const; /** normalize, with optional Jacobian */ - Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const; + GTSAM_EXPORT Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const; /** cross product @return this x q */ - Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // + GTSAM_EXPORT Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // OptionalJacobian<3, 3> H_q = boost::none) const; /** dot product @return this * q*/ - double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, // + GTSAM_EXPORT double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, // OptionalJacobian<1, 3> H_q = boost::none) const; /// @} @@ -130,9 +130,9 @@ class GTSAM_EXPORT Point3 : public Vector3 { static Point3 Expmap(const Vector3& v) { return Point3(v);} inline double dist(const Point3& q) const { return (q - *this).norm(); } Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);} - Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none, + GTSAM_EXPORT Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; - Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none, + GTSAM_EXPORT Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; /// @} #endif diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 4e5085cfe7..9c41a76c89 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -142,7 +142,7 @@ Matrix3 Pose2::adjointMap(const Vector3& v) { Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { double alpha = v[2]; Matrix3 J; - if (fabs(alpha) > 1e-5) { + if (std::abs(alpha) > 1e-5) { // Chirikjian11book2, pg. 36 /* !!!Warning!!! Compare Iserles05an, formula 2.42 and Chirikjian11book2 pg.26 * Iserles' right-trivialization dexpR is actually the left Jacobian J_l in Chirikjian's notation @@ -174,7 +174,7 @@ Matrix3 Pose2::LogmapDerivative(const Pose2& p) { Vector3 v = Logmap(p); double alpha = v[2]; Matrix3 J; - if (fabs(alpha) > 1e-5) { + if (std::abs(alpha) > 1e-5) { double alphaInv = 1/alpha; double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha)); double v1 = v[0], v2 = v[1]; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index efd6a7f88d..388318827d 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -33,7 +33,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Pose2: public LieGroup { +class Pose2: public LieGroup { public: @@ -97,10 +97,10 @@ class GTSAM_EXPORT Pose2: public LieGroup { /// @{ /** print with optional string */ - void print(const std::string& s = "") const; + GTSAM_EXPORT void print(const std::string& s = "") const; /** assert equality up to a tolerance */ - bool equals(const Pose2& pose, double tol = 1e-9) const; + GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const; /// @} /// @name Group @@ -110,7 +110,7 @@ class GTSAM_EXPORT Pose2: public LieGroup { inline static Pose2 identity() { return Pose2(); } /// inverse - Pose2 inverse() const; + GTSAM_EXPORT Pose2 inverse() const; /// compose syntactic sugar inline Pose2 operator*(const Pose2& p2) const { @@ -122,16 +122,16 @@ class GTSAM_EXPORT Pose2: public LieGroup { /// @{ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ - static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none); + GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation - static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none); + GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none); /** * Calculate Adjoint map * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ - Matrix3 AdjointMap() const; + GTSAM_EXPORT Matrix3 AdjointMap() const; /// Apply AdjointMap to twist xi inline Vector3 Adjoint(const Vector3& xi) const { @@ -141,7 +141,7 @@ class GTSAM_EXPORT Pose2: public LieGroup { /** * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19 */ - static Matrix3 adjointMap(const Vector3& v); + GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v); /** * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives @@ -177,15 +177,15 @@ class GTSAM_EXPORT Pose2: public LieGroup { } /// Derivative of Expmap - static Matrix3 ExpmapDerivative(const Vector3& v); + GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v); /// Derivative of Logmap - static Matrix3 LogmapDerivative(const Pose2& v); + GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v); // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP struct ChartAtOrigin { - static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none); - static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none); + GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none); + GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none); }; using LieGroup::inverse; // version with derivative @@ -195,12 +195,12 @@ class GTSAM_EXPORT Pose2: public LieGroup { /// @{ /** Return point coordinates in pose coordinate frame */ - Point2 transformTo(const Point2& point, + GTSAM_EXPORT Point2 transformTo(const Point2& point, OptionalJacobian<2, 3> Dpose = boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const; /** Return point coordinates in global frame */ - Point2 transformFrom(const Point2& point, + GTSAM_EXPORT Point2 transformFrom(const Point2& point, OptionalJacobian<2, 3> Dpose = boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const; @@ -233,14 +233,14 @@ class GTSAM_EXPORT Pose2: public LieGroup { inline const Rot2& rotation() const { return r_; } //// return transformation matrix - Matrix3 matrix() const; + GTSAM_EXPORT Matrix3 matrix() const; /** * Calculate bearing to a landmark * @param point 2D location of landmark * @return 2D rotation \f$ \in SO(2) \f$ */ - Rot2 bearing(const Point2& point, + GTSAM_EXPORT Rot2 bearing(const Point2& point, OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const; /** @@ -248,7 +248,7 @@ class GTSAM_EXPORT Pose2: public LieGroup { * @param point SO(2) location of other pose * @return 2D rotation \f$ \in SO(2) \f$ */ - Rot2 bearing(const Pose2& pose, + GTSAM_EXPORT Rot2 bearing(const Pose2& pose, OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const; /** @@ -256,7 +256,7 @@ class GTSAM_EXPORT Pose2: public LieGroup { * @param point 2D location of landmark * @return range (double) */ - double range(const Point2& point, + GTSAM_EXPORT double range(const Point2& point, OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const; @@ -265,7 +265,7 @@ class GTSAM_EXPORT Pose2: public LieGroup { * @param point 2D location of other pose * @return range (double) */ - double range(const Pose2& point, + GTSAM_EXPORT double range(const Pose2& point, OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index e720fe0b91..01611d7397 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -221,7 +221,7 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { #else // The closed-form formula in Barfoot14tro eq. (102) double phi = w.norm(); - if (fabs(phi)>1e-5) { + 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; // Invert the sign of odd-order terms to have the right Jacobian diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 9f3ed35b07..04ed16774f 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -25,7 +25,7 @@ namespace gtsam { /* ************************************************************************* */ Rot2 Rot2::fromCosSin(double c, double s) { - if (fabs(c * c + s * s - 1.0) > 1e-9) { + if (std::abs(c * c + s * s - 1.0) > 1e-9) { double norm_cs = sqrt(c*c + s*s); c = c/norm_cs; s = s/norm_cs; @@ -46,13 +46,13 @@ void Rot2::print(const string& s) const { /* ************************************************************************* */ bool Rot2::equals(const Rot2& R, double tol) const { - return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol; + return std::abs(c_ - R.c_) <= tol && std::abs(s_ - R.s_) <= tol; } /* ************************************************************************* */ Rot2& Rot2::normalize() { double scale = c_*c_ + s_*s_; - if(fabs(scale-1.0)>1e-10) { + if(std::abs(scale-1.0)>1e-10) { scale = pow(scale, -0.5); c_ *= scale; s_ *= scale; @@ -115,7 +115,7 @@ Point2 Rot2::unrotate(const Point2& p, /* ************************************************************************* */ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) { double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); - if(fabs(n) > 1e-5) { + if(std::abs(n) > 1e-5) { if (H) { *H << -y / d2, x / d2; } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 3b27d45c51..5f1c7d1bf5 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -62,13 +62,13 @@ class SO3: public Matrix3, public LieGroup { } /// Static, named constructor TODO think about relation with above - static SO3 AxisAngle(const Vector3& axis, double theta); + GTSAM_EXPORT static SO3 AxisAngle(const Vector3& axis, double theta); /// @} /// @name Testable /// @{ - void print(const std::string& s) const; + GTSAM_EXPORT void print(const std::string& s) const; bool equals(const SO3 & R, double tol) const { return equal_with_abs_tol(*this, R, tol); @@ -96,19 +96,19 @@ class SO3: public Matrix3, public LieGroup { * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula */ - static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none); + GTSAM_EXPORT static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none); /// Derivative of Expmap - static Matrix3 ExpmapDerivative(const Vector3& omega); + GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& omega); /** * Log map at identity - returns the canonical coordinates * \f$ [R_x,R_y,R_z] \f$ of this rotation */ - static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none); + GTSAM_EXPORT static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none); /// Derivative of Logmap - static Matrix3 LogmapDerivative(const Vector3& omega); + GTSAM_EXPORT static Matrix3 LogmapDerivative(const Vector3& omega); Matrix3 AdjointMap() const { return *this; @@ -156,14 +156,14 @@ class GTSAM_EXPORT ExpmapFunctor { }; /// Functor that implements Exponential map *and* its derivatives -class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { +class DexpFunctor : public ExpmapFunctor { const Vector3 omega; double a, b; Matrix3 dexp_; public: /// Constructor with element of Lie algebra so(3) - DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); + GTSAM_EXPORT DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, @@ -174,11 +174,11 @@ class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Matrix3& dexp() const { return dexp_; } /// Multiplies with dexp(), with optional derivatives - Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, + GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; /// Multiplies with dexp().inverse(), with optional derivatives - Vector3 applyInvDexp(const Vector3& v, + GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; }; diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index e0f3c34242..9eef01577d 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -62,8 +62,8 @@ class GTSAM_EXPORT StereoPoint2 { /** equals */ bool equals(const StereoPoint2& q, double tol = 1e-9) const { - return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol - && fabs(v_ - q.v_) < tol); + return (std::abs(uL_ - q.uL_) < tol && std::abs(uR_ - q.uR_) < tol + && std::abs(v_ - q.v_) < tol); } /// @} diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index f661f819db..533ee500ef 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -68,7 +68,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { /* ************************************************************************* */ // Get the axis of rotation with the minimum projected length of the point static Point3 CalculateBestAxis(const Point3& n) { - double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); + double mx = std::abs(n.x()), my = std::abs(n.y()), mz = std::abs(n.z()); if ((mx <= my) && (mx <= mz)) { return Point3(1.0, 0.0, 0.0); } else if ((my <= mx) && (my <= mz)) { diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index f182df2859..2116988068 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -39,7 +39,7 @@ namespace gtsam { /// Represents a 3D point on a unit sphere. -class GTSAM_EXPORT Unit3 { +class Unit3 { private: @@ -94,11 +94,11 @@ class GTSAM_EXPORT Unit3 { } /// Named constructor from Point3 with optional Jacobian - static Unit3 FromPoint3(const Point3& point, // + GTSAM_EXPORT static Unit3 FromPoint3(const Point3& point, // OptionalJacobian<2, 3> H = boost::none); /// Random direction, using boost::uniform_on_sphere - static Unit3 Random(boost::mt19937 & rng); + GTSAM_EXPORT static Unit3 Random(boost::mt19937 & rng); /// @} @@ -108,7 +108,7 @@ class GTSAM_EXPORT Unit3 { friend std::ostream& operator<<(std::ostream& os, const Unit3& pair); /// The print fuction - void print(const std::string& s = std::string()) const; + GTSAM_EXPORT void print(const std::string& s = std::string()) const; /// The equals function with tolerance bool equals(const Unit3& s, double tol = 1e-9) const { @@ -125,16 +125,16 @@ class GTSAM_EXPORT Unit3 { * tangent to the sphere at the current direction. * Provides derivatives of the basis with the two basis vectors stacked up as a 6x1. */ - const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const; + GTSAM_EXPORT const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const; /// Return skew-symmetric associated with 3D point on unit sphere - Matrix3 skew() const; + GTSAM_EXPORT Matrix3 skew() const; /// Return unit-norm Point3 - Point3 point3(OptionalJacobian<3, 2> H = boost::none) const; + GTSAM_EXPORT Point3 point3(OptionalJacobian<3, 2> H = boost::none) const; /// Return unit-norm Vector - Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const; + GTSAM_EXPORT Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const; /// Return scaled direction as Point3 friend Point3 operator*(double s, const Unit3& d) { @@ -142,20 +142,20 @@ class GTSAM_EXPORT Unit3 { } /// Return dot product with q - double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, // + GTSAM_EXPORT double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, // OptionalJacobian<1,2> H2 = boost::none) const; /// Signed, vector-valued error between two directions /// @deprecated, errorVector has the proper derivatives, this confusingly has only the second. - Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const; + GTSAM_EXPORT Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const; /// Signed, vector-valued error between two directions /// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal. - Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, // + GTSAM_EXPORT Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, // OptionalJacobian<2, 2> H_q = boost::none) const; /// Distance between two directions - double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; + GTSAM_EXPORT double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; /// Cross-product between two Unit3s Unit3 cross(const Unit3& q) const { @@ -188,10 +188,10 @@ class GTSAM_EXPORT Unit3 { }; /// The retract function - Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const; + GTSAM_EXPORT Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const; /// The local coordinates function - Vector2 localCoordinates(const Unit3& s) const; + GTSAM_EXPORT Vector2 localCoordinates(const Unit3& s) const; /// @} diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index f5f824d059..1982b8f505 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -206,7 +206,7 @@ TEST( CalibratedCamera, DBackprojectFromCamera) static Point3 backproject(const Pose3& pose, const Point2& point, const double& depth) { return CalibratedCamera(pose).backproject(point, depth); } -TEST( PinholePose, Dbackproject) +TEST( PinholePose, DbackprojectCalibCamera) { Matrix36 Dpose; Matrix31 Ddepth; diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 7ba8851155..c923e398ba 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -218,10 +218,10 @@ TEST (EssentialMatrix, epipoles) { } // check rank 2 constraint - CHECK(fabs(S(2))<1e-10); + CHECK(std::abs(S(2))<1e-10); // check epipoles not at infinity - CHECK(fabs(U(2,2))>1e-10 && fabs(V(2,2))>1e-10); + CHECK(std::abs(U(2,2))>1e-10 && std::abs(V(2,2))>1e-10); // Check epipoles diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 05d48f4ccd..1cf2f4a3fd 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -58,6 +58,7 @@ TEST( PinholePose, constructor) } //****************************************************************************** +/* Already in testPinholeCamera??? TEST(PinholeCamera, Pose) { Matrix actualH; @@ -69,6 +70,7 @@ TEST(PinholeCamera, Pose) { Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } +*/ /* ************************************************************************* */ TEST( PinholePose, lookat) @@ -207,7 +209,7 @@ static Point3 backproject(const Pose3& pose, const Cal3_S2& cal, return Camera(pose, cal.vector()).backproject(p, depth); } -TEST( PinholePose, Dbackproject) +TEST( PinholePose, DbackprojectRegCamera) { Matrix36 Dpose; Matrix31 Ddepth; diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 843dc6cc18..e862b94adf 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -81,13 +81,14 @@ TEST(Quaternion , Compose) { } //****************************************************************************** -Vector3 z_axis(0, 0, 1); -Q id(Eigen::AngleAxisd(0, z_axis)); -Q R1(Eigen::AngleAxisd(1, z_axis)); +Vector3 Q_z_axis(0, 0, 1); +Q id(Eigen::AngleAxisd(0, Q_z_axis)); +Q R1(Eigen::AngleAxisd(1, Q_z_axis)); Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); //****************************************************************************** TEST(Quaternion , Between) { + Vector3 z_axis(0, 0, 1); Q q1(Eigen::AngleAxisd(0.2, z_axis)); Q q2(Eigen::AngleAxisd(0.1, z_axis)); @@ -98,6 +99,7 @@ TEST(Quaternion , Between) { //****************************************************************************** TEST(Quaternion , Inverse) { + Vector3 z_axis(0, 0, 1); Q q1(Eigen::AngleAxisd(0.1, z_axis)); Q expected(Eigen::AngleAxisd(-0.1, z_axis)); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 851c5e4d3e..ed61c75b59 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -386,7 +386,7 @@ struct GTSAM_EXPORT TriangulationParameters { /** * TriangulationResult is an optional point, along with the reasons why it is invalid. */ -class GTSAM_EXPORT TriangulationResult: public boost::optional { +class TriangulationResult: public boost::optional { enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 9935776a5e..4df234004f 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -36,19 +36,20 @@ namespace gtsam { /* ************************************************************************* */ template BayesTreeCliqueData BayesTree::getCliqueData() const { - BayesTreeCliqueData data; - for(const sharedClique& root: roots_) - getCliqueData(data, root); - return data; + BayesTreeCliqueData stats; + for (const sharedClique& root : roots_) getCliqueData(root, &stats); + return stats; } /* ************************************************************************* */ - template - void BayesTree::getCliqueData(BayesTreeCliqueData& data, sharedClique clique) const { - data.conditionalSizes.push_back(clique->conditional()->nrFrontals()); - data.separatorSizes.push_back(clique->conditional()->nrParents()); - for(sharedClique c: clique->children) { - getCliqueData(data, c); + template + void BayesTree::getCliqueData(sharedClique clique, + BayesTreeCliqueData* stats) const { + const auto conditional = clique->conditional(); + stats->conditionalSizes.push_back(conditional->nrFrontals()); + stats->separatorSizes.push_back(conditional->nrParents()); + for (sharedClique c : clique->children) { + getCliqueData(c, stats); } } @@ -133,34 +134,26 @@ namespace gtsam { } /* ************************************************************************* */ - // TODO: Clean up namespace { - template - int _pushClique(FactorGraph& fg, const boost::shared_ptr& clique) { - fg.push_back(clique->conditional_); + template + struct _pushCliqueFunctor { + _pushCliqueFunctor(FactorGraph* graph_) : graph(graph_) {} + FactorGraph* graph; + int operator()(const boost::shared_ptr& clique, int dummy) { + graph->push_back(clique->conditional_); return 0; } - - template - struct _pushCliqueFunctor { - _pushCliqueFunctor(FactorGraph& graph_) : graph(graph_) {} - FactorGraph& graph; - int operator()(const boost::shared_ptr& clique, int dummy) { - graph.push_back(clique->conditional_); - return 0; - } - }; - } + }; + } // namespace /* ************************************************************************* */ - template - void BayesTree::addFactorsToGraph(FactorGraph& graph) const - { + template + void BayesTree::addFactorsToGraph( + FactorGraph* graph) const { // Traverse the BayesTree and add all conditionals to this graph - int data = 0; // Unused - _pushCliqueFunctor functor(graph); - treeTraversal::DepthFirstForest(*this, data, functor); // FIXME: sort of works? -// treeTraversal::DepthFirstForest(*this, data, boost::bind(&_pushClique, boost::ref(graph), _1)); + int data = 0; // Unused + _pushCliqueFunctor functor(graph); + treeTraversal::DepthFirstForest(*this, data, functor); } /* ************************************************************************* */ @@ -434,50 +427,51 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans) - { + template + void BayesTree::removePath(sharedClique clique, BayesNetType* bn, + Cliques* orphans) { // base case is NULL, if so we do nothing and return empties above if (clique) { - // remove the clique from orphans in case it has been added earlier - orphans.remove(clique); + orphans->remove(clique); // remove me this->removeClique(clique); // remove path above me - this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans); + this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, + orphans); - // add children to list of orphans (splice also removed them from clique->children_) - orphans.insert(orphans.begin(), clique->children.begin(), clique->children.end()); + // add children to list of orphans (splice also removed them from + // clique->children_) + orphans->insert(orphans->begin(), clique->children.begin(), + clique->children.end()); clique->children.clear(); - bn.push_back(clique->conditional_); - + bn->push_back(clique->conditional_); } } - /* ************************************************************************* */ - template - void BayesTree::removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans) - { + /* ************************************************************************* + */ + template + void BayesTree::removeTop(const KeyVector& keys, BayesNetType* bn, + Cliques* orphans) { + gttic(removetop); // process each key of the new factor - for(const Key& j: keys) - { + for (const Key& j : keys) { // get the clique - // TODO: Nodes will be searched again in removeClique + // TODO(frank): Nodes will be searched again in removeClique typename Nodes::const_iterator node = nodes_.find(j); - if(node != nodes_.end()) { + if (node != nodes_.end()) { // remove path from clique to root this->removePath(node->second, bn, orphans); } } // Delete cachedShortcuts for each orphan subtree - //TODO: Consider Improving - for(sharedClique& orphan: orphans) - orphan->deleteCachedShortcuts(); + // TODO(frank): Consider Improving + for (sharedClique& orphan : *orphans) orphan->deleteCachedShortcuts(); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 892ac5f31c..9d632ff06f 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -89,14 +89,14 @@ namespace gtsam { /** Map from keys to Clique */ typedef ConcurrentMap Nodes; + /** Root cliques */ + typedef FastVector Roots; + protected: /** Map from indices to Clique */ Nodes nodes_; - /** Root cliques */ - typedef FastVector Roots; - /** Root cliques */ Roots roots_; @@ -208,13 +208,13 @@ namespace gtsam { * Remove path from clique to root and return that path as factors * plus a list of orphaned subtree roots. Used in removeTop below. */ - void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans); + void removePath(sharedClique clique, BayesNetType* bn, Cliques* orphans); /** * Given a list of indices, turn "contaminated" part of the tree back into a factor graph. * Factors and orphans are added to the in/out arguments. */ - void removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans); + void removeTop(const KeyVector& keys, BayesNetType* bn, Cliques* orphans); /** * Remove the requested subtree. */ @@ -229,7 +229,7 @@ namespace gtsam { void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique()); /** Add all cliques in this BayesTree to the specified factor graph */ - void addFactorsToGraph(FactorGraph& graph) const; + void addFactorsToGraph(FactorGraph* graph) const; protected: @@ -238,7 +238,7 @@ namespace gtsam { int parentnum = 0) const; /** Gather data on a single clique */ - void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const; + void getCliqueData(sharedClique clique, BayesTreeCliqueData* stats) const; /** remove a clique: warning, can result in a forest */ void removeClique(sharedClique clique); @@ -249,7 +249,26 @@ namespace gtsam { // Friend JunctionTree because it directly fills roots and nodes index. template friend class EliminatableClusterTree; - private: +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + public: + /// @name Deprecated + /// @{ + void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans) { + removePath(clique, &bn, &orphans); + } + void removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans) { + removeTop(keys, &bn, &orphans); + } + void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const { + getCliqueData(clique, &stats); + } + void addFactorsToGraph(FactorGraph& graph) const{ + addFactorsToGraph(& graph); + } + /// @} +#endif + + private: /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index 33ba6e18da..3390a3aac8 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -98,7 +98,7 @@ namespace gtsam { for (size_t j = 0; j < n; j++) { // Retrieve the factors involving this variable and create the current node - const VariableIndex::Factors& factors = structure[order[j]]; + const FactorIndices& factors = structure[order[j]]; const sharedNode node = boost::make_shared(); node->key = order[j]; diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index 0aa4af2e16..34ca8ab7fd 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -26,74 +26,105 @@ #include #include +#include +#include // for cout :-( #include -#include // for cout :-( +#include namespace gtsam { - /* ************************************************************************* */ - template - void FactorGraph::print(const std::string& s, const KeyFormatter& formatter) const { - std::cout << s << std::endl; - std::cout << "size: " << size() << std::endl; - for (size_t i = 0; i < factors_.size(); i++) { - std::stringstream ss; - ss << "factor " << i << ": "; - if (factors_[i]) - factors_[i]->print(ss.str(), formatter); - } +/* ************************************************************************* */ +template +void FactorGraph::print(const std::string& s, + const KeyFormatter& formatter) const { + std::cout << s << std::endl; + std::cout << "size: " << size() << std::endl; + for (size_t i = 0; i < factors_.size(); i++) { + std::stringstream ss; + ss << "factor " << i << ": "; + if (factors_[i]) factors_[i]->print(ss.str(), formatter); } - - /* ************************************************************************* */ - template - bool FactorGraph::equals(const This& fg, double tol) const { - /** check whether the two factor graphs have the same number of factors_ */ - if (factors_.size() != fg.size()) return false; - - /** check whether the factors_ are the same */ - for (size_t i = 0; i < factors_.size(); i++) { - // TODO: Doesn't this force order of factor insertion? - sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; - if (f1 == NULL && f2 == NULL) continue; - if (f1 == NULL || f2 == NULL) return false; - if (!f1->equals(*f2, tol)) return false; - } - return true; +} + +/* ************************************************************************* */ +template +bool FactorGraph::equals(const This& fg, double tol) const { + // check whether the two factor graphs have the same number of factors. + if (factors_.size() != fg.size()) return false; + + // check whether the factors are the same, in same order. + for (size_t i = 0; i < factors_.size(); i++) { + sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; + if (f1 == NULL && f2 == NULL) continue; + if (f1 == NULL || f2 == NULL) return false; + if (!f1->equals(*f2, tol)) return false; } - - /* ************************************************************************* */ - template - size_t FactorGraph::nrFactors() const { - size_t size_ = 0; - for(const sharedFactor& factor: factors_) - if (factor) size_++; - return size_; + return true; +} + +/* ************************************************************************* */ +template +size_t FactorGraph::nrFactors() const { + size_t size_ = 0; + for (const sharedFactor& factor : factors_) + if (factor) size_++; + return size_; +} + +/* ************************************************************************* */ +template +KeySet FactorGraph::keys() const { + KeySet keys; + for (const sharedFactor& factor : this->factors_) { + if (factor) keys.insert(factor->begin(), factor->end()); } - - /* ************************************************************************* */ - template - KeySet FactorGraph::keys() const { - KeySet keys; - for(const sharedFactor& factor: this->factors_) { - if(factor) - keys.insert(factor->begin(), factor->end()); + return keys; +} + +/* ************************************************************************* */ +template +KeyVector FactorGraph::keyVector() const { + KeyVector keys; + keys.reserve(2 * size()); // guess at size + for (const sharedFactor& factor : factors_) + if (factor) keys.insert(keys.end(), factor->begin(), factor->end()); + std::sort(keys.begin(), keys.end()); + auto last = std::unique(keys.begin(), keys.end()); + keys.erase(last, keys.end()); + return keys; +} + +/* ************************************************************************* */ +template +template +FactorIndices FactorGraph::add_factors(const CONTAINER& factors, + bool useEmptySlots) { + const size_t num_factors = factors.size(); + FactorIndices newFactorIndices(num_factors); + if (useEmptySlots) { + size_t i = 0; + for (size_t j = 0; j < num_factors; ++j) { + // Loop to find the next available factor slot + do { + if (i >= size()) + // Make room for remaining factors, happens only once. + resize(size() + num_factors - j); + else if (at(i)) + ++i; // Move on to the next slot or past end. + else + break; // We found an empty slot, break to fill it. + } while (true); + + // Use the current slot, updating graph and newFactorSlots. + at(i) = factors[j]; + newFactorIndices[j] = i; } - return keys; - } - - /* ************************************************************************* */ - template - KeyVector FactorGraph::keyVector() const { - KeyVector keys; - keys.reserve(2 * size()); // guess at size - for (const sharedFactor& factor: factors_) - if (factor) - keys.insert(keys.end(), factor->begin(), factor->end()); - std::sort(keys.begin(), keys.end()); - auto last = std::unique(keys.begin(), keys.end()); - keys.erase(last, keys.end()); - return keys; + } else { + // We're not looking for unused slots, so just add the factors at the end. + for (size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size(); + push_back(factors); } + return newFactorIndices; +} - /* ************************************************************************* */ -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 0ecfd87f19..600e3f9ed2 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -22,350 +22,382 @@ #pragma once -#include #include +#include #include #include // for Eigen::aligned_allocator -#include -#include #include #include #include +#include +#include +#include #include #include namespace gtsam { +/// Define collection type: +typedef FastVector FactorIndices; + +// Forward declarations +template +class BayesTree; + +/** Helper */ +template +class CRefCallPushBack { + C& obj; + + public: + explicit CRefCallPushBack(C& obj) : obj(obj) {} + template + void operator()(const A& a) { + obj.push_back(a); + } +}; + +/** Helper */ +template +class RefCallPushBack { + C& obj; + + public: + explicit RefCallPushBack(C& obj) : obj(obj) {} + template + void operator()(A& a) { + obj.push_back(a); + } +}; + +/** Helper */ +template +class CRefCallAddCopy { + C& obj; + + public: + explicit CRefCallAddCopy(C& obj) : obj(obj) {} + template + void operator()(const A& a) { + obj.addCopy(a); + } +}; - // Forward declarations - template class BayesTree; - - /** Helper */ - template - class CRefCallPushBack - { - C& obj; - public: - CRefCallPushBack(C& obj) : obj(obj) {} - template - void operator()(const A& a) { obj.push_back(a); } - }; - - /** Helper */ - template - class RefCallPushBack - { - C& obj; - public: - RefCallPushBack(C& obj) : obj(obj) {} - template - void operator()(A& a) { obj.push_back(a); } - }; - - /** Helper */ - template - class CRefCallAddCopy - { - C& obj; - public: - CRefCallAddCopy(C& obj) : obj(obj) {} - template - void operator()(const A& a) { obj.addCopy(a); } - }; +/** + * A factor graph is a bipartite graph with factor nodes connected to variable + * nodes. In this class, however, only factor nodes are kept around. + * \nosubgrouping + */ +template +class FactorGraph { + public: + typedef FACTOR FactorType; ///< factor type + typedef boost::shared_ptr + sharedFactor; ///< Shared pointer to a factor + typedef sharedFactor value_type; + typedef typename FastVector::iterator iterator; + typedef typename FastVector::const_iterator const_iterator; + + private: + typedef FactorGraph This; ///< Typedef for this class + typedef boost::shared_ptr + shared_ptr; ///< Shared pointer for this class + + /// Check if a DERIVEDFACTOR is in fact derived from FactorType. + template + using IsDerived = typename std::enable_if< + std::is_base_of::value>::type; + + /// Check if T has a value_type derived from FactorType. + template + using HasDerivedValueType = typename std::enable_if< + std::is_base_of::value>::type; + + /// Check if T has a value_type derived from FactorType. + template + using HasDerivedElementType = typename std::enable_if::value>::type; + + protected: + /** concept check, makes sure FACTOR defines print and equals */ + GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) + + /** Collection of factors */ + FastVector factors_; + + /// @name Standard Constructors + /// @{ + + /** Default constructor */ + FactorGraph() {} + + /** Constructor from iterator over factors (shared_ptr or plain objects) */ + template + FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) { + push_back(firstFactor, lastFactor); + } + + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit FactorGraph(const CONTAINER& factors) { + push_back(factors); + } + + /// @} + + public: + /// @name Adding Single Factors + /// @{ /** - * A factor graph is a bipartite graph with factor nodes connected to variable nodes. - * In this class, however, only factor nodes are kept around. - * \nosubgrouping + * Reserve space for the specified number of factors if you know in + * advance how many there will be (works like FastVector::reserve). */ - template - class FactorGraph { - - public: - typedef FACTOR FactorType; ///< factor type - typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor - typedef sharedFactor value_type; - typedef typename FastVector::iterator iterator; - typedef typename FastVector::const_iterator const_iterator; - - private: - typedef FactorGraph This; ///< Typedef for this class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer for this class - - protected: - /** concept check, makes sure FACTOR defines print and equals */ - GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) - - /** Collection of factors */ - FastVector factors_; - - /// @name Standard Constructors - /// @{ - - /** Default constructor */ - FactorGraph() {} - - /** Constructor from iterator over factors (shared_ptr or plain objects) */ - template - FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) { push_back(firstFactor, lastFactor); } - - /** Construct from container of factors (shared_ptr or plain objects) */ - template - explicit FactorGraph(const CONTAINER& factors) { push_back(factors); } - - /// @} - /// @name Advanced Constructors - /// @{ - - // TODO: are these needed? - - ///** - // * @brief Constructor from a Bayes net - // * @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor - // * @return a factor graph with all the conditionals, as factors - // */ - //template - //FactorGraph(const BayesNet& bayesNet); - - ///** convert from Bayes tree */ - //template - //FactorGraph(const BayesTree& bayesTree); - - ///** convert from a derived type */ - //template - //FactorGraph(const FactorGraph& factors) { - // factors_.assign(factors.begin(), factors.end()); - //} - - /// @} - - public: - /// @name Adding Factors - /// @{ - - /** - * Reserve space for the specified number of factors if you know in - * advance how many there will be (works like FastVector::reserve). - */ - void reserve(size_t size) { factors_.reserve(size); } - - // TODO: are these needed? - - /** Add a factor directly using a shared_ptr */ - template - typename std::enable_if::value>::type - push_back(boost::shared_ptr factor) { - factors_.push_back(boost::shared_ptr(factor)); } - - /** Add a factor directly using a shared_ptr */ - void push_back(const sharedFactor& factor) { - factors_.push_back(factor); } - - /** Emplace a factor */ - template - typename std::enable_if::value>::type - emplace_shared(Args&&... args) { - factors_.push_back(boost::allocate_shared(Eigen::aligned_allocator(), std::forward(args)...)); - } - - /** push back many factors with an iterator over shared_ptr (factors are not copied) */ - template - typename std::enable_if::value>::type - push_back(ITERATOR firstFactor, ITERATOR lastFactor) { - factors_.insert(end(), firstFactor, lastFactor); } - - /** push back many factors as shared_ptr's in a container (factors are not copied) */ - template - typename std::enable_if::value>::type - push_back(const CONTAINER& container) { - push_back(container.begin(), container.end()); - } - - /** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived - * classes in favor of a type-specialized version that calls this templated function. */ - template - typename std::enable_if::value>::type - push_back(const BayesTree& bayesTree) { - bayesTree.addFactorsToGraph(*this); - } - -//#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - /** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid - * the copy). */ - template - typename std::enable_if::value>::type - push_back(const DERIVEDFACTOR& factor) { - factors_.push_back(boost::allocate_shared(Eigen::aligned_allocator(), factor)); - } -//#endif - - /** push back many factors with an iterator over plain factors (factors are copied) */ - template - typename std::enable_if::value>::type - push_back(ITERATOR firstFactor, ITERATOR lastFactor) { - for (ITERATOR f = firstFactor; f != lastFactor; ++f) - push_back(*f); - } - - /** push back many factors as non-pointer objects in a container (factors are copied) */ - template - typename std::enable_if::value>::type - push_back(const CONTAINER& container) { - push_back(container.begin(), container.end()); - } - - /** Add a factor directly using a shared_ptr */ - template - typename std::enable_if::value, - boost::assign::list_inserter > >::type - operator+=(boost::shared_ptr factor) { - return boost::assign::make_list_inserter(RefCallPushBack(*this))(factor); - } - - /** Add a factor directly using a shared_ptr */ - boost::assign::list_inserter > - operator+=(const sharedFactor& factor) { - return boost::assign::make_list_inserter(CRefCallPushBack(*this))(factor); - } - - /** Add a factor or container of factors, including STL collections, BayesTrees, etc. */ - template - boost::assign::list_inserter > - operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) { - return boost::assign::make_list_inserter(CRefCallPushBack(*this))(factorOrContainer); - } - - /** Add a factor directly using a shared_ptr */ - template - typename std::enable_if::value>::type - add(boost::shared_ptr factor) { - push_back(factor); - } - - /** Add a factor directly using a shared_ptr */ - void add(const sharedFactor& factor) { - push_back(factor); - } - - /** Add a factor or container of factors, including STL collections, BayesTrees, etc. */ - template - void add(const FACTOR_OR_CONTAINER& factorOrContainer) { - push_back(factorOrContainer); - } - - /// @} - /// @name Testable - /// @{ - - /** print out graph */ - void print(const std::string& s = "FactorGraph", - const KeyFormatter& formatter = DefaultKeyFormatter) const; - - /** Check equality */ - bool equals(const This& fg, double tol = 1e-9) const; - /// @} - - public: - /// @name Standard Interface - /// @{ - - /** return the number of factors (including any null factors set by remove() ). */ - size_t size() const { return factors_.size(); } - - /** Check if the graph is empty (null factors set by remove() will cause this to return false). */ - bool empty() const { return factors_.empty(); } - - /** Get a specific factor by index (this checks array bounds and may throw an exception, as - * opposed to operator[] which does not). - */ - const sharedFactor at(size_t i) const { return factors_.at(i); } - - /** Get a specific factor by index (this checks array bounds and may throw an exception, as - * opposed to operator[] which does not). - */ - sharedFactor& at(size_t i) { return factors_.at(i); } - - /** Get a specific factor by index (this does not check array bounds, as opposed to at() which - * does). - */ - const sharedFactor operator[](size_t i) const { return at(i); } - - /** Get a specific factor by index (this does not check array bounds, as opposed to at() which - * does). - */ - sharedFactor& operator[](size_t i) { return at(i); } - - /** Iterator to beginning of factors. */ - const_iterator begin() const { return factors_.begin();} - - /** Iterator to end of factors. */ - const_iterator end() const { return factors_.end(); } - - /** Get the first factor */ - sharedFactor front() const { return factors_.front(); } - - /** Get the last factor */ - sharedFactor back() const { return factors_.back(); } + void reserve(size_t size) { factors_.reserve(size); } + + /// Add a factor directly using a shared_ptr. + template + IsDerived push_back(boost::shared_ptr factor) { + factors_.push_back(boost::shared_ptr(factor)); + } + + /// Emplace a shared pointer to factor of given type. + template + IsDerived emplace_shared(Args&&... args) { + factors_.push_back(boost::allocate_shared( + Eigen::aligned_allocator(), + std::forward(args)...)); + } - /// @} - /// @name Modifying Factor Graphs (imperative, discouraged) - /// @{ + /** + * Add a factor by value, will be copy-constructed (use push_back with a + * shared_ptr to avoid the copy). + */ + template + IsDerived push_back(const DERIVEDFACTOR& factor) { + factors_.push_back(boost::allocate_shared( + Eigen::aligned_allocator(), factor)); + } + + /// `add` is a synonym for push_back. + template + IsDerived add(boost::shared_ptr factor) { + push_back(factor); + } + + /// `+=` works well with boost::assign list inserter. + template + typename std::enable_if< + std::is_base_of::value, + boost::assign::list_inserter>>::type + operator+=(boost::shared_ptr factor) { + return boost::assign::make_list_inserter(RefCallPushBack(*this))( + factor); + } + + /// @} + /// @name Adding via iterators + /// @{ - /** non-const STL-style begin() */ - iterator begin() { return factors_.begin();} + /** + * Push back many factors with an iterator over shared_ptr (factors are not + * copied) + */ + template + HasDerivedElementType push_back(ITERATOR firstFactor, + ITERATOR lastFactor) { + factors_.insert(end(), firstFactor, lastFactor); + } + + /// Push back many factors with an iterator (factors are copied) + template + HasDerivedValueType push_back(ITERATOR firstFactor, + ITERATOR lastFactor) { + for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f); + } + + /// @} + /// @name Adding via container + /// @{ - /** non-const STL-style end() */ - iterator end() { return factors_.end(); } + /** + * Push back many factors as shared_ptr's in a container (factors are not + * copied) + */ + template + HasDerivedElementType push_back(const CONTAINER& container) { + push_back(container.begin(), container.end()); + } - /** Directly resize the number of factors in the graph. If the new size is less than the - * original, factors at the end will be removed. If the new size is larger than the original, - * null factors will be appended. - */ - void resize(size_t size) { factors_.resize(size); } + /// Push back non-pointer objects in a container (factors are copied). + template + HasDerivedValueType push_back(const CONTAINER& container) { + push_back(container.begin(), container.end()); + } - /** delete factor without re-arranging indexes by inserting a NULL pointer */ - void remove(size_t i) { factors_[i].reset();} + /** + * Add a factor or container of factors, including STL collections, + * BayesTrees, etc. + */ + template + void add(const FACTOR_OR_CONTAINER& factorOrContainer) { + push_back(factorOrContainer); + } - /** replace a factor by index */ - void replace(size_t index, sharedFactor factor) { at(index) = factor; } + /** + * Add a factor or container of factors, including STL collections, + * BayesTrees, etc. + */ + template + boost::assign::list_inserter> operator+=( + const FACTOR_OR_CONTAINER& factorOrContainer) { + return boost::assign::make_list_inserter(CRefCallPushBack(*this))( + factorOrContainer); + } - /** Erase factor and rearrange other factors to take up the empty space */ - iterator erase(iterator item) { return factors_.erase(item); } + /// @} + /// @name Specialized versions + /// @{ - /** Erase factors and rearrange other factors to take up the empty space */ - iterator erase(iterator first, iterator last) { return factors_.erase(first, last); } + /** + * Push back a BayesTree as a collection of factors. + * NOTE: This should be hidden in derived classes in favor of a + * type-specialized version that calls this templated function. + */ + template + typename std::enable_if< + std::is_base_of::value>::type + push_back(const BayesTree& bayesTree) { + bayesTree.addFactorsToGraph(this); + } - /// @} - /// @name Advanced Interface - /// @{ + /** + * Add new factors to a factor graph and returns a list of new factor indices, + * optionally finding and reusing empty factor slots. + */ + template > + FactorIndices add_factors(const CONTAINER& factors, + bool useEmptySlots = false); - /** return the number of non-null factors */ - size_t nrFactors() const; + /// @} + /// @name Testable + /// @{ - /** Potentially slow function to return all keys involved, sorted, as a set */ - KeySet keys() const; + /** print out graph */ + void print(const std::string& s = "FactorGraph", + const KeyFormatter& formatter = DefaultKeyFormatter) const; - /** Potentially slow function to return all keys involved, sorted, as a vector */ - KeyVector keyVector() const; + /** Check equality */ + bool equals(const This& fg, double tol = 1e-9) const; + /// @} - /** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */ - inline bool exists(size_t idx) const { return idx < size() && at(idx); } + public: + /// @name Standard Interface + /// @{ - private: + /** return the number of factors (including any null factors set by remove() + * ). */ + size_t size() const { return factors_.size(); } - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(factors_); - } + /** Check if the graph is empty (null factors set by remove() will cause + * this to return false). */ + bool empty() const { return factors_.empty(); } - /// @} + /** Get a specific factor by index (this checks array bounds and may throw + * an exception, as opposed to operator[] which does not). + */ + const sharedFactor at(size_t i) const { return factors_.at(i); } - }; // FactorGraph + /** Get a specific factor by index (this checks array bounds and may throw + * an exception, as opposed to operator[] which does not). + */ + sharedFactor& at(size_t i) { return factors_.at(i); } -} // namespace gtsam + /** Get a specific factor by index (this does not check array bounds, as + * opposed to at() which does). + */ + const sharedFactor operator[](size_t i) const { return at(i); } + + /** Get a specific factor by index (this does not check array bounds, as + * opposed to at() which does). + */ + sharedFactor& operator[](size_t i) { return at(i); } + + /** Iterator to beginning of factors. */ + const_iterator begin() const { return factors_.begin(); } + + /** Iterator to end of factors. */ + const_iterator end() const { return factors_.end(); } + + /** Get the first factor */ + sharedFactor front() const { return factors_.front(); } + + /** Get the last factor */ + sharedFactor back() const { return factors_.back(); } + + /// @} + /// @name Modifying Factor Graphs (imperative, discouraged) + /// @{ + + /** non-const STL-style begin() */ + iterator begin() { return factors_.begin(); } + + /** non-const STL-style end() */ + iterator end() { return factors_.end(); } + + /** Directly resize the number of factors in the graph. If the new size is + * less than the original, factors at the end will be removed. If the new + * size is larger than the original, null factors will be appended. + */ + void resize(size_t size) { factors_.resize(size); } + + /** delete factor without re-arranging indexes by inserting a NULL pointer + */ + void remove(size_t i) { factors_[i].reset(); } + + /** replace a factor by index */ + void replace(size_t index, sharedFactor factor) { at(index) = factor; } + + /** Erase factor and rearrange other factors to take up the empty space */ + iterator erase(iterator item) { return factors_.erase(item); } + + /** Erase factors and rearrange other factors to take up the empty space */ + iterator erase(iterator first, iterator last) { + return factors_.erase(first, last); + } + + /// @} + /// @name Advanced Interface + /// @{ + + /** return the number of non-null factors */ + size_t nrFactors() const; + + /** Potentially slow function to return all keys involved, sorted, as a set + */ + KeySet keys() const; + + /** Potentially slow function to return all keys involved, sorted, as a + * vector + */ + KeyVector keyVector() const; + + /** MATLAB interface utility: Checks whether a factor index idx exists in + * the graph and is a live pointer */ + inline bool exists(size_t idx) const { return idx < size() && at(idx); } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(factors_); + } + + /// @} +}; // FactorGraph +} // namespace gtsam #include diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 8ed41826e0..1c183b70f8 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -24,14 +24,14 @@ namespace gtsam { /* ************************************************************************* */ template -void ISAM::update_internal(const FactorGraphType& newFactors, - Cliques& orphans, const Eliminate& function) { +void ISAM::updateInternal(const FactorGraphType& newFactors, + Cliques* orphans, const Eliminate& function) { // Remove the contaminated part of the Bayes tree BayesNetType bn; const KeySet newFactorKeys = newFactors.keys(); if (!this->empty()) { KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end()); - this->removeTop(keyVector, bn, orphans); + this->removeTop(keyVector, &bn, orphans); } // Add the removed top and the new factors @@ -40,7 +40,7 @@ void ISAM::update_internal(const FactorGraphType& newFactors, factors += newFactors; // Add the orphaned subtrees - for (const sharedClique& orphan : orphans) + for (const sharedClique& orphan : *orphans) factors += boost::make_shared >(orphan); // Get an ordering where the new keys are eliminated last @@ -62,7 +62,7 @@ template void ISAM::update(const FactorGraphType& newFactors, const Eliminate& function) { Cliques orphans; - this->update_internal(newFactors, orphans, function); + this->updateInternal(newFactors, &orphans, function); } } diff --git a/gtsam/inference/ISAM.h b/gtsam/inference/ISAM.h index d6a40b5393..fe6763a131 100644 --- a/gtsam/inference/ISAM.h +++ b/gtsam/inference/ISAM.h @@ -22,56 +22,67 @@ namespace gtsam { +/** + * A Bayes tree with an update methods that implements the iSAM algorithm. + * Given a set of new factors, it re-eliminates the invalidated part of the + * tree. \nosubgrouping + */ +template +class ISAM : public BAYESTREE { + public: + typedef BAYESTREE Base; + typedef typename Base::BayesNetType BayesNetType; + typedef typename Base::FactorGraphType FactorGraphType; + typedef typename Base::Clique Clique; + typedef typename Base::sharedClique sharedClique; + typedef typename Base::Cliques Cliques; + + private: + typedef typename Base::Eliminate Eliminate; + typedef typename Base::EliminationTraitsType EliminationTraitsType; + + public: + /// @name Standard Constructors + /// @{ + + /** Create an empty Bayes Tree */ + ISAM() {} + + /** Copy constructor */ + explicit ISAM(const Base& bayesTree) : Base(bayesTree) {} + + /// @} + /// @name Advanced Interface Interface + /// @{ + /** - * A Bayes tree with an update methods that implements the iSAM algorithm. - * Given a set of new factors, it re-eliminates the invalidated part of the tree. - * \nosubgrouping + * update the Bayes tree with a set of new factors, typically derived from + * measurements + * @param newFactors is a factor graph that contains the new factors + * @param function an elimination routine */ - template - class ISAM: public BAYESTREE - { - public: - - typedef BAYESTREE Base; - typedef typename Base::BayesNetType BayesNetType; - typedef typename Base::FactorGraphType FactorGraphType; - typedef typename Base::Clique Clique; - typedef typename Base::sharedClique sharedClique; - typedef typename Base::Cliques Cliques; - - private: - - typedef typename Base::Eliminate Eliminate; - typedef typename Base::EliminationTraitsType EliminationTraitsType; - - public: - - /// @name Standard Constructors - /// @{ - - /** Create an empty Bayes Tree */ - ISAM() {} - - /** Copy constructor */ - ISAM(const Base& bayesTree) : Base(bayesTree) {} - - /// @} - /// @name Advanced Interface Interface - /// @{ - - /** - * update the Bayes tree with a set of new factors, typically derived from measurements - * @param newFactors is a factor graph that contains the new factors - * @param function an elimination routine - */ - void update(const FactorGraphType& newFactors, const Eliminate& function = EliminationTraitsType::DefaultEliminate); - - /** update_internal provides access to list of orphans for drawing purposes */ - void update_internal(const FactorGraphType& newFactors, Cliques& orphans, + void update( + const FactorGraphType& newFactors, const Eliminate& function = EliminationTraitsType::DefaultEliminate); - /// @} - - }; + /** updateInternal provides access to list of orphans for drawing purposes + */ + void updateInternal( + const FactorGraphType& newFactors, Cliques* orphans, + const Eliminate& function = EliminationTraitsType::DefaultEliminate); -}/// namespace gtsam + /// @} + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + void update_internal( + const FactorGraphType& newFactors, Cliques& orphans, + const Eliminate& function = EliminationTraitsType::DefaultEliminate) { + updateInternal(newFactors, &orphans, function); + } + /// @} +#endif +}; + +} // namespace gtsam diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp index f257274410..dd433ff094 100644 --- a/gtsam/inference/Key.cpp +++ b/gtsam/inference/Key.cpp @@ -56,7 +56,7 @@ string _multirobotKeyFormatter(Key key) { /* ************************************************************************* */ template -static void Print(const CONTAINER& keys, const string& s, +void Print(const CONTAINER& keys, const string& s, const KeyFormatter& keyFormatter) { cout << s << " "; if (keys.empty()) @@ -83,6 +83,44 @@ void PrintKeySet(const KeySet& keys, const string& s, const KeyFormatter& keyFormatter) { Print(keys, s, keyFormatter); } + +/* ************************************************************************* */ +// Access to custom stream property. +void *&key_formatter::property(ios_base &s) { + static int kUniqueIndex = ios_base::xalloc(); + return s.pword(kUniqueIndex); +} + +/* ************************************************************************* */ +// Store pointer to formatter in property. +void key_formatter::set_property(ios_base &s, const KeyFormatter &f) { + property(s) = (void *)(&f); +} + +/* ************************************************************************* */ +// Get pointer to formatter from property. +KeyFormatter *key_formatter::get_property(ios_base &s) { + return (KeyFormatter *)(property(s)); +} + +/* ************************************************************************* */ +// Stream operator that will take a key_formatter and set the stream property. +ostream &operator<<(ostream &os, const key_formatter &m) { + key_formatter::set_property(os, m.formatter_); + return os; +} + +/* ************************************************************************* */ +// Stream operator that takes a StreamedKey and properly formats it +ostream &operator<<(ostream &os, const StreamedKey &streamedKey) { + const KeyFormatter *formatter = key_formatter::get_property(os); + if (formatter == nullptr) { + formatter = &DefaultKeyFormatter; + } + os << (*formatter)(streamedKey.key_); + return (os); +} + /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index d400a33c06..8b13f0b4ca 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -27,6 +27,8 @@ #include +#include + namespace gtsam { /// Typedef for a function to format a key, i.e. to convert it to a string @@ -52,6 +54,34 @@ GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key); static const gtsam::KeyFormatter MultiRobotKeyFormatter = &_multirobotKeyFormatter; +/// To use the key_formatter on Keys, they must be wrapped in a StreamedKey. +struct StreamedKey { + const Key &key_; + explicit StreamedKey(const Key &key) : key_(key) {} + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const StreamedKey &); +}; + +/** + * Output stream manipulator that will format gtsam::Keys according to the given + * KeyFormatter, as long as Key values are wrapped in a gtsam::StreamedKey. + * LabeledSymbol and Symbol values do not have to be wrapped. + * usage: + * Key key = LabeledSymbol('x', 'A', 5); // cast to key type + * cout << key_formatter(MultiRobotKeyFormatter) << StreamedKey(key); + */ +class key_formatter { + public: + explicit key_formatter(KeyFormatter v) : formatter_(v) {} + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const key_formatter &); + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const StreamedKey &); + + private: + KeyFormatter formatter_; + static void *&property(std::ios_base &s); + static void set_property(std::ios_base &s, const KeyFormatter &f); + static KeyFormatter *get_property(std::ios_base &s); +}; + /// Define collection type once and for all - also used in wrappers typedef FastVector KeyVector; diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index 0d9c35d2c7..6d34883fa3 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -121,6 +121,13 @@ boost::function LabeledSymbol::TypeLabelTest(unsigned char c, return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c && boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label; } + +/* ************************************************************************* */ +std::ostream &operator<<(std::ostream &os, const LabeledSymbol &symbol) { + os << StreamedKey(symbol); + return os; +} + /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 8c521b0678..5b3ec8766f 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -100,10 +100,15 @@ class GTSAM_EXPORT LabeledSymbol { LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); } LabeledSymbol lower() const { return LabeledSymbol(c_, tolower(label_), j_); } - // Create a new symbol with a different value + // Create a new symbol with a different character. LabeledSymbol newChr(unsigned char c) const { return LabeledSymbol(c, label_, j_); } + + // Create a new symbol with a different label. LabeledSymbol newLabel(unsigned char label) const { return LabeledSymbol(c_, label, j_); } + /// Output stream operator that can be used with key_formatter (see Key.h). + friend std::ostream &operator<<(std::ostream &, const LabeledSymbol &); + private: /** Serialization function */ diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 1165b4a0f7..da61ca57eb 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -79,7 +79,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, size_t index = 0; for (auto key_factors: variableIndex) { // Arrange factor indices into COLAMD format - const VariableIndex::Factors& column = key_factors.second; + const FactorIndices& column = key_factors.second; for(size_t factorIndex: column) { A[count++] = (int) factorIndex; // copy sparse column } diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp index ccabcb07e0..5e41b3eac4 100644 --- a/gtsam/inference/Symbol.cpp +++ b/gtsam/inference/Symbol.cpp @@ -66,5 +66,10 @@ boost::function Symbol::ChrTest(unsigned char c) { return bind(&Symbol::chr, bind(make, _1)) == c; } +std::ostream &operator<<(std::ostream &os, const Symbol &symbol) { + os << StreamedKey(symbol); + return os; +} + } // namespace gtsam diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 8e22202ed9..86574f70d4 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -112,6 +112,9 @@ class GTSAM_EXPORT Symbol { */ static boost::function ChrTest(unsigned char c); + /// Output stream operator that can be used with key_formatter (see Key.h). + friend std::ostream &operator<<(std::ostream &, const Symbol &); + private: /** Serialization function */ diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h index bc8100e4a3..727ef8fd84 100644 --- a/gtsam/inference/VariableIndex-inl.h +++ b/gtsam/inference/VariableIndex-inl.h @@ -67,8 +67,8 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); if (factors[i]) { for(Key j: *factors[i]) { - Factors& factorEntries = internalAt(j); - Factors::iterator entry = std::find(factorEntries.begin(), + FactorIndices& factorEntries = internalAt(j); + auto entry = std::find(factorEntries.begin(), factorEntries.end(), *factorIndex); if (entry == factorEntries.end()) throw std::invalid_argument( diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index a96a532897..f2ba3e31cb 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -41,26 +41,22 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT VariableIndex { -public: - + public: typedef boost::shared_ptr shared_ptr; - typedef FactorIndices Factors; - typedef Factors::iterator Factor_iterator; - typedef Factors::const_iterator Factor_const_iterator; + typedef FactorIndices::iterator Factor_iterator; + typedef FactorIndices::const_iterator Factor_const_iterator; -protected: - typedef FastMap KeyMap; + protected: + typedef FastMap KeyMap; KeyMap index_; - size_t nFactors_; // Number of factors in the original factor graph. - size_t nEntries_; // Sum of involved variable counts of each factor. + size_t nFactors_; // Number of factors in the original factor graph. + size_t nEntries_; // Sum of involved variable counts of each factor. -public: + public: typedef KeyMap::const_iterator const_iterator; typedef KeyMap::const_iterator iterator; typedef KeyMap::value_type value_type; -public: - /// @name Standard Constructors /// @{ @@ -71,8 +67,10 @@ class GTSAM_EXPORT VariableIndex { * Create a VariableIndex that computes and stores the block column structure * of a factor graph. */ - template - VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { augment(factorGraph); } + template + explicit VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { + augment(factorGraph); + } /// @} /// @name Standard Interface @@ -88,7 +86,7 @@ class GTSAM_EXPORT VariableIndex { size_t nEntries() const { return nEntries_; } /// Access a list of factors by variable - const Factors& operator[](Key variable) const { + const FactorIndices& operator[](Key variable) const { KeyMap::const_iterator item = index_.find(variable); if(item == index_.end()) throw std::invalid_argument("Requested non-existent variable from VariableIndex"); @@ -96,6 +94,11 @@ class GTSAM_EXPORT VariableIndex { return item->second; } + /// Return true if no factors associated with a variable + bool empty(Key variable) const { + return (*this)[variable].empty(); + } + /// @} /// @name Testable /// @{ @@ -166,16 +169,18 @@ class GTSAM_EXPORT VariableIndex { Factor_const_iterator factorsEnd(Key variable) const { return internalAt(variable).end(); } /// Internal version of 'at' that asserts existence - const Factors& internalAt(Key variable) const { + const FactorIndices& internalAt(Key variable) const { const KeyMap::const_iterator item = index_.find(variable); assert(item != index_.end()); - return item->second; } + return item->second; + } /// Internal version of 'at' that asserts existence - Factors& internalAt(Key variable) { + FactorIndices& internalAt(Key variable) { const KeyMap::iterator item = index_.find(variable); assert(item != index_.end()); - return item->second; } + return item->second; + } /// @} }; diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index 93a161ccde..a602585814 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -22,6 +22,9 @@ #include #include // for operator += + +#include + using namespace boost::assign; using namespace std; using namespace gtsam; @@ -41,17 +44,15 @@ TEST(Key, KeySymbolConversion) { template Key KeyTestValue(); -template<> -Key KeyTestValue<8>() -{ +template <> +Key KeyTestValue<8>() { return 0x6100000000000005; -}; +} -template<> -Key KeyTestValue<4>() -{ +template <> +Key KeyTestValue<4>() { return 0x61000005; -}; +} /* ************************************************************************* */ TEST(Key, KeySymbolEncoding) { @@ -68,12 +69,41 @@ TEST(Key, KeySymbolEncoding) { /* ************************************************************************* */ TEST(Key, ChrTest) { - Key key = Symbol('c',3); + Symbol key('c', 3); EXPECT(Symbol::ChrTest('c')(key)); EXPECT(!Symbol::ChrTest('d')(key)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +// A custom (nonsensical) formatter. +string keyMyFormatter(Key key) { + return "special"; +} + +TEST(Key, Formatting) { + Symbol key('c', 3); + EXPECT("c3" == DefaultKeyFormatter(key)); + + // Try streaming keys, should be default-formatted. + stringstream ss; + ss << StreamedKey(key); + EXPECT("c3" == ss.str()); + + // use key_formatter with a function pointer + stringstream ss2; + ss2 << key_formatter(keyMyFormatter) << StreamedKey(key); + EXPECT("special" == ss2.str()); + + // use key_formatter with a function object. + stringstream ss3; + ss3 << key_formatter(DefaultKeyFormatter) << StreamedKey(key); + EXPECT("c3" == ss3.str()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/inference/tests/testLabeledSymbol.cpp b/gtsam/inference/tests/testLabeledSymbol.cpp index b463f41310..4ac171c73d 100644 --- a/gtsam/inference/tests/testLabeledSymbol.cpp +++ b/gtsam/inference/tests/testLabeledSymbol.cpp @@ -80,6 +80,29 @@ TEST(LabeledSymbol, ChrTest) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +// A custom (nonsensical) formatter. +string labeledSymbolMyFormatter(Key key) { + return "special"; +} + +TEST(LabeledSymbol, Formatting) { + LabeledSymbol symbol('c', 'A', 3); + + // use key_formatter with a function pointer + stringstream ss2; + ss2 << key_formatter(labeledSymbolMyFormatter) << symbol; + EXPECT("special" == ss2.str()); + + // use key_formatter with a function object. + stringstream ss3; + ss3 << key_formatter(MultiRobotKeyFormatter) << symbol; + EXPECT("cA3" == ss3.str()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/inference/tests/testSymbol.cpp b/gtsam/inference/tests/testSymbol.cpp new file mode 100644 index 0000000000..bedd690441 --- /dev/null +++ b/gtsam/inference/tests/testSymbol.cpp @@ -0,0 +1,50 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testSymbol.cpp + * @author Frank Dellaert + */ + +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// A custom (nonsensical) formatter. +string symbolMyFormatter(Key key) { + return "special"; +} + +TEST(Symbol, Formatting) { + Symbol symbol('c', 3); + + // use key_formatter with a function pointer + stringstream ss2; + ss2 << key_formatter(symbolMyFormatter) << symbol; + EXPECT("special" == ss2.str()); + + // use key_formatter with a function object. + stringstream ss3; + ss3 << key_formatter(MultiRobotKeyFormatter) << symbol; + EXPECT("c3" == ss3.str()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index 2365388d6d..8d0fafb618 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -35,12 +35,15 @@ namespace gtsam { namespace internal { /* ************************************************************************* */ - double logDeterminant(const GaussianBayesTreeClique::shared_ptr& clique, double& parentSum) - { - parentSum += clique->conditional()->R().diagonal().unaryExpr(std::ptr_fun(log)).sum(); - assert(false); - return 0; - } + double logDeterminant(const GaussianBayesTreeClique::shared_ptr& clique, + double& parentSum) { + parentSum += clique->conditional() + ->R() + .diagonal() + .unaryExpr(std::ptr_fun(log)) + .sum(); + return 0; + } } /* ************************************************************************* */ diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index d16373c78e..c208259b86 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -505,7 +505,7 @@ VectorValues HessianFactor::solve() { // Do Cholesky Factorization const size_t n = size(); - assert(info_.nBlocks() == n + 1); + assert(size_t(info_.nBlocks()) == n + 1); info_.choleskyPartial(n); auto R = info_.triangularView(0, n); auto eta = linearTerm(); diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index f0fbfbfd20..758d2aec97 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -41,7 +41,7 @@ class VectorValues; /** * parameters for iterative linear solvers */ -class GTSAM_EXPORT IterativeOptimizationParameters { +class IterativeOptimizationParameters { public: @@ -63,27 +63,27 @@ class GTSAM_EXPORT IterativeOptimizationParameters { inline Verbosity verbosity() const { return verbosity_; } - std::string getVerbosity() const; - void setVerbosity(const std::string &s); + GTSAM_EXPORT std::string getVerbosity() const; + GTSAM_EXPORT void setVerbosity(const std::string &s); /* matlab interface */ - void print() const; + GTSAM_EXPORT void print() const; /* virtual print function */ - virtual void print(std::ostream &os) const; + GTSAM_EXPORT virtual void print(std::ostream &os) const; /* for serialization */ friend std::ostream& operator<<(std::ostream &os, const IterativeOptimizationParameters &p); - static Verbosity verbosityTranslator(const std::string &s); - static std::string verbosityTranslator(Verbosity v); + GTSAM_EXPORT static Verbosity verbosityTranslator(const std::string &s); + GTSAM_EXPORT static std::string verbosityTranslator(Verbosity v); }; /** * Base class for Iterative Solvers like SubgraphSolver */ -class GTSAM_EXPORT IterativeSolver { +class IterativeSolver { public: typedef boost::shared_ptr shared_ptr; IterativeSolver() { @@ -92,12 +92,12 @@ class GTSAM_EXPORT IterativeSolver { } /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ - VectorValues optimize(const GaussianFactorGraph &gfg, + GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, boost::optional = boost::none, boost::optional&> lambda = boost::none); /* interface to the nonlinear optimizer, without initial estimate */ - VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, + GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda); /* interface to the nonlinear optimizer that the subclasses have to implement */ diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 4d9c7883b9..a01233fad3 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -808,8 +808,25 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { } /* ************************************************************************* */ -// Welsh +// Welsch /* ************************************************************************* */ +Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} + +void Welsch::print(const std::string &s="") const { + std::cout << s << ": Welsch (" << c_ << ")" << std::endl; +} + +bool Welsch::equals(const Base &expected, double tol) const { + const Welsch* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(c_ - p->c_) < tol; +} + +Welsch::shared_ptr Welsch::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new Welsch(c, reweight)); +} + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} void Welsh::print(const std::string &s="") const { @@ -825,6 +842,7 @@ bool Welsh::equals(const Base &expected, double tol) const { Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Welsh(c, reweight)); } +#endif /* ************************************************************************* */ // GemanMcClure @@ -847,7 +865,7 @@ void GemanMcClure::print(const std::string &s="") const { bool GemanMcClure::equals(const Base &expected, double tol) const { const GemanMcClure* p = dynamic_cast(&expected); if (p == NULL) return false; - return fabs(c_ - p->c_) < tol; + return std::abs(c_ - p->c_) < tol; } GemanMcClure::shared_ptr GemanMcClure::Create(double c, const ReweightScheme reweight) { @@ -879,7 +897,7 @@ void DCS::print(const std::string &s="") const { bool DCS::equals(const Base &expected, double tol) const { const DCS* p = dynamic_cast(&expected); if (p == NULL) return false; - return fabs(c_ - p->c_) < tol; + return std::abs(c_ - p->c_) < tol; } DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) { @@ -903,7 +921,7 @@ void L2WithDeadZone::print(const std::string &s="") const { bool L2WithDeadZone::equals(const Base &expected, double tol) const { const L2WithDeadZone* p = dynamic_cast(&expected); if (p == NULL) return false; - return fabs(k_ - p->k_) < tol; + return std::abs(k_ - p->k_) < tol; } L2WithDeadZone::shared_ptr L2WithDeadZone::Create(double k, const ReweightScheme reweight) { diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index e495921c2d..5c540caa39 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -629,7 +629,7 @@ namespace gtsam { /** * The mEstimator name space contains all robust error functions. * It mirrors the exposition at - * http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html + * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. * * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: @@ -681,7 +681,7 @@ namespace gtsam { /* * This method is responsible for returning the weight function for a given amount of error. * The weight function is related to the analytic derivative of the residual function. See - * http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html + * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf * for details. This method is required when optimizing cost functions with robust penalties * using iteratively re-weighted least squares. */ @@ -750,7 +750,7 @@ namespace gtsam { Fair(double c = 1.3998, const ReweightScheme reweight = Block); double weight(double error) const { - return 1.0 / (1.0 + fabs(error) / c_); + return 1.0 / (1.0 + std::abs(error) / c_); } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; @@ -776,7 +776,8 @@ namespace gtsam { Huber(double k = 1.345, const ReweightScheme reweight = Block); double weight(double error) const { - return (error < k_) ? (1.0) : (k_ / fabs(error)); + double absError = std::abs(error); + return (absError < k_) ? (1.0) : (k_ / absError); } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; @@ -832,7 +833,7 @@ namespace gtsam { Tukey(double c = 4.6851, const ReweightScheme reweight = Block); double weight(double error) const { - if (std::fabs(error) <= c_) { + if (std::abs(error) <= c_) { double xc2 = error*error/csquared_; return (1.0-xc2)*(1.0-xc2); } @@ -852,7 +853,38 @@ namespace gtsam { } }; - /// Welsh implements the "Welsh" robust error model (Zhang97ivc) + /// Welsch implements the "Welsch" robust error model (Zhang97ivc) + class GTSAM_EXPORT Welsch : public Base { + protected: + double c_, csquared_; + + public: + typedef boost::shared_ptr shared_ptr; + + Welsch(double c = 2.9846, const ReweightScheme reweight = Block); + double weight(double error) const { + double xc2 = (error*error)/csquared_; + return std::exp(-xc2); + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(c_); + } + }; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + // Welsh implements the "Welsch" robust error model (Zhang97ivc) + // This was misspelled in previous versions of gtsam and should be + // removed in the future. class GTSAM_EXPORT Welsh : public Base { protected: double c_, csquared_; @@ -878,6 +910,7 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(c_); } }; +#endif /// GemanMcClure implements the "Geman-McClure" robust error model /// (Zhang97ivc). @@ -952,14 +985,14 @@ namespace gtsam { L2WithDeadZone(double k, const ReweightScheme reweight = Block); double residual(double error) const { - const double abs_error = fabs(error); + const double abs_error = std::abs(error); return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); } double weight(double error) const { // note that this code is slightly uglier than above, because there are three distinct // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two // cases (deadzone, non-deadzone) above. - if (fabs(error) <= k_) return 0.0; + if (std::abs(error) <= k_) return 0.0; else if (error > k_) return (-k_+error)/error; else return (k_+error)/error; } diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index 22ad89cd88..a999b3a719 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -107,7 +107,7 @@ static vector UniqueSampler(const vector &weight, const size_t m = weight.size(); if (n > m) throw std::invalid_argument("UniqueSampler: invalid input size"); - vector samples; + vector results; size_t count = 0; vector touched(m, false); @@ -130,12 +130,12 @@ static vector UniqueSampler(const vector &weight, for (const size_t &index : samples) { if (touched[index] == false) { touched[index] = true; - samples.push_back(index); + results.push_back(index); if (++count >= n) break; } } } - return samples; + return results; } /****************************************************************************/ diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index e8304e6e72..cd3ae815b7 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -129,12 +129,31 @@ namespace gtsam { } /* ************************************************************************* */ - void VectorValues::print(const string& str, const KeyFormatter& formatter) const { + ostream& operator<<(ostream& os, const VectorValues& v) { + // Change print depending on whether we are using TBB +#ifdef GTSAM_USE_TBB + map sorted; + for (const auto& key_value : v) { + sorted.emplace(key_value.first, key_value.second); + } + for (const auto& key_value : sorted) +#else + for (const auto& key_value : v) +#endif + { + os << " " << StreamedKey(key_value.first) << ": " << key_value.second.transpose() + << "\n"; + } + return os; + } + + /* ************************************************************************* */ + void VectorValues::print(const string& str, + const KeyFormatter& formatter) const { cout << str << ": " << size() << " elements\n"; - for(const value_type& key_value: *this) - cout << " " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n"; + cout << key_formatter(formatter) << *this; cout.flush(); - } +} /* ************************************************************************* */ bool VectorValues::equals(const VectorValues& x, double tol) const { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index b1d9de585d..fe6b5fcb2c 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -29,6 +29,7 @@ #include #include +#include namespace gtsam { @@ -228,8 +229,11 @@ namespace gtsam { */ const_iterator find(Key j) const { return values_.find(j); } + /// overload operator << to print to stringstream + friend std::ostream& operator<<(std::ostream&, const VectorValues&); + /** print required by Testable for unit testing */ - void print(const std::string& str = "VectorValues: ", + void print(const std::string& str = "VectorValues", const KeyFormatter& formatter = DefaultKeyFormatter) const; /** equals required by Testable for unit testing */ diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 6b130bea0c..85dc4735dd 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -452,19 +452,20 @@ TEST(NoiseModel, WhitenInPlace) /* * These tests are responsible for testing the weight functions for the m-estimators in GTSAM. * The weight function is related to the analytic derivative of the residual function. See - * http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html + * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf * for details. This weight function is required when optimizing cost functions with robust * penalties using iteratively re-weighted least squares. */ TEST(NoiseModel, robustFunctionHuber) { - const double k = 5.0, error1 = 1.0, error2 = 10.0; + const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; const mEstimator::Huber::shared_ptr huber = mEstimator::Huber::Create(k); - const double weight1 = huber->weight(error1), - weight2 = huber->weight(error2); - DOUBLES_EQUAL(1.0, weight1, 1e-8); - DOUBLES_EQUAL(0.5, weight2, 1e-8); + DOUBLES_EQUAL(1.0, huber->weight(error1), 1e-8); + DOUBLES_EQUAL(0.5, huber->weight(error2), 1e-8); + // Test negative value to ensure we take absolute value of error. + DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8); + DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8); } TEST(NoiseModel, robustFunctionGemanMcClure) diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index d1d9990b0a..f97f96aafd 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -17,6 +17,7 @@ #include #include +#include #include @@ -24,6 +25,8 @@ #include #include +#include + using namespace std; using namespace boost::assign; using boost::adaptors::map_keys; @@ -228,6 +231,23 @@ TEST(VectorValues, vector_sub) EXPECT(assert_equal(expected, vv.vector(dims))); } +/* ************************************************************************* */ +TEST(VectorValues, print) +{ + VectorValues vv; + vv.insert(0, (Vector(1) << 1).finished()); + vv.insert(1, Vector2(2, 3)); + vv.insert(2, Vector2(4, 5)); + vv.insert(5, Vector2(6, 7)); + vv.insert(7, Vector2(8, 9)); + + string expected = + " 0: 1\n 1: 2 3\n 2: 4 5\n 5: 6 7\n 7: 8 9\n"; + stringstream actual; + actual << vv; + EXPECT(expected == actual.str()); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 8ed695622a..1418ab687d 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -55,6 +55,26 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation resetIntegration(); } + /** + * Non-Default constructor, initialize with measurements + * @param p: Parameters for AHRS pre-integration + * @param bias_hat: Current estimate of acceleration and rotation rate biases + * @param deltaTij: Delta time in pre-integration + * @param deltaRij: Delta rotation in pre-integration + * @param delRdelBiasOmega: Jacobian of rotation wrt. to gyro bias + * @param preint_meas_cov: Pre-integration covariance + */ + PreintegratedAhrsMeasurements( + const boost::shared_ptr& p, + const Vector3& bias_hat, + double deltaTij, + const Rot3& deltaRij, + const Matrix3& delRdelBiasOmega, + const Matrix3& preint_meas_cov) : + PreintegratedRotation(p, deltaTij, deltaRij, delRdelBiasOmega), + biasHat_(bias_hat), + preintMeasCov_(preint_meas_cov) {} + const Params& p() const { return *boost::static_pointer_cast(p_);} const Vector3& biasHat() const { return biasHat_; } const Matrix3& preintMeasCov() const { return preintMeasCov_; } diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index a7802120c9..2ad71cb3cb 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -64,7 +64,7 @@ typedef ManifoldPreintegration PreintegrationType; * * @addtogroup SLAM */ -class PreintegratedCombinedMeasurements : public PreintegrationType { +class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType { public: @@ -222,7 +222,7 @@ class PreintegratedCombinedMeasurements : public PreintegrationType { * * @addtogroup SLAM */ -class CombinedImuFactor: public NoiseModelFactor6 { public: diff --git a/gtsam/navigation/ManifoldPreintegration.cpp b/gtsam/navigation/ManifoldPreintegration.cpp index cc88d91014..165acdaf0e 100644 --- a/gtsam/navigation/ManifoldPreintegration.cpp +++ b/gtsam/navigation/ManifoldPreintegration.cpp @@ -46,7 +46,7 @@ void ManifoldPreintegration::resetIntegration() { //------------------------------------------------------------------------------ bool ManifoldPreintegration::equals(const ManifoldPreintegration& other, double tol) const { - return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol + return p_->equals(*other.p_, tol) && std::abs(deltaTij_ - other.deltaTij_) < tol && biasHat_.equals(other.biasHat_, tol) && deltaXij_.equals(other.deltaXij_, tol) && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index ec91d69b83..8c29d85ddb 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -65,7 +65,7 @@ bool PreintegratedRotation::equals(const PreintegratedRotation& other, double tol) const { return this->matchesParamsWith(other) && deltaRij_.equals(other.deltaRij_, tol) - && fabs(deltaTij_ - other.deltaTij_) < tol + && std::abs(deltaTij_ - other.deltaTij_) < tol && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); } diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index bf2f5c0c8e..71ef5d08fb 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -35,6 +35,13 @@ struct GTSAM_EXPORT PreintegratedRotationParams { PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {} + PreintegratedRotationParams(const Matrix3& gyroscope_covariance, + boost::optional omega_coriolis) + : gyroscopeCovariance(gyroscope_covariance) { + if (omega_coriolis) + omegaCoriolis.reset(omega_coriolis.get()); + } + virtual ~PreintegratedRotationParams() {} virtual void print(const std::string& s) const; diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index fcba92f60a..3938ce86c4 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -31,6 +31,7 @@ static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; PreintegratedImuMeasurements ScenarioRunner::integrate( double T, const Bias& estimatedBias, bool corrupted) const { + gttic_(integrate); PreintegratedImuMeasurements pim(p_, estimatedBias); const double dt = imuSampleTime(); diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index de451b7e35..5fb9f78d77 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -36,7 +36,7 @@ static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) { * Simple class to test navigation scenarios. * Takes a trajectory scenario as input, and can generate IMU measurements */ -class ScenarioRunner { +class GTSAM_EXPORT ScenarioRunner { public: typedef imuBias::ConstantBias Bias; typedef boost::shared_ptr SharedParams; diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp index 0ac57d05c7..4229d4f0ce 100644 --- a/gtsam/navigation/TangentPreintegration.cpp +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -41,7 +41,7 @@ void TangentPreintegration::resetIntegration() { //------------------------------------------------------------------------------ bool TangentPreintegration::equals(const TangentPreintegration& other, double tol) const { - return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol + return p_->equals(*other.p_, tol) && std::abs(deltaTij_ - other.deltaTij_) < tol && biasHat_.equals(other.biasHat_, tol) && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) && equal_with_abs_tol(preintegrated_H_biasAcc_, diff --git a/gtsam/navigation/expressions.h b/gtsam/navigation/expressions.h new file mode 100644 index 0000000000..231ccb20d2 --- /dev/null +++ b/gtsam/navigation/expressions.h @@ -0,0 +1,44 @@ +/** + * @file expressions.h + * @brief Common expressions for solving navigation problems + * @date May, 2019 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +typedef Expression NavState_; +typedef Expression Velocity3_; + +namespace internal { +// define getters that return a value rather than a reference +Rot3 attitude(const NavState& X, OptionalJacobian<3, 9> H) { + return X.attitude(H); +} +Point3 position(const NavState& X, OptionalJacobian<3, 9> H) { + return X.position(H); +} +Velocity3 velocity(const NavState& X, OptionalJacobian<3, 9> H) { + return X.velocity(H); +} +} // namespace internal + +// overloads for getters +inline Rot3_ attitude(const NavState_& X) { + return Rot3_(internal::attitude, X); +} +inline Point3_ position(const NavState_& X) { + return Point3_(internal::position, X); +} +inline Velocity3_ velocity(const NavState_& X) { + return Velocity3_(internal::velocity, X); +} + +} // namespace gtsam diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 208d0e7098..d32553b94a 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -119,6 +119,34 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); } +//****************************************************************************** +TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) { + Matrix3 gyroscopeCovariance = Matrix3::Ones()*0.4; + Vector3 omegaCoriolis(0.1, 0.5, 0.9); + PreintegratedRotationParams params(gyroscopeCovariance, omegaCoriolis); + Vector3 bias(1.0,2.0,3.0); ///< Current estimate of angular rate bias + Rot3 deltaRij(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); + double deltaTij = 0.02; + Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5; + Matrix3 preintMeasCov = Matrix3::Ones()*0.2; + PreintegratedAhrsMeasurements actualPim( + boost::make_shared(params), + bias, + deltaTij, + deltaRij, + delRdelBiasOmega, + preintMeasCov); + EXPECT(assert_equal(gyroscopeCovariance, + actualPim.p().getGyroscopeCovariance(), 1e-6)); + EXPECT(assert_equal(omegaCoriolis, + actualPim.p().getOmegaCoriolis().get(), 1e-6)); + EXPECT(assert_equal(bias, actualPim.biasHat(), 1e-6)); + DOUBLES_EQUAL(deltaTij, actualPim.deltaTij(), 1e-6); + EXPECT(assert_equal(deltaRij, Rot3(actualPim.deltaRij()), 1e-6)); + EXPECT(assert_equal(delRdelBiasOmega, actualPim.delRdelBiasOmega(), 1e-6)); + EXPECT(assert_equal(preintMeasCov, actualPim.preintMeasCov(), 1e-6)); +} + /* ************************************************************************* */ TEST(AHRSFactor, Error) { // Linearization point diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index c1c17a6d40..3ef810cad9 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -163,11 +163,11 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) { // Measurements const Vector3 measuredOmega(0, 0.1, 0); // M_PI/10.0+0.3; const Vector3 measuredAcc(0, 1.1, -kGravity); - const double deltaT = 0.001; + const double deltaT = 0.01; PreintegratedCombinedMeasurements pim(p, bias); - for (int i = 0; i < 1000; ++i) + for (int i = 0; i < 100; ++i) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor @@ -190,9 +190,9 @@ TEST(CombinedImuFactor, PredictRotation) { PreintegratedCombinedMeasurements pim(p, bias); const Vector3 measuredAcc = - kGravityAlongNavZDown; const Vector3 measuredOmega(0, 0, M_PI / 10.0); - const double deltaT = 0.001; + const double deltaT = 0.01; const double tol = 1e-4; - for (int i = 0; i < 1000; ++i) + for (int i = 0; i < 100; ++i) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 48e4dd5e33..460695ec69 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -18,6 +18,8 @@ * @author Stephen Williams */ +// #define ENABLE_TIMING // uncomment for timing results + #include #include #include @@ -111,7 +113,7 @@ TEST(ImuFactor, Accelerating) { PreintegratedImuMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } @@ -569,6 +571,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { /* ************************************************************************* */ TEST(ImuFactor, PredictPositionAndVelocity) { + gttic(PredictPositionAndVelocity); Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements @@ -597,6 +600,7 @@ TEST(ImuFactor, PredictPositionAndVelocity) { /* ************************************************************************* */ TEST(ImuFactor, PredictRotation) { + gttic(PredictRotation); Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements @@ -623,6 +627,7 @@ TEST(ImuFactor, PredictRotation) { /* ************************************************************************* */ TEST(ImuFactor, PredictArbitrary) { + gttic(PredictArbitrary); Pose3 x1; const Vector3 v1(0, 0, 0); @@ -675,6 +680,7 @@ TEST(ImuFactor, PredictArbitrary) { /* ************************************************************************* */ TEST(ImuFactor, bodyPSensorNoBias) { + gttic(bodyPSensorNoBias); Bias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) // Rotate sensor (z-down) to body (same as navigation) i.e. z-up @@ -716,10 +722,11 @@ TEST(ImuFactor, bodyPSensorNoBias) { #include TEST(ImuFactor, bodyPSensorWithBias) { + gttic(bodyPSensorWithBias); using noiseModel::Diagonal; typedef Bias Bias; - int numFactors = 80; + int numFactors = 10; Vector6 noiseBetweenBiasSigma; noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6, 3.0e-6, 3.0e-6); @@ -899,6 +906,7 @@ TEST(ImuFactor, MergeWithCoriolis) { /* ************************************************************************* */ // Same values as pre-integration test but now testing covariance TEST(ImuFactor, CheckCovariance) { + gttic(CheckCovariance); // Measurements Vector3 measuredAcc(0.1, 0.0, 0.0); Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); @@ -922,6 +930,10 @@ TEST(ImuFactor, CheckCovariance) { /* ************************************************************************* */ int main() { TestResult tr; - return TestRegistry::runAllTests(tr); + auto result = TestRegistry::runAllTests(tr); +#ifdef ENABLE_TIMING + tictoc_print_(); +#endif + return result; } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testNavExpressions.cpp b/gtsam/navigation/tests/testNavExpressions.cpp new file mode 100644 index 0000000000..374732fd4a --- /dev/null +++ b/gtsam/navigation/tests/testNavExpressions.cpp @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testExpressions.cpp + * @date May 2019 + * @author Frank Dellaert + * @brief unit tests for navigation expression helpers + */ + +#include +#include +#include +#include +#include +#include + +#include + +using namespace gtsam; + +// A NavState unknown expression wXb with key 42 +Expression wXb_(42); + +/* ************************************************************************* */ +// Example: absolute position measurement +TEST(Expressions, Position) { + auto absolutePosition_ = position(wXb_); + + // Test with some values + Values values; + values.insert(42, NavState(Rot3(), Point3(1, 2, 3), Velocity3(4, 5, 6))); + EXPECT(assert_equal(Point3(1, 2, 3), absolutePosition_.value(values))); +} + +/* ************************************************************************* */ +// Example: velocity measurement in body frame +TEST(Expressions, Velocity) { + // We want to predict h(wXb) = velocity in body frame + // h(wXb) = bRw(wXb) * wV(wXb) + auto bodyVelocity_ = unrotate(attitude(wXb_), velocity(wXb_)); + + // Test with some values + Values values; + values.insert(42, NavState(Rot3(), Point3(1, 2, 3), Velocity3(4, 5, 6))); + EXPECT(assert_equal(Velocity3(4, 5, 6), bodyVelocity_.value(values))); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarios.cpp index 2129d2d922..adac0fb535 100644 --- a/gtsam/navigation/tests/testScenarios.cpp +++ b/gtsam/navigation/tests/testScenarios.cpp @@ -15,6 +15,8 @@ * @author Frank Dellaert */ +// #define ENABLE_TIMING // uncomment for timing results + #include #include #include @@ -46,6 +48,7 @@ static boost::shared_ptr defaultParams() { /* ************************************************************************* */ TEST(ScenarioRunner, Spin) { + gttic(Spin); // angular velocity 6 degree/sec const double w = 6 * kDegree; const Vector3 W(0, 0, w), V(0, 0, 0); @@ -63,12 +66,12 @@ TEST(ScenarioRunner, Spin) { Matrix6 expected; expected << p->accelerometerCovariance / kDt, Z_3x3, // Z_3x3, p->gyroscopeCovariance / kDt; - Matrix6 actual = runner.estimateNoiseCovariance(10000); + Matrix6 actual = runner.estimateNoiseCovariance(1000); EXPECT(assert_equal(expected, actual, 1e-2)); #endif // Check calculated covariance against Monte Carlo estimate - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); } @@ -79,6 +82,7 @@ ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0)); } /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { + gttic(Forward); using namespace forward; ScenarioRunner runner(scenario, defaultParams(), kDt); const double T = 0.1; // seconds @@ -86,24 +90,26 @@ TEST(ScenarioRunner, Forward) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); } /* ************************************************************************* */ TEST(ScenarioRunner, ForwardWithBias) { + gttic(ForwardWithBias); using namespace forward; ScenarioRunner runner(scenario, defaultParams(), kDt); const double T = 0.1; // seconds auto pim = runner.integrate(T, kNonZeroBias); - Matrix9 estimatedCov = runner.estimateCovariance(T, 1000, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, Circle) { + gttic(Circle); // Forward velocity 2m/s, angular velocity 6 degree/sec const double v = 2, w = 6 * kDegree; ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); @@ -114,13 +120,14 @@ TEST(ScenarioRunner, Circle) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); } /* ************************************************************************* */ TEST(ScenarioRunner, Loop) { + gttic(Loop); // Forward velocity 2m/s // Pitch up with angular velocity 6 degree/sec (negative in FLU) const double v = 2, w = 6 * kDegree; @@ -132,7 +139,7 @@ TEST(ScenarioRunner, Loop) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); } @@ -156,29 +163,32 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating) { + gttic(Accelerating); using namespace accelerating; ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias) { + gttic(AcceleratingWithBias); using namespace accelerating; ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); - Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingAndRotating) { + gttic(AcceleratingAndRotating); using namespace initial; const double a = 0.2; // m/s^2 const Vector3 A(0, a, 0), W(0, 0.1, 0); @@ -190,7 +200,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } @@ -215,29 +225,32 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating2) { + gttic(Accelerating2); using namespace accelerating2; ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias2) { + gttic(AcceleratingWithBias2); using namespace accelerating2; ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); - Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingAndRotating2) { + gttic(AcceleratingAndRotating2); using namespace initial2; const double a = 0.2; // m/s^2 const Vector3 A(0, a, 0), W(0, 0.1, 0); @@ -249,7 +262,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating2) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } @@ -275,29 +288,32 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating3) { + gttic(Accelerating3); using namespace accelerating3; ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias3) { + gttic(AcceleratingWithBias3); using namespace accelerating3; ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); - Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingAndRotating3) { + gttic(AcceleratingAndRotating3); using namespace initial3; const double a = 0.2; // m/s^2 const Vector3 A(0, a, 0), W(0, 0.1, 0); @@ -309,7 +325,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating3) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } @@ -336,29 +352,32 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating4) { + gttic(Accelerating4); using namespace accelerating4; ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias4) { + gttic(AcceleratingWithBias4); using namespace accelerating4; ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); - Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingAndRotating4) { + gttic(AcceleratingAndRotating4); using namespace initial4; const double a = 0.2; // m/s^2 const Vector3 A(0, a, 0), W(0, 0.1, 0); @@ -370,7 +389,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) { auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - Matrix9 estimatedCov = runner.estimateCovariance(T); + Matrix9 estimatedCov = runner.estimateCovariance(T, 100); EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } @@ -379,7 +398,9 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) { int main() { TestResult tr; auto result = TestRegistry::runAllTests(tr); +#ifdef ENABLE_TIMING tictoc_print_(); +#endif return result; } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index 1fa25fd60c..6b30974762 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -41,7 +41,7 @@ class GTSAM_EXPORT DoglegParams : public NonlinearOptimizerParams { VerbosityDL verbosityDL; ///< The verbosity level for Dogleg (default: SILENT), see also NonlinearOptimizerParams::verbosity DoglegParams() : - deltaInitial(10.0), verbosityDL(SILENT) {} + deltaInitial(1.0), verbosityDL(SILENT) {} virtual ~DoglegParams() {} diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 9a70678780..8ce9b361ed 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -177,7 +177,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( gttic(adjust_delta); // Compute gain ratio. Here we take advantage of the invariant that the // Bayes' net error at zero is equal to the nonlinear error - const double rho = fabs(f_error - result.f_error) < 1e-15 || fabs(M_error - new_M_error) < 1e-15 ? + const double rho = std::abs(f_error - result.f_error) < 1e-15 || std::abs(M_error - new_M_error) < 1e-15 ? 0.5 : (f_error - result.f_error) / (M_error - new_M_error); @@ -191,7 +191,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY) stay = false; // If not searching, just return with the new delta else if(mode == SEARCH_EACH_ITERATION) { - if(fabs(newDelta - delta) < 1e-15 || lastAction == DECREASED_DELTA) + if(std::abs(newDelta - delta) < 1e-15 || lastAction == DECREASED_DELTA) stay = false; // Searching, but Newton's solution is within trust region so keep the same trust region else { stay = true; // Searching and increased delta, so try again to increase delta diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 11be14c449..a809cbc083 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -31,150 +31,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, - bool useUnusedSlots, - NonlinearFactorGraph* nonlinearFactors, - FactorIndices* newFactorIndices) { - newFactorIndices->resize(newFactors.size()); - - if (useUnusedSlots) { - size_t globalFactorIndex = 0; - for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); - ++newFactorIndex) { - // Loop to find the next available factor slot - do { - // If we need to add more factors than we have room for, resize - // nonlinearFactors, filling the new slots with NULL factors. Otherwise, - // check if the current factor in nonlinearFactors is already used, and - // if so, increase globalFactorIndex. If the current factor in - // nonlinearFactors is unused, break out of the loop and use the current - // slot. - if (globalFactorIndex >= nonlinearFactors->size()) - nonlinearFactors->resize(nonlinearFactors->size() + - newFactors.size() - newFactorIndex); - else if ((*nonlinearFactors)[globalFactorIndex]) - ++globalFactorIndex; - else - break; - } while (true); - - // Use the current slot, updating nonlinearFactors and newFactorSlots. - (*nonlinearFactors)[globalFactorIndex] = newFactors[newFactorIndex]; - (*newFactorIndices)[newFactorIndex] = globalFactorIndex; - } - } else { - // We're not looking for unused slots, so just add the factors at the end. - for (size_t i = 0; i < newFactors.size(); ++i) - (*newFactorIndices)[i] = i + nonlinearFactors->size(); - nonlinearFactors->push_back(newFactors); - } -} - -/* ************************************************************************* */ -KeySet ISAM2::Impl::CheckRelinearizationFull( - const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { - KeySet relinKeys; - - if (const double* threshold = boost::get(&relinearizeThreshold)) { - for (const VectorValues::KeyValuePair& key_delta : delta) { - double maxDelta = key_delta.second.lpNorm(); - if (maxDelta >= *threshold) relinKeys.insert(key_delta.first); - } - } else if (const FastMap* thresholds = - boost::get >(&relinearizeThreshold)) { - for (const VectorValues::KeyValuePair& key_delta : delta) { - const Vector& threshold = - thresholds->find(Symbol(key_delta.first).chr())->second; - if (threshold.rows() != key_delta.second.rows()) - throw std::invalid_argument( - "Relinearization threshold vector dimensionality for '" + - std::string(1, Symbol(key_delta.first).chr()) + - "' passed into iSAM2 parameters does not match actual variable " - "dimensionality."); - if ((key_delta.second.array().abs() > threshold.array()).any()) - relinKeys.insert(key_delta.first); - } - } - - return relinKeys; -} - -/* ************************************************************************* */ -static void CheckRelinearizationRecursiveDouble( - double threshold, const VectorValues& delta, - const ISAM2::sharedClique& clique, KeySet* relinKeys) { - // Check the current clique for relinearization - bool relinearize = false; - for (Key var : *clique->conditional()) { - double maxDelta = delta[var].lpNorm(); - if (maxDelta >= threshold) { - relinKeys->insert(var); - relinearize = true; - } - } - - // If this node was relinearized, also check its children - if (relinearize) { - for (const ISAM2::sharedClique& child : clique->children) { - CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys); - } - } -} - -/* ************************************************************************* */ -static void CheckRelinearizationRecursiveMap( - const FastMap& thresholds, const VectorValues& delta, - const ISAM2::sharedClique& clique, KeySet* relinKeys) { - // Check the current clique for relinearization - bool relinearize = false; - for (Key var : *clique->conditional()) { - // Find the threshold for this variable type - const Vector& threshold = thresholds.find(Symbol(var).chr())->second; - - const Vector& deltaVar = delta[var]; - - // Verify the threshold vector matches the actual variable size - if (threshold.rows() != deltaVar.rows()) - throw std::invalid_argument( - "Relinearization threshold vector dimensionality for '" + - std::string(1, Symbol(var).chr()) + - "' passed into iSAM2 parameters does not match actual variable " - "dimensionality."); - - // Check for relinearization - if ((deltaVar.array().abs() > threshold.array()).any()) { - relinKeys->insert(var); - relinearize = true; - } - } - - // If this node was relinearized, also check its children - if (relinearize) { - for (const ISAM2::sharedClique& child : clique->children) { - CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys); - } - } -} - -/* ************************************************************************* */ -KeySet ISAM2::Impl::CheckRelinearizationPartial( - const ISAM2::Roots& roots, const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { - KeySet relinKeys; - for (const ISAM2::sharedClique& root : roots) { - if (relinearizeThreshold.type() == typeid(double)) - CheckRelinearizationRecursiveDouble( - boost::get(relinearizeThreshold), delta, root, &relinKeys); - else if (relinearizeThreshold.type() == typeid(FastMap)) - CheckRelinearizationRecursiveMap( - boost::get >(relinearizeThreshold), delta, root, - &relinKeys); - } - return relinKeys; -} - /* ************************************************************************* */ namespace internal { inline static void optimizeInPlace(const ISAM2::sharedClique& clique, @@ -189,7 +45,7 @@ inline static void optimizeInPlace(const ISAM2::sharedClique& clique, } // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots, +size_t DeltaImpl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots, const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta) { @@ -272,7 +128,7 @@ void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys, } // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, +size_t DeltaImpl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, const VectorValues& gradAtZero, VectorValues* RgProd) { @@ -287,7 +143,7 @@ size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, } /* ************************************************************************* */ -VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, +VectorValues DeltaImpl::ComputeGradientSearch(const VectorValues& gradAtZero, const VectorValues& RgProd) { // Compute gradient squared-magnitude const double gradientSqNorm = gradAtZero.dot(gradAtZero); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 8a30fb8cd4..eb2285b281 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -11,18 +11,65 @@ /** * @file ISAM2-impl.h - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. - * @author Michael Kaess, Richard Roberts + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. + * @author Michael Kaess, Richard Roberts, Frank Dellaert */ #pragma once -#include #include +#include + +#include +#include // We need the inst file because we'll make a special JT templated on ISAM2 +#include +#include +#include +#include + +#include +#include +namespace br { +using namespace boost::range; +using namespace boost::adaptors; +} // namespace br + +#include +#include +#include +#include namespace gtsam { -struct GTSAM_EXPORT ISAM2::Impl { +/* ************************************************************************* */ +// Special BayesTree class that uses ISAM2 cliques - this is the result of +// reeliminating ISAM2 subtrees. +class ISAM2BayesTree : public ISAM2::Base { + public: + typedef ISAM2::Base Base; + typedef ISAM2BayesTree This; + typedef boost::shared_ptr shared_ptr; + + ISAM2BayesTree() {} +}; + +/* ************************************************************************* */ +// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for +// reeliminating ISAM2 subtrees. +class ISAM2JunctionTree + : public JunctionTree { + public: + typedef JunctionTree Base; + typedef ISAM2JunctionTree This; + typedef boost::shared_ptr shared_ptr; + + explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) + : Base(eliminationTree) {} +}; + +/* ************************************************************************* */ +struct GTSAM_EXPORT DeltaImpl { struct GTSAM_EXPORT PartialSolveResult { ISAM2::sharedClique bayesTree; }; @@ -34,57 +81,468 @@ struct GTSAM_EXPORT ISAM2::Impl { boost::optional > constrainedKeys; }; - /// Perform the first part of the bookkeeping updates for adding new factors. Adds them to the - /// complete list of nonlinear factors, and populates the list of new factor indices, both - /// optionally finding and reusing empty factor slots. - static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph* nonlinearFactors, FactorIndices* newFactorIndices); - - /** - * Find the set of variables to be relinearized according to relinearizeThreshold. - * Any variables in the VectorValues delta whose vector magnitude is greater than - * or equal to relinearizeThreshold are returned. - * @param delta The linear delta to check against the threshold - * @param keyFormatter Formatter for printing nonlinear keys during debugging - * @return The set of variable indices in delta whose magnitude is greater than or - * equal to relinearizeThreshold - */ - static KeySet CheckRelinearizationFull(const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); - - /** - * Find the set of variables to be relinearized according to relinearizeThreshold. - * This check is performed recursively, starting at the top of the tree. Once a - * variable in the tree does not need to be relinearized, no further checks in - * that branch are performed. This is an approximation of the Full version, designed - * to save time at the expense of accuracy. - * @param delta The linear delta to check against the threshold - * @param keyFormatter Formatter for printing nonlinear keys during debugging - * @return The set of variable indices in delta whose magnitude is greater than or - * equal to relinearizeThreshold - */ - static KeySet CheckRelinearizationPartial(const ISAM2::Roots& roots, - const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); - /** * Update the Newton's method step point, using wildfire */ static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots, - const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta); + const KeySet& replacedKeys, + double wildfireThreshold, + VectorValues* delta); /** * Update the RgProd (R*g) incrementally taking into account which variables * have been recalculated in \c replacedKeys. Only used in Dogleg. */ - static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, - const VectorValues& gradAtZero, VectorValues* RgProd); + static size_t UpdateRgProd(const ISAM2::Roots& roots, + const KeySet& replacedKeys, + const VectorValues& gradAtZero, + VectorValues* RgProd); /** * Compute the gradient-search point. Only used in Dogleg. */ static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero, const VectorValues& RgProd); +}; + +/* ************************************************************************* */ +/** + * Implementation functions for update method + * All of the methods below have clear inputs and outputs, even if not + * functional: iSAM2 is inherintly imperative. + */ +struct GTSAM_EXPORT UpdateImpl { + const ISAM2Params& params_; + const ISAM2UpdateParams& updateParams_; + UpdateImpl(const ISAM2Params& params, const ISAM2UpdateParams& updateParams) + : params_(params), updateParams_(updateParams) {} + + // Provide some debugging information at the start of update + static void LogStartingUpdate(const NonlinearFactorGraph& newFactors, + const ISAM2& isam2) { + gttic(pushBackFactors); + const bool debug = ISDEBUG("ISAM2 update"); + const bool verbose = ISDEBUG("ISAM2 update verbose"); + + if (verbose) { + std::cout << "ISAM2::update\n"; + isam2.print("ISAM2: "); + } + + if (debug || verbose) { + newFactors.print("The new factors are: "); + } + } + + // Check relinearization if we're at the nth step, or we are using a looser + // loop relinerization threshold. + bool relinarizationNeeded(size_t update_count) const { + return updateParams_.force_relinearize || + (params_.enableRelinearization && + update_count % params_.relinearizeSkip == 0); + } + + // Add any new factors \Factors:=\Factors\cup\Factors'. + void pushBackFactors(const NonlinearFactorGraph& newFactors, + NonlinearFactorGraph* nonlinearFactors, + GaussianFactorGraph* linearFactors, + VariableIndex* variableIndex, + FactorIndices* newFactorsIndices, + KeySet* keysWithRemovedFactors) const { + gttic(pushBackFactors); + + // Perform the first part of the bookkeeping updates for adding new factors. + // Adds them to the complete list of nonlinear factors, and populates the + // list of new factor indices, both optionally finding and reusing empty + // factor slots. + *newFactorsIndices = nonlinearFactors->add_factors( + newFactors, params_.findUnusedFactorSlots); + + // Remove the removed factors + NonlinearFactorGraph removedFactors; + removedFactors.reserve(updateParams_.removeFactorIndices.size()); + for (const auto index : updateParams_.removeFactorIndices) { + removedFactors.push_back(nonlinearFactors->at(index)); + nonlinearFactors->remove(index); + if (params_.cacheLinearizedFactors) linearFactors->remove(index); + } + + // Remove removed factors from the variable index so we do not attempt to + // relinearize them + variableIndex->remove(updateParams_.removeFactorIndices.begin(), + updateParams_.removeFactorIndices.end(), + removedFactors); + *keysWithRemovedFactors = removedFactors.keys(); + } + + // Get keys from removed factors and new factors, and compute unused keys, + // i.e., keys that are empty now and do not appear in the new factors. + void computeUnusedKeys(const NonlinearFactorGraph& newFactors, + const VariableIndex& variableIndex, + const KeySet& keysWithRemovedFactors, + KeySet* unusedKeys) const { + gttic(computeUnusedKeys); + KeySet removedAndEmpty; + for (Key key : keysWithRemovedFactors) { + if (variableIndex.empty(key)) + removedAndEmpty.insert(removedAndEmpty.end(), key); + } + KeySet newFactorSymbKeys = newFactors.keys(); + std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), + newFactorSymbKeys.begin(), newFactorSymbKeys.end(), + std::inserter(*unusedKeys, unusedKeys->end())); + } + + // Calculate nonlinear error + void error(const NonlinearFactorGraph& nonlinearFactors, + const Values& estimate, boost::optional* result) const { + gttic(error); + result->reset(nonlinearFactors.error(estimate)); + } + + // Mark linear update + void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors, + const NonlinearFactorGraph& nonlinearFactors, + const KeySet& keysWithRemovedFactors, + KeySet* markedKeys) const { + gttic(gatherInvolvedKeys); + *markedKeys = newFactors.keys(); // Get keys from new factors + // Also mark keys involved in removed factors + markedKeys->insert(keysWithRemovedFactors.begin(), + keysWithRemovedFactors.end()); + + // Also mark any provided extra re-eliminate keys + if (updateParams_.extraReelimKeys) { + for (Key key : *updateParams_.extraReelimKeys) { + markedKeys->insert(key); + } + } + + // Also, keys that were not observed in existing factors, but whose affected + // keys have been extended now (e.g. smart factors) + if (updateParams_.newAffectedKeys) { + for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) { + const auto factorIdx = factorAddedKeys.first; + const auto& affectedKeys = nonlinearFactors.at(factorIdx)->keys(); + markedKeys->insert(affectedKeys.begin(), affectedKeys.end()); + } + } + } + + // Update detail, unused, and observed keys from markedKeys + void updateKeys(const KeySet& markedKeys, ISAM2Result* result) const { + gttic(updateKeys); + // Observed keys for detailed results + if (result->detail && params_.enableDetailedResults) { + for (Key key : markedKeys) { + result->detail->variableStatus[key].isObserved = true; + } + } + + for (Key index : markedKeys) { + // Only add if not unused + if (result->unusedKeys.find(index) == result->unusedKeys.end()) + // Make a copy of these, as we'll soon add to them + result->observedKeys.push_back(index); + } + } + + static void CheckRelinearizationRecursiveMap( + const FastMap& thresholds, const VectorValues& delta, + const ISAM2::sharedClique& clique, KeySet* relinKeys) { + // Check the current clique for relinearization + bool relinearize = false; + for (Key var : *clique->conditional()) { + // Find the threshold for this variable type + const Vector& threshold = thresholds.find(Symbol(var).chr())->second; + + const Vector& deltaVar = delta[var]; + + // Verify the threshold vector matches the actual variable size + if (threshold.rows() != deltaVar.rows()) + throw std::invalid_argument( + "Relinearization threshold vector dimensionality for '" + + std::string(1, Symbol(var).chr()) + + "' passed into iSAM2 parameters does not match actual variable " + "dimensionality."); + + // Check for relinearization + if ((deltaVar.array().abs() > threshold.array()).any()) { + relinKeys->insert(var); + relinearize = true; + } + } + + // If this node was relinearized, also check its children + if (relinearize) { + for (const ISAM2::sharedClique& child : clique->children) { + CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys); + } + } + } + + static void CheckRelinearizationRecursiveDouble( + double threshold, const VectorValues& delta, + const ISAM2::sharedClique& clique, KeySet* relinKeys) { + // Check the current clique for relinearization + bool relinearize = false; + for (Key var : *clique->conditional()) { + double maxDelta = delta[var].lpNorm(); + if (maxDelta >= threshold) { + relinKeys->insert(var); + relinearize = true; + } + } + + // If this node was relinearized, also check its children + if (relinearize) { + for (const ISAM2::sharedClique& child : clique->children) { + CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys); + } + } + } + + /** + * Find the set of variables to be relinearized according to + * relinearizeThreshold. This check is performed recursively, starting at + * the top of the tree. Once a variable in the tree does not need to be + * relinearized, no further checks in that branch are performed. This is an + * approximation of the Full version, designed to save time at the expense + * of accuracy. + * @param delta The linear delta to check against the threshold + * @param keyFormatter Formatter for printing nonlinear keys during + * debugging + * @return The set of variable indices in delta whose magnitude is greater + * than or equal to relinearizeThreshold + */ + static KeySet CheckRelinearizationPartial( + const ISAM2::Roots& roots, const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { + KeySet relinKeys; + for (const ISAM2::sharedClique& root : roots) { + if (relinearizeThreshold.type() == typeid(double)) + CheckRelinearizationRecursiveDouble( + boost::get(relinearizeThreshold), delta, root, &relinKeys); + else if (relinearizeThreshold.type() == typeid(FastMap)) + CheckRelinearizationRecursiveMap( + boost::get >(relinearizeThreshold), delta, + root, &relinKeys); + } + return relinKeys; + } + + /** + * Find the set of variables to be relinearized according to + * relinearizeThreshold. Any variables in the VectorValues delta whose + * vector magnitude is greater than or equal to relinearizeThreshold are + * returned. + * @param delta The linear delta to check against the threshold + * @param keyFormatter Formatter for printing nonlinear keys during + * debugging + * @return The set of variable indices in delta whose magnitude is greater + * than or equal to relinearizeThreshold + */ + static KeySet CheckRelinearizationFull( + const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { + KeySet relinKeys; + + if (const double* threshold = boost::get(&relinearizeThreshold)) { + for (const VectorValues::KeyValuePair& key_delta : delta) { + double maxDelta = key_delta.second.lpNorm(); + if (maxDelta >= *threshold) relinKeys.insert(key_delta.first); + } + } else if (const FastMap* thresholds = + boost::get >(&relinearizeThreshold)) { + for (const VectorValues::KeyValuePair& key_delta : delta) { + const Vector& threshold = + thresholds->find(Symbol(key_delta.first).chr())->second; + if (threshold.rows() != key_delta.second.rows()) + throw std::invalid_argument( + "Relinearization threshold vector dimensionality for '" + + std::string(1, Symbol(key_delta.first).chr()) + + "' passed into iSAM2 parameters does not match actual variable " + "dimensionality."); + if ((key_delta.second.array().abs() > threshold.array()).any()) + relinKeys.insert(key_delta.first); + } + } + + return relinKeys; + } + + // Mark keys in \Delta above threshold \beta: + KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots, + const VectorValues& delta, + const KeySet& fixedVariables, + KeySet* markedKeys) const { + gttic(gatherRelinearizeKeys); + // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. + KeySet relinKeys = + params_.enablePartialRelinearizationCheck + ? CheckRelinearizationPartial(roots, delta, + params_.relinearizeThreshold) + : CheckRelinearizationFull(delta, params_.relinearizeThreshold); + if (updateParams_.forceFullSolve) + relinKeys = CheckRelinearizationFull(delta, 0.0); // for debugging + + // Remove from relinKeys any keys whose linearization points are fixed + for (Key key : fixedVariables) { + relinKeys.erase(key); + } + if (updateParams_.noRelinKeys) { + for (Key key : *updateParams_.noRelinKeys) { + relinKeys.erase(key); + } + } + + // Add the variables being relinearized to the marked keys + markedKeys->insert(relinKeys.begin(), relinKeys.end()); + return relinKeys; + } + + // Record relinerization threshold keys in detailed results + void recordRelinearizeDetail(const KeySet& relinKeys, + ISAM2Result::DetailedResults* detail) const { + if (detail && params_.enableDetailedResults) { + for (Key key : relinKeys) { + detail->variableStatus[key].isAboveRelinThreshold = true; + detail->variableStatus[key].isRelinearized = true; + } + } + } + + // Mark all cliques that involve marked variables \Theta_{J} and all + // their ancestors. + void findFluid(const ISAM2::Roots& roots, const KeySet& relinKeys, + KeySet* markedKeys, + ISAM2Result::DetailedResults* detail) const { + gttic(findFluid); + for (const auto& root : roots) + // add other cliques that have the marked ones in the separator + root->findAll(relinKeys, markedKeys); + + // Relinearization-involved keys for detailed results + if (detail && params_.enableDetailedResults) { + KeySet involvedRelinKeys; + for (const auto& root : roots) + root->findAll(relinKeys, &involvedRelinKeys); + for (Key key : involvedRelinKeys) { + if (!detail->variableStatus[key].isAboveRelinThreshold) { + detail->variableStatus[key].isRelinearizeInvolved = true; + detail->variableStatus[key].isRelinearized = true; + } + } + } + } + + /** + * Apply expmap to the given values, but only for indices appearing in + * \c mask. Values are expmapped in-place. + * \param mask Mask on linear indices, only \c true entries are expmapped + */ + static void ExpmapMasked(const VectorValues& delta, const KeySet& mask, + Values* theta) { + gttic(ExpmapMasked); + assert(theta->size() == delta.size()); + Values::iterator key_value; + VectorValues::const_iterator key_delta; +#ifdef GTSAM_USE_TBB + for (key_value = theta->begin(); key_value != theta->end(); ++key_value) { + key_delta = delta.find(key_value->key); +#else + for (key_value = theta->begin(), key_delta = delta.begin(); + key_value != theta->end(); ++key_value, ++key_delta) { + assert(key_value->key == key_delta->first); +#endif + Key var = key_value->key; + assert(static_cast(delta[var].size()) == key_value->value.dim()); + assert(delta[var].allFinite()); + if (mask.exists(var)) { + Value* retracted = key_value->value.retract_(delta[var]); + key_value->value = *retracted; + retracted->deallocate_(); + } + } + } + + // Linearize new factors + void linearizeNewFactors(const NonlinearFactorGraph& newFactors, + const Values& theta, size_t numNonlinearFactors, + const FactorIndices& newFactorsIndices, + GaussianFactorGraph* linearFactors) const { + gttic(linearizeNewFactors); + auto linearized = newFactors.linearize(theta); + if (params_.findUnusedFactorSlots) { + linearFactors->resize(numNonlinearFactors); + for (size_t i = 0; i < newFactors.size(); ++i) + (*linearFactors)[newFactorsIndices[i]] = (*linearized)[i]; + } else { + linearFactors->push_back(*linearized); + } + assert(linearFactors->size() == numNonlinearFactors); + } + + void augmentVariableIndex(const NonlinearFactorGraph& newFactors, + const FactorIndices& newFactorsIndices, + VariableIndex* variableIndex) const { + gttic(augmentVariableIndex); + // Augment the variable index with the new factors + if (params_.findUnusedFactorSlots) + variableIndex->augment(newFactors, newFactorsIndices); + else + variableIndex->augment(newFactors); + + // Augment it with existing factors which now affect to more variables: + if (updateParams_.newAffectedKeys) { + for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) { + const auto factorIdx = factorAddedKeys.first; + variableIndex->augmentExistingFactor(factorIdx, factorAddedKeys.second); + } + } + } + + static void LogRecalculateKeys(const ISAM2Result& result) { + const bool debug = ISDEBUG("ISAM2 recalculate"); + + if (debug) { + std::cout << "markedKeys: "; + for (const Key key : result.markedKeys) { + std::cout << key << " "; + } + std::cout << std::endl; + std::cout << "observedKeys: "; + for (const Key key : result.observedKeys) { + std::cout << key << " "; + } + std::cout << std::endl; + } + } + + static FactorIndexSet GetAffectedFactors(const KeyList& keys, + const VariableIndex& variableIndex) { + gttic(GetAffectedFactors); + FactorIndexSet indices; + for (const Key key : keys) { + const FactorIndices& factors(variableIndex[key]); + indices.insert(factors.begin(), factors.end()); + } + return indices; + } + + // find intermediate (linearized) factors from cache that are passed into + // the affected area + static GaussianFactorGraph GetCachedBoundaryFactors( + const ISAM2::Cliques& orphans) { + GaussianFactorGraph cachedBoundary; + + for (const auto& orphan : orphans) { + // retrieve the cached factor and add to boundary + cachedBoundary.push_back(orphan->cachedFactor()); + } + return cachedBoundary; + } }; -} +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index c01d1c536d..f56b23777c 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -16,24 +16,16 @@ * @author Michael Kaess, Richard Roberts, Frank Dellaert */ +#include #include +#include #include #include #include -#include // We need the inst file because we'll make a special JT templated on ISAM2 -#include #include -#include -#include -namespace br { -using namespace boost::range; -using namespace boost::adaptors; -} // namespace br - #include -#include #include #include @@ -44,35 +36,6 @@ namespace gtsam { // Instantiate base class template class BayesTree; -static const bool kDisableReordering = false; -static const double kBatchThreshold = 0.65; - -/* ************************************************************************* */ -// Special BayesTree class that uses ISAM2 cliques - this is the result of -// reeliminating ISAM2 subtrees. -class ISAM2BayesTree : public ISAM2::Base { - public: - typedef ISAM2::Base Base; - typedef ISAM2BayesTree This; - typedef boost::shared_ptr shared_ptr; - - ISAM2BayesTree() {} -}; - -/* ************************************************************************* */ -// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for -// reeliminating ISAM2 subtrees. -class ISAM2JunctionTree - : public JunctionTree { - public: - typedef JunctionTree Base; - typedef ISAM2JunctionTree This; - typedef boost::shared_ptr shared_ptr; - - explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) - : Base(eliminationTree) {} -}; - /* ************************************************************************* */ ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) { if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) @@ -96,40 +59,12 @@ bool ISAM2::equals(const ISAM2& other, double tol) const { } /* ************************************************************************* */ -FactorIndexSet ISAM2::getAffectedFactors(const KeyList& keys) const { - static const bool debug = false; - if (debug) cout << "Getting affected factors for "; - if (debug) { - for (const Key key : keys) { - cout << key << " "; - } - } - if (debug) cout << endl; - - FactorIndexSet indices; - for (const Key key : keys) { - const VariableIndex::Factors& factors(variableIndex_[key]); - indices.insert(factors.begin(), factors.end()); - } - if (debug) cout << "Affected factors are: "; - if (debug) { - for (const auto index : indices) { - cout << index << " "; - } - } - if (debug) cout << endl; - return indices; -} - -/* ************************************************************************* */ -// retrieve all factors that ONLY contain the affected variables -// (note that the remaining stuff is summarized in the cached factors) - -GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( - const FastList& affectedKeys, const KeySet& relinKeys) const { - gttic(getAffectedFactors); - FactorIndexSet candidates = getAffectedFactors(affectedKeys); - gttoc(getAffectedFactors); +GaussianFactorGraph ISAM2::relinearizeAffectedFactors( + const ISAM2UpdateParams& updateParams, const FastList& affectedKeys, + const KeySet& relinKeys) { + gttic(relinearizeAffectedFactors); + FactorIndexSet candidates = + UpdateImpl::GetAffectedFactors(affectedKeys, variableIndex_); gttic(affectedKeysSet); // for fast lookup below @@ -138,7 +73,7 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( gttoc(affectedKeysSet); gttic(check_candidates_and_linearize); - auto linearized = boost::make_shared(); + GaussianFactorGraph linearized; for (const FactorIndex idx : candidates) { bool inside = true; bool useCachedLinear = params_.cacheLinearizedFactors; @@ -156,10 +91,10 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( assert(linearFactors_[idx]); assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys()); #endif - linearized->push_back(linearFactors_[idx]); + linearized.push_back(linearFactors_[idx]); } else { auto linearFactor = nonlinearFactors_[idx]->linearize(theta_); - linearized->push_back(linearFactor); + linearized.push_back(linearFactor); if (params_.cacheLinearizedFactors) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS assert(linearFactors_[idx]->keys() == linearFactor->keys()); @@ -175,325 +110,276 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( } /* ************************************************************************* */ -// find intermediate (linearized) factors from cache that are passed into the -// affected area +void ISAM2::recalculate(const ISAM2UpdateParams& updateParams, + const KeySet& relinKeys, ISAM2Result* result) { + gttic(recalculate); + UpdateImpl::LogRecalculateKeys(*result); + + if (!result->markedKeys.empty() || !result->observedKeys.empty()) { + // Remove top of Bayes tree and convert to a factor graph: + // (a) For each affected variable, remove the corresponding clique and all + // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of + // removed cliques. + GaussianBayesNet affectedBayesNet; + Cliques orphans; + this->removeTop( + KeyVector(result->markedKeys.begin(), result->markedKeys.end()), + &affectedBayesNet, &orphans); + + // FactorGraph factors(affectedBayesNet); + // bug was here: we cannot reuse the original factors, because then the + // cached factors get messed up [all the necessary data is actually + // contained in the affectedBayesNet, including what was passed in from the + // boundaries, so this would be correct; however, in the process we also + // generate new cached_ entries that will be wrong (ie. they don't contain + // what would be passed up at a certain point if batch elimination was done, + // but that's what we need); we could choose not to update cached_ from + // here, but then the new information (and potentially different variable + // ordering) is not reflected in the cached_ values which again will be + // wrong] so instead we have to retrieve the original linearized factors AND + // add the cached factors from the boundary + + // ordering provides all keys in conditionals, there cannot be others + // because path to root included + gttic(affectedKeys); + FastList affectedKeys; + for (const auto& conditional : affectedBayesNet) + affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), + conditional->endFrontals()); + gttoc(affectedKeys); + + KeySet affectedKeysSet; + static const double kBatchThreshold = 0.65; + if (affectedKeys.size() >= theta_.size() * kBatchThreshold) { + // Do a batch step - reorder and relinearize all variables + recalculateBatch(updateParams, &affectedKeysSet, result); + } else { + recalculateIncremental(updateParams, relinKeys, affectedKeys, + &affectedKeysSet, &orphans, result); + } -GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) { - GaussianFactorGraph cachedBoundary; + // Root clique variables for detailed results + if (result->detail && params_.enableDetailedResults) { + for (const auto& root : roots_) + for (Key var : *root->conditional()) + result->detail->variableStatus[var].inRootClique = true; + } - for (sharedClique orphan : orphans) { - // retrieve the cached factor and add to boundary - cachedBoundary.push_back(orphan->cachedFactor()); + // Update replaced keys mask (accumulates until back-substitution happens) + deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end()); } - - return cachedBoundary; } /* ************************************************************************* */ -boost::shared_ptr ISAM2::recalculate( - const KeySet& markedKeys, const KeySet& relinKeys, - const KeyVector& observedKeys, const KeySet& unusedIndices, - const boost::optional >& constrainKeys, - ISAM2Result* result) { - // TODO(dellaert): new factors are linearized twice, - // the newFactors passed in are not used. +void ISAM2::recalculateBatch(const ISAM2UpdateParams& updateParams, + KeySet* affectedKeysSet, ISAM2Result* result) { + gttic(recalculateBatch); - const bool debug = ISDEBUG("ISAM2 recalculate"); + gttic(add_keys); + br::copy(variableIndex_ | br::map_keys, + std::inserter(*affectedKeysSet, affectedKeysSet->end())); - // Input: BayesTree(this), newFactors - -// figures for paper, disable for timing -#ifdef PRINT_STATS - static int counter = 0; - int maxClique = 0; - double avgClique = 0; - int numCliques = 0; - int nnzR = 0; - if (counter > 0) { // cannot call on empty tree - GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); - GaussianISAM2_P::CliqueStats cstats = cdata.getStats(); - maxClique = cstats.maxCONDITIONALSize; - avgClique = cstats.avgCONDITIONALSize; - numCliques = cdata.conditionalSizes.size(); - nnzR = calculate_nnz(this->root()); - } - counter++; -#endif + // Removed unused keys: + VariableIndex affectedFactorsVarIndex = variableIndex_; - if (debug) { - cout << "markedKeys: "; - for (const Key key : markedKeys) { - cout << key << " "; - } - cout << endl; - cout << "observedKeys: "; - for (const Key key : observedKeys) { - cout << key << " "; - } - cout << endl; - } + affectedFactorsVarIndex.removeUnusedVariables(result->unusedKeys.begin(), + result->unusedKeys.end()); - // 1. Remove top of Bayes tree and convert to a factor graph: - // (a) For each affected variable, remove the corresponding clique and all - // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of - // removed cliques. - gttic(removetop); - Cliques orphans; - GaussianBayesNet affectedBayesNet; - this->removeTop(KeyVector(markedKeys.begin(), markedKeys.end()), - affectedBayesNet, orphans); - gttoc(removetop); - - // FactorGraph factors(affectedBayesNet); - // bug was here: we cannot reuse the original factors, because then the cached - // factors get messed up [all the necessary data is actually contained in the - // affectedBayesNet, including what was passed in from the boundaries, - // so this would be correct; however, in the process we also generate new - // cached_ entries that will be wrong (ie. they don't contain what would be - // passed up at a certain point if batch elimination was done, but that's - // what we need); we could choose not to update cached_ from here, but then - // the new information (and potentially different variable ordering) is not - // reflected in the cached_ values which again will be wrong] - // so instead we have to retrieve the original linearized factors AND add the - // cached factors from the boundary - - // BEGIN OF COPIED CODE - - // ordering provides all keys in conditionals, there cannot be others because - // path to root included - gttic(affectedKeys); - FastList affectedKeys; - for (const auto& conditional : affectedBayesNet) - affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), - conditional->endFrontals()); - gttoc(affectedKeys); - - boost::shared_ptr affectedKeysSet( - new KeySet()); // Will return this result - - if (affectedKeys.size() >= theta_.size() * kBatchThreshold) { - // Do a batch step - reorder and relinearize all variables - gttic(batch); - - gttic(add_keys); - br::copy(variableIndex_ | br::map_keys, - std::inserter(*affectedKeysSet, affectedKeysSet->end())); - - // Removed unused keys: - VariableIndex affectedFactorsVarIndex = variableIndex_; - - affectedFactorsVarIndex.removeUnusedVariables(unusedIndices.begin(), - unusedIndices.end()); - - for (const Key key : unusedIndices) { - affectedKeysSet->erase(key); - } - gttoc(add_keys); + for (const Key key : result->unusedKeys) { + affectedKeysSet->erase(key); + } + gttoc(add_keys); - gttic(ordering); - Ordering order; - if (constrainKeys) { - order = - Ordering::ColamdConstrained(affectedFactorsVarIndex, *constrainKeys); + gttic(ordering); + Ordering order; + if (updateParams.constrainedKeys) { + order = Ordering::ColamdConstrained(affectedFactorsVarIndex, + *updateParams.constrainedKeys); + } else { + if (theta_.size() > result->observedKeys.size()) { + // Only if some variables are unconstrained + FastMap constraintGroups; + for (Key var : result->observedKeys) constraintGroups[var] = 1; + order = Ordering::ColamdConstrained(affectedFactorsVarIndex, + constraintGroups); } else { - if (theta_.size() > observedKeys.size()) { - // Only if some variables are unconstrained - FastMap constraintGroups; - for (Key var : observedKeys) constraintGroups[var] = 1; - order = Ordering::ColamdConstrained(affectedFactorsVarIndex, - constraintGroups); - } else { - order = Ordering::Colamd(affectedFactorsVarIndex); - } - } - gttoc(ordering); - - gttic(linearize); - GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_); - if (params_.cacheLinearizedFactors) linearFactors_ = linearized; - gttoc(linearize); - - gttic(eliminate); - ISAM2BayesTree::shared_ptr bayesTree = - ISAM2JunctionTree( - GaussianEliminationTree(linearized, affectedFactorsVarIndex, order)) - .eliminate(params_.getEliminationFunction()) - .first; - gttoc(eliminate); - - gttic(insert); - this->clear(); - this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), - bayesTree->roots().end()); - this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); - gttoc(insert); - - result->variablesReeliminated = affectedKeysSet->size(); - result->factorsRecalculated = nonlinearFactors_.size(); - - lastAffectedMarkedCount = markedKeys.size(); - lastAffectedVariableCount = affectedKeysSet->size(); - lastAffectedFactorCount = linearized.size(); - - // Reeliminated keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : theta_.keys()) { - result->detail->variableStatus[key].isReeliminated = true; - } + order = Ordering::Colamd(affectedFactorsVarIndex); } - - gttoc(batch); - - } else { - gttic(incremental); - - // 2. Add the new factors \Factors' into the resulting factor graph - FastList affectedAndNewKeys; - affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), - affectedKeys.end()); - affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), - observedKeys.end()); - gttic(relinearizeAffected); - GaussianFactorGraph factors( - *relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); - if (debug) factors.print("Relinearized factors: "); - gttoc(relinearizeAffected); - - if (debug) { - cout << "Affected keys: "; - for (const Key key : affectedKeys) { - cout << key << " "; - } - cout << endl; + } + gttoc(ordering); + + gttic(linearize); + auto linearized = nonlinearFactors_.linearize(theta_); + if (params_.cacheLinearizedFactors) linearFactors_ = *linearized; + gttoc(linearize); + + gttic(eliminate); + ISAM2BayesTree::shared_ptr bayesTree = + ISAM2JunctionTree( + GaussianEliminationTree(*linearized, affectedFactorsVarIndex, order)) + .eliminate(params_.getEliminationFunction()) + .first; + gttoc(eliminate); + + gttic(insert); + roots_.clear(); + roots_.insert(roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); + nodes_.clear(); + nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); + gttoc(insert); + + result->variablesReeliminated = affectedKeysSet->size(); + result->factorsRecalculated = nonlinearFactors_.size(); + + // Reeliminated keys for detailed results + if (params_.enableDetailedResults) { + for (Key key : theta_.keys()) { + result->detail->variableStatus[key].isReeliminated = true; } + } +} - // Reeliminated keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : affectedAndNewKeys) { - result->detail->variableStatus[key].isReeliminated = true; - } - } +/* ************************************************************************* */ +void ISAM2::recalculateIncremental(const ISAM2UpdateParams& updateParams, + const KeySet& relinKeys, + const FastList& affectedKeys, + KeySet* affectedKeysSet, Cliques* orphans, + ISAM2Result* result) { + gttic(recalculateIncremental); + const bool debug = ISDEBUG("ISAM2 recalculate"); - result->variablesReeliminated = affectedAndNewKeys.size(); - result->factorsRecalculated = factors.size(); - lastAffectedMarkedCount = markedKeys.size(); - lastAffectedVariableCount = affectedKeys.size(); - lastAffectedFactorCount = factors.size(); - -#ifdef PRINT_STATS - // output for generating figures - cout << "linear: #markedKeys: " << markedKeys.size() - << " #affectedVariables: " << affectedKeys.size() - << " #affectedFactors: " << factors.size() - << " maxCliqueSize: " << maxClique << " avgCliqueSize: " << avgClique - << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; -#endif + // 2. Add the new factors \Factors' into the resulting factor graph + FastList affectedAndNewKeys; + affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), + affectedKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), + result->observedKeys.begin(), + result->observedKeys.end()); + GaussianFactorGraph factors = + relinearizeAffectedFactors(updateParams, affectedAndNewKeys, relinKeys); - gttic(cached); - // add the cached intermediate results from the boundary of the orphans ... - GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - if (debug) cachedBoundary.print("Boundary factors: "); - factors.push_back(cachedBoundary); - gttoc(cached); - - gttic(orphans); - // Add the orphaned subtrees - for (const sharedClique& orphan : orphans) - factors += boost::make_shared >(orphan); - gttoc(orphans); - - // END OF COPIED CODE - - // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm - // [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm - // [alg:BayesTree]) - - gttic(reorder_and_eliminate); - - gttic(list_to_set); - // create a partial reordering for the new and contaminated factors - // markedKeys are passed in: those variables will be forced to the end in - // the ordering - affectedKeysSet->insert(markedKeys.begin(), markedKeys.end()); - affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); - gttoc(list_to_set); - - VariableIndex affectedFactorsVarIndex(factors); - - gttic(ordering_constraints); - // Create ordering constraints - FastMap constraintGroups; - if (constrainKeys) { - constraintGroups = *constrainKeys; - } else { - constraintGroups = FastMap(); - const int group = - observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; - for (Key var : observedKeys) - constraintGroups.insert(make_pair(var, group)); + if (debug) { + factors.print("Relinearized factors: "); + std::cout << "Affected keys: "; + for (const Key key : affectedKeys) { + std::cout << key << " "; } + std::cout << std::endl; + } - // Remove unaffected keys from the constraints - for (FastMap::iterator iter = constraintGroups.begin(); - iter != constraintGroups.end(); - /*Incremented in loop ++iter*/) { - if (unusedIndices.exists(iter->first) || - !affectedKeysSet->exists(iter->first)) - constraintGroups.erase(iter++); - else - ++iter; + // Reeliminated keys for detailed results + if (params_.enableDetailedResults) { + for (Key key : affectedAndNewKeys) { + result->detail->variableStatus[key].isReeliminated = true; } - gttoc(ordering_constraints); - - // Generate ordering - gttic(Ordering); - Ordering ordering = - Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); - gttoc(Ordering); - - ISAM2BayesTree::shared_ptr bayesTree = - ISAM2JunctionTree( - GaussianEliminationTree(factors, affectedFactorsVarIndex, ordering)) - .eliminate(params_.getEliminationFunction()) - .first; - - gttoc(reorder_and_eliminate); - - gttic(reassemble); - this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), - bayesTree->roots().end()); - this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); - gttoc(reassemble); - - // 4. The orphans have already been inserted during elimination - - gttoc(incremental); } - // Root clique variables for detailed results - if (params_.enableDetailedResults) { - for (const sharedNode& root : this->roots()) - for (Key var : *root->conditional()) - result->detail->variableStatus[var].inRootClique = true; + result->variablesReeliminated = affectedAndNewKeys.size(); + result->factorsRecalculated = factors.size(); + + gttic(cached); + // Add the cached intermediate results from the boundary of the orphans... + GaussianFactorGraph cachedBoundary = + UpdateImpl::GetCachedBoundaryFactors(*orphans); + if (debug) cachedBoundary.print("Boundary factors: "); + factors.push_back(cachedBoundary); + gttoc(cached); + + gttic(orphans); + // Add the orphaned subtrees + for (const auto& orphan : *orphans) + factors += + boost::make_shared >(orphan); + gttoc(orphans); + + // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm + // [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm + // [alg:BayesTree]) + + gttic(reorder_and_eliminate); + + gttic(list_to_set); + // create a partial reordering for the new and contaminated factors + // result->markedKeys are passed in: those variables will be forced to the + // end in the ordering + affectedKeysSet->insert(result->markedKeys.begin(), result->markedKeys.end()); + affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); + gttoc(list_to_set); + + VariableIndex affectedFactorsVarIndex(factors); + + gttic(ordering_constraints); + // Create ordering constraints + FastMap constraintGroups; + if (updateParams.constrainedKeys) { + constraintGroups = *updateParams.constrainedKeys; + } else { + constraintGroups = FastMap(); + const int group = + result->observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; + for (Key var : result->observedKeys) + constraintGroups.insert(std::make_pair(var, group)); } - return affectedKeysSet; + // Remove unaffected keys from the constraints + for (FastMap::iterator iter = constraintGroups.begin(); + iter != constraintGroups.end(); + /*Incremented in loop ++iter*/) { + if (result->unusedKeys.exists(iter->first) || + !affectedKeysSet->exists(iter->first)) + constraintGroups.erase(iter++); + else + ++iter; + } + gttoc(ordering_constraints); + + // Generate ordering + gttic(Ordering); + const Ordering ordering = + Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); + gttoc(Ordering); + + // Do elimination + GaussianEliminationTree etree(factors, affectedFactorsVarIndex, ordering); + auto bayesTree = ISAM2JunctionTree(etree) + .eliminate(params_.getEliminationFunction()) + .first; + gttoc(reorder_and_eliminate); + + gttic(reassemble); + roots_.insert(roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); + nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); + gttoc(reassemble); + + // 4. The orphans have already been inserted during elimination } /* ************************************************************************* */ -void ISAM2::addVariables(const Values& newTheta) { - const bool debug = ISDEBUG("ISAM2 AddVariables"); +void ISAM2::addVariables(const Values& newTheta, + ISAM2Result::DetailedResults* detail) { + gttic(addNewVariables); theta_.insert(newTheta); - if (debug) newTheta.print("The new variables are: "); + if (ISDEBUG("ISAM2 AddVariables")) newTheta.print("The new variables are: "); // Add zeros into the VectorValues delta_.insert(newTheta.zeroVectors()); deltaNewton_.insert(newTheta.zeroVectors()); RgProd_.insert(newTheta.zeroVectors()); + + // New keys for detailed results + if (detail && params_.enableDetailedResults) { + for (Key key : newTheta.keys()) { + detail->variableStatus[key].isNew = true; + } + } } /* ************************************************************************* */ void ISAM2::removeVariables(const KeySet& unusedKeys) { + gttic(removeVariables); + variableIndex_.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); for (Key key : unusedKeys) { delta_.erase(key); @@ -506,36 +392,6 @@ void ISAM2::removeVariables(const KeySet& unusedKeys) { } } -/* ************************************************************************* */ -void ISAM2::expmapMasked(const KeySet& mask) { - assert(theta_.size() == delta_.size()); - Values::iterator key_value; - VectorValues::const_iterator key_delta; -#ifdef GTSAM_USE_TBB - for (key_value = theta_.begin(); key_value != theta_.end(); ++key_value) { - key_delta = delta_.find(key_value->key); -#else - for (key_value = theta_.begin(), key_delta = delta_.begin(); - key_value != theta_.end(); ++key_value, ++key_delta) { - assert(key_value->key == key_delta->first); -#endif - Key var = key_value->key; - assert(static_cast(delta_[var].size()) == key_value->value.dim()); - assert(delta_[var].allFinite()); - if (mask.exists(var)) { - Value* retracted = key_value->value.retract_(delta_[var]); - key_value->value = *retracted; - retracted->deallocate_(); -#ifndef NDEBUG - // If debugging, invalidate delta_ entries to Inf, to trigger assertions - // if we try to re-use them. - delta_[var] = Vector::Constant(delta_[var].rows(), - numeric_limits::infinity()); -#endif - } - } -} - /* ************************************************************************* */ ISAM2Result ISAM2::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, @@ -544,7 +400,6 @@ ISAM2Result ISAM2::update( const boost::optional >& noRelinKeys, const boost::optional >& extraReelimKeys, bool force_relinearize) { - ISAM2UpdateParams params; params.constrainedKeys = constrainedKeys; params.extraReelimKeys = extraReelimKeys; @@ -557,281 +412,66 @@ ISAM2Result ISAM2::update( /* ************************************************************************* */ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, - const Values& newTheta, - const ISAM2UpdateParams& updateParams) { - const bool debug = ISDEBUG("ISAM2 update"); - const bool verbose = ISDEBUG("ISAM2 update verbose"); - + const Values& newTheta, + const ISAM2UpdateParams& updateParams) { gttic(ISAM2_update); - - this->update_count_++; - - lastAffectedVariableCount = 0; - lastAffectedFactorCount = 0; - lastAffectedCliqueCount = 0; - lastAffectedMarkedCount = 0; - lastBacksubVariableCount = 0; - lastNnzTop = 0; - ISAM2Result result; - if (params_.enableDetailedResults) - result.detail = ISAM2Result::DetailedResults(); - const bool relinearizeThisStep = - updateParams.force_relinearize || (params_.enableRelinearization && - update_count_ % params_.relinearizeSkip == 0); - - if (verbose) { - cout << "ISAM2::update\n"; - this->print("ISAM2: "); - } + this->update_count_ += 1; + UpdateImpl::LogStartingUpdate(newFactors, *this); + ISAM2Result result(params_.enableDetailedResults); + UpdateImpl update(params_, updateParams); // Update delta if we need it to check relinearization later - if (relinearizeThisStep) { - gttic(updateDelta); - updateDelta(kDisableReordering); - gttoc(updateDelta); - } + if (update.relinarizationNeeded(update_count_)) + updateDelta(updateParams.forceFullSolve); - gttic(push_back_factors); // 1. Add any new factors \Factors:=\Factors\cup\Factors'. - // Add the new factor indices to the result struct - if (debug || verbose) newFactors.print("The new factors are: "); - Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, - &nonlinearFactors_, &result.newFactorsIndices); - - // Remove the removed factors - NonlinearFactorGraph removeFactors; - removeFactors.reserve(updateParams.removeFactorIndices.size()); - for (const auto index : updateParams.removeFactorIndices) { - removeFactors.push_back(nonlinearFactors_[index]); - nonlinearFactors_.remove(index); - if (params_.cacheLinearizedFactors) linearFactors_.remove(index); - } + update.pushBackFactors(newFactors, &nonlinearFactors_, &linearFactors_, + &variableIndex_, &result.newFactorsIndices, + &result.keysWithRemovedFactors); + update.computeUnusedKeys(newFactors, variableIndex_, + result.keysWithRemovedFactors, &result.unusedKeys); - // Remove removed factors from the variable index so we do not attempt to - // relinearize them - variableIndex_.remove(updateParams.removeFactorIndices.begin(), - updateParams.removeFactorIndices.end(), - removeFactors); - - // Compute unused keys and indices - KeySet unusedKeys; - KeySet unusedIndices; - { - // Get keys from removed factors and new factors, and compute unused keys, - // i.e., keys that are empty now and do not appear in the new factors. - KeySet removedAndEmpty; - for (Key key : removeFactors.keys()) { - if (variableIndex_[key].empty()) - removedAndEmpty.insert(removedAndEmpty.end(), key); - } - KeySet newFactorSymbKeys = newFactors.keys(); - std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), - newFactorSymbKeys.begin(), newFactorSymbKeys.end(), - std::inserter(unusedKeys, unusedKeys.end())); - - // Get indices for unused keys - for (Key key : unusedKeys) { - unusedIndices.insert(unusedIndices.end(), key); - } - } - gttoc(push_back_factors); - - gttic(add_new_variables); // 2. Initialize any new variables \Theta_{new} and add // \Theta:=\Theta\cup\Theta_{new}. - addVariables(newTheta); - // New keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : newTheta.keys()) { - result.detail->variableStatus[key].isNew = true; - } - } - gttoc(add_new_variables); - - gttic(evaluate_error_before); + addVariables(newTheta, result.details()); if (params_.evaluateNonlinearError) - result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate())); - gttoc(evaluate_error_before); + update.error(nonlinearFactors_, calculateEstimate(), &result.errorBefore); - gttic(gather_involved_keys); // 3. Mark linear update - KeySet markedKeys = newFactors.keys(); // Get keys from new factors - // Also mark keys involved in removed factors - { - KeySet markedRemoveKeys = - removeFactors.keys(); // Get keys involved in removed factors - markedKeys.insert( - markedRemoveKeys.begin(), - markedRemoveKeys.end()); // Add to the overall set of marked keys - } - // Also mark any provided extra re-eliminate keys - if (updateParams.extraReelimKeys) { - for (Key key : *updateParams.extraReelimKeys) { - markedKeys.insert(key); - } - } - // Also, keys that were not observed in existing factors, but whose affected - // keys have been extended now (e.g. smart factors) - if (updateParams.newAffectedKeys) { - for (const auto &factorAddedKeys : *updateParams.newAffectedKeys) { - const auto factorIdx = factorAddedKeys.first; - const auto& affectedKeys = nonlinearFactors_.at(factorIdx)->keys(); - markedKeys.insert(affectedKeys.begin(),affectedKeys.end()); - } - } + update.gatherInvolvedKeys(newFactors, nonlinearFactors_, + result.keysWithRemovedFactors, &result.markedKeys); + update.updateKeys(result.markedKeys, &result); - // Observed keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : markedKeys) { - result.detail->variableStatus[key].isObserved = true; - } - } - - KeyVector observedKeys; - for (Key index : markedKeys) { - // Only add if not unused - if (unusedIndices.find(index) == unusedIndices.end()) - // Make a copy of these, as we'll soon add to them - observedKeys.push_back(index); - } - gttoc(gather_involved_keys); - - // Check relinearization if we're at the nth step, or we are using a looser - // loop relin threshold KeySet relinKeys; - if (relinearizeThisStep) { - gttic(gather_relinearize_keys); + result.variablesRelinearized = 0; + if (update.relinarizationNeeded(update_count_)) { // 4. Mark keys in \Delta above threshold \beta: - // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - if (params_.enablePartialRelinearizationCheck) - relinKeys = Impl::CheckRelinearizationPartial( - roots_, delta_, params_.relinearizeThreshold); - else - relinKeys = - Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold); - if (kDisableReordering) - relinKeys = Impl::CheckRelinearizationFull( - delta_, 0.0); // This is used for debugging - - // Remove from relinKeys any keys whose linearization points are fixed - for (Key key : fixedVariables_) { - relinKeys.erase(key); - } - if (updateParams.noRelinKeys) { - for (Key key : *updateParams.noRelinKeys) { - relinKeys.erase(key); - } - } - - // Above relin threshold keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : relinKeys) { - result.detail->variableStatus[key].isAboveRelinThreshold = true; - result.detail->variableStatus[key].isRelinearized = true; - } - } - - // Add the variables being relinearized to the marked keys - KeySet markedRelinMask; - for (const Key key : relinKeys) markedRelinMask.insert(key); - markedKeys.insert(relinKeys.begin(), relinKeys.end()); - gttoc(gather_relinearize_keys); - - gttic(fluid_find_all); - // 5. Mark all cliques that involve marked variables \Theta_{J} and all - // their ancestors. + relinKeys = update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_, + &result.markedKeys); + update.recordRelinearizeDetail(relinKeys, result.details()); if (!relinKeys.empty()) { - for (const sharedClique& root : roots_) - // add other cliques that have the marked ones in the separator - root->findAll(markedRelinMask, &markedKeys); - - // Relin involved keys for detailed results - if (params_.enableDetailedResults) { - KeySet involvedRelinKeys; - for (const sharedClique& root : roots_) - root->findAll(markedRelinMask, &involvedRelinKeys); - for (Key key : involvedRelinKeys) { - if (!result.detail->variableStatus[key].isAboveRelinThreshold) { - result.detail->variableStatus[key].isRelinearizeInvolved = true; - result.detail->variableStatus[key].isRelinearized = true; - } - } - } + // 5. Mark cliques that involve marked variables \Theta_{J} and ancestors. + update.findFluid(roots_, relinKeys, &result.markedKeys, result.details()); + // 6. Update linearization point for marked variables: + // \Theta_{J}:=\Theta_{J}+\Delta_{J}. + UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_); } - gttoc(fluid_find_all); - - gttic(expmap); - // 6. Update linearization point for marked variables: - // \Theta_{J}:=\Theta_{J}+\Delta_{J}. - if (!relinKeys.empty()) expmapMasked(markedRelinMask); - gttoc(expmap); - - result.variablesRelinearized = markedKeys.size(); - } else { - result.variablesRelinearized = 0; + result.variablesRelinearized = result.markedKeys.size(); } - gttic(linearize_new); // 7. Linearize new factors - if (params_.cacheLinearizedFactors) { - gttic(linearize); - auto linearFactors = newFactors.linearize(theta_); - if (params_.findUnusedFactorSlots) { - linearFactors_.resize(nonlinearFactors_.size()); - for (size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI) - linearFactors_[result.newFactorsIndices[newFactorI]] = - (*linearFactors)[newFactorI]; - } else { - linearFactors_.push_back(*linearFactors); - } - assert(nonlinearFactors_.size() == linearFactors_.size()); - gttoc(linearize); - } - gttoc(linearize_new); - - gttic(augment_VI); - // Augment the variable index with the new factors - if (params_.findUnusedFactorSlots) - variableIndex_.augment(newFactors, result.newFactorsIndices); - else - variableIndex_.augment(newFactors); - - // Augment it with existing factors which now affect to more variables: - if (updateParams.newAffectedKeys) { - for (const auto &factorAddedKeys : *updateParams.newAffectedKeys) { - const auto factorIdx = factorAddedKeys.first; - variableIndex_.augmentExistingFactor( - factorIdx, factorAddedKeys.second); - } - } - gttoc(augment_VI); - - gttic(recalculate); - // 8. Redo top of Bayes tree - boost::shared_ptr replacedKeys; - if (!markedKeys.empty() || !observedKeys.empty()) - replacedKeys = recalculate( - markedKeys, relinKeys, observedKeys, unusedIndices, - updateParams.constrainedKeys, &result); - - // Update replaced keys mask (accumulates until back-substitution takes place) - if (replacedKeys) - deltaReplacedMask_.insert(replacedKeys->begin(), replacedKeys->end()); - gttoc(recalculate); - - // Update data structures to remove unused keys - if (!unusedKeys.empty()) { - gttic(remove_variables); - removeVariables(unusedKeys); - gttoc(remove_variables); - } + update.linearizeNewFactors(newFactors, theta_, nonlinearFactors_.size(), + result.newFactorsIndices, &linearFactors_); + update.augmentVariableIndex(newFactors, result.newFactorsIndices, + &variableIndex_); + + // 8. Redo top of Bayes tree and update data structures + recalculate(updateParams, relinKeys, &result); + if (!result.unusedKeys.empty()) removeVariables(result.unusedKeys); result.cliques = this->nodes().size(); - gttic(evaluate_error_after); if (params_.evaluateNonlinearError) - result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); - gttoc(evaluate_error_after); - + update.error(nonlinearFactors_, calculateEstimate(), &result.errorAfter); return result; } @@ -966,7 +606,7 @@ void ISAM2::marginalizeLeaves( } } // Create factor graph from factor indices - for (const auto index: factorsFromMarginalizedInClique_step1) { + for (const auto index : factorsFromMarginalizedInClique_step1) { graph.push_back(nonlinearFactors_[index]->linearize(theta_)); } @@ -1040,7 +680,7 @@ void ISAM2::marginalizeLeaves( // Remove the factors to remove that have been summarized in the newly-added // marginal factors NonlinearFactorGraph removedFactors; - for (const auto index: factorIndicesToRemove) { + for (const auto index : factorIndicesToRemove) { removedFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); if (params_.cacheLinearizedFactors) linearFactors_.remove(index); @@ -1057,6 +697,7 @@ void ISAM2::marginalizeLeaves( } /* ************************************************************************* */ +// Marked const but actually changes mutable delta void ISAM2::updateDelta(bool forceFullSolve) const { gttic(updateDelta); if (params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { @@ -1066,8 +707,8 @@ void ISAM2::updateDelta(bool forceFullSolve) const { const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; gttic(Wildfire_update); - lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( - roots_, deltaReplacedMask_, effectiveWildfireThreshold, &delta_); + DeltaImpl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_, + effectiveWildfireThreshold, &delta_); deltaReplacedMask_.clear(); gttoc(Wildfire_update); @@ -1083,15 +724,15 @@ void ISAM2::updateDelta(bool forceFullSolve) const { // Compute Newton's method step gttic(Wildfire_update); - lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( + DeltaImpl::UpdateGaussNewtonDelta( roots_, deltaReplacedMask_, effectiveWildfireThreshold, &deltaNewton_); gttoc(Wildfire_update); // Compute steepest descent step const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient - Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, - &RgProd_); // Update RgProd - const VectorValues dx_u = Impl::ComputeGradientSearch( + DeltaImpl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, + &RgProd_); // Update RgProd + const VectorValues dx_u = DeltaImpl::ComputeGradientSearch( gradAtZero, RgProd_); // Compute gradient search point // Clear replaced keys mask because now we've updated deltaNewton_ and @@ -1113,6 +754,8 @@ void ISAM2::updateDelta(bool forceFullSolve) const { doglegResult .dx_d; // Copy the VectorValues containing with the linear solution gttoc(Copy_dx_d); + } else { + throw std::runtime_error("iSAM2: unknown ISAM2Params type"); } } @@ -1155,37 +798,13 @@ double ISAM2::error(const VectorValues& x) const { return GaussianFactorGraph(*this).error(x); } -/* ************************************************************************* */ -static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, - VectorValues* g) { - // Loop through variables in each clique, adding contributions - DenseIndex variablePosition = 0; - for (GaussianConditional::const_iterator jit = root->conditional()->begin(); - jit != root->conditional()->end(); ++jit) { - const DenseIndex dim = root->conditional()->getDim(jit); - pair pos_ins = g->tryInsert( - *jit, root->gradientContribution().segment(variablePosition, dim)); - if (!pos_ins.second) - pos_ins.first->second += - root->gradientContribution().segment(variablePosition, dim); - variablePosition += dim; - } - - // Recursively add contributions from children - typedef boost::shared_ptr sharedClique; - for (const sharedClique& child : root->children) { - gradientAtZeroTreeAdder(child, g); - } -} - /* ************************************************************************* */ VectorValues ISAM2::gradientAtZero() const { // Create result VectorValues g; // Sum up contributions for each clique - for (const ISAM2::sharedClique& root : this->roots()) - gradientAtZeroTreeAdder(root, &g); + for (const auto& root : this->roots()) root->addGradientAtZero(&g); return g; } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 60cdc45feb..9f33e757ff 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -20,12 +20,12 @@ #pragma once +#include +#include #include #include -#include #include #include -#include #include @@ -73,8 +73,8 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ - mutable KeySet - deltaReplacedMask_; // TODO(dellaert): Make sure accessed in the right way + mutable KeySet deltaReplacedMask_; // TODO(dellaert): Make sure accessed in + // the right way /** All original nonlinear factors are stored here to use during * relinearization */ @@ -97,11 +97,11 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { ///< periodic relinearization public: - typedef ISAM2 This; ///< This class - typedef BayesTree Base; ///< The BayesTree base class - typedef Base::Clique Clique; ///< A clique - typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique - typedef Base::Cliques Cliques; ///< List of Clique typedef from base class + using This = ISAM2; ///< This class + using Base = BayesTree; ///< The BayesTree base class + using Clique = Base::Clique; ///< A clique + using sharedClique = Base::sharedClique; ///< Shared pointer to a clique + using Cliques = Base::Cliques; ///< List of Cliques /** Create an empty ISAM2 instance */ explicit ISAM2(const ISAM2Params& params); @@ -175,9 +175,9 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { * @return An ISAM2Result struct containing information about the update * @note No default parameters to avoid ambiguous call errors. */ - virtual ISAM2Result update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, - const ISAM2UpdateParams& updateParams); + virtual ISAM2Result update(const NonlinearFactorGraph& newFactors, + const Values& newTheta, + const ISAM2UpdateParams& updateParams); /** Marginalize out variables listed in leafKeys. These keys must be leaves * in the BayesTree. Throws MarginalizeNonleafException if non-leaves are @@ -226,7 +226,6 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { return traits::Retract(theta_.at(key), delta); } - /** Compute an estimate for a single variable using its incomplete linear * delta computed during the last update. This is faster than calling the * no-argument version of calculateEstimate, which operates on all variables. @@ -243,9 +242,6 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /// @name Public members for non-typical usage /// @{ - /** Internal implementation functions */ - struct Impl; - /** Compute an estimate using a complete delta computed by a full * back-substitution. */ @@ -268,13 +264,6 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /** Access the nonlinear variable index */ const KeySet& getFixedVariables() const { return fixedVariables_; } - size_t lastAffectedVariableCount; - size_t lastAffectedFactorCount; - size_t lastAffectedCliqueCount; - size_t lastAffectedMarkedCount; - mutable size_t lastBacksubVariableCount; - size_t lastNnzTop; - const ISAM2Params& params() const { return params_; } /** prints out clique statistics */ @@ -292,40 +281,39 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /// @} protected: + /// Remove marked top and either recalculate in batch or incrementally. + void recalculate(const ISAM2UpdateParams& updateParams, + const KeySet& relinKeys, ISAM2Result* result); + + // Do a batch step - reorder and relinearize all variables + void recalculateBatch(const ISAM2UpdateParams& updateParams, + KeySet* affectedKeysSet, ISAM2Result* result); + + // retrieve all factors that ONLY contain the affected variables + // (note that the remaining stuff is summarized in the cached factors) + GaussianFactorGraph relinearizeAffectedFactors( + const ISAM2UpdateParams& updateParams, const FastList& affectedKeys, + const KeySet& relinKeys); + + void recalculateIncremental(const ISAM2UpdateParams& updateParams, + const KeySet& relinKeys, + const FastList& affectedKeys, + KeySet* affectedKeysSet, Cliques* orphans, + ISAM2Result* result); + /** * Add new variables to the ISAM2 system. * @param newTheta Initial values for new variables - * @param theta Current solution to be augmented with new initialization - * @param delta Current linear delta to be augmented with zeros - * @param deltaNewton - * @param RgProd - * @param keyFormatter Formatter for printing nonlinear keys during debugging + * @param variableStatus optional detailed result structure */ - void addVariables(const Values& newTheta); + void addVariables(const Values& newTheta, + ISAM2Result::DetailedResults* detail = 0); /** * Remove variables from the ISAM2 system. */ void removeVariables(const KeySet& unusedKeys); - /** - * Apply expmap to the given values, but only for indices appearing in - * \c mask. Values are expmapped in-place. - * \param mask Mask on linear indices, only \c true entries are expmapped - */ - void expmapMasked(const KeySet& mask); - - FactorIndexSet getAffectedFactors(const FastList& keys) const; - GaussianFactorGraph::shared_ptr relinearizeAffectedFactors( - const FastList& affectedKeys, const KeySet& relinKeys) const; - GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans); - - virtual boost::shared_ptr recalculate( - const KeySet& markedKeys, const KeySet& relinKeys, - const KeyVector& observedKeys, const KeySet& unusedIndices, - const boost::optional >& constrainKeys, - ISAM2Result* result); - void updateDelta(bool forceFullSolve = false) const; }; // ISAM2 @@ -334,5 +322,3 @@ template <> struct traits : public Testable {}; } // namespace gtsam - -#include diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp index 96212c9238..f7a1c14a03 100644 --- a/gtsam/nonlinear/ISAM2Clique.cpp +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -19,7 +19,9 @@ #include #include #include + #include +#include using namespace std; @@ -304,7 +306,7 @@ void ISAM2Clique::findAll(const KeySet& markedMask, KeySet* keys) const { static const bool debug = false; // does the separator contain any of the variables? bool found = false; - for (Key key : conditional()->parents()) { + for (Key key : conditional_->parents()) { if (markedMask.exists(key)) { found = true; break; @@ -312,14 +314,34 @@ void ISAM2Clique::findAll(const KeySet& markedMask, KeySet* keys) const { } if (found) { // then add this clique - keys->insert(conditional()->beginFrontals(), conditional()->endFrontals()); + keys->insert(conditional_->beginFrontals(), conditional_->endFrontals()); if (debug) print("Key(s) marked in clique "); - if (debug) cout << "so marking key " << conditional()->front() << endl; + if (debug) cout << "so marking key " << conditional_->front() << endl; } for (const auto& child : children) { child->findAll(markedMask, keys); } } +/* ************************************************************************* */ +void ISAM2Clique::addGradientAtZero(VectorValues* g) const { + // Loop through variables in each clique, adding contributions + DenseIndex position = 0; + for (auto it = conditional_->begin(); it != conditional_->end(); ++it) { + const DenseIndex dim = conditional_->getDim(it); + const Vector contribution = gradientContribution_.segment(position, dim); + VectorValues::iterator values_it; + bool success; + std::tie(values_it, success) = g->tryInsert(*it, contribution); + if (!success) values_it->second += contribution; + position += dim; + } + + // Recursively add contributions from children + for (const auto& child : children) { + child->addGradientAtZero(g); + } +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h index 3c53e3d723..53bdaec819 100644 --- a/gtsam/nonlinear/ISAM2Clique.h +++ b/gtsam/nonlinear/ISAM2Clique.h @@ -75,9 +75,12 @@ class GTSAM_EXPORT ISAM2Clique /** Access the cached factor */ Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } - /** Access the gradient contribution */ + /// Access the gradient contribution const Vector& gradientContribution() const { return gradientContribution_; } + /// Recursively add gradient at zero to g + void addGradientAtZero(VectorValues* g) const; + bool equals(const This& other, double tol = 1e-9) const; /** print this node */ diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h index afddd1f8e9..5cf962e43d 100644 --- a/gtsam/nonlinear/ISAM2Params.h +++ b/gtsam/nonlinear/ISAM2Params.h @@ -234,8 +234,8 @@ struct GTSAM_EXPORT ISAM2Params { Factorization _factorization = ISAM2Params::CHOLESKY, bool _cacheLinearizedFactors = true, const KeyFormatter& _keyFormatter = - DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter - ) + DefaultKeyFormatter, ///< see ISAM2::Params::keyFormatter, + bool _enableDetailedResults = false) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold), relinearizeSkip(_relinearizeSkip), @@ -244,7 +244,7 @@ struct GTSAM_EXPORT ISAM2Params { factorization(_factorization), cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter), - enableDetailedResults(false), + enableDetailedResults(_enableDetailedResults), enablePartialRelinearizationCheck(false), findUnusedFactorSlots(false) {} diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index 763a5e1985..e45b17e4ac 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -24,8 +24,8 @@ #include #include -#include #include +#include #include @@ -38,7 +38,7 @@ namespace gtsam { * converging, and about how much work was required for the update. See member * variables for details and information about each entry. */ -struct GTSAM_EXPORT ISAM2Result { +struct ISAM2Result { /** The nonlinear error of all of the factors, \a including new factors and * variables added during the current call to ISAM2::update(). This error is * calculated using the following variable values: @@ -96,7 +96,22 @@ struct GTSAM_EXPORT ISAM2Result { */ FactorIndices newFactorsIndices; - /** A struct holding detailed results, which must be enabled with + /** Unused keys, and indices for unused keys, + * i.e., keys that are empty now and do not appear in the new factors. + */ + KeySet unusedKeys; + + /** keys for variables that were observed, i.e., not unused. */ + KeyVector observedKeys; + + /** Keys of variables that had factors removed. */ + KeySet keysWithRemovedFactors; + + /** All keys that were marked during the update process. */ + KeySet markedKeys; + + /** + * A struct holding detailed results, which must be enabled with * ISAM2Params::enableDetailedResults. */ struct DetailedResults { @@ -132,15 +147,24 @@ struct GTSAM_EXPORT ISAM2Result { inRootClique(false) {} }; - /** The status of each variable during this update, see VariableStatus. - */ - FastMap variableStatus; + using StatusMap = FastMap; + + /// The status of each variable during this update, see VariableStatus. + StatusMap variableStatus; }; /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See * Detail for information about the results data stored here. */ boost::optional detail; + explicit ISAM2Result(bool enableDetailedResults = false) { + if (enableDetailedResults) detail.reset(DetailedResults()); + } + + /// Return pointer to detail, 0 if no detail requested + DetailedResults* details() { return detail.get_ptr(); } + + /// Print results void print(const std::string str = "") const { using std::cout; cout << str << " Reelimintated: " << variablesReeliminated diff --git a/gtsam/nonlinear/ISAM2UpdateParams.h b/gtsam/nonlinear/ISAM2UpdateParams.h index 77f722b12a..44519e3aab 100644 --- a/gtsam/nonlinear/ISAM2UpdateParams.h +++ b/gtsam/nonlinear/ISAM2UpdateParams.h @@ -16,11 +16,11 @@ #pragma once -#include #include -#include // GTSAM_EXPORT -#include // Key, KeySet -#include //FactorIndices +#include // GTSAM_EXPORT +#include // Key, KeySet +#include //FactorIndices +#include namespace gtsam { @@ -29,7 +29,7 @@ namespace gtsam { * This struct is used by ISAM2::update() to pass additional parameters to * give the user a fine-grained control on how factors and relinearized, etc. */ -struct GTSAM_EXPORT ISAM2UpdateParams { +struct ISAM2UpdateParams { ISAM2UpdateParams() = default; /** Indices of factors to remove from system (default: empty) */ @@ -63,8 +63,12 @@ struct GTSAM_EXPORT ISAM2UpdateParams { * depend on Keys `X(2)`, `X(3)`. Next call to ISAM2::update() must include * its `newAffectedKeys` field with the map `13 -> {X(2), X(3)}`. */ - boost::optional> newAffectedKeys{boost::none}; + boost::optional> newAffectedKeys{boost::none}; + /** By default, iSAM2 uses a wildfire update scheme that stops updating when + * the deltas become too small down in the tree. This flagg forces a full + * solve instead. */ + bool forceFullSolve{false}; }; -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index b9579661d4..c85891af2d 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -201,9 +201,9 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, double minAbsoluteTolerance = params_.relativeErrorTol * currentState->error; // if the change is small we terminate - if (fabs(costChange) < minAbsoluteTolerance) { + if (std::abs(costChange) < minAbsoluteTolerance) { if (verbose) - cout << "fabs(costChange)=" << fabs(costChange) + cout << "abs(costChange)=" << std::abs(costChange) << " minAbsoluteTolerance=" << minAbsoluteTolerance << " (relativeErrorTol=" << params_.relativeErrorTol << ")" << endl; stopSearchingLambda = true; diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 928b59e773..8a1f600ffe 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -23,7 +23,7 @@ namespace gtsam { * This factor does have the ability to perform relinearization under small-angle and * linearity assumptions if a linearization point is added. */ -class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor { +class LinearContainerFactor : public NonlinearFactor { protected: GaussianFactor::shared_ptr factor_; @@ -33,7 +33,7 @@ class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor { LinearContainerFactor() {} /** direct copy constructor */ - LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint); + GTSAM_EXPORT LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint); // Some handy typedefs typedef NonlinearFactor Base; @@ -44,13 +44,13 @@ class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor { typedef boost::shared_ptr shared_ptr; /** Primary constructor: store a linear factor with optional linearization point */ - LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values()); + GTSAM_EXPORT LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values()); /** Primary constructor: store a linear factor with optional linearization point */ - LinearContainerFactor(const HessianFactor& factor, const Values& linearizationPoint = Values()); + GTSAM_EXPORT LinearContainerFactor(const HessianFactor& factor, const Values& linearizationPoint = Values()); /** Constructor from shared_ptr */ - LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values()); + GTSAM_EXPORT LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values()); // Access @@ -59,10 +59,10 @@ class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor { // Testable /** print */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + GTSAM_EXPORT void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; /** Check if two factors are equal */ - bool equals(const NonlinearFactor& f, double tol = 1e-9) const; + GTSAM_EXPORT bool equals(const NonlinearFactor& f, double tol = 1e-9) const; // NonlinearFactor @@ -74,10 +74,10 @@ class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor { * * @return nonlinear error if linearizationPoint present, zero otherwise */ - double error(const Values& c) const; + GTSAM_EXPORT double error(const Values& c) const; /** get the dimension of the factor: rows of linear factor */ - size_t dim() const; + GTSAM_EXPORT size_t dim() const; /** Extract the linearization point used in recalculating error */ const boost::optional& linearizationPoint() const { return linearizationPoint_; } @@ -98,17 +98,17 @@ class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor { * TODO: better approximation of relinearization * TODO: switchable modes for approximation technique */ - GaussianFactor::shared_ptr linearize(const Values& c) const; + GTSAM_EXPORT GaussianFactor::shared_ptr linearize(const Values& c) const; /** * Creates an anti-factor directly */ - GaussianFactor::shared_ptr negateToGaussian() const; + GTSAM_EXPORT GaussianFactor::shared_ptr negateToGaussian() const; /** * Creates the equivalent anti-factor as another LinearContainerFactor. */ - NonlinearFactor::shared_ptr negateToNonlinear() const; + GTSAM_EXPORT NonlinearFactor::shared_ptr negateToNonlinear() const; /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow @@ -127,31 +127,31 @@ class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor { /** * Simple checks whether this is a Jacobian or Hessian factor */ - bool isJacobian() const; - bool isHessian() const; + GTSAM_EXPORT bool isJacobian() const; + GTSAM_EXPORT bool isHessian() const; /** Casts to JacobianFactor */ - boost::shared_ptr toJacobian() const; + GTSAM_EXPORT boost::shared_ptr toJacobian() const; /** Casts to HessianFactor */ - boost::shared_ptr toHessian() const; + GTSAM_EXPORT boost::shared_ptr toHessian() const; /** * Utility function for converting linear graphs to nonlinear graphs * consisting of LinearContainerFactors. */ - static NonlinearFactorGraph ConvertLinearGraph(const GaussianFactorGraph& linear_graph, + GTSAM_EXPORT static NonlinearFactorGraph ConvertLinearGraph(const GaussianFactorGraph& linear_graph, const Values& linearizationPoint = Values()); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, + GTSAM_EXPORT static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, const Values& linearizationPoint = Values()) { return ConvertLinearGraph(linear_graph, linearizationPoint); } #endif protected: - void initializeLinearizationPoint(const Values& linearizationPoint); + GTSAM_EXPORT void initializeLinearizationPoint(const Values& linearizationPoint); private: diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 0486c1f296..abf6b257aa 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -107,7 +107,7 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { newStep - resphi * (newStep - minStep); if ((maxStep - minStep) - < tau * (std::fabs(testStep) + std::fabs(newStep))) { + < tau * (std::abs(testStep) + std::abs(newStep))) { return 0.5 * (minStep + maxStep); } diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 3018a14b9f..180f4fb84d 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -31,7 +31,7 @@ namespace gtsam { /** The common parameters for Nonlinear optimizers. Most optimizers * deriving from NonlinearOptimizer also subclass the parameters. */ -class GTSAM_EXPORT NonlinearOptimizerParams { +class NonlinearOptimizerParams { public: /** See NonlinearOptimizerParams::verbosity */ enum Verbosity { @@ -52,7 +52,7 @@ class GTSAM_EXPORT NonlinearOptimizerParams { virtual ~NonlinearOptimizerParams() { } - virtual void print(const std::string& str = "") const; + GTSAM_EXPORT virtual void print(const std::string& str = "") const; size_t getMaxIterations() const { return maxIterations; } double getRelativeErrorTol() const { return relativeErrorTol; } @@ -68,8 +68,8 @@ class GTSAM_EXPORT NonlinearOptimizerParams { verbosity = verbosityTranslator(src); } - static Verbosity verbosityTranslator(const std::string &s) ; - static std::string verbosityTranslator(Verbosity value) ; + GTSAM_EXPORT static Verbosity verbosityTranslator(const std::string &s) ; + GTSAM_EXPORT static std::string verbosityTranslator(Verbosity value) ; /** See NonlinearOptimizerParams::linearSolverType */ enum LinearSolverType { @@ -144,10 +144,10 @@ class GTSAM_EXPORT NonlinearOptimizerParams { } private: - std::string linearSolverTranslator(LinearSolverType linearSolverType) const; - LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; - std::string orderingTypeTranslator(Ordering::OrderingType type) const; - Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; + GTSAM_EXPORT std::string linearSolverTranslator(LinearSolverType linearSolverType) const; + GTSAM_EXPORT LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; + GTSAM_EXPORT std::string orderingTypeTranslator(Ordering::OrderingType type) const; + GTSAM_EXPORT Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; }; // For backward compatibility: diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 7e14578c39..ff220044a0 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -145,13 +145,13 @@ namespace gtsam { boost::make_filter_iterator(filter, ((const Values&) values).begin(), ((const Values&) values).end()), - &ValuesCastHelper::cast)), constEnd_( boost::make_transform_iterator( boost::make_filter_iterator(filter, ((const Values&) values).end(), ((const Values&) values).end()), - &ValuesCastHelper::cast)) { } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 1048cd73a7..4b0fceaf93 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -422,7 +422,7 @@ namespace gtsam { }; /* ************************************************************************* */ - class GTSAM_EXPORT ValuesKeyAlreadyExists : public std::exception { + class ValuesKeyAlreadyExists : public std::exception { protected: const Key key_; ///< The key that already existed @@ -440,11 +440,11 @@ namespace gtsam { Key key() const throw() { return key_; } /// The message to be displayed to the user - virtual const char* what() const throw(); + GTSAM_EXPORT virtual const char* what() const throw(); }; /* ************************************************************************* */ - class GTSAM_EXPORT ValuesKeyDoesNotExist : public std::exception { + class ValuesKeyDoesNotExist : public std::exception { protected: const char* operation_; ///< The operation that attempted to access the key const Key key_; ///< The key that does not exist @@ -463,11 +463,11 @@ namespace gtsam { Key key() const throw() { return key_; } /// The message to be displayed to the user - virtual const char* what() const throw(); + GTSAM_EXPORT virtual const char* what() const throw(); }; /* ************************************************************************* */ - class GTSAM_EXPORT ValuesIncorrectType : public std::exception { + class ValuesIncorrectType : public std::exception { protected: const Key key_; ///< The key requested const std::type_info& storedTypeId_; @@ -494,11 +494,11 @@ namespace gtsam { const std::type_info& requestedTypeId() const { return requestedTypeId_; } /// The message to be displayed to the user - virtual const char* what() const throw(); + GTSAM_EXPORT virtual const char* what() const throw(); }; /* ************************************************************************* */ - class GTSAM_EXPORT DynamicValuesMismatched : public std::exception { + class DynamicValuesMismatched : public std::exception { public: DynamicValuesMismatched() throw() {} @@ -511,7 +511,7 @@ namespace gtsam { }; /* ************************************************************************* */ - class GTSAM_EXPORT NoMatchFoundForFixed: public std::exception { + class NoMatchFoundForFixed: public std::exception { protected: const size_t M1_, N1_; @@ -528,7 +528,7 @@ namespace gtsam { virtual ~NoMatchFoundForFixed() throw () { } - virtual const char* what() const throw (); + GTSAM_EXPORT virtual const char* what() const throw (); }; /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 67e0d1f246..8fcf84a113 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -121,7 +121,7 @@ class Class : public Point3 { return norm3(*this, H); } bool equals(const Class &q, double tol) const { - return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && fabs(z() - q.z()) < tol); + return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol && std::abs(z() - q.z()) < tol); } void print(const string& s) const { cout << s << *this << endl;} }; diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 9c412eb191..f949514e39 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -146,7 +146,7 @@ namespace gtsam { /** Syntactic sugar for constrained version */ BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : BetweenFactor(key1, key2, measured, - noiseModel::Constrained::All(traits::GetDimension(measured), fabs(mu))) + noiseModel::Constrained::All(traits::GetDimension(measured), std::abs(mu))) {} private: diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 8bb7a3ab84..5b085652f1 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -18,6 +18,8 @@ #include #include +#include + namespace gtsam { /** @@ -70,10 +72,14 @@ namespace gtsam { /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; traits::Print(prior_, " prior mean: "); - this->noiseModel_->print(" noise model: "); + if (this->noiseModel_) + this->noiseModel_->print(" noise model: "); + else + std::cout << "no noise model" << std::endl; } /** equals */ diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 489a4adf40..34f9b9e9f1 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -75,15 +75,12 @@ class SmartFactorBase: public NonlinearFactor { */ ZVector measured_; - /// @name Pose of the camera in the body frame - boost::optional body_P_sensor_; ///< Pose of the camera in the body frame - /// @} + boost::optional body_P_sensor_; ///< Pose of the camera in the body frame // Cache for Fblocks, to avoid a malloc ever time we re-linearize mutable FBlocks Fs; -public: - + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor @@ -118,14 +115,17 @@ class SmartFactorBase: public NonlinearFactor { } /** - * Add a new measurement and pose key - * @param measured_i is the 2m dimensional projection of a single landmark - * @param poseKey is the index corresponding to the camera observing the landmark - * @param sharedNoiseModel is the measurement noise + * Add a new measurement and pose/camera key + * @param measured is the 2m dimensional projection of a single landmark + * @param key is the index corresponding to the camera observing the landmark */ - void add(const Z& measured_i, const Key& cameraKey_i) { - this->measured_.push_back(measured_i); - this->keys_.push_back(cameraKey_i); + void add(const Z& measured, const Key& key) { + if(std::find(keys_.begin(), keys_.end(), key) != keys_.end()) { + throw std::invalid_argument( + "SmartFactorBase::add: adding duplicate measurement for key."); + } + this->measured_.push_back(measured); + this->keys_.push_back(key); } /** @@ -133,8 +133,7 @@ class SmartFactorBase: public NonlinearFactor { */ void add(ZVector& measurements, KeyVector& cameraKeys) { for (size_t i = 0; i < measurements.size(); i++) { - this->measured_.push_back(measurements.at(i)); - this->keys_.push_back(cameraKeys.at(i)); + this->add(measurements.at(i), cameraKeys.at(i)); } } @@ -198,17 +197,20 @@ class SmartFactorBase: public NonlinearFactor { return e && Base::equals(p, tol) && areMeasurementsEqual; } - /// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives - template - Vector unwhitenedError(const Cameras& cameras, const POINT& point, - boost::optional Fs = boost::none, // + /// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and + /// derivatives + template + Vector unwhitenedError( + const Cameras& cameras, const POINT& point, + boost::optional Fs = boost::none, // boost::optional E = boost::none) const { Vector ue = cameras.reprojectionError(point, measured_, Fs, E); - if(body_P_sensor_ && Fs){ - for(size_t i=0; i < Fs->size(); i++){ - Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); + if (body_P_sensor_ && Fs) { + const Pose3 sensor_P_body = body_P_sensor_->inverse(); + for (size_t i = 0; i < Fs->size(); i++) { + const Pose3 w_Pose_body = cameras[i].pose() * sensor_P_body; Matrix J(6, 6); - Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); + const Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); Fs->at(i) = Fs->at(i) * J; } } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index cbeed77c50..15d632cda2 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -79,15 +79,15 @@ class SmartProjectionFactor: public SmartFactorBase { /** * Constructor - * @param body_P_sensor pose of the camera in the body frame - * @param params internal parameters of the smart factors + * @param sharedNoiseModel isotropic noise model for the 2D feature measurements + * @param params parameters for the smart projection factors */ - SmartProjectionFactor(const SharedNoiseModel& sharedNoiseModel, - const boost::optional body_P_sensor = boost::none, - const SmartProjectionParams& params = SmartProjectionParams()) : - Base(sharedNoiseModel, body_P_sensor), params_(params), // - result_(TriangulationResult::Degenerate()) { - } + SmartProjectionFactor( + const SharedNoiseModel& sharedNoiseModel, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel), + params_(params), + result_(TriangulationResult::Degenerate()) {} /** Virtual destructor */ virtual ~SmartProjectionFactor() { @@ -443,7 +443,26 @@ class SmartProjectionFactor: public SmartFactorBase { /** return the farPoint state */ bool isFarPoint() const { return result_.farPoint(); } -private: +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + // It does not make sense to optimize for a camera where the pose would not be + // the actual pose of the camera. An unfortunate consequence of deprecating + // this constructor means that we cannot optimize for calibration when the + // camera is offset from the body pose. That would need a new factor with + // (body) pose and calibration as variables. However, that use case is + // unlikely: when a global offset is know, calibration is typically known. + SmartProjectionFactor( + const SharedNoiseModel& sharedNoiseModel, + const boost::optional body_P_sensor, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, body_P_sensor), + params_(params), + result_(TriangulationResult::Degenerate()) {} + /// @} +#endif + + private: /// Serialization function friend class boost::serialization::access; diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index bb261a9c4e..b5be46258a 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -66,16 +66,31 @@ class SmartProjectionPoseFactor: public SmartProjectionFactor< /** * Constructor - * @param Isotropic measurement noise + * @param sharedNoiseModel isotropic noise model for the 2D feature measurements * @param K (fixed) calibration, assumed to be the same for all cameras - * @param body_P_sensor pose of the camera in the body frame - * @param params internal parameters of the smart factors + * @param params parameters for the smart projection factors */ - SmartProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, + SmartProjectionPoseFactor( + const SharedNoiseModel& sharedNoiseModel, const boost::shared_ptr K, - const boost::optional body_P_sensor = boost::none, - const SmartProjectionParams& params = SmartProjectionParams()) : - Base(sharedNoiseModel, body_P_sensor, params), K_(K) { + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, params), K_(K) { + } + + /** + * Constructor + * @param sharedNoiseModel isotropic noise model for the 2D feature measurements + * @param K (fixed) calibration, assumed to be the same for all cameras + * @param body_P_sensor pose of the camera in the body frame (optional) + * @param params parameters for the smart projection factors + */ + SmartProjectionPoseFactor( + const SharedNoiseModel& sharedNoiseModel, + const boost::shared_ptr K, + const boost::optional body_P_sensor, + const SmartProjectionParams& params = SmartProjectionParams()) + : SmartProjectionPoseFactor(sharedNoiseModel, K, params) { + this->body_P_sensor_ = body_P_sensor; } /** Virtual destructor */ @@ -123,18 +138,16 @@ class SmartProjectionPoseFactor: public SmartProjectionFactor< */ typename Base::Cameras cameras(const Values& values) const { typename Base::Cameras cameras; - for(const Key& k: this->keys_) { - Pose3 pose = values.at(k); - if (Base::body_P_sensor_) - pose = pose.compose(*(Base::body_P_sensor_)); - - Camera camera(pose, K_); - cameras.push_back(camera); + for (const Key& k : this->keys_) { + const Pose3 world_P_sensor_k = + Base::body_P_sensor_ ? values.at(k) * *Base::body_P_sensor_ + : values.at(k); + cameras.emplace_back(world_P_sensor_k, K_); } return cameras; } -private: + private: /// Serialization function friend class boost::serialization::access; diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 17a8d849f7..4294d17d11 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -42,7 +42,7 @@ inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) { namespace internal { // define getter that returns value rather than reference -Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) { +inline Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) { return pose.rotation(H); } } // namespace internal @@ -70,12 +70,12 @@ inline Unit3_ unrotate(const Rot3_& x, const Unit3_& p) { #ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS namespace internal { // define a rotate and unrotate for Vector3 -Vector3 rotate(const Rot3& R, const Vector3& v, +inline Vector3 rotate(const Rot3& R, const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) { return R.rotate(v, H1, H2); } -Vector3 unrotate(const Rot3& R, const Vector3& v, +inline Vector3 unrotate(const Rot3& R, const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) { return R.unrotate(v, H1, H2); diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp similarity index 94% rename from gtsam/slam/tests/testSmartProjectionCameraFactor.cpp rename to gtsam/slam/tests/testSmartProjectionFactor.cpp index ddb4d8adb1..c6d1a5b2cf 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testSmartProjectionCameraFactor.cpp - * @brief Unit tests for SmartProjectionCameraFactor Class + * @file testSmartProjectionFactor.cpp + * @brief Unit tests for SmartProjectionFactor Class * @author Chris Beall * @author Luca Carlone * @author Zsolt Kira @@ -29,19 +29,11 @@ using namespace boost::assign; -static bool isDebugTest = false; - -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - -static Key x1(1); -Symbol l1('l', 1), l2('l', 2), l3('l', 3); -Key c1 = 1, c2 = 2, c3 = 3; - -static Point2 measurement1(323.0, 240.0); - -static double rankTol = 1.0; +static const bool isDebugTest = false; +static const Symbol l1('l', 1), l2('l', 2), l3('l', 3); +static const Key c1 = 1, c2 = 2, c3 = 3; +static const Point2 measurement1(323.0, 240.0); +static const double rankTol = 1.0; template PinholeCamera perturbCameraPoseAndCalibration( @@ -58,7 +50,7 @@ PinholeCamera perturbCameraPoseAndCalibration( } /* ************************************************************************* */ -TEST( SmartProjectionCameraFactor, perturbCameraPose) { +TEST(SmartProjectionFactor, perturbCameraPose) { using namespace vanilla; Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); @@ -70,45 +62,45 @@ TEST( SmartProjectionCameraFactor, perturbCameraPose) { } /* ************************************************************************* */ -TEST( SmartProjectionCameraFactor, Constructor) { +TEST(SmartProjectionFactor, Constructor) { using namespace vanilla; SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); } /* ************************************************************************* */ -TEST( SmartProjectionCameraFactor, Constructor2) { +TEST(SmartProjectionFactor, Constructor2) { using namespace vanilla; params.setRankTolerance(rankTol); - SmartFactor factor1(unit2, boost::none, params); + SmartFactor factor1(unit2, params); } /* ************************************************************************* */ -TEST( SmartProjectionCameraFactor, Constructor3) { +TEST(SmartProjectionFactor, Constructor3) { using namespace vanilla; SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); - factor1->add(measurement1, x1); + factor1->add(measurement1, c1); } /* ************************************************************************* */ -TEST( SmartProjectionCameraFactor, Constructor4) { +TEST(SmartProjectionFactor, Constructor4) { using namespace vanilla; params.setRankTolerance(rankTol); - SmartFactor factor1(unit2, boost::none, params); - factor1.add(measurement1, x1); + SmartFactor factor1(unit2, params); + factor1.add(measurement1, c1); } /* ************************************************************************* */ -TEST( SmartProjectionCameraFactor, Equals ) { +TEST(SmartProjectionFactor, Equals ) { using namespace vanilla; SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); - factor1->add(measurement1, x1); + factor1->add(measurement1, c1); SmartFactor::shared_ptr factor2(new SmartFactor(unit2)); - factor2->add(measurement1, x1); + factor2->add(measurement1, c1); } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, noiseless ) { +TEST(SmartProjectionFactor, noiseless ) { using namespace vanilla; Values values; @@ -127,7 +119,7 @@ TEST( SmartProjectionCameraFactor, noiseless ) { } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, noisy ) { +TEST(SmartProjectionFactor, noisy ) { using namespace vanilla; @@ -178,7 +170,7 @@ TEST( SmartProjectionCameraFactor, noisy ) { } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { +TEST(SmartProjectionFactor, perturbPoseAndOptimize ) { using namespace vanilla; @@ -277,7 +269,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { +TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) { using namespace vanilla; @@ -347,7 +339,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { +TEST(SmartProjectionFactor, perturbCamerasAndOptimize ) { using namespace vanilla; @@ -424,7 +416,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, Cal3Bundler ) { +TEST(SmartProjectionFactor, Cal3Bundler ) { using namespace bundler; @@ -497,7 +489,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) { } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { +TEST(SmartProjectionFactor, Cal3Bundler2 ) { using namespace bundler; @@ -570,7 +562,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, noiselessBundler ) { +TEST(SmartProjectionFactor, noiselessBundler ) { using namespace bundler; Values values; @@ -598,7 +590,7 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) { } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { +TEST(SmartProjectionFactor, comparisonGeneralSfMFactor ) { using namespace bundler; Values values; @@ -637,7 +629,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { +TEST(SmartProjectionFactor, comparisonGeneralSfMFactor1 ) { using namespace bundler; Values values; @@ -681,7 +673,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { /* *************************************************************************/ // Have to think about how to compare multiplyHessianAdd in generalSfMFactor and smartFactors -//TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor2 ){ +//TEST(SmartProjectionFactor, comparisonGeneralSfMFactor2 ){ // // Values values; // values.insert(c1, level_camera); @@ -729,7 +721,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { // EXPECT(assert_equal(yActual,yExpected, 1e-7)); //} /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { +TEST(SmartProjectionFactor, computeImplicitJacobian ) { using namespace bundler; Values values; @@ -767,7 +759,7 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { +TEST(SmartProjectionFactor, implicitJacobianFactor ) { using namespace bundler; @@ -785,7 +777,7 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { params.setEnableEPI(useEPI); SmartFactor::shared_ptr explicitFactor( - new SmartFactor(unit2, boost::none, params)); + new SmartFactor(unit2, params)); explicitFactor->add(level_uv, c1); explicitFactor->add(level_uv_right, c2); @@ -797,7 +789,7 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { // Implicit Schur version params.setLinearizationMode(gtsam::IMPLICIT_SCHUR); SmartFactor::shared_ptr implicitFactor( - new SmartFactor(unit2, boost::none, params)); + new SmartFactor(unit2, params)); implicitFactor->add(level_uv, c1); implicitFactor->add(level_uv_right, c2); GaussianFactor::shared_ptr gaussianImplicitSchurFactor = @@ -818,7 +810,6 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { EXPECT(assert_equal(yActual, yExpected, 1e-7)); } - /* ************************************************************************* */ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); @@ -828,7 +819,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropi BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); -TEST( SmartProjectionCameraFactor, serialize) { +TEST(SmartProjectionFactor, serialize) { using namespace vanilla; using namespace gtsam::serializationTestHelpers; SmartFactor factor(unit2); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 080046dd4b..f833941bcc 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -62,7 +62,7 @@ TEST( SmartProjectionPoseFactor, Constructor2) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactor factor1(model, sharedK, boost::none, params); + SmartFactor factor1(model, sharedK, params); } /* ************************************************************************* */ @@ -77,7 +77,7 @@ TEST( SmartProjectionPoseFactor, Constructor4) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactor factor1(model, sharedK, boost::none, params); + SmartFactor factor1(model, sharedK, params); factor1.add(measurement1, x1); } @@ -189,9 +189,7 @@ TEST( SmartProjectionPoseFactor, noisy ) { measurements.push_back(level_uv); measurements.push_back(level_uv_right); - KeyVector views; - views.push_back(x1); - views.push_back(x2); + KeyVector views {x1, x2}; factor2->add(measurements, views); double actualError2 = factor2->error(values); @@ -199,34 +197,20 @@ TEST( SmartProjectionPoseFactor, noisy ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ - // make a realistic calibration matrix - double fov = 60; // degrees - size_t w=640,h=480; - - Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 cameraPose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses - Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); - Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); - - SimpleCamera cam1(cameraPose1, *K); // with camera poses - SimpleCamera cam2(cameraPose2, *K); - SimpleCamera cam3(cameraPose3, *K); +TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { + using namespace vanillaPose; - // create arbitrary body_Pose_sensor (transforms from sensor to body) - Pose3 sensor_to_body = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // + // create arbitrary body_T_sensor (transforms from sensor to body) + Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); // These are the poses we want to estimate, from camera measurements - Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); - Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse()); - Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse()); + const Pose3 sensor_T_body = body_T_sensor.inverse(); + Pose3 wTb1 = cam1.pose() * sensor_T_body; + Pose3 wTb2 = cam2.pose() * sensor_T_body; + Pose3 wTb3 = cam3.pose() * sensor_T_body; // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(5, 0, 3.0); + Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -236,23 +220,20 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create smart factors - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + KeyVector views {x1, x2, x3}; SmartProjectionParams params; params.setRankTolerance(1.0); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setDegeneracyMode(IGNORE_DEGENERACY); params.setEnableEPI(false); - SmartProjectionPoseFactor smartFactor1(model, K, sensor_to_body, params); + SmartFactor smartFactor1(model, sharedK, body_T_sensor, params); smartFactor1.add(measurements_cam1, views); - SmartProjectionPoseFactor smartFactor2(model, K, sensor_to_body, params); + SmartFactor smartFactor2(model, sharedK, body_T_sensor, params); smartFactor2.add(measurements_cam2, views); - SmartProjectionPoseFactor smartFactor3(model, K, sensor_to_body, params); + SmartFactor smartFactor3(model, sharedK, body_T_sensor, params); smartFactor3.add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -262,30 +243,32 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, bodyPose1, noisePrior); - graph.emplace_shared >(x2, bodyPose2, noisePrior); + graph.emplace_shared >(x1, wTb1, noisePrior); + graph.emplace_shared >(x2, wTb2, noisePrior); // Check errors at ground truth poses Values gtValues; - gtValues.insert(x1, bodyPose1); - gtValues.insert(x2, bodyPose2); - gtValues.insert(x3, bodyPose3); + gtValues.insert(x1, wTb1); + gtValues.insert(x2, wTb2); + gtValues.insert(x3, wTb3); double actualError = graph.error(gtValues); double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-7) - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); Values values; - values.insert(x1, bodyPose1); - values.insert(x2, bodyPose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, bodyPose3*noise_pose); + values.insert(x1, wTb1); + values.insert(x2, wTb2); + // initialize third pose with some noise, we expect it to move back to + // original pose3 + values.insert(x3, wTb3 * noise_pose); LevenbergMarquardtParams lmParams; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - EXPECT(assert_equal(bodyPose3,result.at(x3))); + EXPECT(assert_equal(wTb3, result.at(x3))); } /* *************************************************************************/ @@ -370,9 +353,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { measurements_cam1.push_back(cam2.project(landmark1)); // Create smart factors - KeyVector views; - views.push_back(x1); - views.push_back(x2); + KeyVector views {x1, x2}; SmartFactor::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); smartFactor1->add(measurements_cam1, views); @@ -520,10 +501,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { using namespace vanillaPose; - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + KeyVector views {x1, x2, x3}; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -577,10 +555,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { using namespace vanillaPose; - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + KeyVector views {x1, x2, x3}; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -594,18 +569,18 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { params.setLinearizationMode(gtsam::JACOBIAN_SVD); params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); params.setEnableEPI(false); - SmartFactor factor1(model, sharedK, boost::none, params); + SmartFactor factor1(model, sharedK, params); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -638,10 +613,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { double excludeLandmarksFutherThanDist = 2; - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + KeyVector views {x1, x2, x3}; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -658,15 +630,15 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { params.setEnableEPI(false); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -701,10 +673,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + KeyVector views {x1, x2, x3}; // add fourth landmark Point3 landmark4(5, -0.5, 1); @@ -725,19 +694,19 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor3->add(measurements_cam3, views); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor4->add(measurements_cam4, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -767,10 +736,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { using namespace vanillaPose; - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + KeyVector views {x1, x2, x3}; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -783,15 +749,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { params.setLinearizationMode(gtsam::JACOBIAN_Q); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -821,10 +787,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { using namespace vanillaPose2; - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + KeyVector views {x1, x2, x3}; typedef GenericProjectionFactor ProjectionFactor; NonlinearFactorGraph graph; @@ -869,10 +832,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { /* *************************************************************************/ TEST( SmartProjectionPoseFactor, CheckHessian) { - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + KeyVector views {x1, x2, x3}; using namespace vanillaPose; @@ -894,15 +854,15 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { params.setRankTolerance(10); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + new SmartFactor(model, sharedK, params)); // HESSIAN, by default smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + new SmartFactor(model, sharedK, params)); // HESSIAN, by default smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + new SmartFactor(model, sharedK, params)); // HESSIAN, by default smartFactor3->add(measurements_cam3, views); NonlinearFactorGraph graph; @@ -955,10 +915,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { using namespace vanillaPose2; - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + KeyVector views {x1, x2, x3}; // Two different cameras, at the same position, but different rotations Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); @@ -977,11 +934,11 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac params.setDegeneracyMode(gtsam::HANDLE_INFINITY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK2, boost::none, params)); + new SmartFactor(model, sharedK2, params)); smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK2, boost::none, params)); + new SmartFactor(model, sharedK2, params)); smartFactor2->add(measurements_cam2, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1014,10 +971,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) // this test considers a condition in which the cheirality constraint is triggered using namespace vanillaPose; - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + KeyVector views {x1, x2, x3}; // Two different cameras, at the same position, but different rotations Pose3 pose2 = level_pose @@ -1038,15 +992,15 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1099,9 +1053,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) { using namespace vanillaPose2; - KeyVector views; - views.push_back(x1); - views.push_back(x2); + KeyVector views {x1, x2}; // Project three landmarks into 2 cameras Point2 cam1_uv1 = cam1.project(landmark1); @@ -1133,10 +1085,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { using namespace vanillaPose; - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + KeyVector views {x1, x2, x3}; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -1186,10 +1135,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { using namespace vanillaPose2; - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + KeyVector views {x1, x2, x3}; // All cameras have the same pose so will be degenerate ! Camera cam2(level_pose, sharedK2); @@ -1241,7 +1187,7 @@ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; SmartProjectionParams params; params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - SmartFactor factor(model, sharedBundlerK, boost::none, params); + SmartFactor factor(model, sharedBundlerK, params); factor.add(measurement1, x1); } @@ -1260,10 +1206,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + KeyVector views {x1, x2, x3}; SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK)); smartFactor1->add(measurements_cam1, views); @@ -1309,10 +1252,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { using namespace bundlerPose; - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + KeyVector views {x1, x2, x3}; // Two different cameras Pose3 pose2 = level_pose @@ -1336,15 +1276,15 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedBundlerK, boost::none, params)); + new SmartFactor(model, sharedBundlerK, params)); smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedBundlerK, boost::none, params)); + new SmartFactor(model, sharedBundlerK, params)); smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedBundlerK, boost::none, params)); + new SmartFactor(model, sharedBundlerK, params)); smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1405,7 +1345,7 @@ TEST(SmartProjectionPoseFactor, serialize) { using namespace gtsam::serializationTestHelpers; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactor factor(model, sharedK, boost::none, params); + SmartFactor factor(model, sharedK, params); EXPECT(equalsObj(factor)); EXPECT(equalsXML(factor)); diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index ab89a4dbab..ca87b2bbc4 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -27,7 +27,7 @@ namespace gtsam { /** Symbolic Bayes Net * \nosubgrouping */ - class GTSAM_EXPORT SymbolicBayesNet : public FactorGraph { + class SymbolicBayesNet : public FactorGraph { public: @@ -61,14 +61,14 @@ namespace gtsam { /// @{ /** Check equality */ - bool equals(const This& bn, double tol = 1e-9) const; + GTSAM_EXPORT bool equals(const This& bn, double tol = 1e-9) const; /// @} /// @name Standard Interface /// @{ - void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + GTSAM_EXPORT void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// @} diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index e28f287640..5f7bdde7e3 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -30,7 +30,7 @@ namespace gtsam { /* ************************************************************************* */ /// A clique in a SymbolicBayesTree - class GTSAM_EXPORT SymbolicBayesTreeClique : + class SymbolicBayesTreeClique : public BayesTreeCliqueBase { public: @@ -45,7 +45,7 @@ namespace gtsam { /* ************************************************************************* */ /// A Bayes tree that represents the connectivity between variables but is not associated with any /// probability functions. - class GTSAM_EXPORT SymbolicBayesTree : + class SymbolicBayesTree : public BayesTree { private: @@ -59,7 +59,7 @@ namespace gtsam { SymbolicBayesTree() {} /** check equality */ - bool equals(const This& other, double tol = 1e-9) const; + GTSAM_EXPORT bool equals(const This& other, double tol = 1e-9) const; private: /** Serialization function */ diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index fdc28c5c84..33fc3243bb 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -144,7 +144,7 @@ TEST( BayesTree, removePath ) SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removePath(bayesTree[_C_], bn, orphans); + bayesTree.removePath(bayesTree[_C_], &bn, &orphans); SymbolicFactorGraph factors(bn); CHECK(assert_equal(expected, factors)); CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); @@ -161,7 +161,7 @@ TEST( BayesTree, removePath ) SymbolicBayesNet bn2; SymbolicBayesTree::Cliques orphans2; - bayesTree.removePath(bayesTree[_E_], bn2, orphans2); + bayesTree.removePath(bayesTree[_E_], &bn2, &orphans2); SymbolicFactorGraph factors2(bn2); CHECK(assert_equal(expected2, factors2)); CHECK(assert_container_equal(expectedOrphans2|indirected, orphans2|indirected)); @@ -175,7 +175,7 @@ TEST( BayesTree, removePath2 ) // Call remove-path with clique B SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removePath(bayesTree[_B_], bn, orphans); + bayesTree.removePath(bayesTree[_B_], &bn, &orphans); SymbolicFactorGraph factors(bn); // Check expected outcome @@ -195,7 +195,7 @@ TEST(BayesTree, removePath3) // Call remove-path with clique T SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removePath(bayesTree[_T_], bn, orphans); + bayesTree.removePath(bayesTree[_T_], &bn, &orphans); SymbolicFactorGraph factors(bn); // Check expected outcome @@ -279,7 +279,7 @@ TEST( BayesTree, removeTop ) // Remove the contaminated part of the Bayes tree SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(_B_)(_S_), bn, orphans); + bayesTree.removeTop(list_of(_B_)(_S_), &bn, &orphans); // Check expected outcome SymbolicBayesNet expected; @@ -295,7 +295,7 @@ TEST( BayesTree, removeTop ) //boost::shared_ptr newFactor2(new IndexFactor(_B_)); SymbolicBayesNet bn2; SymbolicBayesTree::Cliques orphans2; - bayesTree.removeTop(list_of(_B_), bn2, orphans2); + bayesTree.removeTop(list_of(_B_), &bn2, &orphans2); SymbolicFactorGraph factors2(bn2); SymbolicFactorGraph expected2; CHECK(assert_equal(expected2, factors2)); @@ -316,7 +316,7 @@ TEST( BayesTree, removeTop2 ) // Remove the contaminated part of the Bayes tree SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(_T_), bn, orphans); + bayesTree.removeTop(list_of(_T_), &bn, &orphans); // Check expected outcome SymbolicBayesNet expected = list_of @@ -343,7 +343,7 @@ TEST( BayesTree, removeTop3 ) // remove all SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(L(5))(X(4))(X(2))(X(3)), bn, orphans); + bayesTree.removeTop(list_of(L(5))(X(4))(X(2))(X(3)), &bn, &orphans); SymbolicBayesNet expectedBn = list_of (SymbolicConditional::FromKeys(list_of(X(4))(L(5)), 2)) @@ -367,7 +367,7 @@ TEST( BayesTree, removeTop4 ) // remove all SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(X(2))(L(5))(X(4))(X(3)), bn, orphans); + bayesTree.removeTop(list_of(X(2))(L(5))(X(4))(X(3)), &bn, &orphans); SymbolicBayesNet expectedBn = list_of (SymbolicConditional::FromKeys(list_of(X(4))(L(5)), 2)) @@ -392,7 +392,7 @@ TEST( BayesTree, removeTop5 ) // Remove nonexistant SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(X(10)), bn, orphans); + bayesTree.removeTop(list_of(X(10)), &bn, &orphans); SymbolicBayesNet expectedBn; EXPECT(assert_equal(expectedBn, bn)); diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index 3fd318456d..8f4eb3c08d 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -15,14 +15,16 @@ * @author Christian Potthast **/ -#include - #include + +#include #include #include #include #include +#include + #include using namespace std; @@ -46,11 +48,10 @@ TEST(SymbolicFactorGraph, keys2) { } /* ************************************************************************* */ -TEST(SymbolicFactorGraph, eliminateFullSequential) -{ +TEST(SymbolicFactorGraph, eliminateFullSequential) { // Test with simpleTestGraph1 Ordering order; - order += 0,1,2,3,4; + order += 0, 1, 2, 3, 4; SymbolicBayesNet actual1 = *simpleTestGraph1.eliminateSequential(order); EXPECT(assert_equal(simpleTestGraph1BayesNet, actual1)); @@ -60,24 +61,20 @@ TEST(SymbolicFactorGraph, eliminateFullSequential) } /* ************************************************************************* */ -TEST(SymbolicFactorGraph, eliminatePartialSequential) -{ +TEST(SymbolicFactorGraph, eliminatePartialSequential) { // Eliminate 0 and 1 const Ordering order = list_of(0)(1); - const SymbolicBayesNet expectedBayesNet = list_of - (SymbolicConditional(0,1,2)) - (SymbolicConditional(1,2,3,4)); + const SymbolicBayesNet expectedBayesNet = + list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3, 4)); - const SymbolicFactorGraph expectedSfg = list_of - (SymbolicFactor(2,3)) - (SymbolicFactor(4,5)) - (SymbolicFactor(2,3,4)); + const SymbolicFactorGraph expectedSfg = list_of(SymbolicFactor(2, 3))( + SymbolicFactor(4, 5))(SymbolicFactor(2, 3, 4)); SymbolicBayesNet::shared_ptr actualBayesNet; SymbolicFactorGraph::shared_ptr actualSfg; boost::tie(actualBayesNet, actualSfg) = - simpleTestGraph2.eliminatePartialSequential(Ordering(list_of(0)(1))); + simpleTestGraph2.eliminatePartialSequential(Ordering(list_of(0)(1))); EXPECT(assert_equal(expectedSfg, *actualSfg)); EXPECT(assert_equal(expectedBayesNet, *actualBayesNet)); @@ -85,75 +82,71 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential) SymbolicBayesNet::shared_ptr actualBayesNet2; SymbolicFactorGraph::shared_ptr actualSfg2; boost::tie(actualBayesNet2, actualSfg2) = - simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container()); + simpleTestGraph2.eliminatePartialSequential( + list_of(0)(1).convert_to_container()); EXPECT(assert_equal(expectedSfg, *actualSfg2)); EXPECT(assert_equal(expectedBayesNet, *actualBayesNet2)); } /* ************************************************************************* */ -TEST(SymbolicFactorGraph, eliminateFullMultifrontal) -{ - Ordering ordering; ordering += 0,1,2,3; - SymbolicBayesTree actual1 = - *simpleChain.eliminateMultifrontal(ordering); +TEST(SymbolicFactorGraph, eliminateFullMultifrontal) { + Ordering ordering; + ordering += 0, 1, 2, 3; + SymbolicBayesTree actual1 = *simpleChain.eliminateMultifrontal(ordering); EXPECT(assert_equal(simpleChainBayesTree, actual1)); - SymbolicBayesTree actual2 = - *asiaGraph.eliminateMultifrontal(asiaOrdering); + SymbolicBayesTree actual2 = *asiaGraph.eliminateMultifrontal(asiaOrdering); EXPECT(assert_equal(asiaBayesTree, actual2)); } /* ************************************************************************* */ -TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) -{ +TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) { SymbolicBayesTree expectedBayesTree; - SymbolicConditional::shared_ptr root = boost::make_shared( - SymbolicConditional::FromKeys(list_of(4)(5)(1), 2)); - expectedBayesTree.insertRoot(boost::make_shared(root)); + SymbolicConditional::shared_ptr root = + boost::make_shared( + SymbolicConditional::FromKeys(list_of(4)(5)(1), 2)); + expectedBayesTree.insertRoot( + boost::make_shared(root)); - SymbolicFactorGraph expectedFactorGraph = list_of - (SymbolicFactor(0,1)) - (SymbolicFactor(0,2)) - (SymbolicFactor(1,3)) - (SymbolicFactor(2,3)) - (SymbolicFactor(1)); + SymbolicFactorGraph expectedFactorGraph = + list_of(SymbolicFactor(0, 1))(SymbolicFactor(0, 2))(SymbolicFactor(1, 3))( + SymbolicFactor(2, 3))(SymbolicFactor(1)); SymbolicBayesTree::shared_ptr actualBayesTree; SymbolicFactorGraph::shared_ptr actualFactorGraph; boost::tie(actualBayesTree, actualFactorGraph) = - simpleTestGraph2.eliminatePartialMultifrontal(Ordering(list_of(4)(5))); + simpleTestGraph2.eliminatePartialMultifrontal(Ordering(list_of(4)(5))); EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph)); EXPECT(assert_equal(expectedBayesTree, *actualBayesTree)); SymbolicBayesTree expectedBayesTree2; - SymbolicBayesTreeClique::shared_ptr root2 = boost::make_shared( - boost::make_shared(4,1)); + SymbolicBayesTreeClique::shared_ptr root2 = + boost::make_shared( + boost::make_shared(4, 1)); root2->children.push_back(boost::make_shared( - boost::make_shared(5,4))); + boost::make_shared(5, 4))); expectedBayesTree2.insertRoot(root2); SymbolicBayesTree::shared_ptr actualBayesTree2; SymbolicFactorGraph::shared_ptr actualFactorGraph2; boost::tie(actualBayesTree2, actualFactorGraph2) = - simpleTestGraph2.eliminatePartialMultifrontal(list_of(4)(5).convert_to_container()); + simpleTestGraph2.eliminatePartialMultifrontal( + list_of(4)(5).convert_to_container()); EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2)); EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2)); } /* ************************************************************************* */ -TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) -{ - SymbolicBayesNet expectedBayesNet = list_of - (SymbolicConditional(0, 1, 2)) - (SymbolicConditional(1, 2, 3)) - (SymbolicConditional(2, 3)) - (SymbolicConditional(3)); +TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) { + SymbolicBayesNet expectedBayesNet = + list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3))( + SymbolicConditional(2, 3))(SymbolicConditional(3)); SymbolicBayesNet actual1 = *simpleTestGraph2.marginalMultifrontalBayesNet( - Ordering(list_of(0)(1)(2)(3))); + Ordering(list_of(0)(1)(2)(3))); EXPECT(assert_equal(expectedBayesNet, actual1)); } @@ -167,104 +160,75 @@ TEST(SymbolicFactorGraph, eliminate_disconnected_graph) { // create expected Chordal bayes Net SymbolicBayesNet expected; - expected.push_back(boost::make_shared(0,1,2)); - expected.push_back(boost::make_shared(1,2)); + expected.push_back(boost::make_shared(0, 1, 2)); + expected.push_back(boost::make_shared(1, 2)); expected.push_back(boost::make_shared(2)); - expected.push_back(boost::make_shared(3,4)); + expected.push_back(boost::make_shared(3, 4)); expected.push_back(boost::make_shared(4)); Ordering order; - order += 0,1,2,3,4; + order += 0, 1, 2, 3, 4; SymbolicBayesNet actual = *fg.eliminateSequential(order); - EXPECT(assert_equal(expected,actual)); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ -//TEST(SymbolicFactorGraph, marginals) -//{ -// // Create factor graph -// SymbolicFactorGraph fg; -// fg.push_factor(0, 1); -// fg.push_factor(0, 2); -// fg.push_factor(1, 4); -// fg.push_factor(2, 4); -// fg.push_factor(3, 4); -// -// // eliminate -// SymbolicSequentialSolver solver(fg); -// SymbolicBayesNet::shared_ptr actual = solver.eliminate(); -// SymbolicBayesNet expected; -// expected.push_front(boost::make_shared(4)); -// expected.push_front(boost::make_shared(3, 4)); -// expected.push_front(boost::make_shared(2, 4)); -// expected.push_front(boost::make_shared(1, 2, 4)); -// expected.push_front(boost::make_shared(0, 1, 2)); -// EXPECT(assert_equal(expected,*actual)); -// -// { -// // jointBayesNet -// vector js; -// js.push_back(0); -// js.push_back(4); -// js.push_back(3); -// SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); -// SymbolicBayesNet expectedBN; -// expectedBN.push_front(boost::make_shared(3)); -// expectedBN.push_front(boost::make_shared(4, 3)); -// expectedBN.push_front(boost::make_shared(0, 4)); -// EXPECT( assert_equal(expectedBN,*actualBN)); -// -// // jointFactorGraph -// SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); -// SymbolicFactorGraph expectedFG; -// expectedFG.push_factor(0, 4); -// expectedFG.push_factor(4, 3); -// expectedFG.push_factor(3); -// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); -// } -// -// { -// // jointBayesNet -// vector js; -// js.push_back(0); -// js.push_back(2); -// js.push_back(3); -// SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); -// SymbolicBayesNet expectedBN; -// expectedBN.push_front(boost::make_shared(2)); -// expectedBN.push_front(boost::make_shared(3, 2)); -// expectedBN.push_front(boost::make_shared(0, 3, 2)); -// EXPECT( assert_equal(expectedBN,*actualBN)); -// -// // jointFactorGraph -// SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); -// SymbolicFactorGraph expectedFG; -// expectedFG.push_factor(0, 3, 2); -// expectedFG.push_factor(3, 2); -// expectedFG.push_factor(2); -// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); -// } -// -// { -// // conditionalBayesNet -// vector js; -// js.push_back(0); -// js.push_back(2); -// js.push_back(3); -// size_t nrFrontals = 2; -// SymbolicBayesNet::shared_ptr actualBN = // -// solver.conditionalBayesNet(js, nrFrontals); -// SymbolicBayesNet expectedBN; -// expectedBN.push_front(boost::make_shared(2, 3)); -// expectedBN.push_front(boost::make_shared(0, 2, 3)); -// EXPECT( assert_equal(expectedBN,*actualBN)); -// } -//} +TEST(SymbolicFactorGraph, marginals) { + // Create factor graph + SymbolicFactorGraph fg; + fg.push_factor(0, 1); + fg.push_factor(0, 2); + fg.push_factor(1, 4); + fg.push_factor(2, 4); + fg.push_factor(3, 4); + + // eliminate + Ordering ord(list_of(3)(4)(2)(1)(0)); + auto actual = fg.eliminateSequential(ord); + SymbolicBayesNet expected; + expected.emplace_shared(3, 4); + expected.emplace_shared(4, 1, 2); + expected.emplace_shared(2, 0, 1); + expected.emplace_shared(1, 0); + expected.emplace_shared(0); + EXPECT(assert_equal(expected, *actual)); + + { + // jointBayesNet + Ordering ord(list_of(0)(4)(3)); + auto actual = fg.eliminatePartialSequential(ord); + SymbolicBayesNet expectedBN; + expectedBN.emplace_shared(0, 1, 2); + expectedBN.emplace_shared(4, 1, 2, 3); + expectedBN.emplace_shared(3, 1, 2); + EXPECT(assert_equal(expectedBN, *(actual.first))); + } + + { + // jointBayesNet + Ordering ord(list_of(0)(2)(3)); + auto actual = fg.eliminatePartialSequential(ord); + SymbolicBayesNet expectedBN; + expectedBN.emplace_shared(0, 1, 2); + expectedBN.emplace_shared(2, 1, 4); + expectedBN.emplace_shared(3, 4); + EXPECT(assert_equal(expectedBN, *(actual.first))); + } + + { + // conditionalBayesNet + Ordering ord(list_of(0)(2)); + auto actual = fg.eliminatePartialSequential(ord); + SymbolicBayesNet expectedBN; + expectedBN.emplace_shared(0, 1, 2); + expectedBN.emplace_shared(2, 1, 4); + EXPECT(assert_equal(expectedBN, *(actual.first))); + } +} /* ************************************************************************* */ -TEST( SymbolicFactorGraph, constructFromBayesNet ) -{ +TEST(SymbolicFactorGraph, constructFromBayesNet) { // create expected factor graph SymbolicFactorGraph expected; expected.push_factor(0, 1, 2); @@ -284,8 +248,7 @@ TEST( SymbolicFactorGraph, constructFromBayesNet ) } /* ************************************************************************* */ -TEST( SymbolicFactorGraph, constructFromBayesTree ) -{ +TEST(SymbolicFactorGraph, constructFromBayesTree) { // create expected factor graph SymbolicFactorGraph expected; expected.push_factor(_E_, _L_, _B_); @@ -300,8 +263,7 @@ TEST( SymbolicFactorGraph, constructFromBayesTree ) } /* ************************************************************************* */ -TEST( SymbolicFactorGraph, push_back ) -{ +TEST(SymbolicFactorGraph, push_back) { // Create two factor graphs and expected combined graph SymbolicFactorGraph fg1, fg2, expected; @@ -321,8 +283,47 @@ TEST( SymbolicFactorGraph, push_back ) actual.push_back(fg1); actual.push_back(fg2); CHECK(assert_equal(expected, actual)); + + // combine in second way + SymbolicFactorGraph actual2 = fg1; + actual2.push_back(fg2); + CHECK(assert_equal(expected, actual2)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +TEST(SymbolicFactorGraph, add_factors) { + SymbolicFactorGraph fg1; + fg1.push_factor(10); + fg1 += SymbolicFactor::shared_ptr(); // empty slot! + fg1.push_factor(11); + + SymbolicFactorGraph fg2; + fg2.push_factor(1); + fg2.push_factor(2); + + SymbolicFactorGraph expected; + expected.push_factor(10); + expected.push_factor(1); + expected.push_factor(11); + expected.push_factor(2); + const FactorIndices expectedIndices = list_of(1)(3); + const FactorIndices actualIndices = fg1.add_factors(fg2, true); + + EXPECT(assert_equal(expected, fg1)); + EXPECT(assert_container_equality(expectedIndices, actualIndices)); + + expected.push_factor(1); + expected.push_factor(2); + const FactorIndices expectedIndices2 = list_of(4)(5); + const FactorIndices actualIndices2 = fg1.add_factors(fg2, false); + + EXPECT(assert_equal(expected, fg1)); + EXPECT(assert_container_equality(expectedIndices2, actualIndices2)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 3273262019..53ba83fade 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -1,3 +1,5 @@ +project(gtsam_unstable LANGUAGES CXX) + # Build full gtsam_unstable library as a single library # and also build tests set (gtsam_unstable_subdirs @@ -42,6 +44,12 @@ foreach(subdir ${gtsam_unstable_subdirs}) add_subdirectory(${subdir}) endforeach(subdir) +# dllexport.h +set(library_name GTSAM_UNSTABLE) # For substitution in dllexport.h.in +configure_file("${GTSAM_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") +list(APPEND gtsam_unstable_srcs "${PROJECT_BINARY_DIR}/dllexport.h") +install(FILES "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION include/gtsam_unstable) + # assemble gtsam_unstable components set(gtsam_unstable_srcs ${base_srcs} @@ -65,6 +73,10 @@ message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") # BUILD_SHARED_LIBS automatically defines static/shared libs: add_library(gtsam_unstable ${gtsam_unstable_srcs}) + +# Apply build flags: +gtsam_apply_build_flags(gtsam_unstable) + set_target_properties(gtsam_unstable PROPERTIES OUTPUT_NAME gtsam_unstable CLEAN_DIRECT_OUTPUT 1 diff --git a/gtsam_unstable/base/Dummy.h b/gtsam_unstable/base/Dummy.h index 14b4b48549..a2f544de58 100644 --- a/gtsam_unstable/base/Dummy.h +++ b/gtsam_unstable/base/Dummy.h @@ -18,7 +18,7 @@ */ #include -#include +#include #include namespace gtsam { diff --git a/gtsam_unstable/base/dllexport.h b/gtsam_unstable/base/dllexport.h deleted file mode 100644 index 903996db4a..0000000000 --- a/gtsam_unstable/base/dllexport.h +++ /dev/null @@ -1,36 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file dllexport.h - * @brief Symbols for exporting classes and methods from DLLs - * @author Richard Roberts - * @date Mar 9, 2013 - */ - -#ifdef _WIN32 -# ifdef GTSAM_UNSTABLE_EXPORTS -# define GTSAM_UNSTABLE_EXPORT __declspec(dllexport) -# define GTSAM_UNSTABLE_EXTERN_EXPORT __declspec(dllexport) extern -# else -# ifndef GTSAM_UNSTABLE_IMPORT_STATIC -# define GTSAM_UNSTABLE_EXPORT __declspec(dllimport) -# define GTSAM_UNSTABLE_EXTERN_EXPORT __declspec(dllimport) -# else /* GTSAM_UNSTABLE_IMPORT_STATIC */ -# define GTSAM_UNSTABLE_EXPORT -# define GTSAM_UNSTABLE_EXTERN_EXPORT extern -# endif /* GTSAM_UNSTABLE_IMPORT_STATIC */ -# endif /* GTSAM_UNSTABLE_EXPORTS */ -#else /* _WIN32 */ -# define GTSAM_UNSTABLE_EXPORT -# define GTSAM_UNSTABLE_EXTERN_EXPORT extern -#endif - diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index cf5abdcb11..0223250b54 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -43,7 +43,7 @@ namespace gtsam { // keep track of which domains changed changed[v] = false; // loop over all factors/constraints for variable v - const VariableIndex::Factors& factors = index[v]; + const FactorIndices& factors = index[v]; for(size_t f: factors) { // if not already a singleton if (!domains[v].isSingleton()) { diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index 32fb6f1ce3..136704c2de 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -17,7 +17,7 @@ #pragma once -#include +#include #include #include diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 1aa85b6fe8..1c5ade5b6c 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -61,7 +61,7 @@ class FullIMUFactor : public NoiseModelFactor2 { return f && Base::equals(e) && equal_with_abs_tol(accel_, f->accel_, tol) && equal_with_abs_tol(gyro_, f->gyro_, tol) && - fabs(dt_ - f->dt_) < tol; + std::abs(dt_ - f->dt_) < tol; } void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 5ed079acb2..bb0a354eee 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -54,7 +54,7 @@ class IMUFactor : public NoiseModelFactor2 { return f && Base::equals(e) && equal_with_abs_tol(accel_, f->accel_, tol) && equal_with_abs_tol(gyro_, f->gyro_, tol) && - fabs(dt_ - f->dt_) < tol; + std::abs(dt_ - f->dt_) < tol; } void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index 9ec10e39ff..209456a621 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -37,7 +37,7 @@ class PendulumFactor1: public NoiseModelFactor3 { ///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0) - : Base(noiseModel::Constrained::All(1, fabs(mu)), k1, k, velKey), h_(h) {} + : Base(noiseModel::Constrained::All(1, std::abs(mu)), k1, k, velKey), h_(h) {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -85,7 +85,7 @@ class PendulumFactor2: public NoiseModelFactor3 { ///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0) - : Base(noiseModel::Constrained::All(1, fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {} + : Base(noiseModel::Constrained::All(1, std::abs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -135,7 +135,7 @@ class PendulumFactorPk: public NoiseModelFactor3 { ///Constructor PendulumFactorPk(Key pKey, Key qKey, Key qKey1, double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0) - : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey, qKey, qKey1), + : Base(noiseModel::Constrained::All(1, std::abs(mu)), pKey, qKey, qKey1), h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} /// @return a deep copy of this factor @@ -191,7 +191,7 @@ class PendulumFactorPk1: public NoiseModelFactor3 { ///Constructor PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1, double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0) - : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey1, qKey, qKey1), + : Base(noiseModel::Constrained::All(1, std::abs(mu)), pKey1, qKey, qKey1), h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} /// @return a deep copy of this factor diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index c8c46ee7ba..cf21c315b9 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -99,7 +99,7 @@ PoseRTV PoseRTV::flyingDynamics( double yaw2 = r2.yaw(); double pitch2 = r2.pitch(); double forward_accel = -thrust * sin(pitch2); // r2, pitch (in global frame?) controls forward force - double loss_lift = lift*fabs(sin(pitch2)); + double loss_lift = lift*std::abs(sin(pitch2)); Rot3 yaw_correction_bn = Rot3::Yaw(yaw2); Point3 forward(forward_accel, 0.0, 0.0); Vector Acc_n = diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index 0c00e5d951..b1cfb6f303 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -6,7 +6,7 @@ #pragma once -#include +#include #include #include diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index f98879e410..721d0265b2 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -27,7 +27,7 @@ class VelocityConstraint3 : public NoiseModelFactor3 { ///TODO: comment VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0) - : Base(noiseModel::Constrained::All(1, fabs(mu)), key1, key2, velKey), dt_(dt) {} + : Base(noiseModel::Constrained::All(1, std::abs(mu)), key1, key2, velKey), dt_(dt) {} virtual ~VelocityConstraint3() {} /// @return a deep copy of this factor diff --git a/gtsam_unstable/examples/README b/gtsam_unstable/examples/README.md similarity index 100% rename from gtsam_unstable/examples/README rename to gtsam_unstable/examples/README.md diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 0ee601d269..9dbeeac89b 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartRangeExample_plaza2.cpp + * @file SmartRangeExample_plaza1.cpp * @brief A 2D Range SLAM example * @date June 20, 2013 * @author FRank Dellaert @@ -42,6 +42,9 @@ #include #include +// To find data files, we can use `findExampleDataFile`, declared here: +#include + // Standard headers, added last, so we know headers above work on their own #include #include @@ -59,10 +62,9 @@ namespace NM = gtsam::noiseModel; typedef pair TimedOdometry; list readOdometry() { list odometryList; - ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt"); - if (!is) - throw runtime_error( - "/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt file not found"); + string drFile = findExampleDataFile("Plaza1_DR.txt"); + ifstream is(drFile); + if (!is) throw runtime_error("Plaza1_DR.txt file not found"); while (is) { double t, distance_traveled, delta_heading; @@ -79,10 +81,9 @@ list readOdometry() { typedef boost::tuple RangeTriple; vector readTriples() { vector triples; - ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt"); - if (!is) - throw runtime_error( - "/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt file not found"); + string tdFile = findExampleDataFile("Plaza1_TD.txt"); + ifstream is(tdFile); + if (!is) throw runtime_error("Plaza1_TD.txt file not found"); while (is) { double t, sender, receiver, range; @@ -98,8 +99,6 @@ int main(int argc, char** argv) { // load Plaza1 data list odometry = readOdometry(); -// size_t M = odometry.size(); - vector triples = readTriples(); size_t K = triples.size(); @@ -112,11 +111,11 @@ int main(int argc, char** argv) { // Set Noise parameters Vector priorSigmas = Vector3(1, 1, M_PI); - Vector odoSigmas = Vector3(0.05, 0.01, 0.2); + Vector odoSigmas = Vector3(0.05, 0.01, 0.1); + auto odoNoise = NM::Diagonal::Sigmas(odoSigmas); double sigmaR = 100; // range standard deviation const NM::Base::shared_ptr // all same type priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior - odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust rangeNoise = robust ? tukey : gaussian; @@ -130,10 +129,8 @@ int main(int argc, char** argv) { NonlinearFactorGraph newFactors; newFactors.push_back(PriorFactor(0, pose0, priorNoise)); - ofstream os2( - "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt"); - ofstream os3( - "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultSR.txt"); + ofstream os2("rangeResultLM.txt"); + ofstream os3("rangeResultSR.txt"); // initialize points (Gaussian) Values initial; @@ -170,11 +167,11 @@ int main(int argc, char** argv) { double t; Pose2 odometry; boost::tie(t, odometry) = timedOdometry; + printf("step %d, time = %g\n",(int)i,t); // add odometry factor newFactors.push_back( - BetweenFactor(i - 1, i, odometry, - NM::Diagonal::Sigmas(odoSigmas))); + BetweenFactor(i - 1, i, odometry, odoNoise)); // predict pose and add as initial estimate Pose2 predictedPose = lastPose.compose(odometry); @@ -188,15 +185,20 @@ int main(int argc, char** argv) { double range = boost::get<2>(triples[k]); if (i > start) { if (smart && totalCount < minK) { - smartFactors[j]->addRange(i, range); - printf("adding range %g for %d on %d",range,(int)j,(int)i);cout << endl; + try { + smartFactors[j]->addRange(i, range); + printf("adding range %g for %d",range,(int)j); + } catch (const invalid_argument& e) { + printf("warning: omitting duplicate range %g for %d",range,(int)j); + } + cout << endl; } else { RangeFactor factor(i, symbol('L', j), range, rangeNoise); // Throw out obvious outliers based on current landmark estimates Vector error = factor.unwhitenedError(landmarkEstimates); - if (k <= 200 || fabs(error[0]) < 5) + if (k <= 200 || std::abs(error[0]) < 5) newFactors.push_back(factor); } totalCount += 1; @@ -254,8 +256,7 @@ int main(int argc, char** argv) { // Write result to file Values result = isam.calculateEstimate(); - ofstream os( - "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt"); + ofstream os("rangeResult.txt"); for(const Values::ConstFiltered::KeyValuePair& it: result.filter()) os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" << it.value.theta() << endl; diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index 5f33a215b4..a63a0ba20a 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -41,6 +41,9 @@ #include #include +// To find data files, we can use `findExampleDataFile`, declared here: +#include + // Standard headers, added last, so we know headers above work on their own #include #include @@ -58,10 +61,9 @@ namespace NM = gtsam::noiseModel; typedef pair TimedOdometry; list readOdometry() { list odometryList; - ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt"); - if (!is) - throw runtime_error( - "/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt file not found"); + string drFile = findExampleDataFile("Plaza2_DR.txt"); + ifstream is(drFile); + if (!is) throw runtime_error("Plaza2_DR.txt file not found"); while (is) { double t, distance_traveled, delta_heading; @@ -78,10 +80,9 @@ list readOdometry() { typedef boost::tuple RangeTriple; vector readTriples() { vector triples; - ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt"); - if (!is) - throw runtime_error( - "/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt file not found"); + string tdFile = findExampleDataFile("Plaza2_TD.txt"); + ifstream is(tdFile); + if (!is) throw runtime_error("Plaza2_TD.txt file not found"); while (is) { double t, sender, receiver, range; @@ -166,7 +167,7 @@ int main(int argc, char** argv) { RangeFactor factor(i, symbol('L', j), range, rangeNoise); // Throw out obvious outliers based on current landmark estimates Vector error = factor.unwhitenedError(landmarkEstimates); - if (k <= 200 || fabs(error[0]) < 5) + if (k <= 200 || std::abs(error[0]) < 5) newFactors.push_back(factor); k = k + 1; countK = countK + 1; @@ -201,13 +202,11 @@ int main(int argc, char** argv) { // Write result to file Values result = isam.calculateEstimate(); - ofstream os2( - "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt"); + ofstream os2("rangeResultLM.txt"); for(const Values::ConstFiltered::KeyValuePair& it: result.filter()) os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" << endl; - ofstream os( - "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt"); + ofstream os("rangeResult.txt"); for(const Values::ConstFiltered::KeyValuePair& it: result.filter()) os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" << it.value.theta() << endl; diff --git a/gtsam_unstable/geometry/BearingS2.h b/gtsam_unstable/geometry/BearingS2.h index 70a22b9a5b..197d4910d0 100644 --- a/gtsam_unstable/geometry/BearingS2.h +++ b/gtsam_unstable/geometry/BearingS2.h @@ -9,7 +9,7 @@ #pragma once -#include +#include #include #include diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index d9bacd106c..fc186857fe 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { @@ -60,10 +61,10 @@ class Event { } /** print with optional string */ - void print(const std::string& s = "") const; + GTSAM_UNSTABLE_EXPORT void print(const std::string& s = "") const; /** equals with an tolerance */ - bool equals(const Event& other, double tol = 1e-9) const; + GTSAM_UNSTABLE_EXPORT bool equals(const Event& other, double tol = 1e-9) const; /// Updates a with tangent space delta inline Event retract(const Vector4& v) const { @@ -94,6 +95,6 @@ class Event { // Define GTSAM traits template<> -struct GTSAM_EXPORT traits : internal::Manifold {}; +struct traits : internal::Manifold {}; } //\ namespace gtsam diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index 4237170f01..2a2afa4769 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -44,7 +44,7 @@ void Pose3Upright::print(const std::string& s) const { /* ************************************************************************* */ bool Pose3Upright::equals(const Pose3Upright& x, double tol) const { - return T_.equals(x.T_, tol) && fabs(z_ - x.z_) < tol; + return T_.equals(x.T_, tol) && std::abs(z_ - x.z_) < tol; } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index 9d01e20a59..d833c9cf46 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -11,7 +11,7 @@ #pragma once -#include +#include #include #include diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index 00bd28ec7d..eb732f2c57 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -77,7 +77,7 @@ bool SimPolygon2D::contains(const Point2& c) const { Point2 dab = ab.b() - ab.a(); Point2 dac = c - ab.a(); double cross = dab.x() * dac.y() - dab.y() * dac.x(); - if (fabs(cross) < 1e-6) // check for on one of the edges + if (std::abs(cross) < 1e-6) // check for on one of the edges return true; bool side = cross > 0; // save the first side found @@ -241,14 +241,14 @@ double SimPolygon2D::randomDistance(double mu, double sigma, double min_dist) { boost::variate_generator > gen_d(rng, norm_dist); double d = -10.0; for (size_t i=0; i min_dist) return d; } cout << "Non viable distance: " << d << " with mu = " << mu << " sigma = " << sigma << " min_dist = " << min_dist << endl; throw runtime_error("Failed to find a viable distance"); - return fabs(norm_dist(rng)); + return std::abs(norm_dist(rng)); } /* ***************************************************************** */ @@ -313,7 +313,7 @@ Pose2 SimPolygon2D::randomFreePose(double boundary_size, const vector pt) cons } // handle vertical case to avoid calculating slope - if (fabs(Ba.x() - Bb.x()) < 1e-5) { + if (std::abs(Ba.x() - Bb.x()) < 1e-5) { if (debug) cout << "vertical line" << endl; if (Ba.x() < len && Ba.x() > 0.0) { if (pt) *pt = transform.transformFrom(Point2(Ba.x(), 0.0)); @@ -88,7 +88,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons // find x-intercept double slope = (high.y() - low.y())/(high.x() - low.x()); if (debug) cout << "slope " << slope << endl; - double xint = (low.x() < high.x()) ? low.x() + fabs(low.y())/slope : high.x() - fabs(high.y())/slope; + double xint = (low.x() < high.x()) ? low.x() + std::abs(low.y())/slope : high.x() - std::abs(high.y())/slope; if (debug) cout << "xintercept " << xint << endl; if (xint > 0.0 && xint < len) { if (pt) *pt = transform.transformFrom(Point2(xint, 0.0)); diff --git a/gtsam_unstable/geometry/SimWall2D.h b/gtsam_unstable/geometry/SimWall2D.h index c143bc36d0..fd5afbc54b 100644 --- a/gtsam_unstable/geometry/SimWall2D.h +++ b/gtsam_unstable/geometry/SimWall2D.h @@ -6,7 +6,7 @@ #pragma once -#include +#include #include #include diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index f7ba53d874..bf4937ed44 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -21,6 +21,7 @@ #include #include #include +#include namespace gtsam { @@ -49,59 +50,59 @@ class Similarity3: public LieGroup { /// @{ /// Default constructor - Similarity3(); + GTSAM_UNSTABLE_EXPORT Similarity3(); /// Construct pure scaling - Similarity3(double s); + GTSAM_UNSTABLE_EXPORT Similarity3(double s); /// Construct from GTSAM types - Similarity3(const Rot3& R, const Point3& t, double s); + GTSAM_UNSTABLE_EXPORT Similarity3(const Rot3& R, const Point3& t, double s); /// Construct from Eigen types - Similarity3(const Matrix3& R, const Vector3& t, double s); + GTSAM_UNSTABLE_EXPORT Similarity3(const Matrix3& R, const Vector3& t, double s); /// Construct from matrix [R t; 0 s^-1] - Similarity3(const Matrix4& T); + GTSAM_UNSTABLE_EXPORT Similarity3(const Matrix4& T); /// @} /// @name Testable /// @{ /// Compare with tolerance - bool equals(const Similarity3& sim, double tol) const; + GTSAM_UNSTABLE_EXPORT bool equals(const Similarity3& sim, double tol) const; /// Exact equality - bool operator==(const Similarity3& other) const; + GTSAM_UNSTABLE_EXPORT bool operator==(const Similarity3& other) const; /// Print with optional string - void print(const std::string& s) const; + GTSAM_UNSTABLE_EXPORT void print(const std::string& s) const; - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p); + GTSAM_UNSTABLE_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p); /// @} /// @name Group /// @{ /// Return an identity transform - static Similarity3 identity(); + GTSAM_UNSTABLE_EXPORT static Similarity3 identity(); /// Composition - Similarity3 operator*(const Similarity3& T) const; + GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& T) const; /// Return the inverse - Similarity3 inverse() const; + GTSAM_UNSTABLE_EXPORT Similarity3 inverse() const; /// @} /// @name Group action on Point3 /// @{ /// Action on a point p is s*(R*p+t) - Point3 transformFrom(const Point3& p, // + GTSAM_UNSTABLE_EXPORT Point3 transformFrom(const Point3& p, // OptionalJacobian<3, 7> H1 = boost::none, // OptionalJacobian<3, 3> H2 = boost::none) const; /** syntactic sugar for transformFrom */ - Point3 operator*(const Point3& p) const; + GTSAM_UNSTABLE_EXPORT Point3 operator*(const Point3& p) const; /// @} /// @name Lie Group @@ -110,12 +111,12 @@ class Similarity3: public LieGroup { /** Log map at the identity * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$ */ - static Vector7 Logmap(const Similarity3& s, // + GTSAM_UNSTABLE_EXPORT static Vector7 Logmap(const Similarity3& s, // OptionalJacobian<7, 7> Hm = boost::none); /** Exponential map at the identity */ - static Similarity3 Expmap(const Vector7& v, // + GTSAM_UNSTABLE_EXPORT static Similarity3 Expmap(const Vector7& v, // OptionalJacobian<7, 7> Hm = boost::none); /// Chart at the origin @@ -136,17 +137,17 @@ class Similarity3: public LieGroup { * @return 4*4 element of Lie algebra that can be exponentiated * TODO(frank): rename to Hat, make part of traits */ - static Matrix4 wedge(const Vector7& xi); + GTSAM_UNSTABLE_EXPORT static Matrix4 wedge(const Vector7& xi); /// Project from one tangent space to another - Matrix7 AdjointMap() const; + GTSAM_UNSTABLE_EXPORT Matrix7 AdjointMap() const; /// @} /// @name Standard interface /// @{ /// Calculate 4*4 matrix group equivalent - const Matrix4 matrix() const; + GTSAM_UNSTABLE_EXPORT const Matrix4 matrix() const; /// Return a GTSAM rotation const Rot3& rotation() const { @@ -165,7 +166,7 @@ class Similarity3: public LieGroup { /// Convert to a rigid body pose (R, s*t) /// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast. - operator Pose3() const; + GTSAM_UNSTABLE_EXPORT operator Pose3() const; /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes inline static size_t Dim() { diff --git a/gtsam_unstable/linear/ActiveSetSolver-inl.h b/gtsam_unstable/linear/ActiveSetSolver-inl.h index 18dc07aec5..602012090a 100644 --- a/gtsam_unstable/linear/ActiveSetSolver-inl.h +++ b/gtsam_unstable/linear/ActiveSetSolver-inl.h @@ -252,7 +252,7 @@ Template InequalityFactorGraph This::identifyActiveConstraints( double error = workingFactor->error(initialValues); // Safety guard. This should not happen unless users provide a bad init if (error > 0) throw InfeasibleInitialValues(); - if (fabs(error) < 1e-7) + if (std::abs(error) < 1e-7) workingFactor->activate(); else workingFactor->inactivate(); diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h index 42f82f52f6..316db921ae 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h @@ -20,7 +20,7 @@ // \callgraph #pragma once -#include +#include #include #include #include diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp index 9c04f0eecc..34a23982f2 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp @@ -38,7 +38,7 @@ void FixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormat /* ************************************************************************* */ bool FixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { - return std::fabs(smootherLag_ - rhs.smootherLag_) < tol + return std::abs(smootherLag_ - rhs.smootherLag_) < tol && std::equal(timestampKeyMap_.begin(), timestampKeyMap_.end(), rhs.timestampKeyMap_.begin()); } diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index bef35ffcee..362cfae968 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -20,7 +20,7 @@ // \callgraph #pragma once -#include +#include #include #include #include diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index ece8cd2f6c..128889b829 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include #include #include diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index 2088ee6466..297057e3fe 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -175,7 +175,6 @@ namespace gtsam { namespace partition { void prepareMetisGraph(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) { - typedef int Weight; typedef std::vector Weights; typedef std::vector Neighbors; typedef std::pair NeighborsInfo; diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 0a3fa02839..ef03eb2c2d 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -152,7 +152,7 @@ bool AHRS::isAidingAvailable(const Mechanization_bRn2& mech, double mu_f = f_.norm() - ge; // accelerometer same magnitude as local gravity ? double mu_u = u_.norm(); // gyro says we are not maneuvering ? - return (fabs(mu_f)<0.5 && mu_u < 5.0 / 180.0 * M_PI); + return (std::abs(mu_f)<0.5 && mu_u < 5.0 / 180.0 * M_PI); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index e15e6e0f73..f22de48cfd 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -9,7 +9,7 @@ #define AHRS_H_ #include "Mechanization_bRn2.h" -#include +#include #include namespace gtsam { diff --git a/gtsam_unstable/slam/DummyFactor.h b/gtsam_unstable/slam/DummyFactor.h index 9f749e9e59..574efabea6 100644 --- a/gtsam_unstable/slam/DummyFactor.h +++ b/gtsam_unstable/slam/DummyFactor.h @@ -9,7 +9,7 @@ #pragma once -#include +#include #include namespace gtsam { diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index a228b23478..5a6f1df6d2 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -9,7 +9,7 @@ #include #include -#include +#include #include namespace gtsam { diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index fa06d47a3d..c71ee7abd7 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -121,7 +121,7 @@ namespace gtsam { // access const Vector& prior() const { return prior_; } - const std::vector& mask() const { return mask_; } + const std::vector& mask() const { return mask_; } const Matrix& H() const { return H_; } protected: diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp index db077994fe..32e8731cd8 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -38,7 +38,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p /* ************************************************************************* */ bool RelativeElevationFactor::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->measured_) < tol; + return e != NULL && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol; } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h index 3d81fbab3d..3507a44929 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.h +++ b/gtsam_unstable/slam/RelativeElevationFactor.h @@ -9,7 +9,7 @@ #pragma once -#include +#include #include #include diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index b3d71d05fd..5511a02091 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -9,11 +9,10 @@ #pragma once -#include +#include #include #include #include -#include #include #include @@ -60,6 +59,10 @@ class SmartRangeFactor: public NoiseModelFactor { /// Add a range measurement to a pose with given key. void addRange(Key key, double measuredRange) { + if(std::find(keys_.begin(), keys_.end(), key) != keys_.end()) { + throw std::invalid_argument( + "SmartRangeFactor::addRange: adding duplicate measurement for key."); + } keys_.push_back(key); measurements_.push_back(measuredRange); size_t n = keys_.size(); @@ -89,7 +92,6 @@ class SmartRangeFactor: public NoiseModelFactor { * Raise runtime_error if not well defined. */ Point2 triangulate(const Values& x) const { - // gttic_(triangulate); // create n circles corresponding to measured range around each pose std::list circles; size_t n = size(); @@ -100,7 +102,7 @@ class SmartRangeFactor: public NoiseModelFactor { Circle2 circle1 = circles.front(); boost::optional best_fh; - boost::optional bestCircle2; + auto bestCircle2 = boost::make_optional(false, circle1); // fixes issue #38 // loop over all circles for (const Circle2& it : circles) { @@ -136,7 +138,6 @@ class SmartRangeFactor: public NoiseModelFactor { } else { throw std::runtime_error("triangulate failed"); } - // gttoc_(triangulate); } /** diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index ead8071381..8a6aab6b73 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -46,7 +46,7 @@ TEST( SmartRangeFactor, constructor ) { TEST( SmartRangeFactor, addRange ) { SmartRangeFactor f(sigma); f.addRange(1, 10); - f.addRange(1, 12); + f.addRange(2, 12); LONGS_EQUAL(2, f.size()) } /* ************************************************************************* */ diff --git a/gtsampy.h b/gtsampy.h deleted file mode 100644 index cc5ccb1b4b..0000000000 --- a/gtsampy.h +++ /dev/null @@ -1,2658 +0,0 @@ -/** - - * GTSAM Wrap Module Definition - * - * These are the current classes available through the matlab toolbox interface, - * add more functions/classes as they are available. - * - * Requirements: - * Classes must start with an uppercase letter - * - Can wrap a typedef - * Only one Method/Constructor per line, though methods/constructors can extend across multiple lines - * Methods can return - * - Eigen types: Matrix, Vector - * - C/C++ basic types: string, bool, size_t, int, double, char, unsigned char - * - void - * - Any class with which be copied with boost::make_shared() - * - boost::shared_ptr of any object type - * Constructors - * - Overloads are supported - * - A class with no constructors can be returned from other functions but not allocated directly in MATLAB - * Methods - * - Constness has no effect - * - Specify by-value (not reference) return types, even if C++ method returns reference - * - Must start with a letter (upper or lowercase) - * - Overloads are supported - * Static methods - * - Must start with a letter (upper or lowercase) and use the "static" keyword - * - The first letter will be made uppercase in the generated MATLAB interface - * - Overloads are supported - * Arguments to functions any of - * - Eigen types: Matrix, Vector - * - Eigen types and classes as an optionally const reference - * - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char - * - Any class with which be copied with boost::make_shared() (except Eigen) - * - boost::shared_ptr of any object type (except Eigen) - * Comments can use either C++ or C style, with multiple lines - * Namespace definitions - * - Names of namespaces must start with a lowercase letter - * - start a namespace with "namespace {" - * - end a namespace with exactly "}" - * - Namespaces can be nested - * Namespace usage - * - Namespaces can be specified for classes in arguments and return values - * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" - * Includes in C++ wrappers - * - All includes will be collected and added in a single file - * - All namespaces must have angle brackets: - * - No default includes will be added - * Global/Namespace functions - * - Functions specified outside of a class are global - * - Can be overloaded with different arguments - * - Can have multiple functions of the same name in different namespaces - * Using classes defined in other modules - * - If you are using a class 'OtherClass' not wrapped in this definition file, add "class OtherClass;" to avoid a dependency error - * Virtual inheritance - * - Specify fully-qualified base classes, i.e. "virtual class Derived : ns::Base {" where "ns" is the namespace - * - Mark with 'virtual' keyword, e.g. "virtual class Base {", and also "virtual class Derived : ns::Base {" - * - Forward declarations must also be marked virtual, e.g. "virtual class ns::Base;" and - * also "virtual class ns::Derived;" - * - Pure virtual (abstract) classes should list no constructors in this interface file - * - Virtual classes must have a clone() function in C++ (though it does not have to be included - * in the MATLAB interface). clone() will be called whenever an object copy is needed, instead - * of using the copy constructor (which is used for non-virtual objects). - * - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree - * virtual boost::shared_ptr clone() const; - * Class Templates - * - Basic templates are supported either with an explicit list of types to instantiate, - * e.g. template class Class1 { ... }; - * or with typedefs, e.g. - * template class Class2 { ... }; - * typedef Class2 MyInstantiatedClass; - * - In the class definition, appearances of the template argument(s) will be replaced with their - * instantiated types, e.g. 'void setValue(const T& value);'. - * - To refer to the instantiation of the template class itself, use 'This', i.e. 'static This Create();' - * - To create new instantiations in other modules, you must copy-and-paste the whole class definition - * into the new module, but use only your new instantiation types. - * - When forward-declaring template instantiations, use the generated/typedefed name, e.g. - * class gtsam::Class1Pose2; - * class gtsam::MyInstantiatedClass; - * Boost.serialization within Matlab: - * - you need to mark classes as being serializable in the markup file (see this file for an example). - * - There are two options currently, depending on the class. To "mark" a class as serializable, - * add a function with a particular signature so that wrap will catch it. - * - Add "void serialize()" to a class to create serialization functions for a class. - * Adding this flag subsumes the serializable() flag below. Requirements: - * - A default constructor must be publicly accessible - * - Must not be an abstract base class - * - The class must have an actual boost.serialization serialize() function. - * - 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. - */ - -/** - * Status: - * - TODO: default values for arguments - * - WORKAROUND: make multiple versions of the same function for different configurations of default arguments - * - TODO: Handle gtsam::Rot3M conversions to quaternions - * - TODO: Parse return of const ref arguments - * - TODO: Parse std::string variants and convert directly to special string - * - TODO: Add enum support - * - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load - */ - -namespace std { - #include - template - class vector - { - //Do we need these? - //Capacity - /*size_t size() const; - size_t max_size() const; - //void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - //Element access - T* at(size_t n); - T* front(); - T* back(); - - //Modifiers - void assign(size_t n, const T& u); - void push_back(const T& x); - void pop_back();*/ - }; - //typedef std::vector - - #include - template - class list - { - - - }; - -} - -namespace gtsam { - -//************************************************************************* -// base -//************************************************************************* - -/** gtsam namespace functions */ -bool linear_independent(Matrix A, Matrix B, double tol); - -virtual class Value { - // No constructors because this is an abstract class - - // Testable - void print(string s) const; - - // Manifold - size_t dim() const; -}; - -#include -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 -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 -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 -//************************************************************************* - -class Point2 { - // Standard Constructors - Point2(); - Point2(double x, double y); - Point2(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Point2& pose, double tol) const; - - // Group - static gtsam::Point2 identity(); - gtsam::Point2 inverse() const; - gtsam::Point2 compose(const gtsam::Point2& p2) const; - gtsam::Point2 between(const gtsam::Point2& p2) const; - - // Manifold - gtsam::Point2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Point2& p) const; - - // Lie Group - static gtsam::Point2 Expmap(Vector v); - static Vector Logmap(const gtsam::Point2& p); - - // Standard Interface - double x() const; - double y() const; - Vector vector() const; - double dist(const gtsam::Point2& p2) const; - double norm() const; - - // enabling serialization functionality - void serialize() const; -}; - -// std::vector -class Point2Vector -{ - // Constructors - Point2Vector(); - Point2Vector(const gtsam::Point2Vector& v); - - //Capacity - size_t size() const; - size_t max_size() const; - void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - //Element access - gtsam::Point2 at(size_t n) const; - gtsam::Point2 front() const; - gtsam::Point2 back() const; - - //Modifiers - void assign(size_t n, const gtsam::Point2& u); - void push_back(const gtsam::Point2& x); - void pop_back(); -}; - -class StereoPoint2 { - // Standard Constructors - StereoPoint2(); - StereoPoint2(double uL, double uR, double v); - - // Testable - void print(string s) const; - bool equals(const gtsam::StereoPoint2& point, double tol) const; - - // Group - static gtsam::StereoPoint2 identity(); - gtsam::StereoPoint2 inverse() const; - gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; - gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; - - // Manifold - gtsam::StereoPoint2 retract(Vector v) const; - Vector localCoordinates(const gtsam::StereoPoint2& p) const; - - // Lie Group - static gtsam::StereoPoint2 Expmap(Vector v); - static Vector Logmap(const gtsam::StereoPoint2& p); - - // Standard Interface - Vector vector() const; - double uL() const; - double uR() const; - double v() const; - - // enabling serialization functionality - void serialize() const; -}; - -class Point3 { - // Standard Constructors - Point3(); - Point3(double x, double y, double z); - Point3(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Point3& p, double tol) const; - - // Group - static gtsam::Point3 identity(); - - // Standard Interface - Vector vector() const; - double x() const; - double y() const; - double z() const; - - // enabling serialization functionality - void serialize() const; -}; - -class Rot2 { - // Standard Constructors and Named Constructors - Rot2(); - Rot2(double theta); - static gtsam::Rot2 fromAngle(double theta); - static gtsam::Rot2 fromDegrees(double theta); - static gtsam::Rot2 fromCosSin(double c, double s); - - // Testable - void print(string s) const; - bool equals(const gtsam::Rot2& rot, double tol) const; - - // Group - static gtsam::Rot2 identity(); - gtsam::Rot2 inverse(); - gtsam::Rot2 compose(const gtsam::Rot2& p2) const; - gtsam::Rot2 between(const gtsam::Rot2& p2) const; - - // Manifold - gtsam::Rot2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot2& p) const; - - // Lie Group - static gtsam::Rot2 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot2& p); - - // Group Action on Point2 - gtsam::Point2 rotate(const gtsam::Point2& point) const; - gtsam::Point2 unrotate(const gtsam::Point2& point) const; - - // Standard Interface - static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative - static gtsam::Rot2 atan2(double y, double x); - double theta() const; - double degrees() const; - double c() const; - double s() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; -}; - -class Rot3 { - // Standard Constructors and Named Constructors - Rot3(); - Rot3(Matrix R); - static gtsam::Rot3 Rx(double t); - static gtsam::Rot3 Ry(double t); - static gtsam::Rot3 Rz(double t); - static gtsam::Rot3 RzRyRx(double x, double y, double z); - static gtsam::Rot3 RzRyRx(Vector xyz); - static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading) - static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) - static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft) - static gtsam::Rot3 ypr(double y, double p, double r); - static gtsam::Rot3 quaternion(double w, double x, double y, double z); - static gtsam::Rot3 Rodrigues(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Rot3& rot, double tol) const; - - // Group - static gtsam::Rot3 identity(); - gtsam::Rot3 inverse() const; - gtsam::Rot3 compose(const gtsam::Rot3& p2) const; - gtsam::Rot3 between(const gtsam::Rot3& p2) const; - - // Manifold - //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options - gtsam::Rot3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot3& p) const; - - // Group Action on Point3 - gtsam::Point3 rotate(const gtsam::Point3& p) const; - gtsam::Point3 unrotate(const gtsam::Point3& p) const; - - // Standard Interface - static gtsam::Rot3 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot3& p); - Matrix matrix() const; - Matrix transpose() const; - gtsam::Point3 column(size_t index) const; - Vector xyz() const; - Vector ypr() const; - Vector rpy() const; - double roll() const; - double pitch() const; - double yaw() const; -// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly - Vector quaternion() const; - - // enabling serialization functionality - void serialize() const; -}; - -class Pose2 { - // Standard Constructor - Pose2(); - Pose2(const gtsam::Pose2& pose); - Pose2(double x, double y, double theta); - Pose2(double theta, const gtsam::Point2& t); - Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); - Pose2(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Pose2& pose, double tol) const; - - // Group - static gtsam::Pose2 identity(); - gtsam::Pose2 inverse() const; - gtsam::Pose2 compose(const gtsam::Pose2& p2) const; - gtsam::Pose2 between(const gtsam::Pose2& p2) const; - - // Manifold - gtsam::Pose2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose2& p) const; - - // Lie Group - static gtsam::Pose2 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose2& p); - Matrix AdjointMap() const; - Vector Adjoint(const Vector& xi) const; - static Matrix wedge(double vx, double vy, double w); - - // Group Actions on Point2 - gtsam::Point2 transformFrom(const gtsam::Point2& p) const; - gtsam::Point2 transformTo(const gtsam::Point2& p) const; - - // Standard Interface - double x() const; - double y() const; - double theta() const; - gtsam::Rot2 bearing(const gtsam::Point2& point) const; - double range(const gtsam::Point2& point) const; - gtsam::Point2 translation() const; - gtsam::Rot2 rotation() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; -}; - -class Pose3 { - // Standard Constructors - Pose3(); - Pose3(const gtsam::Pose3& pose); - Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); - Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) - Pose3(Matrix t); - - // Testable - void print(string s) const; - bool equals(const gtsam::Pose3& pose, double tol) const; - - // Group - static gtsam::Pose3 identity(); - gtsam::Pose3 inverse() const; - gtsam::Pose3 compose(const gtsam::Pose3& p2) const; - gtsam::Pose3 between(const gtsam::Pose3& p2) const; - - // Manifold - gtsam::Pose3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose3& T2) const; - - // Lie Group - static gtsam::Pose3 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose3& p); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); - - // Group Action on Point3 - gtsam::Point3 transformFrom(const gtsam::Point3& p) const; - gtsam::Point3 transformTo(const gtsam::Point3& p) const; - - // Standard Interface - gtsam::Rot3 rotation() const; - gtsam::Point3 translation() const; - double x() const; - double y() const; - double z() const; - Matrix matrix() const; - gtsam::Pose3 transformTo(const gtsam::Pose3& pose) const; // FIXME: shadows other transformTo() - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); - - // enabling serialization functionality - void serialize() const; -}; - -// std::vector -class Pose3Vector -{ - Pose3Vector(); - size_t size() const; - bool empty() const; - gtsam::Pose3 at(size_t n) const; - void push_back(const gtsam::Pose3& x); -}; - -#include -class Unit3 { - // Standard Constructors - Unit3(); - Unit3(const gtsam::Point3& pose); - - // Testable - void print(string s) const; - bool equals(const gtsam::Unit3& pose, double tol) const; - - // Other functionality - Matrix basis() const; - Matrix skew() const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Unit3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Unit3& s) const; -}; - -#include -class EssentialMatrix { - // Standard Constructors - EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); - - // Testable - void print(string s) const; - bool equals(const gtsam::EssentialMatrix& pose, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::EssentialMatrix retract(Vector v) const; - Vector localCoordinates(const gtsam::EssentialMatrix& s) const; - - // Other methods: - gtsam::Rot3 rotation() const; - gtsam::Unit3 direction() const; - Matrix matrix() const; - double error(Vector vA, Vector vB); -}; - -class Cal3_S2 { - // Standard Constructors - Cal3_S2(); - Cal3_S2(double fx, double fy, double s, double u0, double v0); - Cal3_S2(Vector v); - Cal3_S2(double fov, int w, int h); - - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3_S2& rhs, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3_S2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3_S2& c) const; - - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - Vector vector() const; - Matrix matrix() const; - Matrix matrix_inverse() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class Cal3DS2_Base { - // Standard Constructors - Cal3DS2_Base(); - - // Testable - void print(string s) const; - - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - double k1() const; - double k2() const; - - // Action on Point2 - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class Cal3DS2 : gtsam::Cal3DS2_Base { - // Standard Constructors - Cal3DS2(); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); - Cal3DS2(Vector v); - - // Testable - bool equals(const gtsam::Cal3DS2& rhs, double tol) const; - - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3DS2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3DS2& c) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class Cal3Unified : gtsam::Cal3DS2_Base { - // Standard Constructors - Cal3Unified(); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); - Cal3Unified(Vector v); - - // Testable - bool equals(const gtsam::Cal3Unified& rhs, double tol) const; - - // Standard Interface - double xi() const; - gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; - gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; - - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3Unified retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3Unified& c) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Cal3_S2Stereo { - // Standard Constructors - Cal3_S2Stereo(); - Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); - Cal3_S2Stereo(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; - - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - double baseline() const; -}; - -#include -class Cal3Bundler { - // Standard Constructors - Cal3Bundler(); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0); - - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3Bundler retract(Vector v) const; - 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 - double fx() const; - double fy() const; - double k1() const; - double k2() const; - double u0() const; - double v0() const; - Vector vector() const; - Vector k() const; - //Matrix K() const; //FIXME: Uppercase - - // enabling serialization functionality - void serialize() const; -}; - -class CalibratedCamera { - // Standard Constructors and Named Constructors - CalibratedCamera(); - CalibratedCamera(const gtsam::Pose3& pose); - CalibratedCamera(const Vector& v); - static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); - - // Testable - void print(string s) const; - bool equals(const gtsam::CalibratedCamera& camera, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::CalibratedCamera retract(const Vector& d) const; - Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; - - // Action on Point3 - gtsam::Point2 project(const gtsam::Point3& point) const; - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - - // Standard Interface - gtsam::Pose3 pose() const; - double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods - - // enabling serialization functionality - void serialize() const; -}; - -template -class PinholeCamera { - // Standard Constructors and Named Constructors - PinholeCamera(); - PinholeCamera(const gtsam::Pose3& pose); - PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); - static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); - static This Level(const gtsam::Pose2& pose, double height); - static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, - const gtsam::Point3& upVector, const CALIBRATION& K); - - // Testable - void print(string s) const; - bool equals(const This& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - CALIBRATION calibration() const; - - // Manifold - This retract(const Vector& d) const; - Vector localCoordinates(const This& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& point); - - // enabling serialization functionality - void serialize() const; -}; - -virtual class SimpleCamera { - // Standard Constructors and Named Constructors - SimpleCamera(); - SimpleCamera(const gtsam::Pose3& pose); - SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); - static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, - const gtsam::Point3& upVector, const gtsam::Cal3_S2& K); - - // Testable - void print(string s) const; - bool equals(const gtsam::SimpleCamera& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - gtsam::Cal3_S2 calibration() const; - - // Manifold - gtsam::SimpleCamera retract(const Vector& d) const; - Vector localCoordinates(const gtsam::SimpleCamera& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& point); - - // enabling serialization functionality - void serialize() const; - -}; - -// Some typedefs for common camera types -// PinholeCameraCal3_S2 is the same as SimpleCamera above -typedef gtsam::PinholeCamera PinholeCameraCal3_S2; -typedef gtsam::PinholeCamera PinholeCameraCal3DS2; -typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; - -class StereoCamera { - // Standard Constructors and Named Constructors - StereoCamera(); - StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); - - // Testable - void print(string s) const; - bool equals(const gtsam::StereoCamera& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - double baseline() const; - gtsam::Cal3_S2Stereo calibration() const; - - // Manifold - gtsam::StereoCamera retract(const Vector& d) const; - Vector localCoordinates(const gtsam::StereoCamera& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - gtsam::StereoPoint2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include - -// Templates appear not yet supported for free functions -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); - -//************************************************************************* -// Symbolic -//************************************************************************* - -#include -virtual class SymbolicFactor { - // Standard Constructors and Named Constructors - SymbolicFactor(const gtsam::SymbolicFactor& f); - SymbolicFactor(); - SymbolicFactor(size_t j); - SymbolicFactor(size_t j1, size_t j2); - SymbolicFactor(size_t j1, size_t j2, size_t j3); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); - static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); - - // From Factor - size_t size() const; - void print(string s) const; - bool equals(const gtsam::SymbolicFactor& other, double tol) const; - gtsam::KeyVector keys(); -}; - -#include -virtual class SymbolicFactorGraph { - SymbolicFactorGraph(); - SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); - SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); - - // From FactorGraph - void push_back(gtsam::SymbolicFactor* factor); - void print(string s) const; - bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; - size_t size() const; - bool exists(size_t idx) const; - - // Standard interface - gtsam::KeySet keys() const; - void push_back(gtsam::SymbolicFactor* factor); - void push_back(const gtsam::SymbolicFactorGraph& graph); - void push_back(const gtsam::SymbolicBayesNet& bayesNet); - void push_back(const gtsam::SymbolicBayesTree& bayesTree); - - //Advanced Interface - void push_factor(size_t key); - void push_factor(size_t key1, size_t key2); - void push_factor(size_t key1, size_t key2, size_t key3); - void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); - - gtsam::SymbolicBayesNet* eliminateSequential(); - gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - gtsam::SymbolicBayesTree* eliminateMultifrontal(); - gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector& keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering& ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector& keys); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& variables); -}; - -#include -virtual class SymbolicConditional : gtsam::SymbolicFactor { - // Standard Constructors and Named Constructors - SymbolicConditional(); - SymbolicConditional(const gtsam::SymbolicConditional& other); - SymbolicConditional(size_t key); - SymbolicConditional(size_t key, size_t parent); - SymbolicConditional(size_t key, size_t parent1, size_t parent2); - SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); - static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); - - // Testable - void print(string s) const; - bool equals(const gtsam::SymbolicConditional& other, double tol) const; - - // Standard interface - size_t nrFrontals() const; - size_t nrParents() const; -}; - -#include -class SymbolicBayesNet { - SymbolicBayesNet(); - SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); - // Testable - void print(string s) const; - bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; - - // Standard interface - size_t size() const; - void saveGraph(string s) const; - gtsam::SymbolicConditional* at(size_t idx) const; - gtsam::SymbolicConditional* front() const; - gtsam::SymbolicConditional* back() const; - void push_back(gtsam::SymbolicConditional* conditional); - void push_back(const gtsam::SymbolicBayesNet& bayesNet); -}; - -#include -class SymbolicBayesTree { - - //Constructors - SymbolicBayesTree(); - SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); - - // Testable - void print(string s); - bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; - - //Standard Interface - //size_t findParentClique(const gtsam::IndexVector& parents) const; - size_t size(); - void saveGraph(string s) const; - void clear(); - void deleteCachedShortcuts(); - size_t numCachedSeparatorMarginals() const; - - gtsam::SymbolicConditional* marginalFactor(size_t key) const; - gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; -}; - -// class SymbolicBayesTreeClique { -// BayesTreeClique(); -// BayesTreeClique(CONDITIONAL* conditional); -// // BayesTreeClique(const std::pair& result) : Base(result) {} -// -// bool equals(const This& other, double tol) const; -// void print(string s) const; -// void printTree() const; // Default indent of "" -// void printTree(string indent) const; -// size_t numCachedSeparatorMarginals() const; -// -// CONDITIONAL* conditional() const; -// bool isRoot() const; -// size_t treeSize() const; -// // const std::list& children() const { return children_; } -// // derived_ptr parent() const { return parent_.lock(); } -// -// // FIXME: need wrapped versions graphs, BayesNet -// // BayesNet shortcut(derived_ptr root, Eliminate function) const; -// // FactorGraph marginal(derived_ptr root, Eliminate function) const; -// // FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; -// -// void deleteCachedShortcuts(); -// }; - -#include -class VariableIndex { - // Standard Constructors and Named Constructors - VariableIndex(); - // TODO: Templetize constructor when wrap supports it - //template - //VariableIndex(const T& factorGraph, size_t nVariables); - //VariableIndex(const T& factorGraph); - VariableIndex(const gtsam::SymbolicFactorGraph& factorGraph); - VariableIndex(const gtsam::GaussianFactorGraph& factorGraph); - VariableIndex(const gtsam::NonlinearFactorGraph& factorGraph); - VariableIndex(const gtsam::VariableIndex& other); - - // Testable - bool equals(const gtsam::VariableIndex& other, double tol) const; - void print(string s) const; - - // Standard interface - size_t size() const; - size_t nFactors() const; - size_t nEntries() const; -}; - -//************************************************************************* -// linear -//************************************************************************* - -namespace noiseModel { -#include -virtual class Base { -}; - -virtual class Gaussian : gtsam::noiseModel::Base { - static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian* Covariance(Matrix R); - Matrix R() const; - bool equals(gtsam::noiseModel::Base& expected, double tol); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Diagonal : gtsam::noiseModel::Gaussian { - static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal* Variances(Vector variances); - static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); - Matrix R() const; - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Constrained : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas); - static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas); - static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances); - static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances); - static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions); - static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions); - - static gtsam::noiseModel::Constrained* All(size_t dim); - static gtsam::noiseModel::Constrained* All(size_t dim, double mu); - - gtsam::noiseModel::Constrained* unit() const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Isotropic : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Unit : gtsam::noiseModel::Isotropic { - static gtsam::noiseModel::Unit* Create(size_t dim); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -namespace mEstimator { -virtual class Base { -}; - -virtual class Null: gtsam::noiseModel::mEstimator::Base { - Null(); - void print(string s) const; - static gtsam::noiseModel::mEstimator::Null* Create(); - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Fair: gtsam::noiseModel::mEstimator::Base { - Fair(double c); - void print(string s) const; - static gtsam::noiseModel::mEstimator::Fair* Create(double c); - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Huber: gtsam::noiseModel::mEstimator::Base { - Huber(double k); - void print(string s) const; - static gtsam::noiseModel::mEstimator::Huber* Create(double k); - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Tukey: gtsam::noiseModel::mEstimator::Base { - Tukey(double k); - void print(string s) const; - static gtsam::noiseModel::mEstimator::Tukey* Create(double k); - - // enabling serialization functionality - void serializable() const; -}; - -}///\namespace mEstimator - -virtual class Robust : gtsam::noiseModel::Base { - Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -}///\namespace noiseModel - -#include -class Sampler { - //Constructors - Sampler(gtsam::noiseModel::Diagonal* model, int seed); - Sampler(Vector sigmas, int seed); - Sampler(int seed); - - //Standard Interface - size_t dim() const; - Vector sigmas() const; - gtsam::noiseModel::Diagonal* model() const; - Vector sample(); - Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); -}; - -#include -class VectorValues { - //Constructors - VectorValues(); - VectorValues(const gtsam::VectorValues& other); - - //Named Constructors - static gtsam::VectorValues Zero(const gtsam::VectorValues& model); - - //Standard Interface - size_t size() const; - size_t dim(size_t j) const; - bool exists(size_t j) const; - void print(string s) const; - bool equals(const gtsam::VectorValues& expected, double tol) const; - void insert(size_t j, Vector value); - Vector vector() const; - Vector at(size_t j) const; - void update(const gtsam::VectorValues& values); - - //Advanced Interface - void setZero(); - - gtsam::VectorValues add(const gtsam::VectorValues& c) const; - void addInPlace(const gtsam::VectorValues& c); - gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; - gtsam::VectorValues scale(double a) const; - void scaleInPlace(double a); - - bool hasSameStructure(const gtsam::VectorValues& other) const; - double dot(const gtsam::VectorValues& V) const; - double norm() const; - double squaredNorm() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class GaussianFactor { - gtsam::KeyVector keys() const; - void print(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - gtsam::GaussianFactor* clone() const; - gtsam::GaussianFactor* negate() const; - Matrix augmentedInformation() const; - Matrix information() const; - Matrix augmentedJacobian() const; - pair jacobian() const; - size_t size() const; - bool empty() const; -}; - -#include -virtual class JacobianFactor : gtsam::GaussianFactor { - //Constructors - JacobianFactor(); - JacobianFactor(const gtsam::GaussianFactor& factor); - JacobianFactor(Vector b_in); - JacobianFactor(size_t i1, Matrix A1, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - JacobianFactor(const gtsam::GaussianFactorGraph& graph); - - //Testable - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - size_t size() const; - Vector unweighted_error(const gtsam::VectorValues& c) const; - Vector error_vector(const gtsam::VectorValues& c) const; - double error(const gtsam::VectorValues& c) const; - - //Standard Interface - Matrix getA() const; - Vector getb() const; - size_t rows() const; - size_t cols() const; - bool isConstrained() const; - pair jacobianUnweighted() const; - Matrix augmentedJacobianUnweighted() const; - - void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; - gtsam::JacobianFactor whiten() const; - - pair eliminate(const gtsam::Ordering& keys) const; - - void setModel(bool anyConstrained, const Vector& sigmas); - - gtsam::noiseModel::Diagonal* get_model() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class HessianFactor : gtsam::GaussianFactor { - //Constructors - HessianFactor(); - HessianFactor(const gtsam::GaussianFactor& factor); - HessianFactor(size_t j, Matrix G, Vector g, double f); - HessianFactor(size_t j, Vector mu, Matrix Sigma); - HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, - Vector g2, double f); - HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, - Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, - double f); - HessianFactor(const gtsam::GaussianFactorGraph& factors); - - //Testable - size_t size() const; - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - - //Standard Interface - size_t rows() const; - Matrix info() const; - double constantTerm() const; - Vector linearTerm() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class GaussianFactorGraph { - GaussianFactorGraph(); - GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); - GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); - - // From FactorGraph - void print(string s) const; - bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; - size_t size() const; - gtsam::GaussianFactor* at(size_t idx) const; - gtsam::KeySet keys() const; - bool exists(size_t idx) const; - - // Building the graph - void push_back(const gtsam::GaussianFactor* factor); - void push_back(const gtsam::GaussianConditional* factor); - void push_back(const gtsam::GaussianFactorGraph& graph); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - void push_back(const gtsam::GaussianBayesTree& bayesTree); - void add(const gtsam::GaussianFactor& factor); - void add(Vector b); - void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - - // error and probability - double error(const gtsam::VectorValues& c) const; - double probPrime(const gtsam::VectorValues& c) const; - - gtsam::GaussianFactorGraph clone() const; - gtsam::GaussianFactorGraph negate() const; - - // Optimizing and linear algebra - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - - // Elimination and marginals - gtsam::GaussianBayesNet* eliminateSequential(); - gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - gtsam::GaussianBayesTree* eliminateMultifrontal(); - gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector& keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering& ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector& keys); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& variables); - - // Conversion to matrices - Matrix sparseJacobian_() const; - Matrix augmentedJacobian() const; - Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; - pair jacobian() const; - pair jacobian(const gtsam::Ordering& ordering) const; - Matrix augmentedHessian() const; - Matrix augmentedHessian(const gtsam::Ordering& ordering) const; - pair hessian() const; - pair hessian(const gtsam::Ordering& ordering) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class GaussianConditional : gtsam::GaussianFactor { - //Constructors - GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); - - //Constructors with no noise model - GaussianConditional(size_t key, Vector d, Matrix R); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T); - - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianConditional &cg, double tol) const; - - //Advanced Interface - gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; - gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; - void solveTransposeInPlace(gtsam::VectorValues& gy) const; - void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class GaussianDensity : gtsam::GaussianConditional { - //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); - - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianDensity &cg, double tol) const; - Vector mean() const; - Matrix covariance() const; -}; - -#include -virtual class GaussianBayesNet { - //Constructors - GaussianBayesNet(); - GaussianBayesNet(const gtsam::GaussianConditional* conditional); - - // Testable - void print(string s) const; - bool equals(const gtsam::GaussianBayesNet& other, double tol) const; - size_t size() const; - - // FactorGraph derived interface - size_t size() const; - gtsam::GaussianConditional* at(size_t idx) const; - gtsam::KeySet keys() const; - bool exists(size_t idx) const; - - gtsam::GaussianConditional* front() const; - gtsam::GaussianConditional* back() const; - void push_back(gtsam::GaussianConditional* conditional); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues& x) const; - double determinant() const; - double logDeterminant() const; - gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; - gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; -}; - -#include -virtual class GaussianBayesTree { - // Standard Constructors and Named Constructors - GaussianBayesTree(); - GaussianBayesTree(const gtsam::GaussianBayesTree& other); - bool equals(const gtsam::GaussianBayesTree& other, double tol) const; - void print(string s); - size_t size() const; - bool empty() const; - size_t numCachedSeparatorMarginals() const; - void saveGraph(string s) const; - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues& x) const; - double determinant() const; - double logDeterminant() const; - Matrix marginalCovariance(size_t key) const; - gtsam::GaussianConditional* marginalFactor(size_t key) const; - gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; -}; - -class Errors { - //Constructors - Errors(); - Errors(const gtsam::VectorValues& V); - - //Testable - void print(string s); - bool equals(const gtsam::Errors& expected, double tol) const; -}; - -class GaussianISAM { - //Constructor - GaussianISAM(); - - //Standard Interface - void update(const gtsam::GaussianFactorGraph& newFactors); - void saveGraph(string s) const; - void clear(); -}; - -#include -virtual class IterativeOptimizationParameters { - string getVerbosity() const; - void setVerbosity(string s) ; - void print() const; -}; - -//virtual class IterativeSolver { -// IterativeSolver(); -// gtsam::VectorValues optimize (); -//}; - -#include -virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { - ConjugateGradientParameters(); - int getMinIterations() const ; - int getMaxIterations() const ; - int getReset() const; - double getEpsilon_rel() const; - double getEpsilon_abs() const; - - void setMinIterations(int value); - void setMaxIterations(int value); - void setReset(int value); - void setEpsilon_rel(double value); - void setEpsilon_abs(double value); - void print(); -}; - -#include -virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { - SubgraphSolverParameters(); - void print() const; -}; - -virtual class SubgraphSolver { - SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); - SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); - gtsam::VectorValues optimize() const; -}; - -#include -class KalmanFilter { - KalmanFilter(size_t n); - // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); - gtsam::GaussianDensity* init(Vector x0, Matrix P0); - void print(string s) const; - static size_t step(gtsam::GaussianDensity* p); - gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, - Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); - gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, - Matrix B, Vector u, Matrix Q); - gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, - Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, - Vector z, const gtsam::noiseModel::Diagonal* model); - gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, - Vector z, Matrix Q); -}; - -//************************************************************************* -// nonlinear -//************************************************************************* - -#include -size_t symbol(char chr, size_t index); -char symbolChr(size_t key); -size_t symbolIndex(size_t key); - -// Default keyformatter -void PrintKeyList (const gtsam::KeyList& keys); -void PrintKeyList (const gtsam::KeyList& keys, string s); -void PrintKeyVector(const gtsam::KeyVector& keys); -void PrintKeyVector(const gtsam::KeyVector& keys, string s); -void PrintKeySet (const gtsam::KeySet& keys); -void PrintKeySet (const gtsam::KeySet& keys, string s); - -#include -class LabeledSymbol { - LabeledSymbol(size_t full_key); - LabeledSymbol(const gtsam::LabeledSymbol& key); - LabeledSymbol(unsigned char valType, unsigned char label, size_t j); - - size_t key() const; - unsigned char label() const; - unsigned char chr() const; - size_t index() const; - - gtsam::LabeledSymbol upper() const; - gtsam::LabeledSymbol lower() const; - gtsam::LabeledSymbol newChr(unsigned char c) const; - gtsam::LabeledSymbol newLabel(unsigned char label) const; - - void print(string s) const; -}; - -size_t mrsymbol(unsigned char c, unsigned char label, size_t j); -unsigned char mrsymbolChr(size_t key); -unsigned char mrsymbolLabel(size_t key); -size_t mrsymbolIndex(size_t key); - -#include -class Ordering { - // Standard Constructors and Named Constructors - Ordering(); - Ordering(const gtsam::Ordering& other); - - // Testable - void print(string s) const; - bool equals(const gtsam::Ordering& ord, double tol) const; - - // Standard interface - size_t size() const; - size_t at(size_t key) const; - void push_back(size_t key); - - // enabling serialization functionality - void serialize() const; -}; - -class NonlinearFactorGraph { - NonlinearFactorGraph(); - NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t idx) const; - void push_back(const gtsam::NonlinearFactorGraph& factors); - void push_back(gtsam::NonlinearFactor* factor); - void add(gtsam::NonlinearFactor* factor); - bool exists(size_t idx) const; - gtsam::KeySet keys() const; - - // NonlinearFactorGraph - double error(const gtsam::Values& values) const; - double probPrime(const gtsam::Values& values) const; - gtsam::Ordering orderingCOLAMD() const; - // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; - gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; - gtsam::NonlinearFactorGraph clone() const; - - // enabling serialization functionality - void serialize() const; -}; - -virtual class NonlinearFactor { - // Factor base class - size_t size() const; - gtsam::KeyVector keys() const; - void print(string s) const; - void printKeys(string s) const; - // NonlinearFactor - void equals(const gtsam::NonlinearFactor& other, double tol) const; - double error(const gtsam::Values& c) const; - size_t dim() const; - bool active(const gtsam::Values& c) const; - gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; - gtsam::NonlinearFactor* clone() const; - // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen -}; - -virtual class NoiseModelFactor: gtsam::NonlinearFactor { - void equals(const gtsam::NoiseModelFactor& other, double tol) const; - gtsam::noiseModel::Base* noiseModel() const; - Vector unwhitenedError(const gtsam::Values& x) const; - Vector whitenedError(const gtsam::Values& x) const; -}; - -#include -class Values { - Values(); - Values(const gtsam::Values& other); - - size_t size() const; - bool empty() const; - void clear(); - size_t dim() const; - - void print(string s) const; - bool equals(const gtsam::Values& other, double tol) const; - - void insert(const gtsam::Values& values); - void update(const gtsam::Values& values); - void erase(size_t j); - void swap(gtsam::Values& values); - - bool exists(size_t j) const; - gtsam::KeyVector keys() const; - - gtsam::VectorValues zeroVectors() const; - - gtsam::Values retract(const gtsam::VectorValues& delta) const; - gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; - - // enabling serialization functionality - void serialize() const; - - // New in 4.0, we have to specialize every insert/update/at to generate wrappers - // Instead of the old: - // void insert(size_t j, const gtsam::Value& value); - // void update(size_t j, const gtsam::Value& val); - // gtsam::Value at(size_t j) const; - - void insert(size_t j, const gtsam::Point2& t); - void insert(size_t j, const gtsam::Point3& t); - void insert(size_t j, const gtsam::Rot2& t); - void insert(size_t j, const gtsam::Pose2& t); - void insert(size_t j, const gtsam::Rot3& t); - void insert(size_t j, const gtsam::Pose3& t); - void insert(size_t j, const gtsam::Cal3_S2& t); - void insert(size_t j, const gtsam::Cal3DS2& t); - void insert(size_t j, const gtsam::Cal3Bundler& t); - void insert(size_t j, const gtsam::EssentialMatrix& t); - void insert(size_t j, const gtsam::SimpleCamera& t); - void insert(size_t j, const gtsam::imuBias::ConstantBias& t); - void insert(size_t j, Vector t); - void insert(size_t j, Matrix t); - - // Fixed size version - void insertFixed(size_t j, Vector t, size_t n); - - void update(size_t j, const gtsam::Point2& t); - void update(size_t j, const gtsam::Point3& t); - void update(size_t j, const gtsam::Rot2& t); - void update(size_t j, const gtsam::Pose2& t); - void update(size_t j, const gtsam::Rot3& t); - void update(size_t j, const gtsam::Pose3& t); - void update(size_t j, const gtsam::Cal3_S2& t); - void update(size_t j, const gtsam::Cal3DS2& t); - void update(size_t j, const gtsam::Cal3Bundler& t); - void update(size_t j, const gtsam::EssentialMatrix& t); - void update(size_t j, const gtsam::imuBias::ConstantBias& t); - void update(size_t j, Vector t); - void update(size_t j, Matrix t); - - template - T at(size_t j); - - /// Fixed size versions, for n in 1..9 - void insertFixed(size_t j, Vector t, size_t n); - - /// Fixed size versions, for n in 1..9 - Vector atFixed(size_t j, size_t n); - - /// version for double - void insertDouble(size_t j, double c); - double atDouble(size_t j) const; -}; - -// Actually a FastList -#include -class KeyList { - KeyList(); - KeyList(const gtsam::KeyList& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t front() const; - size_t back() const; - void push_back(size_t key); - void push_front(size_t key); - void pop_back(); - void pop_front(); - void sort(); - void remove(size_t key); - - void serialize() const; -}; - -// Actually a FastSet -class KeySet { - KeySet(); - KeySet(const gtsam::KeySet& other); - KeySet(const gtsam::KeyVector& other); - KeySet(const gtsam::KeyList& other); - - // Testable - void print(string s) const; - bool equals(const gtsam::KeySet& other) const; - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(size_t key); - void merge(gtsam::KeySet& other); - bool erase(size_t key); // returns true if value was removed - bool count(size_t key) const; // returns true if value exists - - void serialize() const; -}; - -// Actually a vector -class KeyVector { - KeyVector(); - KeyVector(const gtsam::KeyVector& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t at(size_t i) const; - size_t front() const; - size_t back() const; - void push_back(size_t key) const; - - void serialize() const; -}; - -// Actually a FastMap -class KeyGroupMap { - KeyGroupMap(); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t at(size_t key) const; - int erase(size_t key); - bool insert2(size_t key, int val); -}; - -#include -class Marginals { - Marginals(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& solution); - - void print(string s) const; - Matrix marginalCovariance(size_t variable) const; - Matrix marginalInformation(size_t variable) const; - gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; - gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; -}; - -class JointMarginal { - Matrix at(size_t iVariable, size_t jVariable) const; - Matrix fullMatrix() const; - void print(string s) const; - void print() const; -}; - -#include -virtual class LinearContainerFactor : gtsam::NonlinearFactor { - - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); - LinearContainerFactor(gtsam::GaussianFactor* factor); - - gtsam::GaussianFactor* factor() const; -// const boost::optional& linearizationPoint() const; - - bool isJacobian() const; - gtsam::JacobianFactor* toJacobian() const; - gtsam::HessianFactor* toHessian() const; - - static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, - const gtsam::Values& linearizationPoint); - - static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); - - // enabling serialization functionality - void serializable() const; -}; // \class LinearContainerFactor - -// Summarization functionality -//#include -// -//// Uses partial QR approach by default -//gtsam::GaussianFactorGraph summarize( -// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, -// const gtsam::KeySet& saved_keys); -// -//gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer( -// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, -// const gtsam::KeySet& saved_keys); - -//************************************************************************* -// Nonlinear optimizers -//************************************************************************* - -#include -#include -virtual class NonlinearOptimizerParams { - NonlinearOptimizerParams(); - void print(string s) const; - - int getMaxIterations() const; - double getRelativeErrorTol() const; - double getAbsoluteErrorTol() const; - double getErrorTol() const; - string getVerbosity() const; - - void setMaxIterations(int value); - void setRelativeErrorTol(double value); - void setAbsoluteErrorTol(double value); - void setErrorTol(double value); - void setVerbosity(string s); - - string getLinearSolverType() const; - - void setLinearSolverType(string solver); - void setOrdering(const gtsam::Ordering& ordering); - void setIterativeParams(gtsam::IterativeOptimizationParameters* params); - - bool isMultifrontal() const; - bool isSequential() const; - bool isCholmod() const; - bool isIterative() const; -}; - -bool checkConvergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double errorThreshold, - double currentError, double newError); - -#include -virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { - GaussNewtonParams(); -}; - -#include -virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { - LevenbergMarquardtParams(); - - double getlambdaInitial() const; - double getlambdaFactor() const; - double getlambdaUpperBound() const; - string getVerbosityLM() const; - - void setlambdaInitial(double value); - void setlambdaFactor(double value); - void setlambdaUpperBound(double value); - void setVerbosityLM(string s); -}; - -#include -virtual class DoglegParams : gtsam::NonlinearOptimizerParams { - DoglegParams(); - - double getDeltaInitial() const; - string getVerbosityDL() const; - - void setDeltaInitial(double deltaInitial) const; - void setVerbosityDL(string verbosityDL) const; -}; - -virtual class NonlinearOptimizer { - gtsam::Values optimize(); - gtsam::Values optimizeSafely(); - double error() const; - int iterations() const; - gtsam::Values values() const; - gtsam::GaussianFactorGraph* iterate() const; -}; - -virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); -}; - -virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { - DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); - double getDelta() const; -}; - -virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); - double lambda() const; - void print(string str) const; -}; - -#include -class ISAM2GaussNewtonParams { - ISAM2GaussNewtonParams(); - - void print(string str) const; - - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); -}; - -class ISAM2DoglegParams { - ISAM2DoglegParams(); - - void print(string str) const; - - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); - double getInitialDelta() const; - void setInitialDelta(double initialDelta); - string getAdaptationMode() const; - void setAdaptationMode(string adaptationMode); - bool isVerbose() const; - void setVerbose(bool verbose); -}; - -class ISAM2ThresholdMapValue { - ISAM2ThresholdMapValue(char c, Vector thresholds); - ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); -}; - -class ISAM2ThresholdMap { - ISAM2ThresholdMap(); - ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(const gtsam::ISAM2ThresholdMapValue& value) const; -}; - -class ISAM2Params { - ISAM2Params(); - - void print(string str) const; - - /** Getters and Setters for all properties */ - void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& params); - void setOptimizationParams(const gtsam::ISAM2DoglegParams& params); - void setRelinearizeThreshold(double relinearizeThreshold); - void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& relinearizeThreshold); - int getRelinearizeSkip() const; - void setRelinearizeSkip(int relinearizeSkip); - bool isEnableRelinearization() const; - void setEnableRelinearization(bool enableRelinearization); - bool isEvaluateNonlinearError() const; - void setEvaluateNonlinearError(bool evaluateNonlinearError); - string getFactorization() const; - void setFactorization(string factorization); - bool isCacheLinearizedFactors() const; - void setCacheLinearizedFactors(bool cacheLinearizedFactors); - bool isEnableDetailedResults() const; - void setEnableDetailedResults(bool enableDetailedResults); - bool isEnablePartialRelinearizationCheck() const; - void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); -}; - -class ISAM2Clique { - - //Constructors - ISAM2Clique(); - - //Standard Interface - Vector gradientContribution() const; - void print(string s); -}; - -class ISAM2Result { - ISAM2Result(); - - void print(string str) const; - - /** Getters and Setters for all properties */ - size_t getVariablesRelinearized() const; - size_t getVariablesReeliminated() const; - size_t getCliques() const; -}; - -class ISAM2 { - ISAM2(); - ISAM2(const gtsam::ISAM2Params& params); - ISAM2(const gtsam::ISAM2& other); - - bool equals(const gtsam::ISAM2& other, double tol) const; - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; - - gtsam::ISAM2Result update(); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& 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& constrainedKeys); - //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys, bool force_relinearize); - - gtsam::Values getLinearizationPoint() const; - gtsam::Values calculateEstimate() const; - gtsam::Value calculateEstimate(size_t key) const; - gtsam::Values calculateBestEstimate() const; - Matrix marginalCovariance(size_t key) const; - gtsam::VectorValues getDelta() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; - gtsam::VariableIndex getVariableIndex() const; - gtsam::ISAM2Params params() const; -}; - -#include -class NonlinearISAM { - NonlinearISAM(); - NonlinearISAM(int reorderInterval); - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; - gtsam::Values estimate() const; - Matrix marginalCovariance(size_t key) const; - int reorderInterval() const; - int reorderCounter() const; - void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues); - void reorder_relinearize(); - - // These might be expensive as instead of a reference the wrapper will make a copy - gtsam::GaussianISAM bayesTree() const; - gtsam::Values getLinearizationPoint() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; -}; - -//************************************************************************* -// Nonlinear factor types -//************************************************************************* -#include -#include -#include - -#include -template -virtual class PriorFactor : gtsam::NoiseModelFactor { - PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); - T prior() const; - - // enabling serialization functionality - void serialize() const; -}; - - -#include -template -virtual class BetweenFactor : gtsam::NoiseModelFactor { - BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); - T measured() const; - - // enabling serialization functionality - void serialize() const; -}; - - - -#include -template -virtual class NonlinearEquality : gtsam::NoiseModelFactor { - // Constructor - forces exact evaluation - NonlinearEquality(size_t j, const T& feasible); - // Constructor - allows inexact evaluation - NonlinearEquality(size_t j, const T& feasible, double error_gain); - - // enabling serialization functionality - void serialize() const; -}; - - -#include -template -virtual class RangeFactor : gtsam::NoiseModelFactor { - RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); -}; - -typedef gtsam::RangeFactor RangeFactor2D; -typedef gtsam::RangeFactor RangeFactor3D; -typedef gtsam::RangeFactor RangeFactorPose2; -typedef gtsam::RangeFactor RangeFactorPose3; -typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; -typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -typedef gtsam::RangeFactor RangeFactorSimpleCamera; - - -#include -template -virtual class BearingFactor : gtsam::NoiseModelFactor { - BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::BearingFactor BearingFactor2D; - -#include -template -virtual class BearingRangeFactor : gtsam::NoiseModelFactor { - BearingRangeFactor(size_t poseKey, size_t pointKey, - const BEARING& measuredBearing, const RANGE& measuredRange, - const gtsam::noiseModel::Base* noiseModel); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::BearingRangeFactor BearingRangeFactor2D; - - -#include -template -virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k); - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor); - - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality, - const POSE& body_P_sensor); - - gtsam::Point2 measured() const; - CALIBRATION* calibration() const; - bool verboseCheirality() const; - bool throwCheirality() const; - - // enabling serialization functionality - void serialize() const; -}; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; - - -#include -template -virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { - GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); - gtsam::Point2 measured() const; -}; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; - -template -virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { - GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); - gtsam::Point2 measured() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class SmartProjectionParams { - SmartProjectionParams(); - // TODO(frank): make these work: - // void setLinearizationMode(LinearizationMode linMode); - // void setDegeneracyMode(DegeneracyMode degMode); - void setRankTolerance(double rankTol); - void setEnableEPI(bool enableEPI); - void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); - void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); -}; - -#include -template -virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { - - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::Pose3& body_P_sensor); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::Pose3& body_P_sensor, - const gtsam::SmartProjectionParams& params); - - void add(const gtsam::Point2& measured_i, size_t poseKey_i); - - // enabling serialization functionality - //void serialize() const; -}; - -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; - - -#include -template -virtual class GenericStereoFactor : gtsam::NoiseModelFactor { - GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); - gtsam::StereoPoint2 measured() const; - gtsam::Cal3_S2Stereo* calibration() const; - - // enabling serialization functionality - void serialize() const; -}; -typedef gtsam::GenericStereoFactor GenericStereoFactor3D; - -#include -template -virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { - PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); -}; - -typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; -typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; - -#include -template -virtual class PoseRotationPrior : gtsam::NoiseModelFactor { - PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); -}; - -typedef gtsam::PoseRotationPrior PoseRotationPrior2D; -typedef gtsam::PoseRotationPrior PoseRotationPrior3D; - -#include -virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { - EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, - const gtsam::noiseModel::Base* noiseModel); -}; - -#include -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model); -pair load2D(string filename); -pair load2D_robust(string filename, - gtsam::noiseModel::Base* model); -void save2D(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, - string filename); - -pair readG2o(string filename); -void writeG2o(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& estimate, string filename); - -//************************************************************************* -// Navigation -//************************************************************************* -namespace imuBias { -#include - -class ConstantBias { - // Standard Constructor - ConstantBias(); - ConstantBias(Vector biasAcc, Vector biasGyro); - - // Testable - void print(string s) const; - bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; - - // Group - static gtsam::imuBias::ConstantBias identity(); - gtsam::imuBias::ConstantBias inverse() const; - gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; - gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; - - // Manifold - gtsam::imuBias::ConstantBias retract(Vector v) const; - Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; - - // Lie Group - static gtsam::imuBias::ConstantBias Expmap(Vector v); - static Vector Logmap(const gtsam::imuBias::ConstantBias& b); - - // Standard Interface - Vector vector() const; - Vector accelerometer() const; - Vector gyroscope() const; - Vector correctAccelerometer(Vector measurement) const; - Vector correctGyroscope(Vector measurement) const; -}; - -}///\namespace imuBias - -#include -class PoseVelocityBias{ - PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); - }; -class PreintegratedImuMeasurements { - // Standard Constructor - PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); - PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); - // PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& rhs); - - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); - - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - Vector biasHatVector() const; - Matrix delPdelBiasAcc() const; - Matrix delPdelBiasOmega() const; - Matrix delVdelBiasAcc() const; - Matrix delVdelBiasOmega() const; - Matrix delRdelBiasOmega() const; - Matrix preintMeasCov() const; - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); - gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, - Vector gravity, Vector omegaCoriolis) const; -}; - -virtual class ImuFactor : gtsam::NonlinearFactor { - ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, - const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); - ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, - const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, - const gtsam::Pose3& body_P_sensor); - // Standard Interface - gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; -}; - -#include -class PreintegratedCombinedMeasurements { - // Standard Constructor - PreintegratedCombinedMeasurements( - const gtsam::imuBias::ConstantBias& bias, - Matrix measuredAccCovariance, - Matrix measuredOmegaCovariance, - Matrix integrationErrorCovariance, - Matrix biasAccCovariance, - Matrix biasOmegaCovariance, - Matrix biasAccOmegaInit); - PreintegratedCombinedMeasurements( - const gtsam::imuBias::ConstantBias& bias, - Matrix measuredAccCovariance, - Matrix measuredOmegaCovariance, - Matrix integrationErrorCovariance, - Matrix biasAccCovariance, - Matrix biasOmegaCovariance, - Matrix biasAccOmegaInit, - bool use2ndOrderIntegration); - // PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& rhs); - - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol); - - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - Vector biasHatVector() const; - Matrix delPdelBiasAcc() const; - Matrix delPdelBiasOmega() const; - Matrix delVdelBiasAcc() const; - Matrix delVdelBiasOmega() const; - Matrix delRdelBiasOmega() const; - Matrix preintMeasCov() const; - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); - gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, - Vector gravity, Vector omegaCoriolis) const; -}; - -virtual class CombinedImuFactor : gtsam::NonlinearFactor { - CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, - const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); - // Standard Interface - gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; -}; - -#include -class PreintegratedAhrsMeasurements { - // Standard Constructor - PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); - PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); - PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); - - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); - - // get Data - gtsam::Rot3 deltaRij() const; - double deltaTij() const; - Vector biasHat() const; - - // Standard Interface - void integrateMeasurement(Vector measuredOmega, double deltaT); - void resetIntegration() ; -}; - -virtual class AHRSFactor : gtsam::NonlinearFactor { - AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); - AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, - const gtsam::Pose3& body_P_sensor); - - // Standard Interface - gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, - Vector bias) const; - gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, - Vector omegaCoriolis) const; -}; - -#include -//virtual class AttitudeFactor : gtsam::NonlinearFactor { -// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); -// AttitudeFactor(); -//}; -virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); - Rot3AttitudeFactor(); - void print(string s) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; -}; - -virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{ - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); - Pose3AttitudeFactor(); - void print(string s) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; -}; - -//************************************************************************* -// Utilities -//************************************************************************* - -namespace utilities { - - #include - gtsam::KeyList createKeyList(Vector I); - gtsam::KeyList createKeyList(string s, Vector I); - gtsam::KeyVector createKeyVector(Vector I); - gtsam::KeyVector createKeyVector(string s, Vector I); - gtsam::KeySet createKeySet(Vector I); - gtsam::KeySet createKeySet(string s, Vector I); - Matrix extractPoint2(const gtsam::Values& values); - Matrix extractPoint3(const gtsam::Values& values); - Matrix extractPose2(const gtsam::Values& values); - gtsam::Values allPose3s(gtsam::Values& values); - Matrix extractPose3(const gtsam::Values& values); - void perturbPoint2(gtsam::Values& values, double sigma, int seed); - void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); - void perturbPoint3(gtsam::Values& values, double sigma, int seed); - void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); - void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); - void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); - Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); - gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); - gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); - -} //\namespace utilities - -} diff --git a/matlab/README-gtsam-toolbox.md b/matlab/README.md similarity index 98% rename from matlab/README-gtsam-toolbox.md rename to matlab/README.md index 66a02e9694..86e7d9fe07 100644 --- a/matlab/README-gtsam-toolbox.md +++ b/matlab/README.md @@ -1,6 +1,4 @@ -# GTSAM - Georgia Tech Smoothing and Mapping Library - -## MATLAB wrapper +# MATLAB Wrapper http://borg.cc.gatech.edu/projects/gtsam diff --git a/package.xml b/package.xml index 5b50b5af9b..e14e77f2ad 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ gtsam - 3.2.1 + 4.0.0 gtsam Frank Dellaert diff --git a/package_scripts/README.md b/package_scripts/README.md new file mode 100644 index 0000000000..1e33369aa6 --- /dev/null +++ b/package_scripts/README.md @@ -0,0 +1,14 @@ + +# How to generate Debian packages + + cd [GTSAM_SOURCE_ROOT] + bash package_scripts/prepare_debian.sh + + +# How to generate Ubuntu packages for a PPA + + cd [GTSAM_SOURCE_ROOT] + bash package_scripts/prepare_ubuntu_pkgs_for_ppa.sh + cd ~/gtsam_ubuntu + bash [GTSAM_SOURCE_ROOT]/upload_all_gtsam_ppa.sh + diff --git a/package_scripts/prepare_debian.sh b/package_scripts/prepare_debian.sh new file mode 100755 index 0000000000..7b083d8bbc --- /dev/null +++ b/package_scripts/prepare_debian.sh @@ -0,0 +1,179 @@ +#!/bin/bash +# Prepare to build a Debian package. +# Jose Luis Blanco Claraco, 2019 (for GTSAM) +# Jose Luis Blanco Claraco, 2008-2018 (for MRPT) + +set -e # end on error +#set -x # for debugging + +APPEND_SNAPSHOT_NUM=0 +IS_FOR_UBUNTU=0 +APPEND_LINUX_DISTRO="" +VALUE_EXTRA_CMAKE_PARAMS="" +while getopts "sud:c:" OPTION +do + case $OPTION in + s) + APPEND_SNAPSHOT_NUM=1 + ;; + u) + IS_FOR_UBUNTU=1 + ;; + d) + APPEND_LINUX_DISTRO=$OPTARG + ;; + c) + VALUE_EXTRA_CMAKE_PARAMS=$OPTARG + ;; + ?) + echo "Unknown command line argument!" + exit 1 + ;; + esac +done + +if [ -f CMakeLists.txt ]; +then + source package_scripts/prepare_debian_gen_snapshot_version.sh +else + echo "Error: cannot find CMakeList.txt. This script is intended to be run from the root of the source tree." + exit 1 +fi + +# Append snapshot? +if [ $APPEND_SNAPSHOT_NUM == "1" ]; +then + CUR_SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" + source $CUR_SCRIPT_DIR/prepare_debian_gen_snapshot_version.sh # populate GTSAM_SNAPSHOT_VERSION + + GTSAM_VERSION_STR="${GTSAM_VERSION_STR}~snapshot${GTSAM_SNAPSHOT_VERSION}${APPEND_LINUX_DISTRO}" +else + GTSAM_VERSION_STR="${GTSAM_VERSION_STR}${APPEND_LINUX_DISTRO}" +fi + +# Call prepare_release +GTSAMSRC=`pwd` + +if [ -f $HOME/gtsam_release/gtsam*.tar.gz ]; +then + echo "## release file already exists. Reusing it." +else + source package_scripts/prepare_release.sh + echo + echo "## Done prepare_release.sh" +fi + +echo "=========== Generating GTSAM ${GTSAM_VER_MMP} Debian package ==============" +cd $GTSAMSRC + +set -x +if [ -z "$GTSAM_DEB_DIR" ]; then + GTSAM_DEB_DIR="$HOME/gtsam_debian" +fi +GTSAM_EXTERN_DEBIAN_DIR="$GTSAMSRC/debian/" +GTSAM_EXTERN_UBUNTU_PPA_DIR="$GTSAMSRC/debian/" + +if [ -f ${GTSAM_EXTERN_DEBIAN_DIR}/control ]; +then + echo "Using debian dir: ${GTSAM_EXTERN_DEBIAN_DIR}" +else + echo "ERROR: Cannot find ${GTSAM_EXTERN_DEBIAN_DIR}" + exit 1 +fi + +GTSAM_DEBSRC_DIR=$GTSAM_DEB_DIR/gtsam-${GTSAM_VERSION_STR} + +echo "GTSAM_VERSION_STR: ${GTSAM_VERSION_STR}" +echo "GTSAM_DEBSRC_DIR: ${GTSAM_DEBSRC_DIR}" + +# Prepare a directory for building the debian package: +# +rm -fR $GTSAM_DEB_DIR || true +mkdir -p $GTSAM_DEB_DIR || true + +# Orig tarball: +echo "Copying orig tarball: gtsam_${GTSAM_VERSION_STR}.orig.tar.gz" +cp $HOME/gtsam_release/gtsam*.tar.gz $GTSAM_DEB_DIR/gtsam_${GTSAM_VERSION_STR}.orig.tar.gz +cd ${GTSAM_DEB_DIR} +tar -xf gtsam_${GTSAM_VERSION_STR}.orig.tar.gz + +if [ ! -d "${GTSAM_DEBSRC_DIR}" ]; +then + mv gtsam-* ${GTSAM_DEBSRC_DIR} # fix different dir names for Ubuntu PPA packages +fi + +if [ ! -f "${GTSAM_DEBSRC_DIR}/CMakeLists.txt" ]; +then + echo "*ERROR*: Seems there was a problem copying sources to ${GTSAM_DEBSRC_DIR}... aborting script." + exit 1 +fi + +cd ${GTSAM_DEBSRC_DIR} + +# Copy debian directory: +#mkdir debian +cp -r ${GTSAM_EXTERN_DEBIAN_DIR}/* debian + +# Use modified control & rules files for Ubuntu PPA packages: +#if [ $IS_FOR_UBUNTU == "1" ]; +#then + # already done: cp ${GTSAM_EXTERN_UBUNTU_PPA_DIR}/control.in debian/ + # Ubuntu: force use of gcc-7: + #sed -i '9i\export CXX=/usr/bin/g++-7\' debian/rules + #sed -i '9i\export CC=/usr/bin/gcc-7\' debian/rules7 +#fi + +# Export signing pub key: +mkdir debian/upstream/ +gpg --export --export-options export-minimal --armor > debian/upstream/signing-key.asc + +# Parse debian/ control.in --> control +#mv debian/control.in debian/control +#sed -i "s/@GTSAM_VER_MM@/${GTSAM_VER_MM}/g" debian/control + +# Replace the text "REPLACE_HERE_EXTRA_CMAKE_PARAMS" in the "debian/rules" file +# with: ${${VALUE_EXTRA_CMAKE_PARAMS}} +RULES_FILE=debian/rules +sed -i -e "s/REPLACE_HERE_EXTRA_CMAKE_PARAMS/${VALUE_EXTRA_CMAKE_PARAMS}/g" $RULES_FILE +echo "Using these extra parameters for CMake: '${VALUE_EXTRA_CMAKE_PARAMS}'" + +# Strip my custom files... +rm debian/*.new || true + + +# Figure out the next Debian version number: +echo "Detecting next Debian version number..." + +CHANGELOG_UPSTREAM_VER=$( dpkg-parsechangelog | sed -n 's/Version:.*\([0-9]\.[0-9]*\.[0-9]*.*snapshot.*\)-.*/\1/p' ) +CHANGELOG_LAST_DEBIAN_VER=$( dpkg-parsechangelog | sed -n 's/Version:.*\([0-9]\.[0-9]*\.[0-9]*\).*-\([0-9]*\).*/\2/p' ) + +echo " -> PREVIOUS UPSTREAM: $CHANGELOG_UPSTREAM_VER -> New: ${GTSAM_VERSION_STR}" +echo " -> PREVIOUS DEBIAN VERSION: $CHANGELOG_LAST_DEBIAN_VER" + +# If we have the same upstream versions, increase the Debian version, otherwise create a new entry: +if [ "$CHANGELOG_UPSTREAM_VER" = "$GTSAM_VERSION_STR" ]; +then + NEW_DEBIAN_VER=$[$CHANGELOG_LAST_DEBIAN_VER + 1] + echo "Changing to a new Debian version: ${GTSAM_VERSION_STR}-${NEW_DEBIAN_VER}" + DEBCHANGE_CMD="--newversion ${GTSAM_VERSION_STR}-${NEW_DEBIAN_VER}" +else + DEBCHANGE_CMD="--newversion ${GTSAM_VERSION_STR}-1" +fi + +echo "Adding a new entry to debian/changelog..." + +DEBEMAIL="Jose Luis Blanco Claraco " debchange $DEBCHANGE_CMD -b --distribution unstable --force-distribution New version of upstream sources. + +echo "Copying back the new changelog to a temporary file in: ${GTSAM_EXTERN_DEBIAN_DIR}changelog.new" +cp debian/changelog ${GTSAM_EXTERN_DEBIAN_DIR}changelog.new + +set +x + +echo "==============================================================" +echo "Now, you can build the source Deb package with 'debuild -S -sa'" +echo "==============================================================" + +cd .. +ls -lh + +exit 0 diff --git a/package_scripts/prepare_debian_gen_snapshot_version.sh b/package_scripts/prepare_debian_gen_snapshot_version.sh new file mode 100755 index 0000000000..589d422fe2 --- /dev/null +++ b/package_scripts/prepare_debian_gen_snapshot_version.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +# See https://reproducible-builds.org/specs/source-date-epoch/ +# get SOURCE_DATE_EPOCH with UNIX time_t +if [ -d ".git" ]; +then + SOURCE_DATE_EPOCH=$(git log -1 --pretty=%ct) +else + echo "Error: intended for use from within a git repository" + exit 1 +fi +GTSAM_SNAPSHOT_VERSION=$(date -d @$SOURCE_DATE_EPOCH +%Y%m%d-%H%M) + +GTSAM_SNAPSHOT_VERSION+="-git-" +GTSAM_SNAPSHOT_VERSION+=`git rev-parse --short=8 HEAD` +GTSAM_SNAPSHOT_VERSION+="-" + +# x.y.z version components: +GTSAM_VERSION_MAJOR=$(grep "(GTSAM_VERSION_MAJOR" CMakeLists.txt | sed -r 's/^.*GTSAM_VERSION_MAJOR\s*([0-9])*.*$/\1/g') +GTSAM_VERSION_MINOR=$(grep "(GTSAM_VERSION_MINOR" CMakeLists.txt | sed -r 's/^.*GTSAM_VERSION_MINOR\s*([0-9])*.*$/\1/g') +GTSAM_VERSION_PATCH=$(grep "(GTSAM_VERSION_PATCH" CMakeLists.txt | sed -r 's/^.*GTSAM_VERSION_PATCH\s*([0-9])*.*$/\1/g') + +GTSAM_VER_MM="${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}" +GTSAM_VER_MMP="${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}" +GTSAM_VERSION_STR=$GTSAM_VER_MMP diff --git a/package_scripts/prepare_release.sh b/package_scripts/prepare_release.sh new file mode 100755 index 0000000000..750fc27b31 --- /dev/null +++ b/package_scripts/prepare_release.sh @@ -0,0 +1,71 @@ +#!/bin/bash +# Export sources from a git tree and prepare it for a public release. +# Jose Luis Blanco Claraco, 2019 (for GTSAM) +# Jose Luis Blanco Claraco, 2008-2018 (for MRPT) + +set -e # exit on error +#set -x # for debugging + +# Checks +# -------------------------------- +if [ -f version_prefix.txt ]; +then + if [ -z ${GTSAM_VERSION_STR+x} ]; + then + source package_scripts/prepare_debian_gen_snapshot_version.sh + fi + echo "ERROR: Run this script from the GTSAM source tree root directory." + exit 1 +fi + +GTSAM_SRC=`pwd` +OUT_RELEASES_DIR="$HOME/gtsam_release" + +OUT_DIR=$OUT_RELEASES_DIR/gtsam-${GTSAM_VERSION_STR} + +echo "=========== Generating GTSAM release ${GTSAM_VER_MMP} ==================" +echo "GTSAM_VERSION_STR : ${GTSAM_VERSION_STR}" +echo "OUT_DIR : ${OUT_DIR}" +echo "============================================================" +echo + +# Prepare output directory: +rm -fR $OUT_RELEASES_DIR || true +mkdir -p ${OUT_DIR} + +# Export / copy sources to target dir: +if [ -d "$GTSAM_SRC/.git" ]; +then + echo "# Exporting git source tree to ${OUT_DIR}" + git archive --format=tar HEAD | tar -x -C ${OUT_DIR} + + # Remove VCS control files: + find ${OUT_DIR} -name '.gitignore' | xargs rm + + # Generate ./SOURCE_DATE_EPOCH with UNIX time_t + SOURCE_DATE_EPOCH=$(git log -1 --pretty=%ct) +else + echo "# Copying sources to ${OUT_DIR}" + cp -R . ${OUT_DIR} + + # Generate ./SOURCE_DATE_EPOCH with UNIX time_t + SOURCE_DATE_EPOCH=$(date +%s) +fi + +# See https://reproducible-builds.org/specs/source-date-epoch/ +echo $SOURCE_DATE_EPOCH > ${OUT_DIR}/SOURCE_DATE_EPOCH + +cd ${OUT_DIR} + +# Dont include Debian files in releases: +rm -fR package_scripts + +# Orig tarball: +cd .. +echo "# Creating orig tarball: gtsam-${GTSAM_VERSION_STR}.tar.gz" +tar czf gtsam-${GTSAM_VERSION_STR}.tar.gz gtsam-${GTSAM_VERSION_STR} + +rm -fr gtsam-${GTSAM_VERSION_STR} + +# GPG signature: +gpg --armor --detach-sign gtsam-${GTSAM_VERSION_STR}.tar.gz diff --git a/package_scripts/prepare_ubuntu_pkgs_for_ppa.sh b/package_scripts/prepare_ubuntu_pkgs_for_ppa.sh new file mode 100755 index 0000000000..553e1884df --- /dev/null +++ b/package_scripts/prepare_ubuntu_pkgs_for_ppa.sh @@ -0,0 +1,90 @@ +#!/bin/bash +# Creates a set of packages for each different Ubuntu distribution, with the +# intention of uploading them to: +# https://launchpad.net/~joseluisblancoc/ +# +# JLBC, 2010 +# [Addition 2012:] +# +# You can declare a variable (in the caller shell) with extra flags for the +# CMake in the final ./configure like: +# GTSAM_PKG_CUSTOM_CMAKE_PARAMS="\"-DDISABLE_SSE3=ON\"" +# + +set -e + +# List of distributions to create PPA packages for: +LST_DISTROS=(xenial bionic disco eoan) + +# Checks +# -------------------------------- +if [ -f CMakeLists.txt ]; +then + source package_scripts/prepare_debian_gen_snapshot_version.sh + echo "GTSAM version: ${GTSAM_VER_MMP}" +else + echo "ERROR: Run this script from the GTSAM root directory." + exit 1 +fi + +if [ -z "${gtsam_ubuntu_OUT_DIR}" ]; then + export gtsam_ubuntu_OUT_DIR="$HOME/gtsam_ubuntu" +fi +GTSAMSRC=`pwd` +if [ -z "${GTSAM_DEB_DIR}" ]; then + export GTSAM_DEB_DIR="$HOME/gtsam_debian" +fi +GTSAM_EXTERN_DEBIAN_DIR="$GTSAMSRC/debian/" +EMAIL4DEB="Jose Luis Blanco (University of Malaga) " + +# Clean out dirs: +rm -fr $gtsam_ubuntu_OUT_DIR/ + +# ------------------------------------------------------------------- +# And now create the custom packages for each Ubuntu distribution: +# ------------------------------------------------------------------- +count=${#LST_DISTROS[@]} +IDXS=$(seq 0 $(expr $count - 1)) + +cp ${GTSAM_EXTERN_DEBIAN_DIR}/changelog /tmp/my_changelog + +for IDX in ${IDXS}; +do + DEBIAN_DIST=${LST_DISTROS[$IDX]} + + # ------------------------------------------------------------------- + # Call the standard "prepare_debian.sh" script: + # ------------------------------------------------------------------- + cd ${GTSAMSRC} + bash package_scripts/prepare_debian.sh -s -u -d ${DEBIAN_DIST} -c "${GTSAM_PKG_CUSTOM_CMAKE_PARAMS}" + + CUR_SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" + source $CUR_SCRIPT_DIR/prepare_debian_gen_snapshot_version.sh # populate GTSAM_SNAPSHOT_VERSION + + echo "===== Distribution: ${DEBIAN_DIST} =========" + cd ${GTSAM_DEB_DIR}/gtsam-${GTSAM_VER_MMP}~snapshot${GTSAM_SNAPSHOT_VERSION}${DEBIAN_DIST}/debian + #cp ${GTSAM_EXTERN_DEBIAN_DIR}/changelog changelog + cp /tmp/my_changelog changelog + DEBCHANGE_CMD="--newversion ${GTSAM_VERSION_STR}~snapshot${GTSAM_SNAPSHOT_VERSION}${DEBIAN_DIST}-1" + echo "Changing to a new Debian version: ${DEBCHANGE_CMD}" + echo "Adding a new entry to debian/changelog for distribution ${DEBIAN_DIST}" + DEBEMAIL="Jose Luis Blanco Claraco " debchange $DEBCHANGE_CMD -b --distribution ${DEBIAN_DIST} --force-distribution New version of upstream sources. + + cp changelog /tmp/my_changelog + + echo "Now, let's build the source Deb package with 'debuild -S -sa':" + cd .. + # -S: source package + # -sa: force inclusion of sources + # -d: don't check dependencies in this system + debuild -S -sa -d + + # Make a copy of all these packages: + cd .. + mkdir -p $gtsam_ubuntu_OUT_DIR/$DEBIAN_DIST + cp gtsam_* $gtsam_ubuntu_OUT_DIR/${DEBIAN_DIST}/ + echo ">>>>>> Saving packages to: $gtsam_ubuntu_OUT_DIR/$DEBIAN_DIST/" +done + + +exit 0 diff --git a/package_scripts/upload_all_gtsam_ppa.sh b/package_scripts/upload_all_gtsam_ppa.sh new file mode 100755 index 0000000000..c9d113f00a --- /dev/null +++ b/package_scripts/upload_all_gtsam_ppa.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +find . -name '*.changes' | xargs -I FIL dput ppa:joseluisblancoc/gtsam-develop FIL diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 954afa2203..b0978feb9c 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -10,15 +10,20 @@ * -------------------------------------------------------------------------- */ /** - * @file DoglegOptimizer.h + * @file testDoglegOptimizer.cpp * @brief Unit tests for DoglegOptimizer * @author Richard Roberts + * @author Frank dellaert */ #include #include +#include +#include #include +#include +#include #include #include #include @@ -147,6 +152,44 @@ TEST(DoglegOptimizer, Iterate) { } } +/* ************************************************************************* */ +TEST(DoglegOptimizer, Constraint) { + // Create a pose-graph graph with a constraint on the first pose + NonlinearFactorGraph graph; + const Pose2 origin(0, 0, 0), pose2(2, 0, 0); + graph.emplace_shared >(1, origin); + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + graph.emplace_shared >(1, 2, pose2, model); + + // Create feasible initial estimate + Values initial; + initial.insert(1, origin); // feasible ! + initial.insert(2, Pose2(2.3, 0.1, -0.2)); + + // Optimize the initial values using DoglegOptimizer + DoglegParams params; + params.setVerbosityDL("VERBOSITY"); + DoglegOptimizer optimizer(graph, initial, params); + Values result = optimizer.optimize(); + + // Check result + EXPECT(assert_equal(pose2, result.at(2))); + + // Create infeasible initial estimate + Values infeasible; + infeasible.insert(1, Pose2(0.1, 0, 0)); // infeasible ! + infeasible.insert(2, Pose2(2.3, 0.1, -0.2)); + + // Try optimizing with infeasible initial estimate + DoglegOptimizer optimizer2(graph, infeasible, params); + +#ifdef GTSAM_USE_TBB + CHECK_EXCEPTION(optimizer2.optimize(), std::exception); +#else + CHECK_EXCEPTION(optimizer2.optimize(), std::invalid_argument); +#endif +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 2b6809adaa..68d10bb7bb 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -4,7 +4,7 @@ * @author Michael Kaess */ -#include +#include #include #include @@ -14,7 +14,6 @@ #include #include #include -#include #include #include #include @@ -23,10 +22,12 @@ #include #include #include + +#include + #include -#include -using namespace boost::assign; #include +using namespace boost::assign; namespace br { using namespace boost::adaptors; using namespace boost::range; } using namespace std; @@ -34,7 +35,6 @@ using namespace gtsam; using boost::shared_ptr; static const SharedNoiseModel model; -static const LieScalar Zero(0); // SETDEBUG("ISAM2 update", true); // SETDEBUG("ISAM2 update verbose", true); @@ -47,9 +47,11 @@ SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, ISAM2 createSlamlikeISAM2( boost::optional init_values = boost::none, boost::optional full_graph = boost::none, - const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true), + const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, + 0, false, true, + ISAM2Params::CHOLESKY, true, + DefaultKeyFormatter, true), size_t maxPoses = 10) { - // These variables will be reused and accumulate factors and values ISAM2 isam(params); Values fullinit; @@ -282,34 +284,6 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent; } -/* ************************************************************************* */ -TEST(ISAM2, AddFactorsStep1) -{ - NonlinearFactorGraph nonlinearFactors; - nonlinearFactors += PriorFactor(10, Zero, model); - nonlinearFactors += NonlinearFactor::shared_ptr(); - nonlinearFactors += PriorFactor(11, Zero, model); - - NonlinearFactorGraph newFactors; - newFactors += PriorFactor(1, Zero, model); - newFactors += PriorFactor(2, Zero, model); - - NonlinearFactorGraph expectedNonlinearFactors; - expectedNonlinearFactors += PriorFactor(10, Zero, model); - expectedNonlinearFactors += PriorFactor(1, Zero, model); - expectedNonlinearFactors += PriorFactor(11, Zero, model); - expectedNonlinearFactors += PriorFactor(2, Zero, model); - - const FactorIndices expectedNewFactorIndices = list_of(1)(3); - - FactorIndices actualNewFactorIndices; - - ISAM2::Impl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices); - - EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); - EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices)); -} - /* ************************************************************************* */ TEST(ISAM2, simple) { @@ -692,25 +666,24 @@ namespace { } /* ************************************************************************* */ -TEST(ISAM2, marginalizeLeaves1) -{ +TEST(ISAM2, marginalizeLeaves1) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, Zero, model); + factors += PriorFactor(0, 0.0, model); - factors += BetweenFactor(0, 1, Zero, model); - factors += BetweenFactor(1, 2, Zero, model); - factors += BetweenFactor(0, 2, Zero, model); + factors += BetweenFactor(0, 1, 0.0, model); + factors += BetweenFactor(1, 2, 0.0, model); + factors += BetweenFactor(0, 2, 0.0, model); Values values; - values.insert(0, Zero); - values.insert(1, Zero); - values.insert(2, Zero); + values.insert(0, 0.0); + values.insert(1, 0.0); + values.insert(2, 0.0); - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0, 0)); + constrainedKeys.insert(make_pair(1, 1)); + constrainedKeys.insert(make_pair(2, 2)); isam.update(factors, values, FactorIndices(), constrainedKeys); @@ -719,29 +692,28 @@ TEST(ISAM2, marginalizeLeaves1) } /* ************************************************************************* */ -TEST(ISAM2, marginalizeLeaves2) -{ +TEST(ISAM2, marginalizeLeaves2) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, Zero, model); + factors += PriorFactor(0, 0.0, model); - factors += BetweenFactor(0, 1, Zero, model); - factors += BetweenFactor(1, 2, Zero, model); - factors += BetweenFactor(0, 2, Zero, model); - factors += BetweenFactor(2, 3, Zero, model); + factors += BetweenFactor(0, 1, 0.0, model); + factors += BetweenFactor(1, 2, 0.0, model); + factors += BetweenFactor(0, 2, 0.0, model); + factors += BetweenFactor(2, 3, 0.0, model); Values values; - values.insert(0, Zero); - values.insert(1, Zero); - values.insert(2, Zero); - values.insert(3, Zero); + values.insert(0, 0.0); + values.insert(1, 0.0); + values.insert(2, 0.0); + values.insert(3, 0.0); - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); - constrainedKeys.insert(make_pair(3,3)); + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0, 0)); + constrainedKeys.insert(make_pair(1, 1)); + constrainedKeys.insert(make_pair(2, 2)); + constrainedKeys.insert(make_pair(3, 3)); isam.update(factors, values, FactorIndices(), constrainedKeys); @@ -750,38 +722,37 @@ TEST(ISAM2, marginalizeLeaves2) } /* ************************************************************************* */ -TEST(ISAM2, marginalizeLeaves3) -{ +TEST(ISAM2, marginalizeLeaves3) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, Zero, model); + factors += PriorFactor(0, 0.0, model); - factors += BetweenFactor(0, 1, Zero, model); - factors += BetweenFactor(1, 2, Zero, model); - factors += BetweenFactor(0, 2, Zero, model); + factors += BetweenFactor(0, 1, 0.0, model); + factors += BetweenFactor(1, 2, 0.0, model); + factors += BetweenFactor(0, 2, 0.0, model); - factors += BetweenFactor(2, 3, Zero, model); + factors += BetweenFactor(2, 3, 0.0, model); - factors += BetweenFactor(3, 4, Zero, model); - factors += BetweenFactor(4, 5, Zero, model); - factors += BetweenFactor(3, 5, Zero, model); + factors += BetweenFactor(3, 4, 0.0, model); + factors += BetweenFactor(4, 5, 0.0, model); + factors += BetweenFactor(3, 5, 0.0, model); Values values; - values.insert(0, Zero); - values.insert(1, Zero); - values.insert(2, Zero); - values.insert(3, Zero); - values.insert(4, Zero); - values.insert(5, Zero); - - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); - constrainedKeys.insert(make_pair(3,3)); - constrainedKeys.insert(make_pair(4,4)); - constrainedKeys.insert(make_pair(5,5)); + values.insert(0, 0.0); + values.insert(1, 0.0); + values.insert(2, 0.0); + values.insert(3, 0.0); + values.insert(4, 0.0); + values.insert(5, 0.0); + + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0, 0)); + constrainedKeys.insert(make_pair(1, 1)); + constrainedKeys.insert(make_pair(2, 2)); + constrainedKeys.insert(make_pair(3, 3)); + constrainedKeys.insert(make_pair(4, 4)); + constrainedKeys.insert(make_pair(5, 5)); isam.update(factors, values, FactorIndices(), constrainedKeys); @@ -790,24 +761,23 @@ TEST(ISAM2, marginalizeLeaves3) } /* ************************************************************************* */ -TEST(ISAM2, marginalizeLeaves4) -{ +TEST(ISAM2, marginalizeLeaves4) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, Zero, model); - factors += BetweenFactor(0, 2, Zero, model); - factors += BetweenFactor(1, 2, Zero, model); + factors += PriorFactor(0, 0.0, model); + factors += BetweenFactor(0, 2, 0.0, model); + factors += BetweenFactor(1, 2, 0.0, model); Values values; - values.insert(0, Zero); - values.insert(1, Zero); - values.insert(2, Zero); - - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); + values.insert(0, 0.0); + values.insert(1, 0.0); + values.insert(2, 0.0); + + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0, 0)); + constrainedKeys.insert(make_pair(1, 1)); + constrainedKeys.insert(make_pair(2, 2)); isam.update(factors, values, FactorIndices(), constrainedKeys); diff --git a/timing/timeGaussianFactorGraph.cpp b/timing/timeGaussianFactorGraph.cpp index 1efdb95427..3258edb493 100644 --- a/timing/timeGaussianFactorGraph.cpp +++ b/timing/timeGaussianFactorGraph.cpp @@ -153,13 +153,13 @@ TEST(timeGaussianFactorGraph, linearTime) // Switch to 100*100 grid = 10K poses // 1879: 15.6498 15.3851 15.5279 -int size = 100; +int grid_size = 100; /* ************************************************************************* */ TEST(timeGaussianFactorGraph, planar_old) { cout << "Timing planar - original version" << endl; - double time = timePlanarSmoother(size); + double time = timePlanarSmoother(grid_size); cout << "timeGaussianFactorGraph : " << time << endl; //DOUBLES_EQUAL(5.97,time,0.1); } @@ -168,7 +168,7 @@ TEST(timeGaussianFactorGraph, planar_old) TEST(timeGaussianFactorGraph, planar_new) { cout << "Timing planar - new version" << endl; - double time = timePlanarSmoother(size, false); + double time = timePlanarSmoother(grid_size, false); cout << "timeGaussianFactorGraph : " << time << endl; //DOUBLES_EQUAL(5.97,time,0.1); } @@ -177,7 +177,7 @@ TEST(timeGaussianFactorGraph, planar_new) TEST(timeGaussianFactorGraph, planar_eliminate_old) { cout << "Timing planar Eliminate - original version" << endl; - double time = timePlanarSmootherEliminate(size); + double time = timePlanarSmootherEliminate(grid_size); cout << "timeGaussianFactorGraph : " << time << endl; //DOUBLES_EQUAL(5.97,time,0.1); } @@ -186,7 +186,7 @@ TEST(timeGaussianFactorGraph, planar_eliminate_old) TEST(timeGaussianFactorGraph, planar_eliminate_new) { cout << "Timing planar Eliminate - new version" << endl; - double time = timePlanarSmootherEliminate(size, false); + double time = timePlanarSmootherEliminate(grid_size, false); cout << "timeGaussianFactorGraph : " << time << endl; //DOUBLES_EQUAL(5.97,time,0.1); } diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 3daa87c7c1..6e046e3ab7 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -26,6 +26,9 @@ if (NOT GTSAM_WRAP_SERIALIZATION) target_compile_definitions(wrap_lib PUBLIC -DWRAP_DISABLE_SERIALIZE) endif() +# Apply build flags: +gtsam_apply_build_flags(wrap_lib) + target_link_libraries(wrap_lib ${WRAP_BOOST_LIBRARIES}) target_include_directories(wrap_lib PUBLIC ${Boost_INCLUDE_DIR}) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 5c1e6187ed..65ce9eab7c 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -342,17 +342,21 @@ vector Class::expandTemplate(Str templateArg, /* ************************************************************************* */ void Class::addMethod(bool verbose, bool is_const, Str methodName, - const ArgumentList& argumentList, const ReturnValue& returnValue, - const Template& tmplate) { + const ArgumentList& argumentList, + const ReturnValue& returnValue, const Template& tmplate) { // Check if templated if (tmplate.valid()) { - templateMethods_[methodName].addOverload(methodName, argumentList, - returnValue, is_const, - tmplate.argName(), verbose); + try { + templateMethods_[methodName].addOverload(methodName, argumentList, + returnValue, is_const, + tmplate.argName(), verbose); + } catch (const std::runtime_error& e) { + throw std::runtime_error("Class::addMethod: error adding " + name_ + + "::" + methodName + "\n" + e.what()); + } // Create method to expand // For all values of the template argument, create a new method - for(const Qualified& instName: tmplate.argValues()) { - + for (const Qualified& instName : tmplate.argValues()) { const TemplateSubstitution ts(tmplate.argName(), instName, *this); // substitute template in arguments ArgumentList expandedArgs = argumentList.expandTemplate(ts); @@ -361,15 +365,27 @@ void Class::addMethod(bool verbose, bool is_const, Str methodName, // Now stick in new overload stack with expandedMethodName key // but note we use the same, unexpanded methodName in overload string expandedMethodName = methodName + instName.name(); - methods_[expandedMethodName].addOverload(methodName, expandedArgs, - expandedRetVal, is_const, instName, verbose); + try { + methods_[expandedMethodName].addOverload(methodName, expandedArgs, + expandedRetVal, is_const, + instName, verbose); + } catch (const std::runtime_error& e) { + throw std::runtime_error("Class::addMethod: error adding " + name_ + + "::" + expandedMethodName + "\n" + e.what()); + } } } else { - // just add overload - methods_[methodName].addOverload(methodName, argumentList, returnValue, - is_const, boost::none, verbose); - nontemplateMethods_[methodName].addOverload(methodName, argumentList, returnValue, - is_const, boost::none, verbose); + try { + // just add overload + methods_[methodName].addOverload(methodName, argumentList, returnValue, + is_const, boost::none, verbose); + nontemplateMethods_[methodName].addOverload(methodName, argumentList, + returnValue, is_const, + boost::none, verbose); + } catch (const std::runtime_error& e) { + throw std::runtime_error("Class::addMethod: error adding " + name_ + + "::" + methodName + "\n" + e.what()); + } } } diff --git a/wrap/FileWriter.cpp b/wrap/FileWriter.cpp index 2c3843b372..c07de0eb01 100644 --- a/wrap/FileWriter.cpp +++ b/wrap/FileWriter.cpp @@ -29,7 +29,7 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const { bool file_exists = true; try { existing_contents = file_contents(filename_.c_str(), add_header); - } catch (const CantOpenFile& e) { + } catch (const CantOpenFile& ) { file_exists = false; } diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 13616d64ad..02ab196572 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -143,6 +143,7 @@ void GlobalFunction::emit_cython_pxd(FileWriter& file) const { "\"("; argumentList(i).emit_cython_pxd(file, "", vector()); file.oss << ")"; + file.oss << " except +"; file.oss << "\n"; } } @@ -206,16 +207,18 @@ void GlobalFunction::emit_cython_pyx(FileWriter& file) const { /// Call corresponding cython function file.oss << argumentList(i).pyx_convertEigenTypeAndStorageOrder(" "); + // catch exception which indicates the parameters passed are incorrect. + file.oss << " except:\n"; + file.oss << " return False, None\n\n"; + string call = pyx_functionCall("", pxdName(), i); if (!returnVals_[i].isVoid()) { - file.oss << " return_value = " << call << "\n"; - file.oss << " return True, " << returnVals_[i].pyx_casting("return_value") << "\n"; + file.oss << " return_value = " << call << "\n"; + file.oss << " return True, " << returnVals_[i].pyx_casting("return_value") << "\n"; } else { - file.oss << " " << call << "\n"; - file.oss << " return True, None\n"; + file.oss << " " << call << "\n"; + file.oss << " return True, None\n"; } - file.oss << " except:\n"; - file.oss << " return False, None\n\n"; } } /* ************************************************************************* */ diff --git a/wrap/Method.cpp b/wrap/Method.cpp index f7247341ce..2a4b0b3afd 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -38,12 +38,12 @@ bool Method::addOverload(Str name, const ArgumentList& args, is_const_ = is_const; else if (is_const && !is_const_) throw std::runtime_error( - "Method::addOverload now designated as const whereas before it was " - "not"); + "Method::addOverload: " + name + + " now designated as const whereas before it was not"); else if (!is_const && is_const_) throw std::runtime_error( - "Method::addOverload now designated as non-const whereas before it " - "was"); + "Method::addOverload: " + name + + " now designated as non-const whereas before it was"); return first; } diff --git a/wrap/README b/wrap/README.md similarity index 71% rename from wrap/README rename to wrap/README.md index 92fd1967cf..014577b5a9 100644 --- a/wrap/README +++ b/wrap/README.md @@ -1,25 +1,21 @@ -Frank Dellaert -October 2011 +# WRAP README The wrap library wraps the GTSAM library into a MATLAB toolbox. -It was designed to be more general than just wrapping GTSAM, but a small amount of -GTSAM specific code exists in matlab.h, the include file that is included by the -mex files. The GTSAM-specific functionality consists primarily of handling of -Eigen Matrix and Vector classes. +It was designed to be more general than just wrapping GTSAM, but a small amount of GTSAM specific code exists in matlab.h, the include file that is included by the mex files. The GTSAM-specific functionality consists primarily of handling of Eigen Matrix and Vector classes. -For notes on creating a wrap interface, see gtsam.h for what features can be -wrapped into a toolbox, as well as the current state of the toolbox for gtsam. -For more technical details on the interface, please read comments in matlab.h +For notes on creating a wrap interface, see gtsam.h for what features can be wrapped into a toolbox, as well as the current state of the toolbox for gtsam. For more technical details on the interface, please read comments in matlab.h Some good things to know: OBJECT CREATION + - Classes are created by special constructors, e.g., new_GaussianFactorGraph_.cpp. These constructors are called from the MATLAB class @GaussianFactorGraph. new_GaussianFactorGraph_ calls wrap_constructed in matlab.h, see documentation there METHOD (AND CONSTRUCTOR) ARGUMENTS + - Simple argument types of methods, such as "double", will be converted in the mex wrappers by calling unwrap, defined in matlab.h - Vector and Matrix arguments are normally passed by reference in GTSAM, but diff --git a/wrap/tests/expected-cython/geometry.pxd b/wrap/tests/expected-cython/geometry.pxd index f2cd513e21..0d9adac5fa 100644 --- a/wrap/tests/expected-cython/geometry.pxd +++ b/wrap/tests/expected-cython/geometry.pxd @@ -145,7 +145,7 @@ cdef extern from "folder/path/to/Test.h": cdef extern from "folder/path/to/Test.h" namespace "": - VectorXd pxd_aGlobalFunction "aGlobalFunction"() + VectorXd pxd_aGlobalFunction "aGlobalFunction"() except + cdef extern from "folder/path/to/Test.h" namespace "": - VectorXd pxd_overloadedGlobalFunction "overloadedGlobalFunction"(int a) - VectorXd pxd_overloadedGlobalFunction "overloadedGlobalFunction"(int a, double b) + VectorXd pxd_overloadedGlobalFunction "overloadedGlobalFunction"(int a) except + + VectorXd pxd_overloadedGlobalFunction "overloadedGlobalFunction"(int a, double b) except + diff --git a/wrap/tests/expected-cython/geometry.pyx b/wrap/tests/expected-cython/geometry.pyx index cae19d600d..606fde3e34 100644 --- a/wrap/tests/expected-cython/geometry.pyx +++ b/wrap/tests/expected-cython/geometry.pyx @@ -464,11 +464,11 @@ def overloadedGlobalFunction_0(args, kwargs): try: __params = process_args(['a'], args, kwargs) a = (__params[0]) - return_value = pxd_overloadedGlobalFunction(a) - return True, ndarray_copy(return_value).squeeze() except: return False, None + return_value = pxd_overloadedGlobalFunction(a) + return True, ndarray_copy(return_value).squeeze() def overloadedGlobalFunction_1(args, kwargs): cdef list __params cdef VectorXd return_value @@ -476,8 +476,8 @@ def overloadedGlobalFunction_1(args, kwargs): __params = process_args(['a', 'b'], args, kwargs) a = (__params[0]) b = (__params[1]) - return_value = pxd_overloadedGlobalFunction(a, b) - return True, ndarray_copy(return_value).squeeze() except: return False, None + return_value = pxd_overloadedGlobalFunction(a, b) + return True, ndarray_copy(return_value).squeeze()