From bf40956592ef0e3e1d3c15fe74270857c9d01c3c Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 3 Oct 2013 19:51:56 +0000 Subject: [PATCH] Merged from Hmf6_unordered 970392249a8c50153f24594c9c81acb740cedd06 --- CMakeLists.txt | 700 +++++++++--------- doc/Doxyfile.in | 2 +- gtsam/linear/NoiseModel.cpp | 26 + gtsam/linear/NoiseModel.h | 25 + ...ConcurrentFilteringAndSmoothingExample.cpp | 4 +- .../nonlinear/ConcurrentBatchFilter.cpp | 13 +- .../nonlinear/ConcurrentBatchFilter.h | 8 +- .../nonlinear/ConcurrentBatchSmoother.cpp | 6 +- .../nonlinear/ConcurrentBatchSmoother.h | 3 +- .../nonlinear/ConcurrentIncrementalFilter.cpp | 3 +- .../nonlinear/ConcurrentIncrementalFilter.h | 8 +- .../ConcurrentIncrementalSmoother.cpp | 13 +- .../nonlinear/ConcurrentIncrementalSmoother.h | 3 +- .../tests/testConcurrentBatchFilter.cpp | 255 +++++++ .../tests/testConcurrentBatchSmoother.cpp | 215 ++++++ .../tests/testConcurrentIncrementalFilter.cpp | 243 ++++++ .../testConcurrentIncrementalSmootherDL.cpp | 1 + .../testConcurrentIncrementalSmootherGN.cpp | 222 ++++++ gtsam_unstable/slam/CombinedImuFactor.h | 673 +++++++++++++++++ .../slam/EquivInertialNavFactor_GlobalVel.h | 9 +- gtsam_unstable/slam/ImuFactor.h | 565 ++++++++++++++ 21 files changed, 2633 insertions(+), 364 deletions(-) create mode 100644 gtsam_unstable/slam/CombinedImuFactor.h create mode 100644 gtsam_unstable/slam/ImuFactor.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 27bed41e6..5b10938ff 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,350 +1,350 @@ - -project(GTSAM CXX C) -cmake_minimum_required(VERSION 2.6) - -# Set the version number for the library -set (GTSAM_VERSION_MAJOR 3) -set (GTSAM_VERSION_MINOR 0) -set (GTSAM_VERSION_PATCH 0) -math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") -set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") - -############################################################################### -# Gather information, perform checks, set defaults - -# Set the default install path to home -#set (CMAKE_INSTALL_PREFIX ${HOME} CACHE PATH "Install prefix for library") - -set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake") -include(GtsamMakeConfigFile) - -# Record the root dir for gtsam - needed during external builds, e.g., ROS -set(GTSAM_SOURCE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}) -message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]") - -# Load build type flags and default to Debug mode -include(GtsamBuildTypes) - -# Use macros for creating tests/timing scripts -include(GtsamTesting) -include(GtsamPrinting) - -# guard against in-source builds -if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) - message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") -endif() - -# See whether gtsam_unstable is available (it will be present only if we're using an SVN checkout) -if(EXISTS "${PROJECT_SOURCE_DIR}/gtsam_unstable" AND IS_DIRECTORY "${PROJECT_SOURCE_DIR}/gtsam_unstable") - set(GTSAM_UNSTABLE_AVAILABLE 1) -else() - set(GTSAM_UNSTABLE_AVAILABLE 0) -endif() - - -############################################################################### -# Set up options - -# Configurable Options -option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON) -option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" OFF) # These do not currently work -option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples" ON) -if(GTSAM_UNSTABLE_AVAILABLE) - option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) -endif() -option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON) -option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" OFF) -option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) -option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF) -option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF) -option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) -option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) - -# Options relating to MATLAB wrapper -# TODO: Check for matlab mex binary before handling building of binaries -option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) -option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON) -option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility for wrapping other libraries" ON) - -# Check / set dependent variables for MATLAB wrapper -set(GTSAM_WRAP_HEADER_PATH "${PROJECT_SOURCE_DIR}/wrap") -if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT GTSAM_BUILD_WRAP) - message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP") -endif() -if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP) - message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP") -endif() - -# Flags for choosing default packaging tools -set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator") -set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") - -# Sanity check building of libraries -if (NOT GTSAM_BUILD_SHARED_LIBRARY AND NOT GTSAM_BUILD_STATIC_LIBRARY) - message(FATAL_ERROR "Both shared and static version of GTSAM library disabled - need to choose at least one!") -endif() - -# Flags to determine whether tests and examples are build during 'make install' -# Note that these remove the targets from the 'all' -option(GTSAM_DISABLE_TESTS_ON_INSTALL "Disables building tests during install" ON) -option(GTSAM_DISABLE_EXAMPLES_ON_INSTALL "Disables building examples during install" OFF) - -# Pull in infrastructure -if (GTSAM_BUILD_TESTS) - enable_testing() - include(Dart) - include(CTest) -endif() - -############################################################################### -# Find boost - -# To change the path for boost, you will need to set: -# BOOST_ROOT: path to install prefix for boost -# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT - -# If using Boost shared libs, disable auto linking -if(MSVC) - # Some libraries, at least Boost Program Options, rely on this to export DLL symbols - add_definitions(-DBOOST_ALL_DYN_LINK) - # Disable autolinking - if(NOT Boost_USE_STATIC_LIBS) - add_definitions(-DBOOST_ALL_NO_LIB) - endif() -endif() - -find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono) - -# Required components -if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR - NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY OR NOT Boost_REGEX_LIBRARY) - message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.") -endif() - -option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) -# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) -set(GTSAM_BOOST_LIBRARIES - ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} - ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY}) -if (GTSAM_DISABLE_NEW_TIMERS) - message("WARNING: GTSAM timing instrumentation manually disabled") - add_definitions(-DGTSAM_DISABLE_NEW_TIMERS) -else() - if(Boost_TIMER_LIBRARY) - list(APPEND GTSAM_BOOST_LIBRARIES ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY}) - else() - message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.") - endif() -endif() - - -############################################################################### -# Find TBB -find_package(TBB) - -# Set up variables if we're using TBB -if(TBB_FOUND AND GTSAM_WITH_TBB) - set(GTSAM_USE_TBB 1) # This will go into config.h - set(GTSAM_TBB_LIBRARIES "") - if(TBB_DEBUG_LIBRARIES) - foreach(lib ${TBB_LIBRARIES}) - list(APPEND GTSAM_TBB_LIBRARIES optimized "${lib}") - endforeach() - foreach(lib ${TBB_DEBUG_LIBRARIES}) - list(APPEND GTSAM_TBB_LIBRARIES debug "${lib}") - endforeach() - else() - set(GTSAM_TBB_LIBRARIES ${TBB_LIBRARIES}) - endif() -endif() - - -############################################################################### -# Option for using system Eigen or GTSAM-bundled Eigen -option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) - -# Switch for using system Eigen or GTSAM-bundled Eigen -if(GTSAM_USE_SYSTEM_EIGEN) - # Use generic Eigen include paths e.g. - set(GTSAM_EIGEN_INCLUDE_PREFIX "") - - find_package(Eigen3 REQUIRED) - include_directories(AFTER "${EIGEN3_INCLUDE_DIR}") -else() - # Use bundled Eigen include paths e.g. - set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/") - - # Clear any variables set by FindEigen3 - if(EIGEN3_INCLUDE_DIR) - set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE) - endif() -endif() - -# Write Eigen include file with the paths for either the system Eigen or the GTSAM-bundled Eigen -configure_file(gtsam/3rdparty/gtsam_eigen_includes.h.in gtsam/3rdparty/gtsam_eigen_includes.h) - -# Install the configuration file for Eigen -install(FILES ${CMAKE_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINATION include/gtsam/3rdparty) - - -############################################################################### -# Global compile options - -# Include boost - use 'BEFORE' so that a specific boost specified to CMake -# takes precedence over a system-installed one. -include_directories(BEFORE ${TBB_INCLUDE_DIRS} ${Boost_INCLUDE_DIR}) - -# Add includes for source directories 'BEFORE' boost and any system include -# paths so that the compiler uses GTSAM headers in our source directory instead -# of any previously installed GTSAM headers. -include_directories(BEFORE - gtsam/3rdparty/UFconfig - gtsam/3rdparty/CCOLAMD/Include - ${CMAKE_SOURCE_DIR} - ${CMAKE_BINARY_DIR} # So we can include generated config header files - CppUnitLite) - -if(MSVC) - add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS) - add_definitions(/wd4251 /wd4275 /wd4251 /wd4661) # Disable non-DLL-exported base class and other warnings -endif() - -if(GTSAM_ENABLE_CONSISTENCY_CHECKS) - add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS) -endif() - -############################################################################### -# Add components - -# Set default library - static or shared, before adding subdirectories -if(GTSAM_BUILD_SHARED_LIBRARY) - set(gtsam-default gtsam-shared) - if(GTSAM_BUILD_UNSTABLE) - set(gtsam_unstable-default gtsam_unstable-shared) - endif() -else() - set(gtsam-default gtsam-static) - if(GTSAM_BUILD_UNSTABLE) - set(gtsam_unstable-default gtsam_unstable-static) - endif() -endif() - -# Build CppUnitLite -add_subdirectory(CppUnitLite) - -# Build wrap -if (GTSAM_BUILD_WRAP) - add_subdirectory(wrap) -endif(GTSAM_BUILD_WRAP) - -# Build GTSAM library -add_subdirectory(gtsam) - -# Build Tests -add_subdirectory(tests) - -# Build examples -if (GTSAM_BUILD_EXAMPLES) - add_subdirectory(examples) -endif(GTSAM_BUILD_EXAMPLES) - -# Matlab toolbox -if (GTSAM_INSTALL_MATLAB_TOOLBOX) - add_subdirectory(matlab) -endif() - -# Build gtsam_unstable -if (GTSAM_BUILD_UNSTABLE) - add_subdirectory(gtsam_unstable) -endif(GTSAM_BUILD_UNSTABLE) - -# Install config and export files -GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") -export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake) - -# Check for doxygen availability - optional dependency -find_package(Doxygen) - -# Doxygen documentation - enabling options in subfolder -if (DOXYGEN_FOUND) - add_subdirectory(doc) -endif() - - -############################################################################### -# Set up CPack -set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") -set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") -set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu") -set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README") -set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") -set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) -set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR}) -set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH}) -set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}") -#set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory -#set(CPACK_INSTALLED_DIRECTORIES ".") # FIXME: throws error -set(CPACK_SOURCE_IGNORE_FILES "/build*;/\\\\.;/makestats.sh$") -set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/") -set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/package_scripts/") -set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") -#set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs - -# Deb-package specific cpack -set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev") -set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") - - -############################################################################### -# Print configuration variables -message(STATUS "===============================================================") -message(STATUS "================ Configuration Options ======================") -message(STATUS "Build flags ") -print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ") -print_config_flag(${GTSAM_BUILD_EXAMPLES} "Build Examples ") -print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ") -if (DOXYGEN_FOUND) - print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ") -endif() -print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ") -print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ") -print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name ") -if(GTSAM_UNSTABLE_AVAILABLE) - print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ") -endif() -print_config_flag(${GTSAM_DISABLE_TESTS_ON_INSTALL} "No tests in all or install ") -print_config_flag(${GTSAM_DISABLE_EXAMPLES_ON_INSTALL} "No examples in all or install ") -string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper) -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}}") -if(GTSAM_USE_TBB) - message(STATUS " Use Intel TBB : Yes") -elseif(TBB_FOUND) - message(STATUS " Use Intel TBB : TBB found but GTSAM_WITH_TBB is disabled") -else() - message(STATUS " Use Intel TBB : TBB not found") -endif() - - -message(STATUS "Packaging flags ") -message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}") -message(STATUS " CPack Generator : ${CPACK_GENERATOR}") - -message(STATUS "GTSAM flags ") -print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") -print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") -print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") - -message(STATUS "MATLAB toolbox flags ") -print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") -print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") -print_config_flag(${GTSAM_INSTALL_WRAP} "Install wrap utility ") -message(STATUS "===============================================================") - -if(GTSAM_WITH_TBB AND NOT TBB_FOUND) - message(WARNING "GTSAM_USE_TBB is set to 'On' but TBB was not found. This is ok, but GTSAM parallelization will be disabled. Set GTSAM_USE_TBB to 'Off' to avoid this warning.") -endif() - -# Include CPack *after* all flags -include(CPack) + +project(GTSAM CXX C) +cmake_minimum_required(VERSION 2.6) + +# Set the version number for the library +set (GTSAM_VERSION_MAJOR 3) +set (GTSAM_VERSION_MINOR 0) +set (GTSAM_VERSION_PATCH 0) +math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") +set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") + +############################################################################### +# Gather information, perform checks, set defaults + +# Set the default install path to home +#set (CMAKE_INSTALL_PREFIX ${HOME} CACHE PATH "Install prefix for library") + +set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake") +include(GtsamMakeConfigFile) + +# Record the root dir for gtsam - needed during external builds, e.g., ROS +set(GTSAM_SOURCE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}) +message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]") + +# Load build type flags and default to Debug mode +include(GtsamBuildTypes) + +# Use macros for creating tests/timing scripts +include(GtsamTesting) +include(GtsamPrinting) + +# guard against in-source builds +if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) + message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") +endif() + +# See whether gtsam_unstable is available (it will be present only if we're using an SVN checkout) +if(EXISTS "${PROJECT_SOURCE_DIR}/gtsam_unstable" AND IS_DIRECTORY "${PROJECT_SOURCE_DIR}/gtsam_unstable") + set(GTSAM_UNSTABLE_AVAILABLE 1) +else() + set(GTSAM_UNSTABLE_AVAILABLE 0) +endif() + + +############################################################################### +# Set up options + +# Configurable Options +option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON) +option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" OFF) # These do not currently work +option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples" ON) +if(GTSAM_UNSTABLE_AVAILABLE) + option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) +endif() +option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON) +option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" OFF) +option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) +option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF) +option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF) +option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) +option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) + +# Options relating to MATLAB wrapper +# TODO: Check for matlab mex binary before handling building of binaries +option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) +option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON) +option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility for wrapping other libraries" ON) + +# Check / set dependent variables for MATLAB wrapper +set(GTSAM_WRAP_HEADER_PATH "${PROJECT_SOURCE_DIR}/wrap") +if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT GTSAM_BUILD_WRAP) + message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP") +endif() +if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP) + message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP") +endif() + +# Flags for choosing default packaging tools +set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator") +set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") + +# Sanity check building of libraries +if (NOT GTSAM_BUILD_SHARED_LIBRARY AND NOT GTSAM_BUILD_STATIC_LIBRARY) + message(FATAL_ERROR "Both shared and static version of GTSAM library disabled - need to choose at least one!") +endif() + +# Flags to determine whether tests and examples are build during 'make install' +# Note that these remove the targets from the 'all' +option(GTSAM_DISABLE_TESTS_ON_INSTALL "Disables building tests during install" ON) +option(GTSAM_DISABLE_EXAMPLES_ON_INSTALL "Disables building examples during install" OFF) + +# Pull in infrastructure +if (GTSAM_BUILD_TESTS) + enable_testing() + include(Dart) + include(CTest) +endif() + +############################################################################### +# Find boost + +# To change the path for boost, you will need to set: +# BOOST_ROOT: path to install prefix for boost +# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT + +# If using Boost shared libs, disable auto linking +if(MSVC) + # Some libraries, at least Boost Program Options, rely on this to export DLL symbols + add_definitions(-DBOOST_ALL_DYN_LINK) + # Disable autolinking + if(NOT Boost_USE_STATIC_LIBS) + add_definitions(-DBOOST_ALL_NO_LIB) + endif() +endif() + +find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono) + +# Required components +if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR + NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY OR NOT Boost_REGEX_LIBRARY) + message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.") +endif() + +option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) +# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) +set(GTSAM_BOOST_LIBRARIES + ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} + ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY}) +if (GTSAM_DISABLE_NEW_TIMERS) + message("WARNING: GTSAM timing instrumentation manually disabled") + add_definitions(-DGTSAM_DISABLE_NEW_TIMERS) +else() + if(Boost_TIMER_LIBRARY) + list(APPEND GTSAM_BOOST_LIBRARIES ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY}) + else() + message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.") + endif() +endif() + + +############################################################################### +# Find TBB +find_package(TBB) + +# Set up variables if we're using TBB +if(TBB_FOUND AND GTSAM_WITH_TBB) + set(GTSAM_USE_TBB 1) # This will go into config.h + set(GTSAM_TBB_LIBRARIES "") + if(TBB_DEBUG_LIBRARIES) + foreach(lib ${TBB_LIBRARIES}) + list(APPEND GTSAM_TBB_LIBRARIES optimized "${lib}") + endforeach() + foreach(lib ${TBB_DEBUG_LIBRARIES}) + list(APPEND GTSAM_TBB_LIBRARIES debug "${lib}") + endforeach() + else() + set(GTSAM_TBB_LIBRARIES ${TBB_LIBRARIES}) + endif() +endif() + + +############################################################################### +# Option for using system Eigen or GTSAM-bundled Eigen +option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) + +# Switch for using system Eigen or GTSAM-bundled Eigen +if(GTSAM_USE_SYSTEM_EIGEN) + # Use generic Eigen include paths e.g. + set(GTSAM_EIGEN_INCLUDE_PREFIX "") + + find_package(Eigen3 REQUIRED) + include_directories(AFTER "${EIGEN3_INCLUDE_DIR}") +else() + # Use bundled Eigen include paths e.g. + set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/") + + # Clear any variables set by FindEigen3 + if(EIGEN3_INCLUDE_DIR) + set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE) + endif() +endif() + +# Write Eigen include file with the paths for either the system Eigen or the GTSAM-bundled Eigen +configure_file(gtsam/3rdparty/gtsam_eigen_includes.h.in gtsam/3rdparty/gtsam_eigen_includes.h) + +# Install the configuration file for Eigen +install(FILES ${CMAKE_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINATION include/gtsam/3rdparty) + + +############################################################################### +# Global compile options + +# Include boost - use 'BEFORE' so that a specific boost specified to CMake +# takes precedence over a system-installed one. +include_directories(BEFORE ${TBB_INCLUDE_DIRS} ${Boost_INCLUDE_DIR}) + +# Add includes for source directories 'BEFORE' boost and any system include +# paths so that the compiler uses GTSAM headers in our source directory instead +# of any previously installed GTSAM headers. +include_directories(BEFORE + gtsam/3rdparty/UFconfig + gtsam/3rdparty/CCOLAMD/Include + ${CMAKE_SOURCE_DIR} + ${CMAKE_BINARY_DIR} # So we can include generated config header files + CppUnitLite) + +if(MSVC) + add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS) + add_definitions(/wd4251 /wd4275 /wd4251 /wd4661) # Disable non-DLL-exported base class and other warnings +endif() + +if(GTSAM_ENABLE_CONSISTENCY_CHECKS) + add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS) +endif() + +############################################################################### +# Add components + +# Set default library - static or shared, before adding subdirectories +if(GTSAM_BUILD_SHARED_LIBRARY) + set(gtsam-default gtsam-shared) + if(GTSAM_BUILD_UNSTABLE) + set(gtsam_unstable-default gtsam_unstable-shared) + endif() +else() + set(gtsam-default gtsam-static) + if(GTSAM_BUILD_UNSTABLE) + set(gtsam_unstable-default gtsam_unstable-static) + endif() +endif() + +# Build CppUnitLite +add_subdirectory(CppUnitLite) + +# Build wrap +if (GTSAM_BUILD_WRAP) + add_subdirectory(wrap) +endif(GTSAM_BUILD_WRAP) + +# Build GTSAM library +add_subdirectory(gtsam) + +# Build Tests +add_subdirectory(tests) + +# Build examples +if (GTSAM_BUILD_EXAMPLES) + add_subdirectory(examples) +endif(GTSAM_BUILD_EXAMPLES) + +# Matlab toolbox +if (GTSAM_INSTALL_MATLAB_TOOLBOX) + add_subdirectory(matlab) +endif() + +# Build gtsam_unstable +if (GTSAM_BUILD_UNSTABLE) + add_subdirectory(gtsam_unstable) +endif(GTSAM_BUILD_UNSTABLE) + +# Install config and export files +GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") +export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake) + +# Check for doxygen availability - optional dependency +find_package(Doxygen) + +# Doxygen documentation - enabling options in subfolder +if (DOXYGEN_FOUND) + add_subdirectory(doc) +endif() + + +############################################################################### +# Set up CPack +set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") +set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") +set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu") +set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README") +set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") +set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) +set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR}) +set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH}) +set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}") +#set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory +#set(CPACK_INSTALLED_DIRECTORIES ".") # FIXME: throws error +set(CPACK_SOURCE_IGNORE_FILES "/build*;/\\\\.;/makestats.sh$") +set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/") +set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/package_scripts/") +set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") +#set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs + +# Deb-package specific cpack +set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev") +set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") + + +############################################################################### +# Print configuration variables +message(STATUS "===============================================================") +message(STATUS "================ Configuration Options ======================") +message(STATUS "Build flags ") +print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ") +print_config_flag(${GTSAM_BUILD_EXAMPLES} "Build Examples ") +print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ") +if (DOXYGEN_FOUND) + print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ") +endif() +print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ") +print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ") +print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name ") +if(GTSAM_UNSTABLE_AVAILABLE) + print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ") +endif() +print_config_flag(${GTSAM_DISABLE_TESTS_ON_INSTALL} "No tests in all or install ") +print_config_flag(${GTSAM_DISABLE_EXAMPLES_ON_INSTALL} "No examples in all or install ") +string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper) +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}}") +if(GTSAM_USE_TBB) + message(STATUS " Use Intel TBB : Yes") +elseif(TBB_FOUND) + message(STATUS " Use Intel TBB : TBB found but GTSAM_WITH_TBB is disabled") +else() + message(STATUS " Use Intel TBB : TBB not found") +endif() + + +message(STATUS "Packaging flags ") +message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}") +message(STATUS " CPack Generator : ${CPACK_GENERATOR}") + +message(STATUS "GTSAM flags ") +print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") +print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") +print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") + +message(STATUS "MATLAB toolbox flags ") +print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") +print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") +print_config_flag(${GTSAM_INSTALL_WRAP} "Install wrap utility ") +message(STATUS "===============================================================") + +if(GTSAM_WITH_TBB AND NOT TBB_FOUND) + message(WARNING "GTSAM_USE_TBB is set to 'On' but TBB was not found. This is ok, but GTSAM parallelization will be disabled. Set GTSAM_USE_TBB to 'Off' to avoid this warning.") +endif() + +# Include CPack *after* all flags +include(CPack) diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in index f12e12217..d5c969a8a 100644 --- a/doc/Doxyfile.in +++ b/doc/Doxyfile.in @@ -1554,7 +1554,7 @@ TAGFILES = # When a file name is specified after GENERATE_TAGFILE, doxygen will create # a tag file that is based on the input files it reads. -GENERATE_TAGFILE = +GENERATE_TAGFILE = html/gtsam.tag # If the ALLEXTERNALS tag is set to YES all external classes will be listed # in the class index. If set to NO only the inherited external classes diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 56628c55d..72e1ef9de 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -709,6 +709,32 @@ Tukey::shared_ptr Tukey::Create(const double c, const ReweightScheme reweight) { return shared_ptr(new Tukey(c, reweight)); } +/* ************************************************************************* */ +// Welsh +/* ************************************************************************* */ +Welsh::Welsh(const double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double Welsh::weight(const double &error) const { + double xc2 = (error/c_)*(error/c_); + return std::exp(-xc2); +} + +void Welsh::print(const std::string &s="") const { + std::cout << s << ": Welsh (" << c_ << ")" << std::endl; +} + +bool Welsh::equals(const Base &expected, const double tol) const { + const Welsh* p = dynamic_cast(&expected); + if (p == NULL) return false; + return fabs(c_ - p->c_) < tol; +} + +Welsh::shared_ptr Welsh::Create(const double c, const ReweightScheme reweight) { + return shared_ptr(new Welsh(c, reweight)); +} + } // namespace mEstimator /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index f73360579..4e09e4dbd 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -763,6 +763,31 @@ namespace gtsam { } }; + /// Welsh implements the "Welsh" robust error model (Zhang97ivc) + class GTSAM_EXPORT Welsh : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + Welsh(const double c = 2.9846, const ReweightScheme reweight = Block); + virtual ~Welsh() {} + virtual double weight(const double &error) const ; + virtual void print(const std::string &s) const ; + virtual bool equals(const Base& expected, const double tol=1e-8) const ; + static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + + protected: + double c_; + + 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_); + } + }; + } ///\namespace mEstimator /// Base class for robust error models diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index 4e6e1a524..4b441fc5a 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -245,7 +245,7 @@ int main(int argc, char** argv) { // Now run for a few more seconds so the concurrent smoother and filter have to to re-sync // Continue adding odometry factors until the loop closure may be incorporated into the concurrent smoother - for(double time = 8.0+deltaT; time <= 10.0; time += deltaT) { + for(double time = 8.0+deltaT; time <= 15.0; time += deltaT) { // Define the keys related to this timestamp Key previousKey(1000 * (time-deltaT)); @@ -307,7 +307,7 @@ int main(int argc, char** argv) { // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds - cout << "After 10.0 seconds, each version contains to the following keys:" << endl; + cout << "After 15.0 seconds, each version contains to the following keys:" << endl; cout << " Concurrent Filter Keys: " << endl; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, concurrentFilter.getLinearizationPoint()) { cout << setprecision(5) << " Key: " << key_value.key << endl; diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 71085ad8b..4ab77c8d8 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -161,9 +161,16 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto // Add the new factors to the graph, updating the variable index result.newFactorsIndices = insertFactors(newFactors); - // Remove any user-specified factors from the graph - if(removeFactorIndices) + + if(removeFactorIndices){ + if(debug){ + BOOST_FOREACH(size_t slot, *removeFactorIndices) { + std::cout << "ConcurrentBatchFilter::update removeFactorIndices " << std::endl; + } + } removeFactors(*removeFactorIndices); + } + gttoc(augment_system); if(debug) std::cout << "ConcurrentBatchFilter::update Reordering System ..." << std::endl; @@ -667,4 +674,4 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { } /* ************************************************************************* */ -}/// namespace gtsam +}/// namespace gtsam \ No newline at end of file diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index a69ecfee0..745631fac 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -40,7 +40,13 @@ public: size_t lambdas; ///< The number of different L-M lambda factors that were tried during optimization size_t nonlinearVariables; ///< The number of variables that can be relinearized size_t linearVariables; ///< The number of variables that must keep a constant linearization point - std::vector newFactorsIndices; ///< The indices of the newly-added factors, in 1-to-1 correspondence with the factors passed in + + /** The indices of the newly-added factors, in 1-to-1 correspondence with the + * factors passed as \c newFactors update(). These indices may be + * used later to refer to the factors in order to remove them. + */ + std::vector newFactorsIndices; + double error; ///< The final factor graph error /// Constructor diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 71ae5249e..3dde8e1f6 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -46,7 +46,8 @@ bool ConcurrentBatchSmoother::equals(const ConcurrentSmoother& rhs, double tol) } /* ************************************************************************* */ -ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta) { +ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, + const boost::optional< std::vector >& removeFactorIndices) { gttic(update); @@ -69,6 +70,9 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF // Add the new factors to the graph, updating the variable index insertFactors(newFactors); + + if(removeFactorIndices) + removeFactors(*removeFactorIndices); } gttoc(augment_system); diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index 4160a88fc..d85109605 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -117,7 +117,8 @@ public: * in the smoother). There must not be any variables here that do not occur in newFactors, * and additionally, variables that were already in the system must not be included here. */ - virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values()); + virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), + const boost::optional< std::vector >& removeFactorIndices = boost::none); /** * Perform any required operations before the synchronization process starts. diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index 7963d0e00..d7c6e4ecf 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -55,8 +55,9 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No // Create the return result meta-data Result result; - // Remove any user-provided factors from iSAM2 + // We do not need to remove any factors at this time gtsam::FastVector removedFactors; + if(removeFactorIndices){ removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end()); } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index 364d30c78..1ff4eb299 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -38,7 +38,13 @@ public: size_t iterations; ///< The number of optimizer iterations performed size_t nonlinearVariables; ///< The number of variables that can be relinearized size_t linearVariables; ///< The number of variables that must keep a constant linearization point - std::vector newFactorsIndices; ///< The indices of the newly-added factors, in 1-to-1 correspondence with the factors passed in + + /** The indices of the newly-added factors, in 1-to-1 correspondence with the + * factors passed as \c newFactors update(). These indices may be + * used later to refer to the factors in order to remove them. + */ + std::vector newFactorsIndices; + double error; ///< The final factor graph error /// Constructor diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index 0e68461a3..16a336695 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -44,13 +44,24 @@ bool ConcurrentIncrementalSmoother::equals(const ConcurrentSmoother& rhs, double } /* ************************************************************************* */ -ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta) { +ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, + const boost::optional< std::vector >& removeFactorIndices) { gttic(update); // Create the return result meta-data Result result; + gtsam::FastVector removedFactors; + + if(removeFactorIndices){ + // Be very careful to this line + std::cout << "ConcurrentIncrementalSmoother::update removeFactorIndices - not implemented yet" << std::endl; + filterSummarizationSlots_.insert(filterSummarizationSlots_.end(), removeFactorIndices->begin(), removeFactorIndices->end()); + // before it was: + // removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end()); + } + // Constrain the separator keys to remain in the root // Also, mark the separator keys as fixed linearization points FastMap constrainedKeys; diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h index 990ae21c1..5fe6effb2 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -108,7 +108,8 @@ public: * in the smoother). There must not be any variables here that do not occur in newFactors, * and additionally, variables that were already in the system must not be included here. */ - Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values()); + Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), + const boost::optional< std::vector >& removeFactorIndices = boost::none); /** * Perform any required operations before the synchronization process starts. diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index c60c5bb37..ced9721f1 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -540,6 +540,7 @@ TEST( ConcurrentBatchFilter, synchronize_1 ) /* ************************************************************************* */ TEST( ConcurrentBatchFilter, synchronize_2 ) { + std::cout << "*********************** synchronize_2 ************************" << std::endl; // Create a set of optimizer parameters LevenbergMarquardtParams parameters; //parameters.maxIterations = 1; @@ -607,6 +608,7 @@ TEST( ConcurrentBatchFilter, synchronize_2 ) /* ************************************************************************* */ TEST( ConcurrentBatchFilter, synchronize_3 ) { + std::cout << "*********************** synchronize_3 ************************" << std::endl; // Create a set of optimizer parameters LevenbergMarquardtParams parameters; //parameters.maxIterations = 1; @@ -784,6 +786,7 @@ TEST( ConcurrentBatchFilter, synchronize_4 ) /* ************************************************************************* */ TEST( ConcurrentBatchFilter, synchronize_5 ) { + std::cout << "*********************** synchronize_5 ************************" << std::endl; // Create a set of optimizer parameters LevenbergMarquardtParams parameters; parameters.maxIterations = 1; @@ -1081,6 +1084,258 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_2 ) CHECK(assert_equal(expectedMarginals, actualMarginals, 1e-6)); } +///* ************************************************************************* */ +TEST( ConcurrentBatchFilter, removeFactors_topology_1 ) +{ + std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl; + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + ConcurrentBatchFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,1); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + +///* ************************************************************************* */ +TEST( ConcurrentBatchFilter, removeFactors_topology_2 ) +{ + std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl; + // we try removing the last factor + + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + ConcurrentBatchFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,4); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + + +///* ************************************************************************* */ +TEST( ConcurrentBatchFilter, removeFactors_topology_3 ) +{ + std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl; + // we try removing the first factor + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + ConcurrentBatchFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 0, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,0); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + + NonlinearFactorGraph expectedGraph; + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + +///* ************************************************************************* */ +TEST( ConcurrentBatchFilter, removeFactors_values ) +{ + std::cout << "*********************** removeFactors_values ************************" << std::endl; + // we try removing the last factor + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + ConcurrentBatchFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 4, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,4); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + Values actualValues = filter.calculateEstimate(); + + // note: factors are removed before the optimization + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + + // Calculate expected factor graph and values + Values expectedValues = BatchOptimize(expectedGraph, newValues); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); + CHECK(assert_equal(expectedValues, actualValues, 1e-6)); +} + +///* ************************************************************************* */ +//TEST( ConcurrentBatchFilter, synchronize_10 ) +//{ +// // Create a set of optimizer parameters +// LevenbergMarquardtParams parameters; +// parameters.maxIterations = 1; +// +// // Create a Concurrent Batch Filter +// ConcurrentBatchFilter filter(parameters); +// +// // Insert factors into the filter, and marginalize out several variables. +// // This test places several factors in the smoother side, while leaving +// // several factors on the filter side +//} +// +///* ************************************************************************* */ +//TEST( ConcurrentBatchFilter, synchronize_11 ) +//{ +// // Create a set of optimizer parameters +// LevenbergMarquardtParams parameters; +// parameters.maxIterations = 1; +// +// // Create a Concurrent Batch Filter +// ConcurrentBatchFilter filter(parameters); +// +// // Insert factors into the filter, and marginalize out several variables. +// +// // Generate a non-empty smoother update, simulating synchronizing with a non-empty smoother +//} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index c6d7a0a0e..d2a74f1a6 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -575,6 +575,221 @@ TEST( ConcurrentBatchSmoother, synchronize_3 ) } +///* ************************************************************************* */ +TEST( ConcurrentBatchSmoother, removeFactors_topology_1 ) +{ + std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl; + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + + // Create a Concurrent Batch Smoother + ConcurrentBatchSmoother smoother(parameters); + + // Add some factors to the smoother + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + + // Update the smoother: add all factors + smoother.update(newFactors, newValues); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,1); + + // Add no factors to the smoother (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + smoother.update(noFactors, noValues, removeFactorIndices); + + NonlinearFactorGraph actualGraph = smoother.getFactors(); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + +///* ************************************************************************* */ +TEST( ConcurrentBatchSmoother, removeFactors_topology_2 ) +{ + std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl; + // we try removing the last factor + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + + // Create a Concurrent Batch Smoother + ConcurrentBatchSmoother smoother(parameters); + + // Add some factors to the smoother + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Update the smoother: add all factors + smoother.update(newFactors, newValues); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,4); + + // Add no factors to the smoother (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + smoother.update(noFactors, noValues, removeFactorIndices); + + NonlinearFactorGraph actualGraph = smoother.getFactors(); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + + +///* ************************************************************************* */ +TEST( ConcurrentBatchSmoother, removeFactors_topology_3 ) +{ + std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl; + // we try removing the first factor + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + ConcurrentBatchSmoother Smoother(parameters); + + // Add some factors to the Smoother + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Update the Smoother: add all factors + Smoother.update(newFactors, newValues); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 0, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,0); + + // Add no factors to the Smoother (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + Smoother.update(noFactors, noValues, removeFactorIndices); + + NonlinearFactorGraph actualGraph = Smoother.getFactors(); + + NonlinearFactorGraph expectedGraph; + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + +///* ************************************************************************* */ +TEST( ConcurrentBatchSmoother, removeFactors_values ) +{ + std::cout << "*********************** removeFactors_values ************************" << std::endl; + // we try removing the last factor + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + ConcurrentBatchSmoother Smoother(parameters); + + // Add some factors to the Smoother + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Update the Smoother: add all factors + Smoother.update(newFactors, newValues); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 4, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,4); + + // Add no factors to the Smoother (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + Smoother.update(noFactors, noValues, removeFactorIndices); + + NonlinearFactorGraph actualGraph = Smoother.getFactors(); + Values actualValues = Smoother.calculateEstimate(); + + // note: factors are removed before the optimization + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + + // Calculate expected factor graph and values + Values expectedValues = BatchOptimize(expectedGraph, newValues); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); + CHECK(assert_equal(expectedValues, actualValues, 1e-6)); +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index eeb6a8563..c831e9d94 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -1196,6 +1196,249 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 ) +///* ************************************************************************* */ +TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) +{ + std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl; + + // Create a set of optimizer parameters + ISAM2Params parameters; + parameters.relinearizeThreshold = 0; + // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the + // default value for that is 10 (if you set that to zero the code will crash) + parameters.relinearizeSkip = 1; + + // Create a Concurrent IncrementalFilter + ConcurrentIncrementalFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,1); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + +/////* ************************************************************************* */ +TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) +{ + std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl; + // we try removing the last factor + + ISAM2Params parameters; + parameters.relinearizeThreshold = 0; + // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the + // default value for that is 10 (if you set that to zero the code will crash) + parameters.relinearizeSkip = 1; + + // Create a Concurrent IncrementalFilter + ConcurrentIncrementalFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,4); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + + +/////* ************************************************************************* */ +TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) +{ + std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl; + // we try removing the first factor + + ISAM2Params parameters; + parameters.relinearizeThreshold = 0; + // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the + // default value for that is 10 (if you set that to zero the code will crash) + parameters.relinearizeSkip = 1; + + // Create a Concurrent IncrementalFilter + ConcurrentIncrementalFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 0, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,0); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + + NonlinearFactorGraph expectedGraph; + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + +/////* ************************************************************************* */ +TEST( ConcurrentIncrementalFilter, removeFactors_values ) +{ + std::cout << "*********************** removeFactors_values ************************" << std::endl; + // we try removing the last factor + + ISAM2Params parameters; + parameters.relinearizeThreshold = 0; + // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the + // default value for that is 10 (if you set that to zero the code will crash) + parameters.relinearizeSkip = 1; + + // Create a Concurrent IncrementalFilter + ConcurrentIncrementalFilter filter(parameters); + + // Add some factors to the filter + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Specify a subset of variables to marginalize/move to the smoother + FastList keysToMove; + + // Update the filter: add all factors + filter.update(newFactors, newValues, keysToMove); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 4, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(1,4); + + // Add no factors to the filter (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + filter.update(noFactors, noValues, keysToMove, removeFactorIndices); + + NonlinearFactorGraph actualGraph = filter.getFactors(); + Values actualValues = filter.getLinearizationPoint(); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + + // Calculate expected factor graph and values + Values expectedValues = BatchOptimize(expectedGraph, newValues); + + CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); + CHECK(assert_equal(expectedValues, actualValues, 1e-6)); +} + + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index 969af2560..b17344ef1 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -562,6 +562,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 ) // Check CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6)); + CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6)); // Update the smoother diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 56935b876..158c584b5 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -596,6 +596,228 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 ) } +// ========================================================================================================= +///* ************************************************************************* */ +TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) +{ + std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl; + + // Create a set of optimizer parameters + ISAM2Params parameters; + parameters.optimizationParams = ISAM2GaussNewtonParams(); + + // Create a Concurrent Batch Smoother + ConcurrentIncrementalSmoother smoother(parameters); + + // Add some factors to the smoother + NonlinearFactorGraph newFactors; + newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + + Values newValues; + newValues.insert(1, Pose3().compose(poseError)); + newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); + newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); + newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); + + // Update the smoother: add all factors + smoother.update(newFactors, newValues); + + // factor we want to remove + // NOTE: we can remove factors, paying attention that the remaining graph remains connected + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); + std::vector removeFactorIndices(2,1); + + + + // Add no factors to the smoother (we only want to test the removal) + NonlinearFactorGraph noFactors; + Values noValues; + smoother.update(noFactors, noValues, removeFactorIndices); + + NonlinearFactorGraph actualGraph = smoother.getFactors(); + + actualGraph.print("actual graph: \n"); + + NonlinearFactorGraph expectedGraph; + expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we should add an empty one, so that the ordering and labeling of the factors is preserved + expectedGraph.push_back(NonlinearFactor::shared_ptr()); + expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + +// CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +} + +/////* ************************************************************************* */ +//TEST( ConcurrentIncrementalSmoother, removeFactors_topology_2 ) +//{ +// std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl; +// // we try removing the last factor +// +// // Create a set of optimizer parameters +// LevenbergMarquardtParams parameters; +// +// // Create a Concurrent Batch Smoother +// ConcurrentBatchSmoother smoother(parameters); +// +// // Add some factors to the smoother +// NonlinearFactorGraph newFactors; +// newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); +// newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// +// Values newValues; +// newValues.insert(1, Pose3().compose(poseError)); +// newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); +// newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); +// newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); +// +// // Update the smoother: add all factors +// smoother.update(newFactors, newValues); +// +// // factor we want to remove +// // NOTE: we can remove factors, paying attention that the remaining graph remains connected +// // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); +// std::vector removeFactorIndices(1,4); +// +// // Add no factors to the smoother (we only want to test the removal) +// NonlinearFactorGraph noFactors; +// Values noValues; +// smoother.update(noFactors, noValues, removeFactorIndices); +// +// NonlinearFactorGraph actualGraph = smoother.getFactors(); +// +// NonlinearFactorGraph expectedGraph; +// expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); +// expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); +// expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); +// // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// // we should add an empty one, so that the ordering and labeling of the factors is preserved +// expectedGraph.push_back(NonlinearFactor::shared_ptr()); +// +// CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +//} +// +// +/////* ************************************************************************* */ +//TEST( ConcurrentBatchSmoother, removeFactors_topology_3 ) +//{ +// std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl; +// // we try removing the first factor +// +// // Create a set of optimizer parameters +// LevenbergMarquardtParams parameters; +// ConcurrentBatchSmoother Smoother(parameters); +// +// // Add some factors to the Smoother +// NonlinearFactorGraph newFactors; +// newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); +// newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); +// newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); +// +// Values newValues; +// newValues.insert(1, Pose3().compose(poseError)); +// newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); +// newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); +// newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); +// +// // Update the Smoother: add all factors +// Smoother.update(newFactors, newValues); +// +// // factor we want to remove +// // NOTE: we can remove factors, paying attention that the remaining graph remains connected +// // we remove a single factor, the number 0, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); +// std::vector removeFactorIndices(1,0); +// +// // Add no factors to the Smoother (we only want to test the removal) +// NonlinearFactorGraph noFactors; +// Values noValues; +// Smoother.update(noFactors, noValues, removeFactorIndices); +// +// NonlinearFactorGraph actualGraph = Smoother.getFactors(); +// +// NonlinearFactorGraph expectedGraph; +// // we should add an empty one, so that the ordering and labeling of the factors is preserved +// expectedGraph.push_back(NonlinearFactor::shared_ptr()); +// expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); +// expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); +// expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); +// +// CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +//} +// +/////* ************************************************************************* */ +//TEST( ConcurrentBatchSmoother, removeFactors_values ) +//{ +// std::cout << "*********************** removeFactors_values ************************" << std::endl; +// // we try removing the last factor +// +// // Create a set of optimizer parameters +// LevenbergMarquardtParams parameters; +// ConcurrentBatchSmoother Smoother(parameters); +// +// // Add some factors to the Smoother +// NonlinearFactorGraph newFactors; +// newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); +// newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); +// newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// +// Values newValues; +// newValues.insert(1, Pose3().compose(poseError)); +// newValues.insert(2, newValues.at(1).compose(poseOdometry).compose(poseError)); +// newValues.insert(3, newValues.at(2).compose(poseOdometry).compose(poseError)); +// newValues.insert(4, newValues.at(3).compose(poseOdometry).compose(poseError)); +// +// // Update the Smoother: add all factors +// Smoother.update(newFactors, newValues); +// +// // factor we want to remove +// // NOTE: we can remove factors, paying attention that the remaining graph remains connected +// // we remove a single factor, the number 4, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); +// std::vector removeFactorIndices(1,4); +// +// // Add no factors to the Smoother (we only want to test the removal) +// NonlinearFactorGraph noFactors; +// Values noValues; +// Smoother.update(noFactors, noValues, removeFactorIndices); +// +// NonlinearFactorGraph actualGraph = Smoother.getFactors(); +// Values actualValues = Smoother.calculateEstimate(); +// +// // note: factors are removed before the optimization +// NonlinearFactorGraph expectedGraph; +// expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); +// expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); +// expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); +// // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); +// // we should add an empty one, so that the ordering and labeling of the factors is preserved +// expectedGraph.push_back(NonlinearFactor::shared_ptr()); +// +// // Calculate expected factor graph and values +// Values expectedValues = BatchOptimize(expectedGraph, newValues); +// +// CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); +// CHECK(assert_equal(expectedValues, actualValues, 1e-6)); +//} + + + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/CombinedImuFactor.h b/gtsam_unstable/slam/CombinedImuFactor.h new file mode 100644 index 000000000..b954b82f9 --- /dev/null +++ b/gtsam_unstable/slam/CombinedImuFactor.h @@ -0,0 +1,673 @@ +/* ---------------------------------------------------------------------------- + + * 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 CombinedImuFactor.h + * @author Luca Carlone, Stephen Williams + **/ + +#pragma once + +/* GTSAM includes */ +#include +#include +#include +#include +#include +#include + +/* External or standard includes */ +#include + + +namespace gtsam { + + /** + * + * @addtogroup SLAM + * + * REFERENCES: + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built + * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + */ + + class CombinedImuFactor: public NoiseModelFactor6 { + + public: + + /** Struct to store results of preintegrating IMU measurements. Can be build + * incrementally so as to avoid costly integration at time of factor construction. */ + + /** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */ + static Matrix3 rightJacobianExpMapSO3(const Vector3& x) { + // x is the axis-angle representation (exponential coordinates) for a rotation + double normx = norm_2(x); // rotation angle + Matrix3 Jr; + if (normx < 10e-8){ + Jr = Matrix3::Identity(); + } + else{ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + + ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian + } + return Jr; + } + + /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */ + static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) { + // x is the axis-angle representation (exponential coordinates) for a rotation + double normx = norm_2(x); // rotation angle + Matrix3 Jrinv; + + if (normx < 10e-8){ + Jrinv = Matrix3::Identity(); + } + else{ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + Jrinv = Matrix3::Identity() + + 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; + } + return Jrinv; + } + + /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) + * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ + class CombinedPreintegratedMeasurements { + public: + imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration + Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector + ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) + + Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) + Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame) + Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i) + double deltaTij; ///< Time interval from i to j + + Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + + Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + ///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases + ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] + /** Default constructor, initialize with no IMU measurements */ + CombinedPreintegratedMeasurements( + const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases + const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc + const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc + const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc + const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) + const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) + const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements + ///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements) + ) : biasHat(bias), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), + delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), + delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)) + { + // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21) + measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3); + } + + CombinedPreintegratedMeasurements() : + biasHat(imuBias::ConstantBias()), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), + delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), + delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)) + { + } + + /** print */ + void print(const std::string& s = "Preintegrated Measurements:") const { + std::cout << s << std::endl; + biasHat.print(" biasHat"); + std::cout << " deltaTij " << deltaTij << std::endl; + std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl; + std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl; + deltaRij.print(" deltaRij "); + std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl; + std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl; + } + + /** equals */ + bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const { + return biasHat.equals(expected.biasHat, tol) + && equal_with_abs_tol(measurementCovariance, expected.measurementCovariance, tol) + && equal_with_abs_tol(deltaPij, expected.deltaPij, tol) + && equal_with_abs_tol(deltaVij, expected.deltaVij, tol) + && deltaRij.equals(expected.deltaRij, tol) + && std::fabs(deltaTij - expected.deltaTij) < tol + && equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol) + && equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol) + && equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol) + && equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol) + && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); + } + + /** Add a single IMU measurement to the preintegration. */ + void integrateMeasurement( + const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) + const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) + double deltaT, ///< Time step + boost::optional body_P_sensor = boost::none ///< Sensor frame + ) { + // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. + // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty + Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc); + Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega); + + // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame + if(body_P_sensor){ + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); + correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + // linear acceleration vector in the body frame + } + + const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement + const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + + // Update Jacobians + /* ----------------------------------------------------------------------------------------------------------------------- */ + delPdelBiasAcc += delVdelBiasAcc * deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT; + delVdelBiasAcc += -deltaRij.matrix() * deltaT; + delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; + delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; + + // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that + // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we + // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements + /* ----------------------------------------------------------------------------------------------------------------------- */ + Matrix3 Z_3x3 = Matrix3::Zero(); + Matrix3 I_3x3 = Matrix3::Identity(); + const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3) + const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i); + + Rot3 Rot_j = deltaRij * Rincr; + const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j); + + // Single Jacobians to propagate covariance + Matrix3 H_pos_pos = I_3x3; + Matrix3 H_pos_vel = I_3x3 * deltaT; + Matrix3 H_pos_angles = Z_3x3; + + Matrix3 H_vel_pos = Z_3x3; + Matrix3 H_vel_vel = I_3x3; + Matrix3 H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + // analytic expression corresponding to the following numerical derivative + // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); + Matrix3 H_vel_biasacc = - deltaRij.matrix() * deltaT; + + Matrix3 H_angles_pos = Z_3x3; + Matrix3 H_angles_vel = Z_3x3; + Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; + Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; + // analytic expression corresponding to the following numerical derivative + // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + + // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix F(15,15); + F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, + H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, + H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, + Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; + + + // first order uncertainty propagation + // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() + + Matrix G_measCov_Gt = Matrix::Zero(15,15); + // BLOCK DIAGONAL TERMS + G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance.block(0,0,3,3); + +// G_measCov_Gt.block(3,3,3,3) = (H_vel_biasacc) * (1/deltaT) * measurementCovariance.block(3,3,3,3) * (H_vel_biasacc.transpose()) + +// (H_vel_biasacc) * (1/deltaT) * +// ( measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) * +// (H_vel_biasacc.transpose()); + + G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) * + (measurementCovariance.block(3,3,3,3) + measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) * + (H_vel_biasacc.transpose()); + + G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) * + (measurementCovariance.block(6,6,3,3) + measurementCovariance.block(12,12,3,3) + measurementCovariance.block(18,18,3,3) ) * + (H_angles_biasomega.transpose()); + + G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance.block(9,9,3,3); + + G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance.block(12,12,3,3); + + // OFF BLOCK DIAGONAL TERMS + Matrix3 block24 = H_vel_biasacc * measurementCovariance.block(9,9,3,3); + G_measCov_Gt.block(3,9,3,3) = block24; + G_measCov_Gt.block(9,3,3,3) = block24.transpose(); + + Matrix3 block35 = H_angles_biasomega * measurementCovariance.block(12,12,3,3); + G_measCov_Gt.block(6,12,3,3) = block35; + G_measCov_Gt.block(12,6,3,3) = block35.transpose(); + + /* + // overall Jacobian wrt raw measurements (df/du) + Matrix3 H_vel_initbiasacc = H_vel_biasacc; + Matrix3 H_angles_initbiasomega = H_angles_biasomega; + + // COMBINED IMU FACTOR, preserves correlation with bias evolution and considers initial uncertainty on biases + Matrix G(15,21); + G << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, - H_vel_biasacc, Z_3x3, H_vel_biasacc, Z_3x3, H_vel_initbiasacc, Z_3x3, + Z_3x3, Z_3x3, - H_angles_biasomega, Z_3x3, H_angles_biasomega, Z_3x3, H_angles_initbiasomega, + Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3; + + Matrix ErrorMatrix = (1/deltaT) * G * measurementCovariance * G.transpose() - G_measCov_Gt; + std::cout << "---- matrix multiplication error = [" << ErrorMatrix << "];"<< std::endl; + double max_err=0; + for(int i=0;i<15;i++) + { + for(int j=0;j<15;j++) + { + if(fabs(ErrorMatrix(i,j))>max_err) + max_err = fabs(ErrorMatrix(i,j)); + } + } + std::cout << "---- max matrix multiplication error = [" << max_err << "];"<< std::endl; + + if(max_err>10e-15) + std::cout << "---- max matrix multiplication error *large* = [" << max_err << "];"<< std::endl; + + PreintMeasCov = F * PreintMeasCov * F.transpose() + (1/deltaT) * G * measurementCovariance * G.transpose(); + */ + + PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt; + + // Update preintegrated measurements + /* ----------------------------------------------------------------------------------------------------------------------- */ + deltaPij += deltaVij * deltaT; + deltaVij += deltaRij.matrix() * correctedAcc * deltaT; + deltaRij = deltaRij * Rincr; + deltaTij += deltaT; + } + + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, + const Vector3& delta_angles, const Vector& delta_vel_in_t0){ + + // Note: all delta terms refer to an IMU\sensor system at t0 + + Vector body_t_a_body = msr_acc_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; + } + + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles){ + + // Note: all delta terms refer to an IMU\sensor system at t0 + + // Calculate the corrected measurements using the Bias object + Vector body_t_omega_body= msr_gyro_t; + + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); + return Rot3::Logmap(R_t_to_t0); + } + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(biasHat); + ar & BOOST_SERIALIZATION_NVP(measurementCovariance); + ar & BOOST_SERIALIZATION_NVP(deltaPij); + ar & BOOST_SERIALIZATION_NVP(deltaVij); + ar & BOOST_SERIALIZATION_NVP(deltaRij); + ar & BOOST_SERIALIZATION_NVP(deltaTij); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega); + } + }; + + private: + + typedef CombinedImuFactor This; + typedef NoiseModelFactor6 Base; + + CombinedPreintegratedMeasurements preintegratedMeasurements_; + Vector3 gravity_; + Vector3 omegaCoriolis_; + + public: + + /** Shorthand for a smart pointer to a factor */ +#ifndef _MSC_VER + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::shared_ptr shared_ptr; +#endif + /** Default constructor - only use for serialization */ + CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} + + /** Constructor */ + CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, + const SharedNoiseModel& model) : + Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), + preintegratedMeasurements_(preintegratedMeasurements), + gravity_(gravity), + omegaCoriolis_(omegaCoriolis) { + } + + virtual ~CombinedImuFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "CombinedImuFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << "," + << keyFormatter(this->key6()) << ")\n"; + preintegratedMeasurements_.print(" preintegrated measurements:"); + std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; + std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) + && equal_with_abs_tol(gravity_, e->gravity_, tol) + && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol); + } + + /** Access the preintegrated measurements. */ + const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { + return preintegratedMeasurements_; } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none, + boost::optional H6 = boost::none) const + { + + const double& deltaTij = preintegratedMeasurements_.deltaTij; + const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3 Rot_i = pose_i.rotation(); + const Rot3 Rot_j = pose_j.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + const Vector3 pos_j = pose_j.translation().vector(); + + // We compute factor's Jacobians, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term + + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + + const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); + + const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); + + const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + + if(H1) { + H1->resize(15,6); + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + - Rot_i.matrix(), + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + Matrix3::Zero(), + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(), + //dBiasAcc/dPi + Matrix3::Zero(), Matrix3::Zero(), + //dBiasOmega/dPi + Matrix3::Zero(), Matrix3::Zero(); + } + + if(H2) { + H2->resize(15,3); + (*H2) << + // dfP/dVi + - Matrix3::Identity() * deltaTij + + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - Matrix3::Identity() + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(), + //dBiasAcc/dVi + Matrix3::Zero(), + //dBiasOmega/dVi + Matrix3::Zero(); + + } + + if(H3) { + + H3->resize(15,6); + (*H3) << + // dfP/dPosej + Matrix3::Zero(), Rot_j.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(), + //dBiasAcc/dPosej + Matrix3::Zero(), Matrix3::Zero(), + //dBiasOmega/dPosej + Matrix3::Zero(), Matrix3::Zero(); + } + + if(H4) { + H4->resize(15,3); + (*H4) << + // dfP/dVj + Matrix3::Zero(), + // dfV/dVj + Matrix3::Identity(), + // dfR/dVj + Matrix3::Zero(), + //dBiasAcc/dVj + Matrix3::Zero(), + //dBiasOmega/dVj + Matrix3::Zero(); + } + + if(H5) { + const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); + const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; + + H5->resize(15,6); + (*H5) << + // dfP/dBias_i + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc, + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega, + // dfV/dBias_i + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc, + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega, + // dfR/dBias_i + Matrix::Zero(3,3), + Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega), + //dBiasAcc/dBias_i + -Matrix3::Identity(), Matrix3::Zero(), + //dBiasOmega/dBias_i + Matrix3::Zero(), -Matrix3::Identity(); + } + + if(H6) { + + H6->resize(15,6); + (*H6) << + // dfP/dBias_j + Matrix3::Zero(), Matrix3::Zero(), + // dfV/dBias_j + Matrix3::Zero(), Matrix3::Zero(), + // dfR/dBias_j + Matrix3::Zero(), Matrix3::Zero(), + //dBiasAcc/dBias_j + Matrix3::Identity(), Matrix3::Zero(), + //dBiasOmega/dBias_j + Matrix3::Zero(), Matrix3::Identity(); + } + + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 fp = + pos_j - pos_i + - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr) + - vel_i * deltaTij + + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + - 0.5 * gravity_ * deltaTij*deltaTij; + + const Vector3 fv = + vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr) + + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + - gravity_ * deltaTij; + + const Vector3 fR = Rot3::Logmap(fRhat); + + const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); + + const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); + + Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15 + + return r; + } + + + /** predicted states from IMU */ + static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, + const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j, + const CombinedPreintegratedMeasurements preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis) + { + + const double& deltaTij = preintegratedMeasurements.deltaTij; + const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat.accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat.gyroscope(); + + const Rot3 Rot_i = pose_i.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij + + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; + + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); + + pose_j = Pose3( Rot_j, Point3(pos_j) ); + + bias_j = bias_i; + } + + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor6", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(gravity_); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); + } + }; // \class CombinedImuFactor + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 564f5a2c8..650e11ff0 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -282,8 +282,12 @@ public: // Predict POSE Pose2Pred = predictPose(Pose1, Vel1, Bias1); + // Luca: difference between Pose2 and Pose2Pred + POSE DiffPose( Pose2.rotation().between(Pose2Pred.rotation()), Pose2Pred.translation() - Pose2.translation() ); +// DiffPose = Pose2.between(Pose2Pred); + return DiffPose; // Calculate error - return Pose2.between(Pose2Pred); + //return Pose2.between(Pose2Pred); } VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const { @@ -454,10 +458,13 @@ public: Matrix F_bias_g = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3); Matrix F = stack(5, &F_angles, &F_pos, &F_vel, &F_bias_a, &F_bias_g); + noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt ); Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() ); EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d; + // Luca: force identity covariance matrix (for testing purposes) + // EquivCov_Overall = Matrix::Identity(15,15); // Update Jacobian_wrt_t0_Overall Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall; diff --git a/gtsam_unstable/slam/ImuFactor.h b/gtsam_unstable/slam/ImuFactor.h new file mode 100644 index 000000000..0c08ed7be --- /dev/null +++ b/gtsam_unstable/slam/ImuFactor.h @@ -0,0 +1,565 @@ +/* ---------------------------------------------------------------------------- + + * 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 ImuFactor.h + * @author Luca Carlone, Stephen Williams, Richard Roberts + **/ + +#pragma once + +/* GTSAM includes */ +#include +#include +#include +#include +#include +#include + +/* External or standard includes */ +#include + + +namespace gtsam { + + /** + * + * @addtogroup SLAM + * * REFERENCES: + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built + * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + */ + + class ImuFactor: public NoiseModelFactor5 { + + public: + + /** Struct to store results of preintegrating IMU measurements. Can be build + * incrementally so as to avoid costly integration at time of factor construction. */ + + /** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */ + static Matrix3 rightJacobianExpMapSO3(const Vector3& x) { + // x is the axis-angle representation (exponential coordinates) for a rotation + double normx = norm_2(x); // rotation angle + Matrix3 Jr; + if (normx < 10e-8){ + Jr = Matrix3::Identity(); + } + else{ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + + ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian + } + return Jr; + } + + /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */ + static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) { + // x is the axis-angle representation (exponential coordinates) for a rotation + double normx = norm_2(x); // rotation angle + Matrix3 Jrinv; + + if (normx < 10e-8){ + Jrinv = Matrix3::Identity(); + } + else{ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + Jrinv = Matrix3::Identity() + + 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; + } + return Jrinv; + } + + /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) + * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ + class PreintegratedMeasurements { + public: + imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration + Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) + + Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, , in [2]) (in frame i) + Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame) + Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i) + double deltaTij; ///< Time interval from i to j + + Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + + Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + + Vector3 initialRotationRate; ///< initial rotation rate reading from the IMU (at time i) + Vector3 finalRotationRate; ///< final rotation rate reading from the IMU (at time j) + + /** Default constructor, initialize with no IMU measurements */ + PreintegratedMeasurements( + const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases + const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc + const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc + const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc + const Vector3& initialRotationRate = Vector3::Zero() ///< initial rotation rate reading from the IMU (at time i) + ) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), + delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), + delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9), + initialRotationRate(initialRotationRate), finalRotationRate(initialRotationRate) + { + measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance; + PreintMeasCov = Matrix::Zero(9,9); + } + + PreintegratedMeasurements() : + biasHat(imuBias::ConstantBias()), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), + delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), + delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9), + initialRotationRate(Vector3::Zero()), finalRotationRate(Vector3::Zero()) + { + measurementCovariance = Matrix::Zero(9,9); + PreintMeasCov = Matrix::Zero(9,9); + } + + /** print */ + void print(const std::string& s = "Preintegrated Measurements:") const { + std::cout << s << std::endl; + biasHat.print(" biasHat"); + std::cout << " deltaTij " << deltaTij << std::endl; + std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl; + std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl; + deltaRij.print(" deltaRij "); + std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl; + std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl; + } + + /** equals */ + bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const { + return biasHat.equals(expected.biasHat, tol) + && equal_with_abs_tol(measurementCovariance, expected.measurementCovariance, tol) + && equal_with_abs_tol(deltaPij, expected.deltaPij, tol) + && equal_with_abs_tol(deltaVij, expected.deltaVij, tol) + && deltaRij.equals(expected.deltaRij, tol) + && std::fabs(deltaTij - expected.deltaTij) < tol + && equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol) + && equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol) + && equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol) + && equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol) + && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); + } + + /** Add a single IMU measurement to the preintegration. */ + void integrateMeasurement( + const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) + const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) + double deltaT, ///< Time step + boost::optional body_P_sensor = boost::none ///< Sensor frame + ) { + + // NOTE: order is important here because each update uses old values. + // First we compensate the measurements for the bias + Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc); + Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega); + + finalRotationRate = correctedOmega; + + // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame + if(body_P_sensor){ + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + + Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); + + correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + // linear acceleration vector in the body frame + } + + const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement + + const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + + // Update Jacobians + /* ----------------------------------------------------------------------------------------------------------------------- */ + delPdelBiasAcc += delVdelBiasAcc * deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT; + delVdelBiasAcc += -deltaRij.matrix() * deltaT; + delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; + delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; + + // Update preintegrated mesurements covariance + /* ----------------------------------------------------------------------------------------------------------------------- */ + Matrix3 Z_3x3 = Matrix3::Zero(); + Matrix3 I_3x3 = Matrix3::Identity(); + const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3) + const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i); + + Rot3 Rot_j = deltaRij * Rincr; + const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j); + + // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that + // can be seen as a prediction phase in an EKF framework + Matrix H_pos_pos = I_3x3; + Matrix H_pos_vel = I_3x3 * deltaT; + Matrix H_pos_angles = Z_3x3; + + Matrix H_vel_pos = Z_3x3; + Matrix H_vel_vel = I_3x3; + Matrix H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + // analytic expression corresponding to the following numerical derivative + // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); + + Matrix H_angles_pos = Z_3x3; + Matrix H_angles_vel = Z_3x3; + Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; + // analytic expression corresponding to the following numerical derivative + // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + + // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix F(9,9); + F << H_pos_pos, H_pos_vel, H_pos_angles, + H_vel_pos, H_vel_vel, H_vel_angles, + H_angles_pos, H_angles_vel, H_angles_angles; + + + // first order uncertainty propagation + // the deltaT allows to pass from continuous time noise to discrete time noise + PreintMeasCov = F * PreintMeasCov * F.transpose() + measurementCovariance * deltaT ; + + // Update preintegrated measurements + /* ----------------------------------------------------------------------------------------------------------------------- */ + deltaPij += deltaVij * deltaT; + deltaVij += deltaRij.matrix() * correctedAcc * deltaT; + deltaRij = deltaRij * Rincr; + deltaTij += deltaT; + } + + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, + const Vector3& delta_angles, const Vector& delta_vel_in_t0){ + + // Note: all delta terms refer to an IMU\sensor system at t0 + + Vector body_t_a_body = msr_acc_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; + } + + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles){ + + // Note: all delta terms refer to an IMU\sensor system at t0 + + // Calculate the corrected measurements using the Bias object + Vector body_t_omega_body= msr_gyro_t; + + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); + return Rot3::Logmap(R_t_to_t0); + } + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(biasHat); + ar & BOOST_SERIALIZATION_NVP(measurementCovariance); + ar & BOOST_SERIALIZATION_NVP(deltaPij); + ar & BOOST_SERIALIZATION_NVP(deltaVij); + ar & BOOST_SERIALIZATION_NVP(deltaRij); + ar & BOOST_SERIALIZATION_NVP(deltaTij); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega); + } + }; + + private: + + typedef ImuFactor This; + typedef NoiseModelFactor5 Base; + + PreintegratedMeasurements preintegratedMeasurements_; + Vector3 gravity_; + Vector3 omegaCoriolis_; + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + public: + + /** Shorthand for a smart pointer to a factor */ +#ifndef _MSC_VER + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::shared_ptr shared_ptr; +#endif + /** Default constructor - only use for serialization */ + ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} + + /** Constructor */ + ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, + const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : + Base(model, pose_i, vel_i, pose_j, vel_j, bias), + preintegratedMeasurements_(preintegratedMeasurements), + gravity_(gravity), + omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor) { + } + + virtual ~ImuFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "ImuFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << ")\n"; + preintegratedMeasurements_.print(" preintegrated measurements:"); + std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; + std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; + this->noiseModel_->print(" noise model: "); + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) + && equal_with_abs_tol(gravity_, e->gravity_, tol) + && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + /** Access the preintegrated measurements. */ + const PreintegratedMeasurements& preintegratedMeasurements() const { + return preintegratedMeasurements_; } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const imuBias::ConstantBias& bias, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none) const + { + + const double& deltaTij = preintegratedMeasurements_.deltaTij; + const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer(); + const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3 Rot_i = pose_i.rotation(); + const Rot3 Rot_j = pose_j.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + const Vector3 pos_j = pose_j.translation().vector(); + + // We compute factor's Jacobians + /* ---------------------------------------------------------------------------------------------------- */ + const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term + + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + + const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); + + const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); + + const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + + if(H1) { + H1->resize(9,6); + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + - Rot_i.matrix(), + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + Matrix3::Zero(), + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(); + } + if(H2) { + H2->resize(9,3); + (*H2) << + // dfP/dVi + - Matrix3::Identity() * deltaTij + + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - Matrix3::Identity() + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(); + + } + if(H3) { + + H3->resize(9,6); + (*H3) << + // dfP/dPosej + Matrix3::Zero(), Rot_j.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(); + } + if(H4) { + H4->resize(9,3); + (*H4) << + // dfP/dVj + Matrix3::Zero(), + // dfV/dVj + Matrix3::Identity(), + // dfR/dVj + Matrix3::Zero(); + } + if(H5) { + + const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); + const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; + + H5->resize(9,6); + (*H5) << + // dfP/dBias + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc, + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega, + // dfV/dBias + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc, + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega, + // dfR/dBias + Matrix::Zero(3,3), + Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); + } + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 fp = + pos_j - pos_i + - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr) + - vel_i * deltaTij + + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + - 0.5 * gravity_ * deltaTij*deltaTij; + + const Vector3 fv = + vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr) + + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + - gravity_ * deltaTij; + + const Vector3 fR = Rot3::Logmap(fRhat); + + Vector r(9); r << fp, fv, fR; + return r; + } + + + /** predicted states from IMU */ + static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, + const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none) + { + + const double& deltaTij = preintegratedMeasurements.deltaTij; + const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat.accelerometer(); + const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat.gyroscope(); + + const Rot3 Rot_i = pose_i.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij + + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; + + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); + + pose_j = Pose3( Rot_j, Point3(pos_j) ); + } + + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor5", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(gravity_); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } + }; // \class ImuFactor + +} /// namespace gtsam