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

Merged from Hmf6_unordered 970392249a8c50153f24594c9c81acb740cedd06

release/4.3a0
Richard Roberts 2013-10-03 19:51:56 +00:00
parent 2ad0cd4417
commit bf40956592
21 changed files with 2633 additions and 364 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

700
CMakeLists.txt
View File

@ -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. <Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_PREFIX "")
find_package(Eigen3 REQUIRED)
include_directories(AFTER "${EIGEN3_INCLUDE_DIR}")
else()
# Use bundled Eigen include paths e.g. <gtsam/3rdparty/Eigen/Eigen/Core>
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. <Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_PREFIX "")
find_package(Eigen3 REQUIRED)
include_directories(AFTER "${EIGEN3_INCLUDE_DIR}")
else()
# Use bundled Eigen include paths e.g. <gtsam/3rdparty/Eigen/Eigen/Core>
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)

2
doc/Doxyfile.in
View File

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

26
gtsam/linear/NoiseModel.cpp
View File

@ -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<const Welsh*>(&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
/* ************************************************************************* */

25
gtsam/linear/NoiseModel.h
View File

@ -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<Welsh> 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<class ARCHIVE>
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

4
gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp
View File

@ -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;

13
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -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<Key>& keysToMove) {
}
/* ************************************************************************* */
}/// namespace gtsam
}/// namespace gtsam

8
gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
View File

@ -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<size_t> 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<size_t> newFactorsIndices;
double error; ///< The final factor graph error
/// Constructor

6
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -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<size_t> >& 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);

3
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h
View File

@ -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<size_t> >& removeFactorIndices = boost::none);
/**
* Perform any required operations before the synchronization process starts.

3
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -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<size_t> removedFactors;
if(removeFactorIndices){
removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end());
}

8
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h
View File

@ -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<size_t> 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<size_t> newFactorsIndices;
double error; ///< The final factor graph error
/// Constructor

13
gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
View File

@ -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<size_t> >& removeFactorIndices) {
gttic(update);
// Create the return result meta-data
Result result;
gtsam::FastVector<size_t> 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<Key,int> constrainedKeys;

3
gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h
View File

@ -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<size_t> >& removeFactorIndices = boost::none);
/**
* Perform any required operations before the synchronization process starts.

255
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
View File

@ -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<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
// Specify a subset of variables to marginalize/move to the smoother
FastList<Key> 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<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
// Specify a subset of variables to marginalize/move to the smoother
FastList<Key> 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<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
// Specify a subset of variables to marginalize/move to the smoother
FastList<Key> 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<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
// Specify a subset of variables to marginalize/move to the smoother
FastList<Key> 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<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(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);}
/* ************************************************************************* */

215
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
View File

@ -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<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
newValues.insert(4, newValues.at<Pose3>(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<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
newValues.insert(4, newValues.at<Pose3>(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<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
newValues.insert(4, newValues.at<Pose3>(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<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
newValues.insert(4, newValues.at<Pose3>(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<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(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);}
/* ************************************************************************* */

243
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
View File

@ -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<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
// Specify a subset of variables to marginalize/move to the smoother
FastList<Key> 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<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
// Specify a subset of variables to marginalize/move to the smoother
FastList<Key> 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<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
// Specify a subset of variables to marginalize/move to the smoother
FastList<Key> 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<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
// Specify a subset of variables to marginalize/move to the smoother
FastList<Key> 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<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(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);}
/* ************************************************************************* */

1
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
View File

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

222
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
View File

@ -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<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
newValues.insert(4, newValues.at<Pose3>(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<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(1, poseInitial, noisePrior));
// newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
// newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
// newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
//
// Values newValues;
// newValues.insert(1, Pose3().compose(poseError));
// newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
// newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
// newValues.insert(4, newValues.at<Pose3>(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<Pose3>(1, 2, poseOdometry, noiseOdometery);
// std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
// expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
// expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
// expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(1, poseInitial, noisePrior));
// newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
// newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
// newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
// newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
//
// Values newValues;
// newValues.insert(1, Pose3().compose(poseError));
// newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
// newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
// newValues.insert(4, newValues.at<Pose3>(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<Pose3>(1, 2, poseOdometry, noiseOdometery);
// std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
// expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
// expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
// expectedGraph.push_back(BetweenFactor<Pose3>(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<Pose3>(1, poseInitial, noisePrior));
// newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
// newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
// newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
//
// Values newValues;
// newValues.insert(1, Pose3().compose(poseError));
// newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
// newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
// newValues.insert(4, newValues.at<Pose3>(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<Pose3>(1, 2, poseOdometry, noiseOdometery);
// std::vector<size_t> 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<Pose3>(1, poseInitial, noisePrior));
// expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
// expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
// expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(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);}
/* ************************************************************************* */

673
gtsam_unstable/slam/CombinedImuFactor.h Normal file
View File

@ -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 <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/debug.h>
/* External or standard includes */
#include <ostream>
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<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias,imuBias::ConstantBias> {
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<Pose3> 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<LieVector, LieVector>(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<LieVector, LieVector>(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<class ARCHIVE>
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<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias,imuBias::ConstantBias> Base;
CombinedPreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_;
Vector3 omegaCoriolis_;
public:
/** Shorthand for a smart pointer to a factor */
#ifndef _MSC_VER
typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
#else
typedef boost::shared_ptr<CombinedImuFactor> 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>(
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<const This*> (&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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none,
boost::optional<Matrix&> 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<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor6",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
}
}; // \class CombinedImuFactor
} /// namespace gtsam

9
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -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;

565
gtsam_unstable/slam/ImuFactor.h Normal file
View File

@ -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 <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/debug.h>
/* External or standard includes */
#include <ostream>
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<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> {
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<Pose3> 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<LieVector, LieVector>(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<LieVector, LieVector>(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<class ARCHIVE>
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<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> Base;
PreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_;
Vector3 omegaCoriolis_;
boost::optional<Pose3> 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<ImuFactor> shared_ptr;
#else
typedef boost::shared_ptr<ImuFactor> 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<Pose3> 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>(
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<const This*> (&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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> 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<Pose3> 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<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor5",
boost::serialization::base_object<Base>(*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