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

Many small improvements, bug-fixes, and tests

release/4.3a0
Frank 2016-02-24 11:01:19 -08:00
parent 9e4b8017ba
commit 0372a959ee
152 changed files with 3223 additions and 1332 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
CMakeLists.txt
View File

@ -66,7 +66,8 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON)
option(GTSAM_USE_VECTOR3_POINTS "Symply typdef Point3 to eigen::Vector3d" OFF) option(GTSAM_USE_VECTOR3_POINTS "Simply typdef Point3 to eigen::Vector3d" OFF)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
# Options relating to MATLAB wrapper # Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries # TODO: Check for matlab mex binary before handling building of binaries
@ -289,18 +290,24 @@ endif()
# Include boost - use 'BEFORE' so that a specific boost specified to CMake # Include boost - use 'BEFORE' so that a specific boost specified to CMake
# takes precedence over a system-installed one. # takes precedence over a system-installed one.
# Use 'SYSTEM' to ignore compiler warnings in Boost headers
include_directories(BEFORE SYSTEM ${Boost_INCLUDE_DIR}) include_directories(BEFORE SYSTEM ${Boost_INCLUDE_DIR})
if(GTSAM_SUPPORT_NESTED_DISSECTION)
set(METIS_INCLUDE_DIRECTORIES
gtsam/3rdparty/metis/include
gtsam/3rdparty/metis/libmetis
gtsam/3rdparty/metis/GKlib)
else()
set(METIS_INCLUDE_DIRECTORIES)
endif()
# Add includes for source directories 'BEFORE' boost and any system include # Add includes for source directories 'BEFORE' boost and any system include
# paths so that the compiler uses GTSAM headers in our source directory instead # paths so that the compiler uses GTSAM headers in our source directory instead
# of any previously installed GTSAM headers. # of any previously installed GTSAM headers.
include_directories(BEFORE include_directories(BEFORE
gtsam/3rdparty/UFconfig gtsam/3rdparty/UFconfig
gtsam/3rdparty/CCOLAMD/Include gtsam/3rdparty/CCOLAMD/Include
gtsam/3rdparty/metis/include ${METIS_INCLUDE_DIRECTORIES}
gtsam/3rdparty/metis/libmetis
gtsam/3rdparty/metis/GKlib
${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}
${PROJECT_BINARY_DIR} # So we can include generated config header files ${PROJECT_BINARY_DIR} # So we can include generated config header files
CppUnitLite) CppUnitLite)
@ -484,6 +491,7 @@ print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full Exp
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ")
print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ") print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
message(STATUS "MATLAB toolbox flags ") message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")

2
GTSAM-Concepts.md
View File

@ -19,7 +19,7 @@ To optimize over continuous types, we assume they are manifolds. This is central
[Manifolds](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) and [charts](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) are intimately linked concepts. We are only interested here in [differentiable manifolds](http://en.wikipedia.org/wiki/Differentiable_manifold#Definition), continuous spaces that can be locally approximated *at any point* using a local vector space, called the [tangent space](http://en.wikipedia.org/wiki/Tangent_space). A *chart* is an invertible map from the manifold to that tangent space. [Manifolds](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) and [charts](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) are intimately linked concepts. We are only interested here in [differentiable manifolds](http://en.wikipedia.org/wiki/Differentiable_manifold#Definition), continuous spaces that can be locally approximated *at any point* using a local vector space, called the [tangent space](http://en.wikipedia.org/wiki/Tangent_space). A *chart* is an invertible map from the manifold to that tangent space.
In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented. In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented.
In detail, we ask the following are defined in the traits object (although, not all are needed for optimization): In detail, we ask that the following items are defined in the traits object (although, not all are needed for optimization):
* values: * values:
* `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1. * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1.

121
cmake/GtsamBuildTypes.cmake
View File

@ -3,56 +3,76 @@
list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}") list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}")
# Default to Release mode # Default to Release mode
if(NOT FIRST_PASS_DONE AND NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION) if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
set(CMAKE_BUILD_TYPE "Release" CACHE STRING set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING
"Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo." "Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo.")
FORCE) set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE})
endif() endif()
# Add option for using build type postfixes to allow installing multiple build modes # Add option for using build type postfixes to allow installing multiple build modes
option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON) option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON)
# Add debugging flags but only on the first pass # Set custom compilation flags.
if(NOT FIRST_PASS_DONE) # NOTE: We set all the CACHE variables with a GTSAM prefix, and then set a normal local variable below
if(MSVC) # so that we don't "pollute" the global variable namespace in the cmake cache.
set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) if(MSVC)
set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) set(GTSAM_CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.")
set(CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.")
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.")
set(CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE) set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.")
set(CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE) set(GTSAM_CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds.")
set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds.")
set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /Zi" CACHE STRING "Flags used by the compiler during profiling builds.")
set(CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /Zi" CACHE STRING "Flags used by the compiler during profiling builds.")
set(CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.")
set(CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.")
mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING CMAKE_EXE_LINKER_FLAGS_TIMING CMAKE_SHARED_LINKER_FLAGS_TIMING CMAKE_MODULE_LINKER_FLAGS_TIMING) else()
set(CMAKE_C_FLAGS_PROFILING "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) set(GTSAM_CMAKE_C_FLAGS_DEBUG "-std=c11 -Wall -g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.")
set(CMAKE_CXX_FLAGS_PROFILING "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "-std=c++11 -Wall -g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.")
set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "-std=c11 -Wall -g -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.")
set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "-std=c++11 -Wall -g -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.")
set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) set(GTSAM_CMAKE_C_FLAGS_RELEASE "-std=c11 -Wall -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during release builds.")
mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING CMAKE_MODULE_LINKER_FLAGS_PROFILING) set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "-std=c++11 -Wall -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during release builds.")
else() set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE}" CACHE STRING "Flags used by the compiler during profiling builds.")
set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -std=c11 -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE}" CACHE STRING "Flags used by the compiler during profiling builds.")
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -std=c++11 -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.")
set(CMAKE_C_FLAGS_RELWITHDEBINFO "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.")
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
set(CMAKE_C_FLAGS_RELEASE "-std=c11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)
set(CMAKE_CXX_FLAGS_RELEASE "-std=c++11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)
set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
set(CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
set(CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING CMAKE_EXE_LINKER_FLAGS_TIMING CMAKE_SHARED_LINKER_FLAGS_TIMING)
set(CMAKE_C_FLAGS_PROFILING "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
set(CMAKE_CXX_FLAGS_PROFILING "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE__LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING)
endif()
endif() endif()
set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
set(GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
set(GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING
GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING
GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_CMAKE_CXX_FLAGS_PROFILING GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING
GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING)
# Apply the gtsam specific build flags as normal variables. This makes it so that they only
# apply to the gtsam part of the build if gtsam is built as a subproject
set(CMAKE_C_FLAGS_DEBUG ${GTSAM_CMAKE_C_FLAGS_DEBUG})
set(CMAKE_CXX_FLAGS_DEBUG ${GTSAM_CMAKE_CXX_FLAGS_DEBUG})
set(CMAKE_C_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO})
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO})
set(CMAKE_C_FLAGS_RELEASE ${GTSAM_CMAKE_C_FLAGS_RELEASE})
set(CMAKE_CXX_FLAGS_RELEASE ${GTSAM_CMAKE_CXX_FLAGS_RELEASE})
set(CMAKE_C_FLAGS_PROFILING ${GTSAM_CMAKE_C_FLAGS_PROFILING})
set(CMAKE_CXX_FLAGS_PROFILING ${GTSAM_CMAKE_CXX_FLAGS_PROFILING})
set(CMAKE_C_FLAGS_TIMING ${GTSAM_CMAKE_C_FLAGS_TIMING})
set(CMAKE_CXX_FLAGS_TIMING ${GTSAM_CMAKE_CXX_FLAGS_TIMING})
set(CMAKE_SHARED_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING})
set(CMAKE_MODULE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING})
set(CMAKE_EXE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING})
set(CMAKE_SHARED_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING})
set(CMAKE_MODULE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING})
set(CMAKE_EXE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING})
# Clang uses a template depth that is less than standard and is too small # Clang uses a template depth that is less than standard and is too small
if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
# Apple Clang before 5.0 does not support -ftemplate-depth. # Apple Clang before 5.0 does not support -ftemplate-depth.
@ -84,11 +104,10 @@ if(NOT "${CMAKE_BUILD_TYPE}" STREQUAL "")
endif() endif()
# Set up build types for MSVC and XCode # Set up build types for MSVC and XCode
if(NOT FIRST_PASS_DONE) set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel
set(CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel CACHE STRING "Build types available to MSVC and XCode")
CACHE STRING "Build types available to MSVC and XCode" FORCE) mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES)
mark_as_advanced(FORCE CMAKE_CONFIGURATION_TYPES) set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES})
endif()
# Check build types # Check build types
string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower) string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower)
@ -100,13 +119,9 @@ if( NOT cmake_build_type_tolower STREQUAL ""
AND NOT cmake_build_type_tolower STREQUAL "profiling" AND NOT cmake_build_type_tolower STREQUAL "profiling"
AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo" AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo"
AND NOT cmake_build_type_tolower STREQUAL "minsizerel") AND NOT cmake_build_type_tolower STREQUAL "minsizerel")
message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).") message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo, MinSizeRel (case-insensitive).")
endif() endif()
# Mark that first pass is done
set(FIRST_PASS_DONE TRUE CACHE INTERNAL "Internally used to mark whether cmake has been run multiple times")
mark_as_advanced(FIRST_PASS_DONE)
# Enable Visual Studio solution folders # Enable Visual Studio solution folders
set_property(GLOBAL PROPERTY USE_FOLDERS On) set_property(GLOBAL PROPERTY USE_FOLDERS On)

4
cmake/GtsamTesting.cmake
View File

@ -166,7 +166,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
add_dependencies(check ${script_name}) add_dependencies(check ${script_name})
add_dependencies(all.tests ${script_name}) add_dependencies(all.tests ${script_name})
if(NOT MSVC AND NOT XCODE_VERSION) if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name}) add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name})
endif() endif()
# Add TOPSRCDIR # Add TOPSRCDIR
@ -254,7 +254,7 @@ macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName b
# Add target dependencies # Add target dependencies
add_dependencies(${groupName} ${script_name}) add_dependencies(${groupName} ${script_name})
if(NOT MSVC AND NOT XCODE_VERSION) if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name}) add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name})
endif() endif()
# Add TOPSRCDIR # Add TOPSRCDIR

3
gtsam.h
View File

@ -1742,7 +1742,8 @@ virtual class NonlinearFactor {
virtual class NoiseModelFactor: gtsam::NonlinearFactor { virtual class NoiseModelFactor: gtsam::NonlinearFactor {
void equals(const gtsam::NoiseModelFactor& other, double tol) const; void equals(const gtsam::NoiseModelFactor& other, double tol) const;
gtsam::noiseModel::Base* get_noiseModel() const; gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below
gtsam::noiseModel::Base* noiseModel() const;
Vector unwhitenedError(const gtsam::Values& x) const; Vector unwhitenedError(const gtsam::Values& x) const;
Vector whitenedError(const gtsam::Values& x) const; Vector whitenedError(const gtsam::Values& x) const;
}; };

4
gtsam/3rdparty/CMakeLists.txt vendored
View File

@ -43,7 +43,9 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
endif() endif()
option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF)
add_subdirectory(metis) if(GTSAM_SUPPORT_NESTED_DISSECTION)
add_subdirectory(metis)
endif()
add_subdirectory(ceres) add_subdirectory(ceres)

2
gtsam/3rdparty/ceres/fixed_array.h vendored
View File

@ -33,7 +33,7 @@
#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ #define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
#include <cstddef> #include <cstddef>
#include <Eigen/Core> #include <gtsam/3rdparty/ceres/eigen.h>
#include <gtsam/3rdparty/ceres/macros.h> #include <gtsam/3rdparty/ceres/macros.h>
#include <gtsam/3rdparty/ceres/manual_constructor.h> #include <gtsam/3rdparty/ceres/manual_constructor.h>

2
gtsam/3rdparty/ceres/jet.h vendored
View File

@ -162,7 +162,7 @@
#include <limits> #include <limits>
#include <string> #include <string>
#include <Eigen/Core> #include <gtsam/3rdparty/ceres/eigen.h>
#include <gtsam/3rdparty/ceres/fpclassify.h> #include <gtsam/3rdparty/ceres/fpclassify.h>
namespace ceres { namespace ceres {

6
gtsam/3rdparty/metis/CMakeLists.txt vendored
View File

@ -20,7 +20,7 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
endif() endif()
set(GKLIB_PATH ${PROJECT_SOURCE_DIR}/GKlib CACHE PATH "path to GKlib") set(GKLIB_PATH ${PROJECT_SOURCE_DIR}/GKlib CACHE PATH "path to GKlib")
set(SHARED FALSE CACHE BOOL "build a shared library") set(METIS_SHARED TRUE CACHE BOOL "build a shared library")
if(MSVC) if(MSVC)
set(METIS_INSTALL FALSE) set(METIS_INSTALL FALSE)
@ -29,11 +29,11 @@ else()
endif() endif()
# Configure libmetis library. # Configure libmetis library.
if(SHARED) if(METIS_SHARED)
set(METIS_LIBRARY_TYPE SHARED) set(METIS_LIBRARY_TYPE SHARED)
else() else()
set(METIS_LIBRARY_TYPE STATIC) set(METIS_LIBRARY_TYPE STATIC)
endif(SHARED) endif(METIS_SHARED)
include(${GKLIB_PATH}/GKlibSystem.cmake) include(${GKLIB_PATH}/GKlibSystem.cmake)
# Add include directories. # Add include directories.

4
gtsam/CMakeLists.txt
View File

@ -88,7 +88,9 @@ configure_file("${PROJECT_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h")
list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h") list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h")
install(FILES "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h" DESTINATION include/gtsam) install(FILES "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h" DESTINATION include/gtsam)
list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis) if(GTSAM_SUPPORT_NESTED_DISSECTION)
list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis)
endif()
# Versions # Versions
set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}) set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH})

9
gtsam/base/Lie.h
View File

@ -327,6 +327,15 @@ T expm(const Vector& x, int K=7) {
return T(expm(xhat,K)); return T(expm(xhat,K));
} }
/**
* Linear interpolation between X and Y by coefficient t in [0, 1].
*/
template <typename T>
T interpolate(const T& X, const T& Y, double t) {
assert(t >= 0 && t <= 1);
return traits<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
}
} // namespace gtsam } // namespace gtsam
/** /**

44
gtsam/base/Matrix.h
View File

@ -46,29 +46,29 @@ typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> M
// Create handy typedefs and constants for square-size matrices // Create handy typedefs and constants for square-size matrices
// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9 // MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9
#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX) \ #define GTSAM_MAKE_MATRIX_DEFS(N) \
typedef Eigen::Matrix<double, SIZE, SIZE> Matrix##SUFFIX; \ typedef Eigen::Matrix<double, N, N> Matrix##N; \
typedef Eigen::Matrix<double, 1, SIZE> Matrix1##SUFFIX; \ typedef Eigen::Matrix<double, 1, N> Matrix1##N; \
typedef Eigen::Matrix<double, 2, SIZE> Matrix2##SUFFIX; \ typedef Eigen::Matrix<double, 2, N> Matrix2##N; \
typedef Eigen::Matrix<double, 3, SIZE> Matrix3##SUFFIX; \ typedef Eigen::Matrix<double, 3, N> Matrix3##N; \
typedef Eigen::Matrix<double, 4, SIZE> Matrix4##SUFFIX; \ typedef Eigen::Matrix<double, 4, N> Matrix4##N; \
typedef Eigen::Matrix<double, 5, SIZE> Matrix5##SUFFIX; \ typedef Eigen::Matrix<double, 5, N> Matrix5##N; \
typedef Eigen::Matrix<double, 6, SIZE> Matrix6##SUFFIX; \ typedef Eigen::Matrix<double, 6, N> Matrix6##N; \
typedef Eigen::Matrix<double, 7, SIZE> Matrix7##SUFFIX; \ typedef Eigen::Matrix<double, 7, N> Matrix7##N; \
typedef Eigen::Matrix<double, 8, SIZE> Matrix8##SUFFIX; \ typedef Eigen::Matrix<double, 8, N> Matrix8##N; \
typedef Eigen::Matrix<double, 9, SIZE> Matrix9##SUFFIX; \ typedef Eigen::Matrix<double, 9, N> Matrix9##N; \
static const Eigen::MatrixBase<Matrix##SUFFIX>::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \ static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
static const Eigen::MatrixBase<Matrix##SUFFIX>::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero(); static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Zero();
GTSAM_MAKE_TYPEDEFS(1,1); GTSAM_MAKE_MATRIX_DEFS(1);
GTSAM_MAKE_TYPEDEFS(2,2); GTSAM_MAKE_MATRIX_DEFS(2);
GTSAM_MAKE_TYPEDEFS(3,3); GTSAM_MAKE_MATRIX_DEFS(3);
GTSAM_MAKE_TYPEDEFS(4,4); GTSAM_MAKE_MATRIX_DEFS(4);
GTSAM_MAKE_TYPEDEFS(5,5); GTSAM_MAKE_MATRIX_DEFS(5);
GTSAM_MAKE_TYPEDEFS(6,6); GTSAM_MAKE_MATRIX_DEFS(6);
GTSAM_MAKE_TYPEDEFS(7,7); GTSAM_MAKE_MATRIX_DEFS(7);
GTSAM_MAKE_TYPEDEFS(8,8); GTSAM_MAKE_MATRIX_DEFS(8);
GTSAM_MAKE_TYPEDEFS(9,9); GTSAM_MAKE_MATRIX_DEFS(9);
// Matrix expressions for accessing parts of matrices // Matrix expressions for accessing parts of matrices
typedef Eigen::Block<Matrix> SubMatrix; typedef Eigen::Block<Matrix> SubMatrix;

2
gtsam/base/Testable.h
View File

@ -123,7 +123,7 @@ namespace gtsam {
double tol_; double tol_;
equals_star(double tol = 1e-9) : tol_(tol) {} equals_star(double tol = 1e-9) : tol_(tol) {}
bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) { bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
if (!actual || !expected) return true; if (!actual && !expected) return true;
return actual && expected && traits<V>::Equals(*actual,*expected, tol_); return actual && expected && traits<V>::Equals(*actual,*expected, tol_);
} }
}; };

2
gtsam/base/Vector.cpp
View File

@ -292,7 +292,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights,
// Basically, instead of doing a normal QR step with the weighted // Basically, instead of doing a normal QR step with the weighted
// pseudoinverse, we enforce the constraint by turning // pseudoinverse, we enforce the constraint by turning
// ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0 // ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0
pseudo = delta(m, i, 1 / a[i]); pseudo = delta(m, i, 1.0 / a[i]);
return inf; return inf;
} }
} }

26
gtsam/base/Vector.h
View File

@ -41,13 +41,25 @@ typedef Eigen::VectorXd Vector;
typedef Eigen::Matrix<double, 1, 1> Vector1; typedef Eigen::Matrix<double, 1, 1> Vector1;
typedef Eigen::Vector2d Vector2; typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3; typedef Eigen::Vector3d Vector3;
typedef Eigen::Matrix<double, 4, 1> Vector4;
typedef Eigen::Matrix<double, 5, 1> Vector5; static const Eigen::MatrixBase<Vector2>::ConstantReturnType Z_2x1 = Vector2::Zero();
typedef Eigen::Matrix<double, 6, 1> Vector6; static const Eigen::MatrixBase<Vector3>::ConstantReturnType Z_3x1 = Vector3::Zero();
typedef Eigen::Matrix<double, 7, 1> Vector7;
typedef Eigen::Matrix<double, 8, 1> Vector8; // Create handy typedefs and constants for vectors with N>3
typedef Eigen::Matrix<double, 9, 1> Vector9; // VectorN and Z_Nx1, for N=1..9
typedef Eigen::Matrix<double, 10, 1> Vector10; #define GTSAM_MAKE_VECTOR_DEFS(N) \
typedef Eigen::Matrix<double, N, 1> Vector##N; \
static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero();
GTSAM_MAKE_VECTOR_DEFS(4);
GTSAM_MAKE_VECTOR_DEFS(5);
GTSAM_MAKE_VECTOR_DEFS(6);
GTSAM_MAKE_VECTOR_DEFS(7);
GTSAM_MAKE_VECTOR_DEFS(8);
GTSAM_MAKE_VECTOR_DEFS(9);
GTSAM_MAKE_VECTOR_DEFS(10);
GTSAM_MAKE_VECTOR_DEFS(11);
GTSAM_MAKE_VECTOR_DEFS(12);
typedef Eigen::VectorBlock<Vector> SubVector; typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector; typedef Eigen::VectorBlock<const Vector> ConstSubVector;

37
gtsam/base/VectorSpace.h
View File

@ -43,7 +43,7 @@ struct VectorSpaceImpl {
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Jacobian::Identity(); if (H1) *H1 = Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity();
return origin + (Class)v; return origin + v;
} }
/// @} /// @}
@ -117,7 +117,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(origin); if (H1) *H1 = Eye(origin);
if (H2) *H2 = Eye(origin); if (H2) *H2 = Eye(origin);
return origin + Class(v); return origin + v;
} }
/// @} /// @}
@ -159,12 +159,34 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
/// @} /// @}
}; };
/// A helper that implements the traits interface for GTSAM vector space types. /// Requirements on type to pass it to Manifold template below
/// To use this for your gtsam type, define: template<class Class>
/// template<> struct traits<Type> : public VectorSpaceTraits<Type> { }; struct HasVectorSpacePrereqs {
enum { dim = Class::dimension };
Class p, q;
Vector v;
BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) {
p = Class::identity(); // identity
q = p + p; // addition
q = p - p; // subtraction
v = p.vector(); // conversion to vector
q = p + v; // addition of a vector on the right
}
};
/// A helper that implements the traits interface for *classes* that define vector spaces
/// To use this for your class, define:
/// template<> struct traits<Class> : public VectorSpaceTraits<Class> {};
/// The class needs to support the requirements defined by HasVectorSpacePrereqs above
template<class Class> template<class Class>
struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> { struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
// Check that Class has the necessary machinery
BOOST_CONCEPT_ASSERT((HasVectorSpacePrereqs<Class>));
typedef vector_space_tag structure_category; typedef vector_space_tag structure_category;
/// @name Group /// @name Group
@ -180,12 +202,11 @@ struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
/// @} /// @}
}; };
/// Both VectorSpaceTRaits and Testable /// VectorSpace provides both Testable and VectorSpaceTraits
template<class Class> template<class Class>
struct VectorSpace: Testable<Class>, VectorSpaceTraits<Class> {}; struct VectorSpace: Testable<Class>, VectorSpaceTraits<Class> {};
/// A helper that implements the traits interface for GTSAM lie groups. /// A helper that implements the traits interface for scalar vector spaces. Usage:
/// To use this for your gtsam type, define:
/// template<> struct traits<Type> : public ScalarTraits<Type> { }; /// template<> struct traits<Type> : public ScalarTraits<Type> { };
template<typename Scalar> template<typename Scalar>
struct ScalarTraits : VectorSpaceImpl<Scalar, 1> { struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {

18
gtsam/base/VerticalBlockMatrix.h
View File

@ -165,24 +165,24 @@ namespace gtsam {
return variableColOffsets_[actualBlock]; return variableColOffsets_[actualBlock];
} }
/** Get the apparent first row of the underlying matrix for all operations */
const DenseIndex& rowStart() const { return rowStart_; }
/** Get or set the apparent first row of the underlying matrix for all operations */ /** Get or set the apparent first row of the underlying matrix for all operations */
DenseIndex& rowStart() { return rowStart_; } DenseIndex& rowStart() { return rowStart_; }
/** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
const DenseIndex& rowEnd() const { return rowEnd_; }
/** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ /** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
DenseIndex& rowEnd() { return rowEnd_; } DenseIndex& rowEnd() { return rowEnd_; }
/** Get the apparent first block for all operations */
const DenseIndex& firstBlock() const { return blockStart_; }
/** Get or set the apparent first block for all operations */ /** Get or set the apparent first block for all operations */
DenseIndex& firstBlock() { return blockStart_; } DenseIndex& firstBlock() { return blockStart_; }
/** Get the apparent first row of the underlying matrix for all operations */
DenseIndex rowStart() const { return rowStart_; }
/** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
DenseIndex rowEnd() const { return rowEnd_; }
/** Get the apparent first block for all operations */
DenseIndex firstBlock() const { return blockStart_; }
/** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ /** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */
const Matrix& matrix() const { return matrix_; } const Matrix& matrix() const { return matrix_; }

12
gtsam/base/numericalDerivative.h
View File

@ -131,11 +131,11 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
typedef typename TraitsX::TangentVector TangentX; typedef typename TraitsX::TangentVector TangentX;
// get value at x, and corresponding chart // get value at x, and corresponding chart
Y hx = h(x); const Y hx = h(x);
// Bit of a hack for now to find number of rows // Bit of a hack for now to find number of rows
TangentY zeroY = TraitsY::Local(hx, hx); const TangentY zeroY = TraitsY::Local(hx, hx);
size_t m = zeroY.size(); const size_t m = zeroY.size();
// Prepare a tangent vector to perturb x with, only works for fixed size // Prepare a tangent vector to perturb x with, only works for fixed size
TangentX dx; TangentX dx;
@ -143,12 +143,12 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
// Fill in Jacobian H // Fill in Jacobian H
Matrix H = zeros(m, N); Matrix H = zeros(m, N);
double factor = 1.0 / (2.0 * delta); const double factor = 1.0 / (2.0 * delta);
for (int j = 0; j < N; j++) { for (int j = 0; j < N; j++) {
dx(j) = delta; dx(j) = delta;
TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); const TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
dx(j) = -delta; dx(j) = -delta;
TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
dx(j) = 0; dx(j) = 0;
H.col(j) << (dy1 - dy2) * factor; H.col(j) << (dy1 - dy2) * factor;
} }

6
gtsam/base/tests/testFastContainers.cpp
View File

@ -22,11 +22,11 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testFastContainers, KeySet ) { TEST( testFastContainers, KeySet ) {
FastVector<Key> init_vector; KeyVector init_vector;
init_vector += 2, 3, 4, 5; init_vector += 2, 3, 4, 5;
FastSet<size_t> actSet(init_vector); KeySet actSet(init_vector);
FastSet<size_t> expSet; expSet += 2, 3, 4, 5; KeySet expSet; expSet += 2, 3, 4, 5;
EXPECT(actSet == expSet); EXPECT(actSet == expSet);
} }

3
gtsam/base/types.h
View File

@ -25,6 +25,7 @@
#include <gtsam/config.h> // for GTSAM_USE_TBB #include <gtsam/config.h> // for GTSAM_USE_TBB
#include <cstddef> #include <cstddef>
#include <cstdint>
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
#include <tbb/task_scheduler_init.h> #include <tbb/task_scheduler_init.h>
@ -53,7 +54,7 @@
namespace gtsam { namespace gtsam {
/// Integer nonlinear key type /// Integer nonlinear key type
typedef size_t Key; typedef std::uint64_t Key;
/// The index type for Eigen objects /// The index type for Eigen objects
typedef ptrdiff_t DenseIndex; typedef ptrdiff_t DenseIndex;

3
gtsam/config.h.in
View File

@ -65,3 +65,6 @@
// Publish flag about Eigen typedef // Publish flag about Eigen typedef
#cmakedefine GTSAM_USE_VECTOR3_POINTS #cmakedefine GTSAM_USE_VECTOR3_POINTS
// Support Metis-based nested dissection
#cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION

10
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
View File

@ -421,16 +421,16 @@ TEST(ADT, conversion)
ADT fDiscreteKey(X & Y, "0.2 0.5 0.3 0.6"); ADT fDiscreteKey(X & Y, "0.2 0.5 0.3 0.6");
dot(fDiscreteKey, "conversion-f1"); dot(fDiscreteKey, "conversion-f1");
std::map<size_t, size_t> ordering; std::map<Key, Key> keyMap;
ordering[0] = 5; keyMap[0] = 5;
ordering[1] = 2; keyMap[1] = 2;
AlgebraicDecisionTree<size_t> fIndexKey(fDiscreteKey, ordering); AlgebraicDecisionTree<Key> fIndexKey(fDiscreteKey, keyMap);
// f1.print("f1"); // f1.print("f1");
// f2.print("f2"); // f2.print("f2");
dot(fIndexKey, "conversion-f2"); dot(fIndexKey, "conversion-f2");
Assignment<size_t> x00, x01, x02, x10, x11, x12; Assignment<Key> x00, x01, x02, x10, x11, x12;
x00[5] = 0, x00[2] = 0; x00[5] = 0, x00[2] = 0;
x01[5] = 0, x01[2] = 1; x01[5] = 0, x01[2] = 1;
x10[5] = 1, x10[2] = 0; x10[5] = 1, x10[2] = 0;

5
gtsam/geometry/Cal3_S2.h
View File

@ -95,6 +95,11 @@ public:
return fy_; return fy_;
} }
/// aspect ratio
inline double aspectRatio() const {
return fx_/fy_;
}
/// skew /// skew
inline double skew() const { inline double skew() const {
return s_; return s_;

11
gtsam/geometry/EssentialMatrix.cpp
View File

@ -12,6 +12,17 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb,
OptionalJacobian<5, 3> H1,
OptionalJacobian<5, 2> H2) {
if (H1)
*H1 << I_3x3, Matrix23::Zero();
if (H2)
*H2 << Matrix32::Zero(), I_2x2;
return EssentialMatrix(aRb, aTb);
}
/* ************************************************************************* */ /* ************************************************************************* */
EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb, EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb,
OptionalJacobian<5, 6> H) { OptionalJacobian<5, 6> H) {

5
gtsam/geometry/EssentialMatrix.h
View File

@ -52,6 +52,11 @@ public:
Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) { Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) {
} }
/// Named constructor with derivatives
static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb,
OptionalJacobian<5, 3> H1 = boost::none,
OptionalJacobian<5, 2> H2 = boost::none);
/// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
static EssentialMatrix FromPose3(const Pose3& _1P2_, static EssentialMatrix FromPose3(const Pose3& _1P2_,
OptionalJacobian<5, 6> H = boost::none); OptionalJacobian<5, 6> H = boost::none);

20
gtsam/geometry/OrientedPlane3.cpp
View File

@ -57,13 +57,31 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d); return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const { Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const {
Vector2 n_error = -n_.localCoordinates(plane.n_); Vector2 n_error = -n_.localCoordinates(plane.n_);
return Vector3(n_error(0), n_error(1), d_ - plane.d_); return Vector3(n_error(0), n_error(1), d_ - plane.d_);
} }
/* ************************************************************************* */
Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
Matrix22 H_n_error_this, H_n_error_other;
Vector2 n_error = n_.errorVector(other.normal(), H1 ? &H_n_error_this : 0,
H2 ? &H_n_error_other : 0);
double d_error = d_ - other.d_;
if (H1) {
*H1 << H_n_error_this, Vector2::Zero(), 0, 0, 1;
}
if (H2) {
*H2 << H_n_error_other, Vector2::Zero(), 0, 0, -1;
}
return Vector3(n_error(0), n_error(1), d_error);
}
/* ************************************************************************* */ /* ************************************************************************* */
OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const { OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const {
return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2)); return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2));

9
gtsam/geometry/OrientedPlane3.h
View File

@ -114,6 +114,15 @@ public:
*/ */
Vector3 error(const OrientedPlane3& plane) const; Vector3 error(const OrientedPlane3& plane) const;
/** Computes the error between the two planes, with derivatives.
* This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses
* Unit3::localCoordinates. This one has correct derivatives.
* NOTE(hayk): The derivatives are zero when normals are exactly orthogonal.
* @param the other plane
*/
Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, //
OptionalJacobian<3, 3> H2 = boost::none) const;
/// Dimensionality of tangent space = 3 DOF /// Dimensionality of tangent space = 3 DOF
inline static size_t Dim() { inline static size_t Dim() {
return 3; return 3;

5
gtsam/geometry/PinholePose.h
View File

@ -328,6 +328,11 @@ public:
virtual ~PinholePose() { virtual ~PinholePose() {
} }
/// return shared pointer to calibration
const boost::shared_ptr<CALIBRATION>& sharedCalibration() const {
return K_;
}
/// return calibration /// return calibration
virtual const CALIBRATION& calibration() const { virtual const CALIBRATION& calibration() const {
return *K_; return *K_;

6
gtsam/geometry/Point2.cpp
View File

@ -140,4 +140,10 @@ ostream &operator<<(ostream &os, const Point2& p) {
return os; return os;
} }
/* ************************************************************************* */
ostream &operator<<(ostream &os, const gtsam::Point2Pair &p) {
os << p.first << " <-> " << p.second;
return os;
}
} // namespace gtsam } // namespace gtsam

11
gtsam/geometry/Point2.h
View File

@ -50,7 +50,7 @@ public:
/// @{ /// @{
/// construct from 2D vector /// construct from 2D vector
Point2(const Vector2& v) { explicit Point2(const Vector2& v) {
x_ = v(0); x_ = v(0);
y_ = v(1); y_ = v(1);
} }
@ -112,6 +112,11 @@ public:
/// inverse /// inverse
inline Point2 operator- () const {return Point2(-x_,-y_);} inline Point2 operator- () const {return Point2(-x_,-y_);}
/// add vector on right
inline Point2 operator +(const Vector2& v) const {
return Point2(x_ + v[0], y_ + v[1]);
}
/// add /// add
inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
@ -194,6 +199,10 @@ private:
/// @} /// @}
}; };
// Convenience typedef
typedef std::pair<Point2, Point2> Point2Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
// For MATLAB wrapper // For MATLAB wrapper
typedef std::vector<Point2> Point2Vector; typedef std::vector<Point2> Point2Vector;

2
gtsam/geometry/Point3.cpp
View File

@ -73,7 +73,7 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const { OptionalJacobian<3,3> H2) const {
if (H1) *H1 = I_3x3; if (H1) *H1 = I_3x3;
if (H2) *H2 = I_3x3; if (H2) *H2 = -I_3x3;
return *this - q; return *this - q;
} }
#endif #endif

14
gtsam/geometry/Point3.h
View File

@ -51,17 +51,12 @@ class GTSAM_EXPORT Point3 : public Vector3 {
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
#ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// Default constructor no longer initializes, just like Vector3 // Deprecated default constructor initializes to zero, in contrast to new behavior below
Point3() {} Point3() { setZero(); }
#endif #endif
/// Construct from x, y, and z coordinates using Vector3::Vector3;
Point3(double x, double y, double z): Vector3(x,y, z) {}
/// Construct from other vector
template<typename Derived>
inline Point3(const Eigen::MatrixBase<Derived>& v): Vector3(v) {}
/// @} /// @}
/// @name Testable /// @name Testable
@ -126,7 +121,6 @@ class GTSAM_EXPORT Point3 : public Vector3 {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated /// @name Deprecated
/// @{ /// @{
Point3() { setZero(); } // initializes to zero, in contrast to new behavior
Point3 inverse() const { return -(*this);} Point3 inverse() const { return -(*this);}
Point3 compose(const Point3& q) const { return (*this)+q;} Point3 compose(const Point3& q) const { return (*this)+q;}
Point3 between(const Point3& q) const { return q-(*this);} Point3 between(const Point3& q) const { return q-(*this);}

2
gtsam/geometry/Pose2.cpp
View File

@ -216,7 +216,7 @@ Point2 Pose2::transform_from(const Point2& point,
OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0); OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0);
OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2); OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2);
const Point2 q = r_.rotate(point, Hrotation, Hpoint); const Point2 q = r_.rotate(point, Hrotation, Hpoint);
if (Htranslation) *Htranslation = Hpoint ? *Hpoint : r_.matrix(); if (Htranslation) *Htranslation = (Hpoint ? *Hpoint : r_.matrix());
return q + t_; return q + t_;
} }

82
gtsam/geometry/Pose3.cpp
View File

@ -36,6 +36,14 @@ Pose3::Pose3(const Pose2& pose2) :
Point3(pose2.x(), pose2.y(), 0)) { Point3(pose2.x(), pose2.y(), 0)) {
} }
/* ************************************************************************* */
Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> H1,
OptionalJacobian<6, 3> H2) {
if (H1) *H1 << I_3x3, Z_3x3;
if (H2) *H2 << Z_3x3, R.transpose();
return Pose3(R, t);
}
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::inverse() const { Pose3 Pose3::inverse() const {
Rot3 Rt = R_.inverse(); Rot3 Rt = R_.inverse();
@ -193,8 +201,8 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
* The closed-form formula is similar to formula 102 in Barfoot14tro) * The closed-form formula is similar to formula 102 in Barfoot14tro)
*/ */
static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
const Vector3 w = xi.head<3>(); const auto w = xi.head<3>();
const Vector3 v = xi.tail<3>(); const auto v = xi.tail<3>();
const Matrix3 V = skewSymmetric(v); const Matrix3 V = skewSymmetric(v);
const Matrix3 W = skewSymmetric(w); const Matrix3 W = skewSymmetric(w);
@ -260,9 +268,18 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const {
return t_; return t_;
} }
/* ************************************************************************* */
const Rot3& Pose3::rotation(OptionalJacobian<3, 6> H) const {
if (H) {
*H << I_3x3, Z_3x3;
}
return R_;
}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix4 Pose3::matrix() const { Matrix4 Pose3::matrix() const {
static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished(); static const auto A14 = Eigen::RowVector4d(0,0,0,1);
Matrix4 mat; Matrix4 mat;
mat << R_.matrix(), t_, A14; mat << R_.matrix(), t_, A14;
return mat; return mat;
@ -275,6 +292,14 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
return Pose3(cRv, t); return Pose3(cRv, t);
} }
/* ************************************************************************* */
Pose3 Pose3::transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1,
OptionalJacobian<6, 6> H2) const {
if (H1) *H1 = -pose.inverse().AdjointMap() * AdjointMap();
if (H2) *H2 = I_6x6;
return inverse() * pose;
}
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
OptionalJacobian<3,3> Dpoint) const { OptionalJacobian<3,3> Dpoint) const {
@ -355,41 +380,54 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::optional<Pose3> align(const vector<Point3Pair>& pairs) { boost::optional<Pose3> Pose3::Align(const std::vector<Point3Pair>& abPointPairs) {
const size_t n = pairs.size(); const size_t n = abPointPairs.size();
if (n < 3) if (n < 3)
return boost::none; // we need at least three pairs return boost::none; // we need at least three pairs
// calculate centroids // calculate centroids
Point3 cp(0,0,0), cq(0,0,0); Point3 aCentroid(0,0,0), bCentroid(0,0,0);
BOOST_FOREACH(const Point3Pair& pair, pairs) { for(const Point3Pair& abPair: abPointPairs) {
cp += pair.first; aCentroid += abPair.first;
cq += pair.second; bCentroid += abPair.second;
} }
double f = 1.0 / n; double f = 1.0 / n;
cp *= f; aCentroid *= f;
cq *= f; bCentroid *= f;
// Add to form H matrix // Add to form H matrix
Matrix3 H = Z_3x3; Matrix3 H = Z_3x3;
BOOST_FOREACH(const Point3Pair& pair, pairs) { for(const Point3Pair& abPair: abPointPairs) {
Point3 dp = pair.first - cp; Point3 da = abPair.first - aCentroid;
Point3 dq = pair.second - cq; Point3 db = abPair.second - bCentroid;
H += dp * dq.transpose(); H += db * da.transpose();
} }
// Compute SVD // Compute SVD
Matrix U, V; Eigen::JacobiSVD<Matrix> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
Vector S; Matrix U = svd.matrixU();
svd(H, U, S, V); Vector S = svd.singularValues();
Matrix V = svd.matrixV();
// Check rank
if (S[1] < 1e-10)
return boost::none;
// Recover transform with correction from Eggert97machinevisionandapplications // Recover transform with correction from Eggert97machinevisionandapplications
Matrix3 UVtranspose = U * V.transpose(); Matrix3 UVtranspose = U * V.transpose();
Matrix3 detWeighting = I_3x3; Matrix3 detWeighting = I_3x3;
detWeighting(2, 2) = UVtranspose.determinant(); detWeighting(2, 2) = UVtranspose.determinant();
Rot3 R(Matrix3(V * detWeighting * U.transpose())); Rot3 aRb(Matrix(V * detWeighting * U.transpose()));
Point3 t = Point3(cq) - R * Point3(cp); Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid);
return Pose3(R, t); return Pose3(aRb, aTb);
}
boost::optional<Pose3> align(const vector<Point3Pair>& baPointPairs) {
vector<Point3Pair> abPointPairs;
BOOST_FOREACH (const Point3Pair& baPair, baPointPairs) {
abPointPairs.push_back(make_pair(baPair.second, baPair.first));
}
return Pose3::Align(abPointPairs);
} }
/* ************************************************************************* */ /* ************************************************************************* */

35
gtsam/geometry/Pose3.h
View File

@ -73,6 +73,19 @@ public:
T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) { T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
} }
/// Named constructor with derivatives
static Pose3 Create(const Rot3& R, const Point3& t,
OptionalJacobian<6, 3> H1 = boost::none,
OptionalJacobian<6, 3> H2 = boost::none);
/**
* Create Pose3 by aligning two point pairs
* A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point
* Meant to replace the deprecated function 'align', which orders the pairs the opposite way.
* Note this allows for noise on the points but in that case the mapping will not be exact.
*/
static boost::optional<Pose3> Align(const std::vector<Point3Pair>& abPointPairs);
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
@ -213,9 +226,7 @@ public:
/// @{ /// @{
/// get rotation /// get rotation
const Rot3& rotation() const { const Rot3& rotation(OptionalJacobian<3, 6> H = boost::none) const;
return R_;
}
/// get translation /// get translation
const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const; const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const;
@ -238,9 +249,15 @@ public:
/** convert to 4*4 matrix */ /** convert to 4*4 matrix */
Matrix4 matrix() const; Matrix4 matrix() const;
/** receives a pose in world coordinates and transforms it to local coordinates */ /** receives a pose in local coordinates and transforms it to world coordinates
* @deprecated: This is actually equivalent to transform_from, so it is WRONG! Use
* transform_pose_to instead. */
Pose3 transform_to(const Pose3& pose) const; Pose3 transform_to(const Pose3& pose) const;
/** receives a pose in world coordinates and transforms it to local coordinates */
Pose3 transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none,
OptionalJacobian<6, 6> H2 = boost::none) const;
/** /**
* Calculate range to a landmark * Calculate range to a landmark
* @param point 3D location of landmark * @param point 3D location of landmark
@ -291,7 +308,7 @@ public:
GTSAM_EXPORT GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose3& p); friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
@ -317,11 +334,11 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
} }
/** /**
* Calculate pose between a vector of 3D point correspondences (p,q) * Calculate pose between a vector of 3D point correspondences (b_point, a_point)
* where q = Pose3::transform_from(p) = t + R*p * where a_point = Pose3::transform_from(b_point) = t + R*b_point
* @deprecated: use Pose3::Align with point pairs ordered the opposite way
*/ */
typedef std::pair<Point3, Point3> Point3Pair; GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& baPointPairs);
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
// For MATLAB wrapper // For MATLAB wrapper
typedef std::vector<Pose3> Pose3Vector; typedef std::vector<Pose3> Pose3Vector;

57
gtsam/geometry/Rot3.cpp
View File

@ -42,6 +42,58 @@ Rot3 Rot3::Random(boost::mt19937& rng) {
return AxisAngle(axis, angle); return AxisAngle(axis, angle);
} }
/* ************************************************************************* */
Rot3 Rot3::AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p) {
// if a_p is already aligned with b_p, return the identity rotation
if (std::abs(a_p.dot(b_p)) > 0.999999999) {
return Rot3();
}
// Check axis was not degenerate cross product
const Vector3 z = axis.unitVector();
if (z.hasNaN())
throw std::runtime_error("AlignSinglePair: axis has Nans");
// Now, calculate rotation that takes b_p to a_p
const Matrix3 P = I_3x3 - z * z.transpose(); // orthogonal projector
const Vector3 a_po = P * a_p.unitVector(); // point in a orthogonal to axis
const Vector3 b_po = P * b_p.unitVector(); // point in b orthogonal to axis
const Vector3 x = a_po.normalized(); // x-axis in axis-orthogonal plane, along a_p vector
const Vector3 y = z.cross(x); // y-axis in axis-orthogonal plane
const double u = x.dot(b_po); // x-coordinate for b_po
const double v = y.dot(b_po); // y-coordinate for b_po
double angle = std::atan2(v, u);
return Rot3::AxisAngle(z, -angle);
}
/* ************************************************************************* */
Rot3 Rot3::AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, //
const Unit3& a_q, const Unit3& b_q) {
// there are three frames in play:
// a: the first frame in which p and q are measured
// b: the second frame in which p and q are measured
// i: intermediate, after aligning first pair
// First, find rotation around that aligns a_p and b_p
Rot3 i_R_b = AlignPair(a_p.cross(b_p), a_p, b_p);
// Rotate points in frame b to the intermediate frame,
// in which we expect the point p to be aligned now
Unit3 i_q = i_R_b * b_q;
assert(assert_equal(a_p, i_R_b * b_p, 1e-6));
// Now align second pair: we need to align i_q to a_q
Rot3 a_R_i = AlignPair(a_p, a_q, i_q);
assert(assert_equal(a_p, a_R_i * a_p, 1e-6));
assert(assert_equal(a_q, a_R_i * i_q, 1e-6));
// The desired rotation is the product of both
Rot3 a_R_b = a_R_i * i_R_b;
return a_R_b;
}
/* ************************************************************************* */ /* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const { bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol); return equal_with_abs_tol(matrix(), R.matrix(), tol);
@ -172,10 +224,7 @@ ostream &operator<<(ostream &os, const Rot3& R) {
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::slerp(double t, const Rot3& other) const { Rot3 Rot3::slerp(double t, const Rot3& other) const {
// amazingly simple in GTSAM :-) return interpolate(*this, other, t);
assert(t>=0 && t<=1);
Vector3 omega = Logmap(between(other));
return compose(Expmap(t * omega));
} }
/* ************************************************************************* */ /* ************************************************************************* */

10
gtsam/geometry/Rot3.h
View File

@ -215,6 +215,13 @@ namespace gtsam {
return Rodrigues(Vector3(wx, wy, wz)); return Rodrigues(Vector3(wx, wy, wz));
} }
/// Determine a rotation to bring two vectors into alignment, using the rotation axis provided
static Rot3 AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p);
/// Calculate rotation from two pairs of homogeneous points using two successive rotations
static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, //
const Unit3& a_q, const Unit3& b_q);
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
@ -456,9 +463,6 @@ namespace gtsam {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated /// @name Deprecated
/// @{ /// @{
#ifndef GTSAM_USE_VECTOR3_POINTS
static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); }
#endif
static Rot3 rodriguez(const Point3& axis, double angle) { return AxisAngle(axis, angle); } static Rot3 rodriguez(const Point3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); } static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); } static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); }

6
gtsam/geometry/Rot3M.cpp
View File

@ -36,9 +36,9 @@ Rot3::Rot3() : rot_(I_3x3) {}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
rot_.col(0) = (Vector3)col1; rot_.col(0) = col1;
rot_.col(1) = (Vector3)col2; rot_.col(1) = col2;
rot_.col(2) = (Vector3)col3; rot_.col(2) = col3;
} }
/* ************************************************************************* */ /* ************************************************************************* */

5
gtsam/geometry/SO3.cpp
View File

@ -29,11 +29,12 @@ namespace so3 {
void ExpmapFunctor::init(bool nearZeroApprox) { void ExpmapFunctor::init(bool nearZeroApprox) {
nearZero = nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon()); nearZero = nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
if (nearZero) return; if (!nearZero) {
theta = std::sqrt(theta2); // rotation angle theta = std::sqrt(theta2); // rotation angle
sin_theta = std::sin(theta); sin_theta = std::sin(theta);
const double s2 = std::sin(theta / 2.0); const double s2 = std::sin(theta / 2.0);
one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
}
} }
ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
@ -42,7 +43,6 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
init(nearZeroApprox); init(nearZeroApprox);
if (!nearZero) { if (!nearZero) {
theta = std::sqrt(theta2);
K = W / theta; K = W / theta;
KK = K * K; KK = K * K;
} }
@ -55,7 +55,6 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApp
W = K * angle; W = K * angle;
init(nearZeroApprox); init(nearZeroApprox);
if (!nearZero) { if (!nearZero) {
theta = angle;
KK = K * K; KK = K * K;
} }
} }

7
gtsam/geometry/StereoPoint2.h
View File

@ -50,7 +50,7 @@ public:
} }
/// construct from 3D vector /// construct from 3D vector
StereoPoint2(const Vector3& v) : explicit StereoPoint2(const Vector3& v) :
uL_(v(0)), uR_(v(1)), v_(v(2)) {} uL_(v(0)), uR_(v(1)), v_(v(2)) {}
/// @} /// @}
@ -80,6 +80,11 @@ public:
return StereoPoint2(-uL_, -uR_, -v_); return StereoPoint2(-uL_, -uR_, -v_);
} }
/// add vector on right
inline StereoPoint2 operator +(const Vector3& v) const {
return StereoPoint2(uL_ + v[0], uR_ + v[1], v_ + v[2]);
}
/// add /// add
inline StereoPoint2 operator +(const StereoPoint2& b) const { inline StereoPoint2 operator +(const StereoPoint2& b) const {
return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);

52
gtsam/geometry/Unit3.cpp
View File

@ -60,7 +60,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
// around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng). // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> > generator( boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> > generator(
rng, randomDirection); rng, randomDirection);
vector<double> d = generator(); const vector<double> d = generator();
return Unit3(d[0], d[1], d[2]); return Unit3(d[0], d[1], d[2]);
} }
@ -100,16 +100,16 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
// Choose the direction of the first basis vector b1 in the tangent plane by crossing n with // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with
// the chosen axis. // the chosen axis.
Matrix33 H_B1_n; Matrix33 H_B1_n;
Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : 0); Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr);
// Normalize result to get a unit vector: b1 = B1 / |B1|. // Normalize result to get a unit vector: b1 = B1 / |B1|.
Matrix33 H_b1_B1; Matrix33 H_b1_B1;
Point3 b1 = normalize(B1, H ? &H_b1_B1 : 0); Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr);
// Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them.
// No need to normalize this, p and b1 are orthogonal unit vectors. // No need to normalize this, p and b1 are orthogonal unit vectors.
Matrix33 H_b2_n, H_b2_b1; Matrix33 H_b2_n, H_b2_b1;
Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0); Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr);
// Create the basis by stacking b1 and b2. // Create the basis by stacking b1 and b2.
B_.reset(Matrix32()); B_.reset(Matrix32());
@ -118,8 +118,8 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
if (H) { if (H) {
// Chain rule tomfoolery to compute the derivative. // Chain rule tomfoolery to compute the derivative.
const Matrix32& H_n_p = *B_; const Matrix32& H_n_p = *B_;
Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p;
Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;
// Cache the derivative and fill the result. // Cache the derivative and fill the result.
H_B_.reset(Matrix62()); H_B_.reset(Matrix62());
@ -164,14 +164,14 @@ Matrix3 Unit3::skew() const {
double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const { double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const {
// Get the unit vectors of each, and the derivative. // Get the unit vectors of each, and the derivative.
Matrix32 H_pn_p; Matrix32 H_pn_p;
Point3 pn = point3(H_p ? &H_pn_p : 0); Point3 pn = point3(H_p ? &H_pn_p : nullptr);
Matrix32 H_qn_q; Matrix32 H_qn_q;
Point3 qn = q.point3(H_q ? &H_qn_q : 0); const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr);
// Compute the dot product of the Point3s. // Compute the dot product of the Point3s.
Matrix13 H_dot_pn, H_dot_qn; Matrix13 H_dot_pn, H_dot_qn;
double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0); double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : nullptr, H_q ? &H_dot_qn : nullptr);
if (H_p) { if (H_p) {
(*H_p) << H_dot_pn * H_pn_p; (*H_p) << H_dot_pn * H_pn_p;
@ -187,10 +187,9 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,
/* ************************************************************************* */ /* ************************************************************************* */
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const { Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const {
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
Matrix23 Bt = basis().transpose(); const Vector2 xi = basis().transpose() * q.p_;
Vector2 xi = Bt * q.p_;
if (H_q) { if (H_q) {
*H_q = Bt * q.basis(); *H_q = basis().transpose() * q.basis();
} }
return xi; return xi;
} }
@ -199,11 +198,11 @@ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const {
Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const { Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const {
// Get the point3 of this, and the derivative. // Get the point3 of this, and the derivative.
Matrix32 H_qn_q; Matrix32 H_qn_q;
Point3 qn = q.point3(H_q ? &H_qn_q : 0); const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr);
// 2D error here is projecting q into the tangent plane of this (p). // 2D error here is projecting q into the tangent plane of this (p).
Matrix62 H_B_p; Matrix62 H_B_p;
Matrix23 Bt = basis(H_p ? &H_B_p : 0).transpose(); Matrix23 Bt = basis(H_p ? &H_B_p : nullptr).transpose();
Vector2 xi = Bt * qn; Vector2 xi = Bt * qn;
if (H_p) { if (H_p) {
@ -212,18 +211,18 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ
const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0); const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0);
// Derivatives of the two entries of xi wrt the basis vectors. // Derivatives of the two entries of xi wrt the basis vectors.
Matrix13 H_xi1_b1 = qn.transpose(); const Matrix13 H_xi1_b1 = qn.transpose();
Matrix13 H_xi2_b2 = qn.transpose(); const Matrix13 H_xi2_b2 = qn.transpose();
// Assemble dxi/dp = dxi/dB * dB/dp. // Assemble dxi/dp = dxi/dB * dB/dp.
Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p; const Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p;
Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p; const Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p;
*H_p << H_xi1_p, H_xi2_p; *H_p << H_xi1_p, H_xi2_p;
} }
if (H_q) { if (H_q) {
// dxi/dq is given by dxi/dqu * dqu/dq, where qu is the unit vector of q. // dxi/dq is given by dxi/dqu * dqu/dq, where qu is the unit vector of q.
Matrix23 H_xi_qu = Bt; const Matrix23 H_xi_qu = Bt;
*H_q = H_xi_qu * H_qn_q; *H_q = H_xi_qu * H_qn_q;
} }
@ -232,26 +231,27 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ
/* ************************************************************************* */ /* ************************************************************************* */
double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
Matrix2 H_; Matrix2 H_xi_q;
Vector2 xi = error(q, H_); const Vector2 xi = error(q, H ? &H_xi_q : nullptr);
double theta = xi.norm(); const double theta = xi.norm();
if (H) if (H)
*H = (xi.transpose() / theta) * H_; *H = (xi.transpose() / theta) * H_xi_q;
return theta; return theta;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Unit3 Unit3::retract(const Vector2& v) const { Unit3 Unit3::retract(const Vector2& v) const {
// Compute the 3D xi_hat vector // Compute the 3D xi_hat vector
Vector3 xi_hat = basis() * v; const Vector3 xi_hat = basis() * v;
double theta = xi_hat.norm(); const double theta = xi_hat.norm();
// Treat case of very small v differently // Treat case of very small v differently
if (theta < std::numeric_limits<double>::epsilon()) { if (theta < std::numeric_limits<double>::epsilon()) {
return Unit3(Vector3(std::cos(theta) * p_ + xi_hat)); return Unit3(Vector3(std::cos(theta) * p_ + xi_hat));
} }
Vector3 exp_p_xi_hat = std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); const Vector3 exp_p_xi_hat =
std::cos(theta) * p_ + xi_hat * (sin(theta) / theta);
return Unit3(exp_p_xi_hat); return Unit3(exp_p_xi_hat);
} }

7
gtsam/geometry/Unit3.h
View File

@ -65,14 +65,7 @@ public:
p_(1.0, 0.0, 0.0) { p_(1.0, 0.0, 0.0) {
} }
#ifndef GTSAM_USE_VECTOR3_POINTS
/// Construct from point /// Construct from point
explicit Unit3(const Point3& p) :
p_(p.vector().normalized()) {
}
#endif
/// Construct from a vector3
explicit Unit3(const Vector3& p) : explicit Unit3(const Vector3& p) :
p_(p.normalized()) { p_(p.normalized()) {
} }

49
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -20,20 +20,41 @@ GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix)
//************************************************************************* //*************************************************************************
// Create two cameras and corresponding essential matrix E // Create two cameras and corresponding essential matrix E
Rot3 c1Rc2 = Rot3::Yaw(M_PI_2); Rot3 trueRotation = Rot3::Yaw(M_PI_2);
Point3 c1Tc2(0.1, 0, 0); Point3 trueTranslation(0.1, 0, 0);
EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); Unit3 trueDirection(trueTranslation);
EssentialMatrix trueE(trueRotation, trueDirection);
//************************************************************************* //*************************************************************************
TEST (EssentialMatrix, equality) { TEST (EssentialMatrix, equality) {
EssentialMatrix actual(c1Rc2, Unit3(c1Tc2)), expected(c1Rc2, Unit3(c1Tc2)); EssentialMatrix actual(trueRotation, trueDirection), expected(trueRotation, trueDirection);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
//*************************************************************************
TEST(EssentialMatrix, FromRotationAndDirection) {
Matrix actualH1, actualH2;
EXPECT(assert_equal(
trueE, EssentialMatrix::FromRotationAndDirection(trueRotation, trueDirection, actualH1, actualH2),
1e-8));
Matrix expectedH1 = numericalDerivative11<EssentialMatrix, Rot3>(
boost::bind(EssentialMatrix::FromRotationAndDirection, _1, trueDirection, boost::none,
boost::none),
trueRotation);
EXPECT(assert_equal(expectedH1, actualH1, 1e-7));
Matrix expectedH2 = numericalDerivative11<EssentialMatrix, Unit3>(
boost::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, _1, boost::none,
boost::none),
trueDirection);
EXPECT(assert_equal(expectedH2, actualH2, 1e-7));
}
//************************************************************************* //*************************************************************************
TEST (EssentialMatrix, FromPose3) { TEST (EssentialMatrix, FromPose3) {
EssentialMatrix expected(c1Rc2, Unit3(c1Tc2)); EssentialMatrix expected(trueRotation, trueDirection);
Pose3 pose(c1Rc2, c1Tc2); Pose3 pose(trueRotation, trueTranslation);
EssentialMatrix actual = EssentialMatrix::FromPose3(pose); EssentialMatrix actual = EssentialMatrix::FromPose3(pose);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
@ -50,7 +71,7 @@ TEST(EssentialMatrix, localCoordinates0) {
TEST (EssentialMatrix, localCoordinates) { TEST (EssentialMatrix, localCoordinates) {
// Pose between two cameras // Pose between two cameras
Pose3 pose(c1Rc2, c1Tc2); Pose3 pose(trueRotation, trueTranslation);
EssentialMatrix hx = EssentialMatrix::FromPose3(pose); EssentialMatrix hx = EssentialMatrix::FromPose3(pose);
Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose)); Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose));
EXPECT(assert_equal(zero(5), actual, 1e-8)); EXPECT(assert_equal(zero(5), actual, 1e-8));
@ -70,15 +91,15 @@ TEST (EssentialMatrix, retract0) {
//************************************************************************* //*************************************************************************
TEST (EssentialMatrix, retract1) { TEST (EssentialMatrix, retract1) {
EssentialMatrix expected(c1Rc2.retract(Vector3(0.1, 0, 0)), Unit3(c1Tc2)); EssentialMatrix expected(trueRotation.retract(Vector3(0.1, 0, 0)), trueDirection);
EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0).finished()); EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0).finished());
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
//************************************************************************* //*************************************************************************
TEST (EssentialMatrix, retract2) { TEST (EssentialMatrix, retract2) {
EssentialMatrix expected(c1Rc2, EssentialMatrix expected(trueRotation,
Unit3(c1Tc2).retract(Vector2(0.1, 0))); trueDirection.retract(Vector2(0.1, 0)));
EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0).finished()); EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0).finished());
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
@ -124,8 +145,8 @@ TEST (EssentialMatrix, rotate) {
Rot3 bRc(bX, bZ, -bY), cRb = bRc.inverse(); Rot3 bRc(bX, bZ, -bY), cRb = bRc.inverse();
// Let's compute the ground truth E in body frame: // Let's compute the ground truth E in body frame:
Rot3 b1Rb2 = bRc * c1Rc2 * cRb; Rot3 b1Rb2 = bRc * trueRotation * cRb;
Point3 b1Tb2 = bRc * c1Tc2; Point3 b1Tb2 = bRc * trueTranslation;
EssentialMatrix bodyE(b1Rb2, Unit3(b1Tb2)); EssentialMatrix bodyE(b1Rb2, Unit3(b1Tb2));
EXPECT(assert_equal(bodyE, bRc * trueE, 1e-8)); EXPECT(assert_equal(bodyE, bRc * trueE, 1e-8));
EXPECT(assert_equal(bodyE, trueE.rotate(bRc), 1e-8)); EXPECT(assert_equal(bodyE, trueE.rotate(bRc), 1e-8));
@ -149,7 +170,7 @@ TEST (EssentialMatrix, rotate) {
//************************************************************************* //*************************************************************************
TEST (EssentialMatrix, FromPose3_a) { TEST (EssentialMatrix, FromPose3_a) {
Matrix actualH; Matrix actualH;
Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras
EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>( Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
@ -171,7 +192,7 @@ TEST (EssentialMatrix, FromPose3_b) {
//************************************************************************* //*************************************************************************
TEST (EssentialMatrix, streaming) { TEST (EssentialMatrix, streaming) {
EssentialMatrix expected(c1Rc2, Unit3(c1Tc2)), actual; EssentialMatrix expected(trueRotation, trueDirection), actual;
stringstream ss; stringstream ss;
ss << expected; ss << expected;
ss >> actual; ss >> actual;

24
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -139,6 +139,30 @@ TEST(OrientedPlane3, localCoordinates_retract) {
} }
} }
//*******************************************************************************
TEST (OrientedPlane3, error2) {
OrientedPlane3 plane1(-1, 0.1, 0.2, 5);
OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4);
// Hard-coded regression values, to ensure the result doesn't change.
EXPECT(assert_equal(zero(3), plane1.errorVector(plane1), 1e-8));
EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4), plane1.errorVector(plane2), 1e-5));
// Test the jacobians of transform
Matrix33 actualH1, expectedH1, actualH2, expectedH2;
Vector3 actual = plane1.errorVector(plane2, actualH1, actualH2);
EXPECT(assert_equal(plane1.normal().errorVector(plane2.normal()), Vector2(actual[0], actual[1])));
EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2]));
boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f = //
boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
expectedH1 = numericalDerivative21(f, plane1, plane2);
expectedH2 = numericalDerivative22(f, plane1, plane2);
EXPECT(assert_equal(expectedH1, actualH1, 1e-9));
EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
srand(time(NULL)); srand(time(NULL));

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

@ -81,6 +81,71 @@ TEST( Point3, dot) {
Point3 origin(0,0,0), ones(1, 1, 1); Point3 origin(0,0,0), ones(1, 1, 1);
CHECK(origin.dot(Point3(1, 1, 0)) == 0); CHECK(origin.dot(Point3(1, 1, 0)) == 0);
CHECK(ones.dot(Point3(1, 1, 0)) == 2); CHECK(ones.dot(Point3(1, 1, 0)) == 2);
Point3 p(1, 0.2, 0.3);
Point3 q = p + Point3(0.5, 0.2, -3.0);
Point3 r = p + Point3(0.8, 0, 0);
Point3 t = p + Point3(0, 0.3, -0.4);
EXPECT(assert_equal(1.130000, p.dot(p), 1e-8));
EXPECT(assert_equal(0.770000, p.dot(q), 1e-5));
EXPECT(assert_equal(1.930000, p.dot(r), 1e-5));
EXPECT(assert_equal(1.070000, p.dot(t), 1e-5));
// Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2;
boost::function<double(const Point3&, const Point3&)> f = boost::bind(&Point3::dot, _1, _2, //
boost::none, boost::none);
{
p.dot(q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<double,Point3>(f, p, q), H1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<double,Point3>(f, p, q), H2, 1e-9));
}
{
p.dot(r, H1, H2);
EXPECT(assert_equal(numericalDerivative21<double,Point3>(f, p, r), H1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<double,Point3>(f, p, r), H2, 1e-9));
}
{
p.dot(t, H1, H2);
EXPECT(assert_equal(numericalDerivative21<double,Point3>(f, p, t), H1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<double,Point3>(f, p, t), H2, 1e-9));
}
}
/* ************************************************************************* */
TEST(Point3, cross) {
Matrix aH1, aH2;
boost::function<Point3(const Point3&, const Point3&)> f =
boost::bind(&gtsam::cross, _1, _2, boost::none, boost::none);
const Point3 omega(0, 1, 0), theta(4, 6, 8);
cross(omega, theta, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
}
/* ************************************************************************* */
TEST( Point3, cross2) {
Point3 p(1, 0.2, 0.3);
Point3 q = p + Point3(0.5, 0.2, -3.0);
Point3 r = p + Point3(0.8, 0, 0);
EXPECT(assert_equal(Point3(0, 0, 0), p.cross(p), 1e-8));
EXPECT(assert_equal(Point3(-0.66, 3.15, 0.1), p.cross(q), 1e-5));
EXPECT(assert_equal(Point3(0, 0.24, -0.16), p.cross(r), 1e-5));
// Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2;
boost::function<Point3(const Point3&, const Point3&)> f = boost::bind(&Point3::cross, _1, _2, //
boost::none, boost::none);
{
p.cross(q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(f, p, q), H1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<Point3,Point3>(f, p, q), H2, 1e-9));
}
{
p.cross(r, H1, H2);
EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(f, p, r), H1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<Point3,Point3>(f, p, r), H2, 1e-9));
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -135,17 +200,6 @@ TEST (Point3, distance) {
EXPECT(assert_equal(numH2, H2, 1e-8)); EXPECT(assert_equal(numH2, H2, 1e-8));
} }
/* ************************************************************************* */
TEST(Point3, cross) {
Matrix aH1, aH2;
boost::function<Point3(const Point3&, const Point3&)> f =
boost::bind(&gtsam::cross, _1, _2, boost::none, boost::none);
const Point3 omega(0, 1, 0), theta(4, 6, 8);
cross(omega, theta, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

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

@ -31,12 +31,13 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Pose3) GTSAM_CONCEPT_TESTABLE_INST(Pose3)
GTSAM_CONCEPT_LIE_INST(Pose3) GTSAM_CONCEPT_LIE_INST(Pose3)
static Point3 P(0.2,0.7,-2); static const Point3 P(0.2,0.7,-2);
static Rot3 R = Rot3::Rodrigues(0.3,0,0); static const Rot3 R = Rot3::Rodrigues(0.3,0,0);
static Pose3 T(R,Point3(3.5,-8.2,4.2)); static const Point3 P2(3.5,-8.2,4.2);
static Pose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)); static const Pose3 T(R,P2);
static Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3)); static const Pose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),P2);
const double tol=1e-5; static const Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3));
static const double tol=1e-5;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, equals) TEST( Pose3, equals)
@ -197,6 +198,17 @@ TEST(Pose3, translation) {
EXPECT(assert_equal(numericalH, actualH, 1e-6)); EXPECT(assert_equal(numericalH, actualH, 1e-6));
} }
/* ************************************************************************* */
// Check rotation and its pushforward
TEST(Pose3, rotation) {
Matrix actualH;
EXPECT(assert_equal(R, T.rotation(actualH), 1e-8));
Matrix numericalH = numericalDerivative11<Rot3, Pose3>(
boost::bind(&Pose3::rotation, _1, boost::none), T);
EXPECT(assert_equal(numericalH, actualH, 1e-6));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3, Adjoint_compose_full) TEST(Pose3, Adjoint_compose_full)
{ {
@ -403,6 +415,41 @@ TEST( Pose3, transform_to)
EXPECT(assert_equal(expected, actual, 0.001)); EXPECT(assert_equal(expected, actual, 0.001));
} }
Pose3 transform_pose_to_(const Pose3& pose, const Pose3& pose2) { return pose.transform_pose_to(pose2); }
/* ************************************************************************* */
TEST( Pose3, transform_pose_to)
{
Pose3 origin = T.transform_pose_to(T);
EXPECT(assert_equal(Pose3{}, origin));
}
/* ************************************************************************* */
TEST( Pose3, transform_pose_to_with_derivatives)
{
Matrix actH1, actH2;
Pose3 res = T.transform_pose_to(T2,actH1,actH2);
EXPECT(assert_equal(res, T.inverse().compose(T2)));
Matrix expH1 = numericalDerivative21(transform_pose_to_, T, T2),
expH2 = numericalDerivative22(transform_pose_to_, T, T2);
EXPECT(assert_equal(expH1, actH1, 1e-8));
EXPECT(assert_equal(expH2, actH2, 1e-8));
}
/* ************************************************************************* */
TEST( Pose3, transform_pose_to_with_derivatives2)
{
Matrix actH1, actH2;
Pose3 res = T.transform_pose_to(T3,actH1,actH2);
EXPECT(assert_equal(res, T.inverse().compose(T3)));
Matrix expH1 = numericalDerivative21(transform_pose_to_, T, T3),
expH2 = numericalDerivative22(transform_pose_to_, T, T3);
EXPECT(assert_equal(expH1, actH1, 1e-8));
EXPECT(assert_equal(expH2, actH2, 1e-8));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_from) TEST( Pose3, transform_from)
{ {
@ -476,7 +523,7 @@ TEST(Pose3, Retract_LocalCoordinates)
{ {
Vector6 d; Vector6 d;
d << 1,2,3,4,5,6; d/=10; d << 1,2,3,4,5,6; d/=10;
R = Rot3::Retract(d.head<3>()); const Rot3 R = Rot3::Retract(d.head<3>());
Pose3 t = Pose3::Retract(d); Pose3 t = Pose3::Retract(d);
EXPECT(assert_equal(d, Pose3::LocalCoordinates(t))); EXPECT(assert_equal(d, Pose3::LocalCoordinates(t)));
} }
@ -506,6 +553,8 @@ TEST(Pose3, retract_localCoordinates2)
EXPECT(assert_equal(t2, t1.retract(d12))); EXPECT(assert_equal(t2, t1.retract(d12)));
Vector d21 = t2.localCoordinates(t1); Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21))); EXPECT(assert_equal(t1, t2.retract(d21)));
// TODO(hayk): This currently fails!
// EXPECT(assert_equal(d12, -d21));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3, manifold_expmap) TEST(Pose3, manifold_expmap)
@ -637,16 +686,26 @@ TEST( Pose3, range_pose )
Unit3 bearing_proxy(const Pose3& pose, const Point3& point) { Unit3 bearing_proxy(const Pose3& pose, const Point3& point) {
return pose.bearing(point); return pose.bearing(point);
} }
TEST( Pose3, bearing ) TEST(Pose3, Bearing) {
{
Matrix expectedH1, actualH1, expectedH2, actualH2; Matrix expectedH1, actualH1, expectedH2, actualH2;
EXPECT(assert_equal(Unit3(1,0,0),x1.bearing(l1, actualH1, actualH2),1e-9)); EXPECT(assert_equal(Unit3(1, 0, 0), x1.bearing(l1, actualH1, actualH2), 1e-9));
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, x1, l1); expectedH1 = numericalDerivative21(bearing_proxy, x1, l1);
expectedH2 = numericalDerivative22(bearing_proxy, x1, l1); expectedH2 = numericalDerivative22(bearing_proxy, x1, l1);
EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(expectedH1, actualH1));
EXPECT(assert_equal(expectedH2,actualH2)); EXPECT(assert_equal(expectedH2, actualH2));
}
TEST(Pose3, Bearing2) {
Matrix expectedH1, actualH1, expectedH2, actualH2;
EXPECT(assert_equal(Unit3(0,0.6,-0.8), x2.bearing(l4, actualH1, actualH2), 1e-9));
// Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, x2, l4);
expectedH2 = numericalDerivative22(bearing_proxy, x2, l4);
EXPECT(assert_equal(expectedH1, actualH1));
EXPECT(assert_equal(expectedH2, actualH2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -671,21 +730,21 @@ TEST( Pose3, adjointMap) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3, align_1) { TEST(Pose3, Align1) {
Pose3 expected(Rot3(), Point3(10,10,0)); Pose3 expected(Rot3(), Point3(10,10,0));
vector<Point3Pair> correspondences; vector<Point3Pair> correspondences;
Point3Pair pq1(make_pair(Point3(0,0,0), Point3(10,10,0))); Point3Pair ab1(make_pair(Point3(10,10,0), Point3(0,0,0)));
Point3Pair pq2(make_pair(Point3(20,10,0), Point3(30,20,0))); Point3Pair ab2(make_pair(Point3(30,20,0), Point3(20,10,0)));
Point3Pair pq3(make_pair(Point3(10,20,0), Point3(20,30,0))); Point3Pair ab3(make_pair(Point3(20,30,0), Point3(10,20,0)));
correspondences += pq1, pq2, pq3; correspondences += ab1, ab2, ab3;
boost::optional<Pose3> actual = align(correspondences); boost::optional<Pose3> actual = Pose3::Align(correspondences);
EXPECT(assert_equal(expected, *actual)); EXPECT(assert_equal(expected, *actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3, align_2) { TEST(Pose3, Align2) {
Point3 t(20,10,5); Point3 t(20,10,5);
Rot3 R = Rot3::RzRyRx(0.3, 0.2, 0.1); Rot3 R = Rot3::RzRyRx(0.3, 0.2, 0.1);
Pose3 expected(R, t); Pose3 expected(R, t);
@ -695,12 +754,12 @@ TEST(Pose3, align_2) {
Point3 q1 = expected.transform_from(p1), Point3 q1 = expected.transform_from(p1),
q2 = expected.transform_from(p2), q2 = expected.transform_from(p2),
q3 = expected.transform_from(p3); q3 = expected.transform_from(p3);
Point3Pair pq1(make_pair(p1, q1)); Point3Pair ab1(make_pair(q1, p1));
Point3Pair pq2(make_pair(p2, q2)); Point3Pair ab2(make_pair(q2, p2));
Point3Pair pq3(make_pair(p3, q3)); Point3Pair ab3(make_pair(q3, p3));
correspondences += pq1, pq2, pq3; correspondences += ab1, ab2, ab3;
boost::optional<Pose3> actual = align(correspondences); boost::optional<Pose3> actual = Pose3::Align(correspondences);
EXPECT(assert_equal(expected, *actual, 1e-5)); EXPECT(assert_equal(expected, *actual, 1e-5));
} }
@ -843,6 +902,22 @@ TEST(Pose3 , ChartDerivatives) {
} }
} }
/* ************************************************************************* */
TEST(Pose3, interpolate) {
EXPECT(assert_equal(T2, interpolate(T2,T3, 0.0)));
EXPECT(assert_equal(T3, interpolate(T2,T3, 1.0)));
}
/* ************************************************************************* */
TEST(Pose3, Create) {
Matrix63 actualH1, actualH2;
Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2);
EXPECT(assert_equal(T, actual));
boost::function<Pose3(Rot3,Point3)> create = boost::bind(Pose3::Create,_1,_2,boost::none,boost::none);
EXPECT(assert_equal(numericalDerivative21<Pose3,Rot3,Point3>(create, R, P2), actualH1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<Pose3,Rot3,Point3>(create, R, P2), actualH2, 1e-9));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

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

@ -117,11 +117,13 @@ TEST( Rot3, Rodrigues2)
{ {
Vector axis = Vector3(0., 1., 0.); // rotation around Y Vector axis = Vector3(0., 1., 0.); // rotation around Y
double angle = 3.14 / 4.0; double angle = 3.14 / 4.0;
Rot3 actual = Rot3::AxisAngle(axis, angle);
Rot3 expected(0.707388, 0, 0.706825, Rot3 expected(0.707388, 0, 0.706825,
0, 1, 0, 0, 1, 0,
-0.706825, 0, 0.707388); -0.706825, 0, 0.707388);
Rot3 actual = Rot3::AxisAngle(axis, angle);
CHECK(assert_equal(expected,actual,1e-5)); CHECK(assert_equal(expected,actual,1e-5));
Rot3 actual2 = Rot3::Rodrigues(angle*axis);
CHECK(assert_equal(expected,actual2,1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -242,6 +244,7 @@ TEST(Rot3, retract_localCoordinates2)
EXPECT(assert_equal(t2, t1.retract(d12))); EXPECT(assert_equal(t2, t1.retract(d12)));
Vector d21 = t2.localCoordinates(t1); Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21))); EXPECT(assert_equal(t1, t2.retract(d21)));
EXPECT(assert_equal(d12, -d21));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot3, manifold_expmap) TEST(Rot3, manifold_expmap)

2
gtsam/geometry/tests/testUnit3.cpp
View File

@ -303,7 +303,7 @@ TEST(Unit3, localCoordinates) {
} }
//******************************************************************************* //*******************************************************************************
// Wrapper to make basis return a vector6 so we can test numerical derivatives. // Wrapper to make basis return a Vector6 so we can test numerical derivatives.
Vector6 BasisTest(const Unit3& p, OptionalJacobian<6, 2> H) { Vector6 BasisTest(const Unit3& p, OptionalJacobian<6, 2> H) {
Matrix32 B = p.basis(H); Matrix32 B = p.basis(H);
Vector6 B_vec; Vector6 B_vec;

6
gtsam/geometry/triangulation.cpp
View File

@ -56,11 +56,10 @@ Vector4 triangulateHomogeneousDLT(
Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices, Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol) { const std::vector<Point2>& measurements, double rank_tol) {
Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
rank_tol);
// Create 3D point from homogeneous coordinates // Create 3D point from homogeneous coordinates
return Point3(sub((v / v(3)), 0, 3)); return Point3(v.head<3>() / v[3]);
} }
/// ///
@ -92,4 +91,3 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,
} }
} // \namespace gtsam } // \namespace gtsam

46
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -48,16 +48,20 @@ namespace gtsam {
// If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
// for no variable index first so that it's always computed if we need to call COLAMD because // for no variable index first so that it's always computed if we need to call COLAMD because
// no Ordering is provided. // no Ordering is provided.
return eliminateSequential(ordering, function, VariableIndex(asDerived()), orderingType); VariableIndex computedVariableIndex(asDerived());
return eliminateSequential(ordering, function, computedVariableIndex, orderingType);
} }
else /*if(!ordering)*/ { else /*if(!ordering)*/ {
// If no Ordering provided, compute one and call this function again. We are guaranteed to // If no Ordering provided, compute one and call this function again. We are guaranteed to
// have a VariableIndex already here because we computed one if needed in the previous 'else' // have a VariableIndex already here because we computed one if needed in the previous 'else'
// block. // block.
if (orderingType == Ordering::METIS) if (orderingType == Ordering::METIS) {
return eliminateSequential(Ordering::Metis(asDerived()), function, variableIndex, orderingType); Ordering computedOrdering = Ordering::Metis(asDerived());
else return eliminateSequential(computedOrdering, function, variableIndex, orderingType);
return eliminateSequential(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); } else {
Ordering computedOrdering = Ordering::Colamd(*variableIndex);
return eliminateSequential(computedOrdering, function, variableIndex, orderingType);
}
} }
} }
@ -86,16 +90,20 @@ namespace gtsam {
// If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
// for no variable index first so that it's always computed if we need to call COLAMD because // for no variable index first so that it's always computed if we need to call COLAMD because
// no Ordering is provided. // no Ordering is provided.
return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType); VariableIndex computedVariableIndex(asDerived());
return eliminateMultifrontal(ordering, function, computedVariableIndex, orderingType);
} }
else /*if(!ordering)*/ { else /*if(!ordering)*/ {
// If no Ordering provided, compute one and call this function again. We are guaranteed to // If no Ordering provided, compute one and call this function again. We are guaranteed to
// have a VariableIndex already here because we computed one if needed in the previous 'else' // have a VariableIndex already here because we computed one if needed in the previous 'else'
// block. // block.
if (orderingType == Ordering::METIS) if (orderingType == Ordering::METIS) {
return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType); Ordering computedOrdering = Ordering::Metis(asDerived());
else return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType);
return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); } else {
Ordering computedOrdering = Ordering::Colamd(*variableIndex);
return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType);
}
} }
} }
@ -112,7 +120,8 @@ namespace gtsam {
return etree.eliminate(function); return etree.eliminate(function);
} else { } else {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
return eliminatePartialSequential(ordering, function, VariableIndex(asDerived())); VariableIndex computedVariableIndex(asDerived());
return eliminatePartialSequential(ordering, function, computedVariableIndex);
} }
} }
@ -132,7 +141,8 @@ namespace gtsam {
return eliminatePartialSequential(ordering, function, variableIndex); return eliminatePartialSequential(ordering, function, variableIndex);
} else { } else {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
return eliminatePartialSequential(variables, function, VariableIndex(asDerived())); VariableIndex computedVariableIndex(asDerived());
return eliminatePartialSequential(variables, function, computedVariableIndex);
} }
} }
@ -150,7 +160,8 @@ namespace gtsam {
return junctionTree.eliminate(function); return junctionTree.eliminate(function);
} else { } else {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
return eliminatePartialMultifrontal(ordering, function, VariableIndex(asDerived())); VariableIndex computedVariableIndex(asDerived());
return eliminatePartialMultifrontal(ordering, function, computedVariableIndex);
} }
} }
@ -170,7 +181,8 @@ namespace gtsam {
return eliminatePartialMultifrontal(ordering, function, variableIndex); return eliminatePartialMultifrontal(ordering, function, variableIndex);
} else { } else {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
return eliminatePartialMultifrontal(variables, function, VariableIndex(asDerived())); VariableIndex computedVariableIndex(asDerived());
return eliminatePartialMultifrontal(variables, function, computedVariableIndex);
} }
} }
@ -287,7 +299,8 @@ namespace gtsam {
} }
} else { } else {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, VariableIndex(asDerived())); VariableIndex computedVariableIndex(asDerived());
return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex);
} }
} }
@ -312,7 +325,8 @@ namespace gtsam {
else else
{ {
// If no variable index is provided, compute one and call this function again // If no variable index is provided, compute one and call this function again
return marginal(variables, function, VariableIndex(asDerived())); VariableIndex computedVariableIndex(asDerived());
return marginal(variables, function, computedVariableIndex);
} }
} }

23
gtsam/inference/FactorGraph-inst.h
View File

@ -74,11 +74,26 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
KeySet FactorGraph<FACTOR>::keys() const { KeySet FactorGraph<FACTOR>::keys() const {
KeySet allKeys; KeySet keys;
BOOST_FOREACH(const sharedFactor& factor, factors_) BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor)
keys.insert(factor->begin(), factor->end());
}
return keys;
}
/* ************************************************************************* */
template <class FACTOR>
KeyVector FactorGraph<FACTOR>::keyVector() const {
KeyVector keys;
keys.reserve(2 * size()); // guess at size
BOOST_FOREACH (const sharedFactor& factor, factors_)
if (factor) if (factor)
allKeys.insert(factor->begin(), factor->end()); keys.insert(keys.end(), factor->begin(), factor->end());
return allKeys; std::sort(keys.begin(), keys.end());
auto last = std::unique(keys.begin(), keys.end());
keys.erase(last, keys.end());
return keys;
} }
/* ************************************************************************* */ /* ************************************************************************* */

9
gtsam/inference/FactorGraph.h
View File

@ -319,10 +319,10 @@ namespace gtsam {
void replace(size_t index, sharedFactor factor) { at(index) = factor; } void replace(size_t index, sharedFactor factor) { at(index) = factor; }
/** Erase factor and rearrange other factors to take up the empty space */ /** Erase factor and rearrange other factors to take up the empty space */
void erase(iterator item) { factors_.erase(item); } iterator erase(iterator item) { return factors_.erase(item); }
/** Erase factors and rearrange other factors to take up the empty space */ /** Erase factors and rearrange other factors to take up the empty space */
void erase(iterator first, iterator last) { factors_.erase(first, last); } iterator erase(iterator first, iterator last) { return factors_.erase(first, last); }
/// @} /// @}
/// @name Advanced Interface /// @name Advanced Interface
@ -331,9 +331,12 @@ namespace gtsam {
/** return the number of non-null factors */ /** return the number of non-null factors */
size_t nrFactors() const; size_t nrFactors() const;
/** Potentially very slow function to return all keys involved */ /** Potentially slow function to return all keys involved, sorted, as a set */
KeySet keys() const; KeySet keys() const;
/** Potentially slow function to return all keys involved, sorted, as a vector */
KeyVector keyVector() const;
/** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */ /** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */
inline bool exists(size_t idx) const { return idx < size() && at(idx); } inline bool exists(size_t idx) const { return idx < size() && at(idx); }

15
gtsam/inference/Key.h
View File

@ -78,14 +78,19 @@ GTSAM_EXPORT void PrintKeySet(const KeySet& keys, const std::string& s = "",
// Define Key to be Testable by specializing gtsam::traits // Define Key to be Testable by specializing gtsam::traits
template<typename T> struct traits; template<typename T> struct traits;
template<> struct traits<Key> {
static void Print(const Key& key, const std::string& str = "") { template <>
PrintKey(key, str); struct traits<Key> {
static void Print(const Key& val, const std::string& str = "") {
PrintKey(val, str);
} }
static bool Equals(const Key& key1, const Key& key2, double tol = 1e-8) { static bool Equals(const Key& val1, const Key& val2, double tol = 1e-8) {
return key1 == key2; return val1 == val2;
} }
}; };
} // namespace gtsam } // namespace gtsam

2
gtsam/inference/LabeledSymbol.cpp
View File

@ -37,7 +37,7 @@ LabeledSymbol::LabeledSymbol(const LabeledSymbol& key)
: c_(key.c_), label_(key.label_), j_(key.j_) {} : c_(key.c_), label_(key.label_), j_(key.j_) {}
/* ************************************************************************* */ /* ************************************************************************* */
LabeledSymbol::LabeledSymbol(unsigned char c, unsigned char label, size_t j) LabeledSymbol::LabeledSymbol(unsigned char c, unsigned char label, std::uint64_t j)
: c_(c), label_(label), j_(j) {} : c_(c), label_(label), j_(j) {}
/* ************************************************************************* */ /* ************************************************************************* */

4
gtsam/inference/LabeledSymbol.h
View File

@ -34,7 +34,7 @@ namespace gtsam {
class GTSAM_EXPORT LabeledSymbol { class GTSAM_EXPORT LabeledSymbol {
protected: protected:
unsigned char c_, label_; unsigned char c_, label_;
size_t j_; std::uint64_t j_;
public: public:
/** Default constructor */ /** Default constructor */
@ -44,7 +44,7 @@ public:
LabeledSymbol(const LabeledSymbol& key); LabeledSymbol(const LabeledSymbol& key);
/** Constructor */ /** Constructor */
LabeledSymbol(unsigned char c, unsigned char label, size_t j); LabeledSymbol(unsigned char c, unsigned char label, std::uint64_t j);
/** Constructor that decodes an integer gtsam::Key */ /** Constructor that decodes an integer gtsam::Key */
LabeledSymbol(gtsam::Key key); LabeledSymbol(gtsam::Key key);

8
gtsam/inference/Ordering.cpp
View File

@ -23,7 +23,10 @@
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h> #include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
#ifdef GTSAM_SUPPORT_NESTED_DISSECTION
#include <gtsam/3rdparty/metis/include/metis.h> #include <gtsam/3rdparty/metis/include/metis.h>
#endif
using namespace std; using namespace std;
@ -198,6 +201,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
/* ************************************************************************* */ /* ************************************************************************* */
Ordering Ordering::Metis(const MetisIndex& met) { Ordering Ordering::Metis(const MetisIndex& met) {
#ifdef GTSAM_SUPPORT_NESTED_DISSECTION
gttic(Ordering_METIS); gttic(Ordering_METIS);
vector<idx_t> xadj = met.xadj(); vector<idx_t> xadj = met.xadj();
@ -227,6 +231,10 @@ Ordering Ordering::Metis(const MetisIndex& met) {
result[j] = met.intToKey(perm[j]); result[j] = met.intToKey(perm[j]);
} }
return result; return result;
#else
throw runtime_error("GTSAM was built without support for Metis-based "
"nested dissection");
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/inference/Symbol.cpp
View File

@ -33,9 +33,9 @@ static const size_t indexBits = keyBits - chrBits;
static const Key chrMask = Key(UCHAR_MAX) << indexBits; // For some reason, std::numeric_limits<unsigned char>::max() fails static const Key chrMask = Key(UCHAR_MAX) << indexBits; // For some reason, std::numeric_limits<unsigned char>::max() fails
static const Key indexMask = ~chrMask; static const Key indexMask = ~chrMask;
Symbol::Symbol(Key key) { Symbol::Symbol(Key key) :
c_ = (unsigned char) ((key & chrMask) >> indexBits); c_((unsigned char) ((key & chrMask) >> indexBits)),
j_ = key & indexMask; j_ (key & indexMask) {
} }
Key Symbol::key() const { Key Symbol::key() const {

65
gtsam/inference/Symbol.h
View File

@ -21,6 +21,7 @@
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <cstdint>
namespace gtsam { namespace gtsam {
@ -32,8 +33,8 @@ namespace gtsam {
*/ */
class GTSAM_EXPORT Symbol { class GTSAM_EXPORT Symbol {
protected: protected:
unsigned char c_; const unsigned char c_;
size_t j_; const std::uint64_t j_;
public: public:
@ -48,7 +49,7 @@ public:
} }
/** Constructor */ /** Constructor */
Symbol(unsigned char c, size_t j) : Symbol(unsigned char c, std::uint64_t j) :
c_(c), j_(j) { c_(c), j_(j) {
} }
@ -73,7 +74,7 @@ public:
} }
/** Retrieve key index */ /** Retrieve key index */
size_t index() const { std::uint64_t index() const {
return j_; return j_;
} }
@ -124,41 +125,41 @@ private:
}; };
/** Create a symbol key from a character and index, i.e. x5. */ /** Create a symbol key from a character and index, i.e. x5. */
inline Key symbol(unsigned char c, size_t j) { return (Key)Symbol(c,j); } inline Key symbol(unsigned char c, std::uint64_t j) { return (Key)Symbol(c,j); }
/** Return the character portion of a symbol key. */ /** Return the character portion of a symbol key. */
inline unsigned char symbolChr(Key key) { return Symbol(key).chr(); } inline unsigned char symbolChr(Key key) { return Symbol(key).chr(); }
/** Return the index portion of a symbol key. */ /** Return the index portion of a symbol key. */
inline size_t symbolIndex(Key key) { return Symbol(key).index(); } inline std::uint64_t symbolIndex(Key key) { return Symbol(key).index(); }
namespace symbol_shorthand { namespace symbol_shorthand {
inline Key A(size_t j) { return Symbol('a', j); } inline Key A(std::uint64_t j) { return Symbol('a', j); }
inline Key B(size_t j) { return Symbol('b', j); } inline Key B(std::uint64_t j) { return Symbol('b', j); }
inline Key C(size_t j) { return Symbol('c', j); } inline Key C(std::uint64_t j) { return Symbol('c', j); }
inline Key D(size_t j) { return Symbol('d', j); } inline Key D(std::uint64_t j) { return Symbol('d', j); }
inline Key E(size_t j) { return Symbol('e', j); } inline Key E(std::uint64_t j) { return Symbol('e', j); }
inline Key F(size_t j) { return Symbol('f', j); } inline Key F(std::uint64_t j) { return Symbol('f', j); }
inline Key G(size_t j) { return Symbol('g', j); } inline Key G(std::uint64_t j) { return Symbol('g', j); }
inline Key H(size_t j) { return Symbol('h', j); } inline Key H(std::uint64_t j) { return Symbol('h', j); }
inline Key I(size_t j) { return Symbol('i', j); } inline Key I(std::uint64_t j) { return Symbol('i', j); }
inline Key J(size_t j) { return Symbol('j', j); } inline Key J(std::uint64_t j) { return Symbol('j', j); }
inline Key K(size_t j) { return Symbol('k', j); } inline Key K(std::uint64_t j) { return Symbol('k', j); }
inline Key L(size_t j) { return Symbol('l', j); } inline Key L(std::uint64_t j) { return Symbol('l', j); }
inline Key M(size_t j) { return Symbol('m', j); } inline Key M(std::uint64_t j) { return Symbol('m', j); }
inline Key N(size_t j) { return Symbol('n', j); } inline Key N(std::uint64_t j) { return Symbol('n', j); }
inline Key O(size_t j) { return Symbol('o', j); } inline Key O(std::uint64_t j) { return Symbol('o', j); }
inline Key P(size_t j) { return Symbol('p', j); } inline Key P(std::uint64_t j) { return Symbol('p', j); }
inline Key Q(size_t j) { return Symbol('q', j); } inline Key Q(std::uint64_t j) { return Symbol('q', j); }
inline Key R(size_t j) { return Symbol('r', j); } inline Key R(std::uint64_t j) { return Symbol('r', j); }
inline Key S(size_t j) { return Symbol('s', j); } inline Key S(std::uint64_t j) { return Symbol('s', j); }
inline Key T(size_t j) { return Symbol('t', j); } inline Key T(std::uint64_t j) { return Symbol('t', j); }
inline Key U(size_t j) { return Symbol('u', j); } inline Key U(std::uint64_t j) { return Symbol('u', j); }
inline Key V(size_t j) { return Symbol('v', j); } inline Key V(std::uint64_t j) { return Symbol('v', j); }
inline Key W(size_t j) { return Symbol('w', j); } inline Key W(std::uint64_t j) { return Symbol('w', j); }
inline Key X(size_t j) { return Symbol('x', j); } inline Key X(std::uint64_t j) { return Symbol('x', j); }
inline Key Y(size_t j) { return Symbol('y', j); } inline Key Y(std::uint64_t j) { return Symbol('y', j); }
inline Key Z(size_t j) { return Symbol('z', j); } inline Key Z(std::uint64_t j) { return Symbol('z', j); }
} }
/// traits /// traits

2
gtsam/inference/VariableIndex.h
View File

@ -81,7 +81,7 @@ public:
* The number of variable entries. This is one greater than the variable * The number of variable entries. This is one greater than the variable
* with the highest index. * with the highest index.
*/ */
Key size() const { return index_.size(); } size_t size() const { return index_.size(); }
/** The number of factors in the original factor graph */ /** The number of factors in the original factor graph */
size_t nFactors() const { return nFactors_; } size_t nFactors() const { return nFactors_; }

2
gtsam/inference/inference-inst.h
View File

@ -61,7 +61,7 @@ namespace gtsam {
{ {
// Call eliminate on the node and add the result to the parent's gathered factors // Call eliminate on the node and add the result to the parent's gathered factors
typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors); typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors);
if(!childFactor->empty()) if(childFactor && !childFactor->empty())
myData.parentData->childFactors.push_back(childFactor); myData.parentData->childFactors.push_back(childFactor);
} }
}; };

2
gtsam/inference/tests/testOrdering.cpp
View File

@ -71,7 +71,7 @@ TEST(Ordering, grouped_constrained_ordering) {
SymbolicFactorGraph sfg = example::symbolicChain(); SymbolicFactorGraph sfg = example::symbolicChain();
// constrained version - push one set to the end // constrained version - push one set to the end
FastMap<size_t, int> constraints; FastMap<Key, int> constraints;
constraints[2] = 1; constraints[2] = 1;
constraints[4] = 1; constraints[4] = 1;
constraints[5] = 2; constraints[5] = 2;

7
gtsam/linear/GaussianBayesNet.cpp
View File

@ -86,6 +86,8 @@ namespace gtsam {
VectorValues GaussianBayesNet::backSubstitute(const VectorValues& rhs) const VectorValues GaussianBayesNet::backSubstitute(const VectorValues& rhs) const
{ {
VectorValues result; VectorValues result;
// TODO this looks pretty sketchy. result is passed as the parents argument
// as it's filled up by solving the gaussian conditionals.
BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) { BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) {
result.insert(cg->solveOtherRHS(result, rhs)); result.insert(cg->solveOtherRHS(result, rhs));
} }
@ -139,17 +141,18 @@ namespace gtsam {
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix,Vector> GaussianBayesNet::matrix() const pair<Matrix, Vector> GaussianBayesNet::matrix() const {
{
GaussianFactorGraph factorGraph(*this); GaussianFactorGraph factorGraph(*this);
KeySet keys = factorGraph.keys(); KeySet keys = factorGraph.keys();
// add frontal keys in order // add frontal keys in order
Ordering ordering; Ordering ordering;
BOOST_FOREACH (const sharedConditional& cg, *this) BOOST_FOREACH (const sharedConditional& cg, *this)
if (cg) {
BOOST_FOREACH (Key key, cg->frontals()) { BOOST_FOREACH (Key key, cg->frontals()) {
ordering.push_back(key); ordering.push_back(key);
keys.erase(key); keys.erase(key);
} }
}
// add remaining keys in case Bayes net is incomplete // add remaining keys in case Bayes net is incomplete
BOOST_FOREACH (Key key, keys) BOOST_FOREACH (Key key, keys)
ordering.push_back(key); ordering.push_back(key);

16
gtsam/linear/GaussianConditional.cpp
View File

@ -120,24 +120,24 @@ namespace gtsam {
VectorValues GaussianConditional::solve(const VectorValues& x) const VectorValues GaussianConditional::solve(const VectorValues& x) const
{ {
// Concatenate all vector values that correspond to parent variables // Concatenate all vector values that correspond to parent variables
Vector xS = x.vector(FastVector<Key>(beginParents(), endParents())); const Vector xS = x.vector(FastVector<Key>(beginParents(), endParents()));
// Update right-hand-side // Update right-hand-side
xS = getb() - get_S() * xS; const Vector rhs = get_d() - get_S() * xS;
// Solve matrix // Solve matrix
Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS); const Vector solution = get_R().triangularView<Eigen::Upper>().solve(rhs);
// Check for indeterminant solution // Check for indeterminant solution
if(soln.hasNaN()) throw IndeterminantLinearSystemException(keys().front()); if (solution.hasNaN()) {
throw IndeterminantLinearSystemException(keys().front());
// TODO, do we not have to scale by sigmas here? Copy/paste bug }
// Insert solution into a VectorValues // Insert solution into a VectorValues
VectorValues result; VectorValues result;
DenseIndex vectorPosition = 0; DenseIndex vectorPosition = 0;
for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal))); result.insert(*frontal, solution.segment(vectorPosition, getDim(frontal)));
vectorPosition += getDim(frontal); vectorPosition += getDim(frontal);
} }

26
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -30,6 +30,8 @@
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/base/cholesky.h> #include <gtsam/base/cholesky.h>
#include <boost/foreach.hpp>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -68,27 +70,6 @@ namespace gtsam {
return spec; return spec;
} }
/* ************************************************************************* */
vector<size_t> GaussianFactorGraph::getkeydim() const {
// First find dimensions of each variable
vector<size_t> dims;
BOOST_FOREACH(const sharedFactor& factor, *this) {
for (GaussianFactor::const_iterator pos = factor->begin();
pos != factor->end(); ++pos) {
if (dims.size() <= *pos)
dims.resize(*pos + 1, 0);
dims[*pos] = factor->getDim(pos);
}
}
// Find accumulated dimensions for variables
vector<size_t> dims_accumulated;
dims_accumulated.resize(dims.size()+1,0);
dims_accumulated[0]=0;
for (size_t i=1; i<dims_accumulated.size(); i++)
dims_accumulated[i] = dims_accumulated[i-1]+dims[i-1];
return dims_accumulated;
}
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph::shared_ptr GaussianFactorGraph::cloneToPtr() const { GaussianFactorGraph::shared_ptr GaussianFactorGraph::cloneToPtr() const {
gtsam::GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); gtsam::GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
@ -228,7 +209,8 @@ namespace gtsam {
Matrix GaussianFactorGraph::augmentedHessian( Matrix GaussianFactorGraph::augmentedHessian(
boost::optional<const Ordering&> optionalOrdering) const { boost::optional<const Ordering&> optionalOrdering) const {
// combine all factors and get upper-triangular part of Hessian // combine all factors and get upper-triangular part of Hessian
HessianFactor combined(*this, Scatter(*this, optionalOrdering)); Scatter scatter(*this, optionalOrdering);
HessianFactor combined(*this, scatter);
Matrix result = combined.info(); Matrix result = combined.info();
// Fill in lower-triangular part of Hessian // Fill in lower-triangular part of Hessian
result.triangularView<Eigen::StrictlyLower>() = result.transpose(); result.triangularView<Eigen::StrictlyLower>() = result.transpose();

2
gtsam/linear/GaussianFactorGraph.h
View File

@ -141,8 +141,6 @@ namespace gtsam {
/* return a map of (Key, dimension) */ /* return a map of (Key, dimension) */
std::map<Key, size_t> getKeyDimMap() const; std::map<Key, size_t> getKeyDimMap() const;
std::vector<size_t> getkeydim() const;
/** unnormalized error */ /** unnormalized error */
double error(const VectorValues& x) const { double error(const VectorValues& x) const {
double total_error = 0.; double total_error = 0.;

4
gtsam/linear/HessianFactor.cpp
View File

@ -455,8 +455,8 @@ std::pair<boost::shared_ptr<GaussianConditional>,
// Build joint factor // Build joint factor
HessianFactor::shared_ptr jointFactor; HessianFactor::shared_ptr jointFactor;
try { try {
jointFactor = boost::make_shared<HessianFactor>(factors, Scatter scatter(factors, keys);
Scatter(factors, keys)); jointFactor = boost::make_shared<HessianFactor>(factors, scatter);
} catch (std::invalid_argument&) { } catch (std::invalid_argument&) {
throw InvalidDenseElimination( throw InvalidDenseElimination(
"EliminateCholesky was called with a request to eliminate variables that are not\n" "EliminateCholesky was called with a request to eliminate variables that are not\n"

2
gtsam/linear/IterativeSolver.h
View File

@ -111,7 +111,7 @@ public:
* Handy data structure for iterative solvers * Handy data structure for iterative solvers
* key to (index, dimension, colstart) * key to (index, dimension, colstart)
*/ */
class GTSAM_EXPORT KeyInfoEntry: public boost::tuple<size_t, size_t, size_t> { class GTSAM_EXPORT KeyInfoEntry: public boost::tuple<Key, size_t, Key> {
public: public:

91
gtsam/linear/JacobianFactor.cpp
View File

@ -527,15 +527,16 @@ void JacobianFactor::updateHessian(const FastVector<Key>& infoKeys,
// Loop over blocks of A, including RHS with j==n // Loop over blocks of A, including RHS with j==n
vector<DenseIndex> slots(n+1); vector<DenseIndex> slots(n+1);
for (DenseIndex j = 0; j <= n; ++j) { for (DenseIndex j = 0; j <= n; ++j) {
Eigen::Block<const Matrix> Ab_j = Ab_(j);
const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]); const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]);
slots[j] = J; slots[j] = J;
// Fill off-diagonal blocks with Ai'*Aj // Fill off-diagonal blocks with Ai'*Aj
for (DenseIndex i = 0; i < j; ++i) { for (DenseIndex i = 0; i < j; ++i) {
const DenseIndex I = slots[i]; // because i<j, slots[i] is valid. const DenseIndex I = slots[i]; // because i<j, slots[i] is valid.
(*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j); (*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_j;
} }
// Fill diagonal block with Aj'*Aj // Fill diagonal block with Aj'*Aj
(*info)(J, J).selfadjointView().rankUpdate(Ab_(j).transpose()); (*info)(J, J).selfadjointView().rankUpdate(Ab_j.transpose());
} }
} }
} }
@ -691,8 +692,8 @@ GaussianFactor::shared_ptr JacobianFactor::negate() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, std::pair<GaussianConditional::shared_ptr,
boost::shared_ptr<JacobianFactor> > JacobianFactor::eliminate( JacobianFactor::shared_ptr> JacobianFactor::eliminate(
const Ordering& keys) { const Ordering& keys) {
GaussianFactorGraph graph; GaussianFactorGraph graph;
graph.add(*this); graph.add(*this);
@ -710,8 +711,7 @@ void JacobianFactor::setModel(bool anyConstrained, const Vector& sigmas) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, std::pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr> EliminateQR(
boost::shared_ptr<JacobianFactor> > EliminateQR(
const GaussianFactorGraph& factors, const Ordering& keys) { const GaussianFactorGraph& factors, const Ordering& keys) {
gttic(EliminateQR); gttic(EliminateQR);
// Combine and sort variable blocks in elimination order // Combine and sort variable blocks in elimination order
@ -725,14 +725,24 @@ std::pair<boost::shared_ptr<GaussianConditional>,
} }
// Do dense elimination // Do dense elimination
// The following QR variants eliminate to fully triangular or trapezoidal
SharedDiagonal noiseModel; SharedDiagonal noiseModel;
if (jointFactor->model_) VerticalBlockMatrix& Ab = jointFactor->Ab_;
jointFactor->model_ = jointFactor->model_->QR(jointFactor->Ab_.matrix()); if (jointFactor->model_) {
else // The noiseModel QR can, in the case of constraints, yield a "staggered" QR where
inplace_QR(jointFactor->Ab_.matrix()); // some rows have more leading zeros than in an upper triangular matrix.
// In either case, QR will put zeros below the "diagonal".
// Zero below the diagonal jointFactor->model_ = jointFactor->model_->QR(Ab.matrix());
jointFactor->Ab_.matrix().triangularView<Eigen::StrictlyLower>().setZero(); } else {
// The inplace variant will have no valid rows anymore below m==n
// and only entries above the diagonal are valid.
inplace_QR(Ab.matrix());
// We zero below the diagonal to agree with the result from noieModel QR
Ab.matrix().triangularView<Eigen::StrictlyLower>().setZero();
size_t m = Ab.rows(), n = Ab.cols() - 1;
size_t maxRank = min(m, n);
jointFactor->model_ = noiseModel::Unit::Create(maxRank);
}
// Split elimination result into conditional and remaining factor // Split elimination result into conditional and remaining factor
GaussianConditional::shared_ptr conditional = // GaussianConditional::shared_ptr conditional = //
@ -742,56 +752,53 @@ std::pair<boost::shared_ptr<GaussianConditional>,
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditional::shared_ptr JacobianFactor::splitConditional( GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFrontals) {
size_t nrFrontals) {
gttic(JacobianFactor_splitConditional); gttic(JacobianFactor_splitConditional);
if (nrFrontals > size()) if (!model_) {
throw std::invalid_argument( throw std::invalid_argument(
"Requesting to split more variables than exist using JacobianFactor::splitConditional"); "JacobianFactor::splitConditional cannot be given a NULL noise model");
}
if (nrFrontals > size()) {
throw std::invalid_argument(
"JacobianFactor::splitConditional was requested to split off more variables than exist.");
}
// Convert nr of keys to number of scalar columns
DenseIndex frontalDim = Ab_.range(0, nrFrontals).cols(); DenseIndex frontalDim = Ab_.range(0, nrFrontals).cols();
// Check that the noise model has at least this dimension
// If this is *not* the case, we do not have enough information on the frontal variables.
if ((DenseIndex)model_->dim() < frontalDim)
throw IndeterminantLinearSystemException(this->keys().front());
// Restrict the matrix to be in the first nrFrontals variables and create the conditional // Restrict the matrix to be in the first nrFrontals variables and create the conditional
gttic(cond_Rd);
const DenseIndex originalRowEnd = Ab_.rowEnd(); const DenseIndex originalRowEnd = Ab_.rowEnd();
Ab_.rowEnd() = Ab_.rowStart() + frontalDim; Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
SharedDiagonal conditionalNoiseModel; SharedDiagonal conditionalNoiseModel;
if (model_) { conditionalNoiseModel =
if ((DenseIndex) model_->dim() < frontalDim) noiseModel::Diagonal::Sigmas(model_->sigmas().segment(Ab_.rowStart(), Ab_.rows()));
throw IndeterminantLinearSystemException(this->keys().front()); GaussianConditional::shared_ptr conditional =
conditionalNoiseModel = noiseModel::Diagonal::Sigmas( boost::make_shared<GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);
model_->sigmas().segment(Ab_.rowStart(), Ab_.rows()));
} const DenseIndex maxRemainingRows =
GaussianConditional::shared_ptr conditional = boost::make_shared< std::min(Ab_.cols(), originalRowEnd) - Ab_.rowStart() - frontalDim;
GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel); const DenseIndex remainingRows = std::min(model_->sigmas().size() - frontalDim, maxRemainingRows);
const DenseIndex maxRemainingRows = std::min(Ab_.cols(), originalRowEnd)
- Ab_.rowStart() - frontalDim;
const DenseIndex remainingRows =
model_ ? std::min(model_->sigmas().size() - frontalDim,
maxRemainingRows) :
maxRemainingRows;
Ab_.rowStart() += frontalDim; Ab_.rowStart() += frontalDim;
Ab_.rowEnd() = Ab_.rowStart() + remainingRows; Ab_.rowEnd() = Ab_.rowStart() + remainingRows;
Ab_.firstBlock() += nrFrontals; Ab_.firstBlock() += nrFrontals;
gttoc(cond_Rd);
// Take lower-right block of Ab to get the new factor // Take lower-right block of Ab to get the new factor
gttic(remaining_factor);
keys_.erase(begin(), begin() + nrFrontals); keys_.erase(begin(), begin() + nrFrontals);
// Set sigmas with the right model // Set sigmas with the right model
if (model_) {
if (model_->isConstrained()) if (model_->isConstrained())
model_ = noiseModel::Constrained::MixedSigmas( model_ = noiseModel::Constrained::MixedSigmas(model_->sigmas().tail(remainingRows));
model_->sigmas().tail(remainingRows));
else else
model_ = noiseModel::Diagonal::Sigmas( model_ = noiseModel::Diagonal::Sigmas(model_->sigmas().tail(remainingRows));
model_->sigmas().tail(remainingRows));
assert(model_->dim() == (size_t)Ab_.rows()); assert(model_->dim() == (size_t)Ab_.rows());
}
gttoc(remaining_factor);
return conditional; return conditional;
} }
} } // namespace gtsam

6
gtsam/linear/JacobianFactor.h
View File

@ -314,7 +314,7 @@ namespace gtsam {
JacobianFactor whiten() const; JacobianFactor whiten() const;
/** Eliminate the requested variables. */ /** Eliminate the requested variables. */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> > std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
eliminate(const Ordering& keys); eliminate(const Ordering& keys);
/** set noiseModel correctly */ /** set noiseModel correctly */
@ -331,13 +331,15 @@ namespace gtsam {
* @return The conditional and remaining factor * @return The conditional and remaining factor
* *
* \addtogroup LinearSolving */ * \addtogroup LinearSolving */
friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> > friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
/** /**
* splits a pre-factorized factor into a conditional, and changes the current * splits a pre-factorized factor into a conditional, and changes the current
* factor to be the remaining component. Performs same operation as eliminate(), * factor to be the remaining component. Performs same operation as eliminate(),
* but without running QR. * but without running QR.
* NOTE: looks at dimension of noise model to determine how many rows to keep.
* @param nrFrontals number of keys to eliminate
*/ */
boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals); boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals);

239
gtsam/linear/NoiseModel.cpp
View File

@ -21,6 +21,7 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/random/linear_congruential.hpp> #include <boost/random/linear_congruential.hpp>
#include <boost/random/normal_distribution.hpp> #include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp> #include <boost/random/variate_generator.hpp>
@ -58,7 +59,7 @@ boost::optional<Vector> checkIfDiagonal(const Matrix M) {
for (i = 0; i < m; i++) for (i = 0; i < m; i++)
if (!full) if (!full)
for (j = i + 1; j < n; j++) for (j = i + 1; j < n; j++)
if (fabs(M(i, j)) > 1e-9) { if (std::abs(M(i, j)) > 1e-9) {
full = true; full = true;
break; break;
} }
@ -87,7 +88,7 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
if (diagonal) if (diagonal)
return Diagonal::Sigmas(diagonal->array().inverse(), true); return Diagonal::Sigmas(diagonal->array().inverse(), true);
} }
// NOTE(frank): only reaches here if !smart && !diagonal // NOTE(frank): only reaches here if !(smart && diagonal)
return shared_ptr(new Gaussian(R.rows(), R)); return shared_ptr(new Gaussian(R.rows(), R));
} }
@ -190,8 +191,8 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const {
// get size(A) and maxRank // get size(A) and maxRank
// TODO: really no rank problems ? // TODO: really no rank problems ?
// size_t m = Ab.rows(), n = Ab.cols()-1; size_t m = Ab.rows(), n = Ab.cols()-1;
// size_t maxRank = min(m,n); size_t maxRank = min(m,n);
// pre-whiten everything (cheaply if possible) // pre-whiten everything (cheaply if possible)
WhitenInPlace(Ab); WhitenInPlace(Ab);
@ -200,12 +201,13 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const {
// Eigen QR - much faster than older householder approach // Eigen QR - much faster than older householder approach
inplace_QR(Ab); inplace_QR(Ab);
Ab.triangularView<Eigen::StrictlyLower>().setZero();
// hand-coded householder implementation // hand-coded householder implementation
// TODO: necessary to isolate last column? // TODO: necessary to isolate last column?
// householder(Ab, maxRank); // householder(Ab, maxRank);
return SharedDiagonal(); return noiseModel::Unit::Create(maxRank);
} }
void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const { void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const {
@ -414,20 +416,40 @@ Constrained::shared_ptr Constrained::unit() const {
// Special version of QR for Constrained calls slower but smarter code // Special version of QR for Constrained calls slower but smarter code
// that deals with possibly zero sigmas // that deals with possibly zero sigmas
// It is Gram-Schmidt orthogonalization rather than Householder // It is Gram-Schmidt orthogonalization rather than Householder
// Previously Diagonal::QR
// Check whether column a triggers a constraint and corresponding variable is deterministic
// Return constraint_row with maximum element in case variable plays in multiple constraints
template <typename VECTOR>
boost::optional<size_t> check_if_constraint(VECTOR a, const Vector& invsigmas, size_t m) {
boost::optional<size_t> constraint_row;
// not zero, so roundoff errors will not be counted
// TODO(frank): that's a fairly crude way of dealing with roundoff errors :-(
double max_element = 1e-9;
for (size_t i = 0; i < m; i++) {
if (!std::isinf(invsigmas[i]))
continue;
double abs_ai = std::abs(a(i,0));
if (abs_ai > max_element) {
max_element = abs_ai;
constraint_row.reset(i);
}
}
return constraint_row;
}
SharedDiagonal Constrained::QR(Matrix& Ab) const { SharedDiagonal Constrained::QR(Matrix& Ab) const {
bool verbose = false; static const double kInfinity = std::numeric_limits<double>::infinity();
if (verbose) cout << "\nStarting Constrained::QR" << endl;
// get size(A) and maxRank // get size(A) and maxRank
size_t m = Ab.rows(), n = Ab.cols()-1; size_t m = Ab.rows();
size_t maxRank = min(m,n); const size_t n = Ab.cols() - 1;
const size_t maxRank = min(m, n);
// create storage for [R d] // create storage for [R d]
typedef boost::tuple<size_t, Vector, double> Triple; typedef boost::tuple<size_t, Matrix, double> Triple;
list<Triple> Rd; list<Triple> Rd;
Vector pseudo(m); // allocate storage for pseudo-inverse Matrix rd(1, n + 1); // and for row of R
Vector invsigmas = sigmas_.array().inverse(); Vector invsigmas = sigmas_.array().inverse();
Vector weights = invsigmas.array().square(); // calculate weights once Vector weights = invsigmas.array().square(); // calculate weights once
@ -435,60 +457,100 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
// are not necessarily contiguous. For each one, estimate the corresponding // are not necessarily contiguous. For each one, estimate the corresponding
// scalar variable x as d-rS, with S the separator (remaining columns). // scalar variable x as d-rS, with S the separator (remaining columns).
// Then update A and b by substituting x with d-rS, zero-ing out x's column. // Then update A and b by substituting x with d-rS, zero-ing out x's column.
for (size_t j=0; j<n; ++j) { for (size_t j = 0; j < n; ++j) {
// extract the first column of A // extract the first column of A
Vector a = Ab.col(j); Eigen::Block<Matrix> a = Ab.block(0, j, m, 1);
// Check whether we need to handle as a constraint
boost::optional<size_t> constraint_row = check_if_constraint(a, invsigmas, m);
if (constraint_row) {
// Handle this as a constraint, as the i^th row has zero sigma with non-zero entry A(i,j)
// In this case, the row in [R|d] is simply the row in [A|b]
// NOTE(frank): we used to divide by a[i] but there is no need with a constraint
rd = Ab.row(*constraint_row);
// Construct solution (r, d, sigma)
Rd.push_back(boost::make_tuple(j, rd, kInfinity));
// exit after rank exhausted
if (Rd.size() >= maxRank)
break;
// The constraint row will be zeroed out, so we can save work by swapping in the
// last valid row and decreasing m. This will save work on subsequent down-dates, too.
m -= 1;
if (*constraint_row != m) {
Ab.row(*constraint_row) = Ab.row(m);
weights(*constraint_row) = weights(m);
invsigmas(*constraint_row) = invsigmas(m);
}
// get a reduced a-column which is now shorter
Eigen::Block<Matrix> a_reduced = Ab.block(0, j, m, 1);
a_reduced *= (1.0/rd(0, j)); // NOTE(frank): this is the 1/a[i] = 1/rd(0,j) factor we need!
// Rank-1 down-date of Ab, expensive, using outer product
Ab.block(0, j + 1, m, n - j).noalias() -= a_reduced * rd.middleCols(j + 1, n - j);
} else {
// Treat in normal Gram-Schmidt way
// Calculate weighted pseudo-inverse and corresponding precision // Calculate weighted pseudo-inverse and corresponding precision
gttic(constrained_QR_weightedPseudoinverse);
double precision = weightedPseudoinverse(a, weights, pseudo);
gttoc(constrained_QR_weightedPseudoinverse);
// If precision is zero, no information on this column // Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma)
// This is actually not limited to constraints, could happen in Gaussian::QR // For diagonal Sigma, inv(Sigma) = diag(precisions)
// In that case, we're probably hosed. TODO: make sure Householder is rank-revealing double precision = 0;
if (precision < 1e-8) continue; Vector pseudo(m); // allocate storage for pseudo-inverse
for (size_t i = 0; i < m; i++) {
double ai = a(i, 0);
if (std::abs(ai) > 1e-9) { // also catches remaining sigma==0 rows
pseudo[i] = weights[i] * ai;
precision += pseudo[i] * ai;
} else
pseudo[i] = 0;
}
if (precision > 1e-8) {
pseudo /= precision;
gttic(constrained_QR_create_rd);
// create solution [r d], rhs is automatically r(n) // create solution [r d], rhs is automatically r(n)
Vector rd(n+1); // uninitialized ! rd(0, j) = 1.0; // put 1 on diagonal
rd(j)=1.0; // put 1 on diagonal rd.block(0, j + 1, 1, n - j) = pseudo.transpose() * Ab.block(0, j + 1, m, n - j);
for (size_t j2=j+1; j2<n+1; ++j2) // and fill in remainder with dot-products
rd(j2) = pseudo.dot(Ab.col(j2));
gttoc(constrained_QR_create_rd);
// construct solution (r, d, sigma) // construct solution (r, d, sigma)
Rd.push_back(boost::make_tuple(j, rd, precision)); Rd.push_back(boost::make_tuple(j, rd, precision));
} else {
// If precision is zero, no information on this column
// This is actually not limited to constraints, could happen in Gaussian::QR
// In that case, we're probably hosed. TODO: make sure Householder is rank-revealing
continue; // but even if not, no need to update if a==zeros
}
// exit after rank exhausted // exit after rank exhausted
if (Rd.size()>=maxRank) break; if (Rd.size() >= maxRank)
break;
// update Ab, expensive, using outer product // Rank-1 down-date of Ab, expensive, using outer product
gttic(constrained_QR_update_Ab); Ab.block(0, j + 1, m, n - j).noalias() -= a * rd.middleCols(j + 1, n - j);
Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose(); }
gttoc(constrained_QR_update_Ab);
} }
// Create storage for precisions // Create storage for precisions
Vector precisions(Rd.size()); Vector precisions(Rd.size());
gttic(constrained_QR_write_back_into_Ab);
// Write back result in Ab, imperative as we are // Write back result in Ab, imperative as we are
// TODO: test that is correct if a column was skipped !!!!
size_t i = 0; // start with first row size_t i = 0; // start with first row
bool mixed = false; bool mixed = false;
BOOST_FOREACH(const Triple& t, Rd) { Ab.setZero(); // make sure we don't look below
BOOST_FOREACH (const Triple& t, Rd) {
const size_t& j = t.get<0>(); const size_t& j = t.get<0>();
const Vector& rd = t.get<1>(); const Matrix& rd = t.get<1>();
precisions(i) = t.get<2>(); precisions(i) = t.get<2>();
if (constrained(i)) mixed = true; if (std::isinf(precisions(i)))
for (size_t j2=0; j2<j; ++j2) mixed = true;
Ab(i,j2) = 0.0; // fill in zeros below diagonal anway Ab.block(i, j, 1, n + 1 - j) = rd.block(0, j, 1, n + 1 - j);
for (size_t j2=j; j2<n+1; ++j2) i += 1;
Ab(i,j2) = rd(j2);
i+=1;
} }
gttoc(constrained_QR_write_back_into_Ab);
// Must include mu, as the defaults might be higher, resulting in non-convergence // Must include mu, as the defaults might be higher, resulting in non-convergence
return mixed ? Constrained::MixedPrecisions(mu_, precisions) : Diagonal::Precisions(precisions); return mixed ? Constrained::MixedPrecisions(mu_, precisions) : Diagonal::Precisions(precisions);
@ -498,13 +560,13 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
// Isotropic // Isotropic
/* ************************************************************************* */ /* ************************************************************************* */
Isotropic::shared_ptr Isotropic::Sigma(size_t dim, double sigma, bool smart) { Isotropic::shared_ptr Isotropic::Sigma(size_t dim, double sigma, bool smart) {
if (smart && fabs(sigma-1.0)<1e-9) return Unit::Create(dim); if (smart && std::abs(sigma-1.0)<1e-9) return Unit::Create(dim);
return shared_ptr(new Isotropic(dim, sigma)); return shared_ptr(new Isotropic(dim, sigma));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smart) { Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smart) {
if (smart && fabs(variance-1.0)<1e-9) return Unit::Create(dim); if (smart && std::abs(variance-1.0)<1e-9) return Unit::Create(dim);
return shared_ptr(new Isotropic(dim, sqrt(variance))); return shared_ptr(new Isotropic(dim, sqrt(variance)));
} }
@ -561,21 +623,19 @@ void Unit::print(const std::string& name) const {
namespace mEstimator { namespace mEstimator {
/** produce a weight vector according to an error vector and the implemented Vector Base::weight(const Vector& error) const {
* robust function */
Vector Base::weight(const Vector &error) const {
const size_t n = error.rows(); const size_t n = error.rows();
Vector w(n); Vector w(n);
for ( size_t i = 0 ; i < n ; ++i ) for (size_t i = 0; i < n; ++i)
w(i) = weight(error(i)); w(i) = weight(error(i));
return w; return w;
} }
/** The following three functions reweight block matrices and a vector // The following three functions re-weight block matrices and a vector
* according to their weight implementation */ // according to their weight implementation
void Base::reweight(Vector& error) const { void Base::reweight(Vector& error) const {
if(reweight_ == Block) { if (reweight_ == Block) {
const double w = sqrtWeight(error.norm()); const double w = sqrtWeight(error.norm());
error *= w; error *= w;
} else { } else {
@ -583,7 +643,7 @@ void Base::reweight(Vector& error) const {
} }
} }
/** Reweight n block matrices with one error vector */ // Reweight n block matrices with one error vector
void Base::reweight(vector<Matrix> &A, Vector &error) const { void Base::reweight(vector<Matrix> &A, Vector &error) const {
if ( reweight_ == Block ) { if ( reweight_ == Block ) {
const double w = sqrtWeight(error.norm()); const double w = sqrtWeight(error.norm());
@ -601,7 +661,7 @@ void Base::reweight(vector<Matrix> &A, Vector &error) const {
} }
} }
/** Reweight one block matrix with one error vector */ // Reweight one block matrix with one error vector
void Base::reweight(Matrix &A, Vector &error) const { void Base::reweight(Matrix &A, Vector &error) const {
if ( reweight_ == Block ) { if ( reweight_ == Block ) {
const double w = sqrtWeight(error.norm()); const double w = sqrtWeight(error.norm());
@ -615,7 +675,7 @@ void Base::reweight(Matrix &A, Vector &error) const {
} }
} }
/** Reweight two block matrix with one error vector */ // Reweight two block matrix with one error vector
void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const { void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const {
if ( reweight_ == Block ) { if ( reweight_ == Block ) {
const double w = sqrtWeight(error.norm()); const double w = sqrtWeight(error.norm());
@ -631,7 +691,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const {
} }
} }
/** Reweight three block matrix with one error vector */ // Reweight three block matrix with one error vector
void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const { void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const {
if ( reweight_ == Block ) { if ( reweight_ == Block ) {
const double w = sqrtWeight(error.norm()); const double w = sqrtWeight(error.norm());
@ -659,11 +719,9 @@ void Null::print(const std::string &s="") const
Null::shared_ptr Null::Create() Null::shared_ptr Null::Create()
{ return shared_ptr(new Null()); } { return shared_ptr(new Null()); }
Fair::Fair(double c, const ReweightScheme reweight) Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) {
: Base(reweight), c_(c) { if (c_ <= 0) {
if ( c_ <= 0 ) { throw runtime_error("mEstimator Fair takes only positive double in constructor.");
cout << "mEstimator Fair takes only positive double in constructor. forced to 1.0" << endl;
c_ = 1.0;
} }
} }
@ -671,16 +729,13 @@ Fair::Fair(double c, const ReweightScheme reweight)
// Fair // Fair
/* ************************************************************************* */ /* ************************************************************************* */
double Fair::weight(double error) const
{ return 1.0 / (1.0 + fabs(error)/c_); }
void Fair::print(const std::string &s="") const void Fair::print(const std::string &s="") const
{ cout << s << "fair (" << c_ << ")" << endl; } { cout << s << "fair (" << c_ << ")" << endl; }
bool Fair::equals(const Base &expected, double tol) const { bool Fair::equals(const Base &expected, double tol) const {
const Fair* p = dynamic_cast<const Fair*> (&expected); const Fair* p = dynamic_cast<const Fair*> (&expected);
if (p == NULL) return false; if (p == NULL) return false;
return fabs(c_ - p->c_ ) < tol; return std::abs(c_ - p->c_ ) < tol;
} }
Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight) Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight)
@ -690,18 +745,12 @@ Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight)
// Huber // Huber
/* ************************************************************************* */ /* ************************************************************************* */
Huber::Huber(double k, const ReweightScheme reweight) Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) {
: Base(reweight), k_(k) { if (k_ <= 0) {
if ( k_ <= 0 ) { throw runtime_error("mEstimator Huber takes only positive double in constructor.");
cout << "mEstimator Huber takes only positive double in constructor. forced to 1.0" << endl;
k_ = 1.0;
} }
} }
double Huber::weight(double error) const {
return (fabs(error) > k_) ? k_ / fabs(error) : 1.0;
}
void Huber::print(const std::string &s="") const { void Huber::print(const std::string &s="") const {
cout << s << "huber (" << k_ << ")" << endl; cout << s << "huber (" << k_ << ")" << endl;
} }
@ -709,7 +758,7 @@ void Huber::print(const std::string &s="") const {
bool Huber::equals(const Base &expected, double tol) const { bool Huber::equals(const Base &expected, double tol) const {
const Huber* p = dynamic_cast<const Huber*>(&expected); const Huber* p = dynamic_cast<const Huber*>(&expected);
if (p == NULL) return false; if (p == NULL) return false;
return fabs(k_ - p->k_) < tol; return std::abs(k_ - p->k_) < tol;
} }
Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) { Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) {
@ -720,18 +769,12 @@ Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) {
// Cauchy // Cauchy
/* ************************************************************************* */ /* ************************************************************************* */
Cauchy::Cauchy(double k, const ReweightScheme reweight) Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), ksquared_(k * k) {
: Base(reweight), k_(k) { if (k <= 0) {
if ( k_ <= 0 ) { throw runtime_error("mEstimator Cauchy takes only positive double in constructor.");
cout << "mEstimator Cauchy takes only positive double in constructor. forced to 1.0" << endl;
k_ = 1.0;
} }
} }
double Cauchy::weight(double error) const {
return k_*k_ / (k_*k_ + error*error);
}
void Cauchy::print(const std::string &s="") const { void Cauchy::print(const std::string &s="") const {
cout << s << "cauchy (" << k_ << ")" << endl; cout << s << "cauchy (" << k_ << ")" << endl;
} }
@ -739,7 +782,7 @@ void Cauchy::print(const std::string &s="") const {
bool Cauchy::equals(const Base &expected, double tol) const { bool Cauchy::equals(const Base &expected, double tol) const {
const Cauchy* p = dynamic_cast<const Cauchy*>(&expected); const Cauchy* p = dynamic_cast<const Cauchy*>(&expected);
if (p == NULL) return false; if (p == NULL) return false;
return fabs(k_ - p->k_) < tol; return std::abs(ksquared_ - p->ksquared_) < tol;
} }
Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) {
@ -749,18 +792,7 @@ Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) {
/* ************************************************************************* */ /* ************************************************************************* */
// Tukey // Tukey
/* ************************************************************************* */ /* ************************************************************************* */
Tukey::Tukey(double c, const ReweightScheme reweight) Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
: Base(reweight), c_(c) {
}
double Tukey::weight(double error) const {
if (fabs(error) <= c_) {
double xc2 = (error/c_)*(error/c_);
double one_xc22 = (1.0-xc2)*(1.0-xc2);
return one_xc22;
}
return 0.0;
}
void Tukey::print(const std::string &s="") const { void Tukey::print(const std::string &s="") const {
std::cout << s << ": Tukey (" << c_ << ")" << std::endl; std::cout << s << ": Tukey (" << c_ << ")" << std::endl;
@ -769,7 +801,7 @@ void Tukey::print(const std::string &s="") const {
bool Tukey::equals(const Base &expected, double tol) const { bool Tukey::equals(const Base &expected, double tol) const {
const Tukey* p = dynamic_cast<const Tukey*>(&expected); const Tukey* p = dynamic_cast<const Tukey*>(&expected);
if (p == NULL) return false; if (p == NULL) return false;
return fabs(c_ - p->c_) < tol; return std::abs(c_ - p->c_) < tol;
} }
Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) {
@ -779,14 +811,7 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) {
/* ************************************************************************* */ /* ************************************************************************* */
// Welsh // Welsh
/* ************************************************************************* */ /* ************************************************************************* */
Welsh::Welsh(double c, const ReweightScheme reweight) Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
: Base(reweight), c_(c) {
}
double Welsh::weight(double error) const {
double xc2 = (error/c_)*(error/c_);
return std::exp(-xc2);
}
void Welsh::print(const std::string &s="") const { void Welsh::print(const std::string &s="") const {
std::cout << s << ": Welsh (" << c_ << ")" << std::endl; std::cout << s << ": Welsh (" << c_ << ")" << std::endl;
@ -795,7 +820,7 @@ void Welsh::print(const std::string &s="") const {
bool Welsh::equals(const Base &expected, double tol) const { bool Welsh::equals(const Base &expected, double tol) const {
const Welsh* p = dynamic_cast<const Welsh*>(&expected); const Welsh* p = dynamic_cast<const Welsh*>(&expected);
if (p == NULL) return false; if (p == NULL) return false;
return fabs(c_ - p->c_) < tol; return std::abs(c_ - p->c_) < tol;
} }
Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) {

130
gtsam/linear/NoiseModel.h
View File

@ -232,9 +232,11 @@ namespace gtsam {
/** /**
* Apply appropriately weighted QR factorization to the system [A b] * Apply appropriately weighted QR factorization to the system [A b]
* Q' * [A b] = [R d] * Q' * [A b] = [R d]
* Dimensions: (r*m) * m*(n+1) = r*(n+1) * Dimensions: (r*m) * m*(n+1) = r*(n+1), where r = min(m,n).
* This routine performs an in-place factorization on Ab.
* Below-diagonal elements are set to zero by this routine.
* @param Ab is the m*(n+1) augmented system matrix [A b] * @param Ab is the m*(n+1) augmented system matrix [A b]
* @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!! * @return Empty SharedDiagonal() noise model: R,d are whitened
*/ */
virtual boost::shared_ptr<Diagonal> QR(Matrix& Ab) const; virtual boost::shared_ptr<Diagonal> QR(Matrix& Ab) const;
@ -482,6 +484,12 @@ namespace gtsam {
/** /**
* Apply QR factorization to the system [A b], taking into account constraints * Apply QR factorization to the system [A b], taking into account constraints
* Q' * [A b] = [R d]
* Dimensions: (r*m) * m*(n+1) = r*(n+1), where r = min(m,n).
* This routine performs an in-place factorization on Ab.
* Below-diagonal elements are set to zero by this routine.
* @param Ab is the m*(n+1) augmented system matrix [A b]
* @return diagonal noise model can be all zeros, mixed, or not-constrained
*/ */
virtual Diagonal::shared_ptr QR(Matrix& Ab) const; virtual Diagonal::shared_ptr QR(Matrix& Ab) const;
@ -618,8 +626,24 @@ namespace gtsam {
} }
}; };
// TODO: should not really exist /**
/// The mEstimator namespace contains all robust error functions (not models) * The mEstimator name space contains all robust error functions.
* It mirrors the exposition at
* http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html
* which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice.
*
* To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples:
*
* Name Symbol Least-Squares L1-norm Huber
* Residual \rho(x) 0.5*x^2 |x| 0.5*x^2 if x<k, 0.5*k^2 + k|x-k| otherwise
* Derivative \phi(x) x sgn(x) x if x<k, k sgn(x) otherwise
* Weight w(x)=\phi(x)/x 1 1/|x| 1 if x<k, k/|x| otherwise
*
* With these definitions, D(\rho(x), p) = \phi(x) D(x,p) = w(x) x D(x,p) = w(x) D(L2(x), p),
* and hence we can solve the equivalent weighted least squares problem \sum w(r_i) \rho(r_i)
*
* Each M-estimator in the mEstimator name space simply implements the above functions.
*/
namespace mEstimator { namespace mEstimator {
//--------------------------------------------------------------------------------------- //---------------------------------------------------------------------------------------
@ -696,19 +720,20 @@ namespace gtsam {
/// Fair implements the "Fair" robust error model (Zhang97ivc) /// Fair implements the "Fair" robust error model (Zhang97ivc)
class GTSAM_EXPORT Fair : public Base { class GTSAM_EXPORT Fair : public Base {
protected:
double c_;
public: public:
typedef boost::shared_ptr<Fair> shared_ptr; typedef boost::shared_ptr<Fair> shared_ptr;
Fair(double c = 1.3998, const ReweightScheme reweight = Block); Fair(double c = 1.3998, const ReweightScheme reweight = Block);
virtual ~Fair() {} double weight(double error) const {
virtual double weight(double error) const; return 1.0 / (1.0 + fabs(error) / c_);
virtual void print(const std::string &s) const; }
virtual bool equals(const Base& expected, double tol=1e-8) const; void print(const std::string &s) const;
bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double c, const ReweightScheme reweight = Block) ; static shared_ptr Create(double c, const ReweightScheme reweight = Block) ;
protected:
double c_;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -721,19 +746,20 @@ namespace gtsam {
/// Huber implements the "Huber" robust error model (Zhang97ivc) /// Huber implements the "Huber" robust error model (Zhang97ivc)
class GTSAM_EXPORT Huber : public Base { class GTSAM_EXPORT Huber : public Base {
protected:
double k_;
public: public:
typedef boost::shared_ptr<Huber> shared_ptr; typedef boost::shared_ptr<Huber> shared_ptr;
virtual ~Huber() {}
Huber(double k = 1.345, const ReweightScheme reweight = Block); Huber(double k = 1.345, const ReweightScheme reweight = Block);
virtual double weight(double error) const; double weight(double error) const {
virtual void print(const std::string &s) const; return (error < k_) ? (1.0) : (k_ / fabs(error));
virtual bool equals(const Base& expected, double tol=1e-8) const; }
void print(const std::string &s) const;
bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
protected:
double k_;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -750,19 +776,20 @@ namespace gtsam {
/// oberlaender@fzi.de /// oberlaender@fzi.de
/// Thanks Jan! /// Thanks Jan!
class GTSAM_EXPORT Cauchy : public Base { class GTSAM_EXPORT Cauchy : public Base {
protected:
double k_, ksquared_;
public: public:
typedef boost::shared_ptr<Cauchy> shared_ptr; typedef boost::shared_ptr<Cauchy> shared_ptr;
virtual ~Cauchy() {}
Cauchy(double k = 0.1, const ReweightScheme reweight = Block); Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
virtual double weight(double error) const; double weight(double error) const {
virtual void print(const std::string &s) const; return ksquared_ / (ksquared_ + error*error);
virtual bool equals(const Base& expected, double tol=1e-8) const; }
void print(const std::string &s) const;
bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
protected:
double k_;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -775,19 +802,24 @@ namespace gtsam {
/// Tukey implements the "Tukey" robust error model (Zhang97ivc) /// Tukey implements the "Tukey" robust error model (Zhang97ivc)
class GTSAM_EXPORT Tukey : public Base { class GTSAM_EXPORT Tukey : public Base {
protected:
double c_, csquared_;
public: public:
typedef boost::shared_ptr<Tukey> shared_ptr; typedef boost::shared_ptr<Tukey> shared_ptr;
Tukey(double c = 4.6851, const ReweightScheme reweight = Block); Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
virtual ~Tukey() {} double weight(double error) const {
virtual double weight(double error) const; if (std::fabs(error) <= c_) {
virtual void print(const std::string &s) const; double xc2 = error*error/csquared_;
virtual bool equals(const Base& expected, double tol=1e-8) const; return (1.0-xc2)*(1.0-xc2);
}
return 0.0;
}
void print(const std::string &s) const;
bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
protected:
double c_;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -800,19 +832,21 @@ namespace gtsam {
/// Welsh implements the "Welsh" robust error model (Zhang97ivc) /// Welsh implements the "Welsh" robust error model (Zhang97ivc)
class GTSAM_EXPORT Welsh : public Base { class GTSAM_EXPORT Welsh : public Base {
protected:
double c_, csquared_;
public: public:
typedef boost::shared_ptr<Welsh> shared_ptr; typedef boost::shared_ptr<Welsh> shared_ptr;
Welsh(double c = 2.9846, const ReweightScheme reweight = Block); Welsh(double c = 2.9846, const ReweightScheme reweight = Block);
virtual ~Welsh() {} double weight(double error) const {
virtual double weight(double error) const; double xc2 = (error*error)/csquared_;
virtual void print(const std::string &s) const; return std::exp(-xc2);
virtual bool equals(const Base& expected, double tol=1e-8) const; }
void print(const std::string &s) const;
bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
protected:
double c_;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -884,7 +918,23 @@ namespace gtsam {
} ///\namespace mEstimator } ///\namespace mEstimator
/// Base class for robust error models /**
* Base class for robust error models
* The robust M-estimators above simply tell us how to re-weight the residual, and are
* isotropic kernels, in that they do not allow for correlated noise. They also have no way
* to scale the residual values, e.g., dividing by a single standard deviation.
* Hence, the actual robust noise model below does this scaling/whitening in sequence, by
* passing both a standard noise model and a robust estimator.
*
* Taking as an example noise = Isotropic::Create(d, sigma), we first divide the residuals
* uw = |Ax-b| by sigma by "whitening" the system (A,b), obtaining r = |Ax-b|/sigma, and
* then we pass the now whitened residual 'r' through the robust M-estimator.
* This is currently done by multiplying with sqrt(w), because the residuals will be squared
* again in error, yielding 0.5 \sum w(r)*r^2.
*
* In other words, while sigma is expressed in the native residual units, a parameter like
* k in the Huber norm is expressed in whitened units, i.e., "nr of sigmas".
*/
class GTSAM_EXPORT Robust : public Base { class GTSAM_EXPORT Robust : public Base {
public: public:
typedef boost::shared_ptr<Robust> shared_ptr; typedef boost::shared_ptr<Robust> shared_ptr;

11
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -508,9 +508,8 @@ Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
Errors::iterator ei = e.begin(); Errors::iterator ei = e.begin();
for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) { for (size_t i = 0; i < y.size(); ++i, ++ei)
*ei = y[i]; *ei = y[i];
}
// Add A2 contribution // Add A2 contribution
VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y
@ -523,9 +522,9 @@ VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {
Errors::const_iterator it = e.begin(); Errors::const_iterator it = e.begin();
VectorValues y = zero(); VectorValues y = zero();
for ( Key i = 0 ; i < y.size() ; ++i, ++it ) for (size_t i = 0; i < y.size(); ++i, ++it)
y[i] = *it ; y[i] = *it;
transposeMultiplyAdd2(1.0,it,e.end(),y); transposeMultiplyAdd2(1.0, it, e.end(), y);
return y; return y;
} }
@ -535,7 +534,7 @@ void SubgraphPreconditioner::transposeMultiplyAdd
(double alpha, const Errors& e, VectorValues& y) const { (double alpha, const Errors& e, VectorValues& y) const {
Errors::const_iterator it = e.begin(); Errors::const_iterator it = e.begin();
for ( Key i = 0 ; i < y.size() ; ++i, ++it ) { for (size_t i = 0; i < y.size(); ++i, ++it) {
const Vector& ei = *it; const Vector& ei = *it;
axpy(alpha, ei, y[i]); axpy(alpha, ei, y[i]);
} }

2
gtsam/linear/VectorValues.h
View File

@ -135,7 +135,7 @@ namespace gtsam {
/// @{ /// @{
/** Number of variables stored. */ /** Number of variables stored. */
Key size() const { return values_.size(); } size_t size() const { return values_.size(); }
/** Return the dimension of variable \c j. */ /** Return the dimension of variable \c j. */
size_t dim(Key j) const { return at(j).rows(); } size_t dim(Key j) const { return at(j).rows(); }

14
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -314,22 +314,26 @@ TEST(HessianFactor, CombineAndEliminate1) {
Ordering ordering = list_of(1); Ordering ordering = list_of(1);
GaussianConditional::shared_ptr expectedConditional; GaussianConditional::shared_ptr expectedConditional;
JacobianFactor::shared_ptr expectedFactor; JacobianFactor::shared_ptr expectedFactor;
boost::tie(expectedConditional, expectedFactor) = // boost::tie(expectedConditional, expectedFactor) = jacobian.eliminate(ordering);
jacobian.eliminate(ordering); CHECK(expectedFactor);
// Eliminate // Eliminate
GaussianConditional::shared_ptr actualConditional; GaussianConditional::shared_ptr actualConditional;
HessianFactor::shared_ptr actualHessian; HessianFactor::shared_ptr actualHessian;
boost::tie(actualConditional, actualHessian) = // boost::tie(actualConditional, actualHessian) = //
EliminateCholesky(gfg, ordering); EliminateCholesky(gfg, ordering);
actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
#ifdef TEST_ERROR_EQUIVALENCE
// these tests disabled because QR cuts off the all zeros + error row
EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9); EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
EXPECT( EXPECT(
assert_equal(expectedFactor->augmentedInformation(), assert_equal(expectedFactor->augmentedInformation(),
actualHessian->augmentedInformation(), 1e-9)); actualHessian->augmentedInformation(), 1e-9));
EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6)); EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
} #endif
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate2) { TEST(HessianFactor, CombineAndEliminate2) {
@ -381,8 +385,11 @@ TEST(HessianFactor, CombineAndEliminate2) {
HessianFactor::shared_ptr actualHessian; HessianFactor::shared_ptr actualHessian;
boost::tie(actualConditional, actualHessian) = // boost::tie(actualConditional, actualHessian) = //
EliminateCholesky(gfg, ordering); EliminateCholesky(gfg, ordering);
actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
#ifdef TEST_ERROR_EQUIVALENCE
// these tests disabled because QR cuts off the all zeros + error row
VectorValues v; VectorValues v;
v.insert(1, Vector3(1, 2, 3)); v.insert(1, Vector3(1, 2, 3));
EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9); EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
@ -390,6 +397,7 @@ TEST(HessianFactor, CombineAndEliminate2) {
assert_equal(expectedFactor->augmentedInformation(), assert_equal(expectedFactor->augmentedInformation(),
actualHessian->augmentedInformation(), 1e-9)); actualHessian->augmentedInformation(), 1e-9));
EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6)); EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */

118
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
@ -161,37 +162,46 @@ TEST(JabobianFactor, Hessian_conversion) {
EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3)); EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3));
} }
/* ************************************************************************* */
namespace simple_graph {
Key keyX(10), keyY(8), keyZ(12);
double sigma1 = 0.1;
Matrix A11 = Matrix::Identity(2, 2);
Vector2 b1(2, -1);
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1));
double sigma2 = 0.5;
Matrix A21 = -2 * Matrix::Identity(2, 2);
Matrix A22 = 3 * Matrix::Identity(2, 2);
Vector2 b2(4, -5);
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2));
double sigma3 = 1.0;
Matrix A32 = -4 * Matrix::Identity(2, 2);
Matrix A33 = 5 * Matrix::Identity(2, 2);
Vector2 b3(3, -6);
JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3));
GaussianFactorGraph factors(list_of(factor1)(factor2)(factor3));
Ordering ordering(list_of(keyX)(keyY)(keyZ));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( JacobianFactor, construct_from_graph) TEST( JacobianFactor, construct_from_graph)
{ {
GaussianFactorGraph factors; using namespace simple_graph;
double sigma1 = 0.1;
Matrix A11 = Matrix::Identity(2,2);
Vector b1(2); b1 << 2, -1;
factors.add(JacobianFactor(10, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1)));
double sigma2 = 0.5;
Matrix A21 = -2 * Matrix::Identity(2,2);
Matrix A22 = 3 * Matrix::Identity(2,2);
Vector b2(2); b2 << 4, -5;
factors.add(JacobianFactor(10, A21, 8, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2)));
double sigma3 = 1.0;
Matrix A32 = -4 * Matrix::Identity(2,2);
Matrix A33 = 5 * Matrix::Identity(2,2);
Vector b3(2); b3 << 3, -6;
factors.add(JacobianFactor(8, A32, 12, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3)));
Matrix A1(6,2); A1 << A11, A21, Matrix::Zero(2,2); Matrix A1(6,2); A1 << A11, A21, Matrix::Zero(2,2);
Matrix A2(6,2); A2 << Matrix::Zero(2,2), A22, A32; Matrix A2(6,2); A2 << Matrix::Zero(2,2), A22, A32;
Matrix A3(6,2); A3 << Matrix::Zero(4,2), A33; Matrix A3(6,2); A3 << Matrix::Zero(4,2), A33;
Vector b(6); b << b1, b2, b3; Vector b(6); b << b1, b2, b3;
Vector sigmas(6); sigmas << sigma1, sigma1, sigma2, sigma2, sigma3, sigma3; Vector sigmas(6); sigmas << sigma1, sigma1, sigma2, sigma2, sigma3, sigma3;
JacobianFactor expected(10, A1, 8, A2, 12, A3, b, noiseModel::Diagonal::Sigmas(sigmas)); JacobianFactor expected(keyX, A1, keyY, A2, keyZ, A3, b, noiseModel::Diagonal::Sigmas(sigmas));
// The ordering here specifies the order in which the variables will appear in the combined factor // The ordering here specifies the order in which the variables will appear in the combined factor
JacobianFactor actual(factors, Ordering(list_of(10)(8)(12))); JacobianFactor actual(factors, ordering);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
@ -466,7 +476,7 @@ TEST(JacobianFactor, eliminate2 )
+0.00,-0.20,+0.00,-0.80 +0.00,-0.20,+0.00,-0.80
).finished()/oldSigma; ).finished()/oldSigma;
Vector d = Vector2(0.2,-0.14)/oldSigma; Vector d = Vector2(0.2,-0.14)/oldSigma;
GaussianConditional expectedCG(2, d, R11, 11, S12); GaussianConditional expectedCG(2, d, R11, 11, S12, noiseModel::Unit::Create(2));
EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); EXPECT(assert_equal(expectedCG, *actual.first, 1e-4));
@ -478,7 +488,7 @@ TEST(JacobianFactor, eliminate2 )
0.00, 1.00, +0.00, -1.00 0.00, 1.00, +0.00, -1.00
).finished()/sigma; ).finished()/sigma;
Vector b1 = Vector2(0.0, 0.894427); Vector b1 = Vector2(0.0, 0.894427);
JacobianFactor expectedLF(11, Bl1x1, b1); JacobianFactor expectedLF(11, Bl1x1, b1, noiseModel::Unit::Create(2));
EXPECT(assert_equal(expectedLF, *actual.second,1e-3)); EXPECT(assert_equal(expectedLF, *actual.second,1e-3));
} }
@ -516,7 +526,7 @@ TEST(JacobianFactor, EliminateQR)
EXPECT(assert_equal(2.0 * Ab, actualDense)); EXPECT(assert_equal(2.0 * Ab, actualDense));
// Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows) // Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows)
Matrix R = 2.0 * (Matrix(11, 11) << Matrix R = 2.0 * (Matrix(10, 11) <<
-12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611, -12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611,
0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625, 0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625,
0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250, 0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250,
@ -526,24 +536,24 @@ TEST(JacobianFactor, EliminateQR)
0., 0., 0., 0., 0., 0., -11.1139, -0.6521, -2.1943, -7.5529, -0.9081, 0., 0., 0., 0., 0., 0., -11.1139, -0.6521, -2.1943, -7.5529, -0.9081,
0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685, 0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685,
0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476, 0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476,
0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090, 0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090).finished();
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635).finished();
GaussianConditional expectedFragment( GaussianConditional expectedFragment(list_of(3)(5)(7)(9)(11), 3,
list_of(3)(5)(7)(9)(11), 3, VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), R)); VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), R.topRows(6)),
noiseModel::Unit::Create(6));
// Eliminate (3 frontal variables, 6 scalar columns) using QR !!!! // Eliminate (3 frontal variables, 6 scalar columns) using QR !!!!
GaussianFactorGraph::EliminationResult actual = EliminateQR(factors, list_of(3)(5)(7)); GaussianFactorGraph::EliminationResult actual = EliminateQR(factors, list_of(3)(5)(7));
const JacobianFactor &actualJF = dynamic_cast<const JacobianFactor&>(*actual.second); const JacobianFactor &actualJF = dynamic_cast<const JacobianFactor&>(*actual.second);
EXPECT(assert_equal(expectedFragment, *actual.first, 0.001)); EXPECT(assert_equal(expectedFragment, *actual.first, 0.001));
EXPECT(assert_equal(size_t(2), actualJF.keys().size())); EXPECT_LONGS_EQUAL(size_t(2), actualJF.keys().size());
EXPECT(assert_equal(Key(9), actualJF.keys()[0])); EXPECT(assert_equal(Key(9), actualJF.keys()[0]));
EXPECT(assert_equal(Key(11), actualJF.keys()[1])); EXPECT(assert_equal(Key(11), actualJF.keys()[1]));
EXPECT(assert_equal(Matrix(R.block(6, 6, 5, 2)), actualJF.getA(actualJF.begin()), 0.001)); EXPECT(assert_equal(Matrix(R.block(6, 6, 4, 2)), actualJF.getA(actualJF.begin()), 0.001));
EXPECT(assert_equal(Matrix(R.block(6, 8, 5, 2)), actualJF.getA(actualJF.begin()+1), 0.001)); EXPECT(assert_equal(Matrix(R.block(6, 8, 4, 2)), actualJF.getA(actualJF.begin()+1), 0.001));
EXPECT(assert_equal(Vector(R.col(10).segment(6, 5)), actualJF.getb(), 0.001)); EXPECT(assert_equal(Vector(R.col(10).segment(6, 4)), actualJF.getb(), 0.001));
EXPECT(!actualJF.get_model()); EXPECT(assert_equal(*noiseModel::Diagonal::Sigmas(Vector4::Ones()), *actualJF.get_model(), 0.001));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -557,8 +567,8 @@ TEST ( JacobianFactor, constraint_eliminate1 )
pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr> pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr>
actual = lc.eliminate(list_of(1)); actual = lc.eliminate(list_of(1));
// verify linear factor // verify linear factor is a null ptr
EXPECT(actual.second->size() == 0); EXPECT(actual.second->empty());
// verify conditional Gaussian // verify conditional Gaussian
Vector sigmas = Vector2(0.0, 0.0); Vector sigmas = Vector2(0.0, 0.0);
@ -574,14 +584,10 @@ TEST ( JacobianFactor, constraint_eliminate2 )
Vector b(2); b(0)=3.0; b(1)=4.0; Vector b(2); b(0)=3.0; b(1)=4.0;
// A1 - invertible // A1 - invertible
Matrix A1(2,2); Matrix2 A1; A1 << 2,4, 2,1;
A1(0,0) = 1.0 ; A1(0,1) = 2.0;
A1(1,0) = 2.0 ; A1(1,1) = 1.0;
// A2 - not invertible // A2 - not invertible
Matrix A2(2,2); Matrix2 A2; A2 << 2,4, 2,4;
A2(0,0) = 1.0 ; A2(0,1) = 2.0;
A2(1,0) = 2.0 ; A2(1,1) = 4.0;
JacobianFactor lc(1, A1, 2, A2, b, noiseModel::Constrained::All(2)); JacobianFactor lc(1, A1, 2, A2, b, noiseModel::Constrained::All(2));
@ -598,18 +604,38 @@ TEST ( JacobianFactor, constraint_eliminate2 )
EXPECT(assert_equal(expectedLF, *actual.second)); EXPECT(assert_equal(expectedLF, *actual.second));
// verify CG // verify CG
Matrix R = (Matrix(2, 2) << Matrix2 R; R << 1,2, 0,1;
1.0, 2.0, Matrix2 S; S << 1,2, 0,0;
0.0, 1.0).finished();
Matrix S = (Matrix(2, 2) <<
1.0, 2.0,
0.0, 0.0).finished();
Vector d = Vector2(3.0, 0.6666); Vector d = Vector2(3.0, 0.6666);
Vector sigmas = Vector2(0.0, 0.0); Vector sigmas = Vector2(0.0, 0.0);
GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas)); GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas));
EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); EXPECT(assert_equal(expectedCG, *actual.first, 1e-4));
} }
/* ************************************************************************* */
TEST(JacobianFactor, OverdeterminedEliminate) {
Matrix Ab(9, 4);
Ab << 0, 1, 0, 0, //
0, 0, 1, 0, //
Matrix74::Ones();
// Call Gaussian version
Vector9 sigmas = Vector9::Ones();
SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
JacobianFactor factor(0, Ab.leftCols(3), Ab.col(3), diagonal);
GaussianFactorGraph::EliminationResult actual = factor.eliminate(list_of(0));
Matrix expectedRd(3, 4);
expectedRd << -2.64575131, -2.64575131, -2.64575131, -2.64575131, //
0.0, -1, 0, 0, //
0.0, 0.0, -1, 0;
GaussianConditional expectedConditional(0, expectedRd.col(3), expectedRd.leftCols(3),
noiseModel::Unit::Create(3));
EXPECT(assert_equal(expectedConditional, *actual.first, 1e-4));
EXPECT(actual.second->empty());
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */ /* ************************************************************************* */

155
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -222,45 +222,172 @@ namespace exampleQR {
SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas); SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
} }
/* ************************************************************************* */
TEST( NoiseModel, QR ) TEST( NoiseModel, QR )
{ {
Matrix Ab1 = exampleQR::Ab; Matrix Ab1 = exampleQR::Ab;
Matrix Ab2 = exampleQR::Ab; // otherwise overwritten ! Matrix Ab2 = exampleQR::Ab; // otherwise overwritten !
// Expected result
Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished();
SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
// Call Gaussian version // Call Gaussian version
SharedDiagonal actual1 = exampleQR::diagonal->QR(Ab1); SharedDiagonal actual1 = exampleQR::diagonal->QR(Ab1);
EXPECT(!actual1); EXPECT(actual1->isUnit());
EXPECT(linear_dependent(exampleQR::Rd,Ab1,1e-4)); // Ab was modified in place !!! EXPECT(linear_dependent(exampleQR::Rd,Ab1,1e-4)); // Ab was modified in place !!!
// Call Constrained version // Expected result for constrained version
SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(exampleQR::sigmas); Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished();
SharedDiagonal actual2 = constrained->QR(Ab2); SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
SharedDiagonal expectedModel2 = noiseModel::Diagonal::Sigmas(expectedSigmas);
EXPECT(assert_equal(*expectedModel2,*actual2,1e-6));
Matrix expectedRd2 = (Matrix(4, 7) << Matrix expectedRd2 = (Matrix(4, 7) <<
1., 0., -0.2, 0., -0.8, 0., 0.2, 1., 0., -0.2, 0., -0.8, 0., 0.2,
0., 1., 0.,-0.2, 0., -0.8,-0.14, 0., 1., 0.,-0.2, 0., -0.8,-0.14,
0., 0., 1., 0., -1., 0., 0.0, 0., 0., 1., 0., -1., 0., 0.0,
0., 0., 0., 1., 0., -1., 0.2).finished(); 0., 0., 0., 1., 0., -1., 0.2).finished();
EXPECT(linear_dependent(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!!
// Call Constrained version
SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(exampleQR::sigmas);
SharedDiagonal actual2 = constrained->QR(Ab2);
EXPECT(assert_equal(*expectedModel, *actual2, 1e-6));
EXPECT(linear_dependent(expectedRd2, Ab2, 1e-6)); // Ab was modified in place !!!
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(NoiseModel, OverdeterminedQR) {
Matrix Ab1(9, 4);
Ab1 << 0, 1, 0, 0, //
0, 0, 1, 0, //
Matrix74::Ones();
Matrix Ab2 = Ab1; // otherwise overwritten !
// Call Gaussian version
Vector9 sigmas = Vector9::Ones() ;
SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
SharedDiagonal actual1 = diagonal->QR(Ab1);
EXPECT(actual1->isUnit());
Matrix expectedRd(9,4);
expectedRd << -2.64575131, -2.64575131, -2.64575131, -2.64575131, //
0.0, -1, 0, 0, //
0.0, 0.0, -1, 0, //
Matrix64::Zero();
EXPECT(assert_equal(expectedRd, Ab1, 1e-4)); // Ab was modified in place !!!
// Expected result for constrained version
Vector3 expectedSigmas(0.377964473, 1, 1);
SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
// Call Constrained version
SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);
SharedDiagonal actual2 = constrained->QR(Ab2);
EXPECT(assert_equal(*expectedModel, *actual2, 1e-6));
expectedRd.row(0) *= 0.377964473; // not divided by sigma!
EXPECT(assert_equal(-expectedRd, Ab2, 1e-6)); // Ab was modified in place !!!
}
/* ************************************************************************* */
TEST( NoiseModel, MixedQR )
{
// Call Constrained version, with first and third row treated as constraints
// Naming the 6 variables u,v,w,x,y,z, we have
// u = -z
// w = -x
// And let's have simple priors on variables
Matrix Ab(5,6+1);
Ab <<
1,0,0,0,0,1, 0, // u+z = 0
0,0,0,0,1,0, 0, // y^2
0,0,1,1,0,0, 0, // w+x = 0
0,1,0,0,0,0, 0, // v^2
0,0,0,0,0,1, 0; // z^2
Vector mixed_sigmas = (Vector(5) << 0, 1, 0, 1, 1).finished();
SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(mixed_sigmas);
// Expected result
Vector expectedSigmas = (Vector(5) << 0, 1, 0, 1, 1).finished();
SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
Matrix expectedRd(5, 6+1);
expectedRd << 1, 0, 0, 0, 0, 1, 0, //
0, 1, 0, 0, 0, 0, 0, //
0, 0, 1, 1, 0, 0, 0, //
0, 0, 0, 0, 1, 0, 0, //
0, 0, 0, 0, 0, 1, 0; //
SharedDiagonal actual = constrained->QR(Ab);
EXPECT(assert_equal(*expectedModel,*actual,1e-6));
EXPECT(linear_dependent(expectedRd,Ab,1e-6)); // Ab was modified in place !!!
}
/* ************************************************************************* */
TEST( NoiseModel, MixedQR2 )
{
// Let's have three variables x,y,z, but x=z and y=z
// Hence, all non-constraints are really measurements on z
Matrix Ab(11,3+1);
Ab <<
1,0,0, 0, //
0,1,0, 0, //
0,0,1, 0, //
-1,0,1, 0, // x=z
1,0,0, 0, //
0,1,0, 0, //
0,0,1, 0, //
0,-1,1, 0, // y=z
1,0,0, 0, //
0,1,0, 0, //
0,0,1, 0; //
Vector sigmas(11);
sigmas.setOnes();
sigmas[3] = 0;
sigmas[7] = 0;
SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);
// Expected result
Vector3 expectedSigmas(0,0,1.0/3);
SharedDiagonal expectedModel = noiseModel::Constrained::MixedSigmas(expectedSigmas);
Matrix expectedRd(11, 3+1);
expectedRd.setZero();
expectedRd.row(0) << -1, 0, 1, 0; // x=z
expectedRd.row(1) << 0, -1, 1, 0; // y=z
expectedRd.row(2) << 0, 0, 1, 0; // z=0 +/- 1/3
SharedDiagonal actual = constrained->QR(Ab);
EXPECT(assert_equal(*expectedModel,*actual,1e-6));
EXPECT(assert_equal(expectedRd,Ab,1e-6)); // Ab was modified in place !!!
}
/* ************************************************************************* */
TEST( NoiseModel, FullyConstrained )
{
Matrix Ab(3,7);
Ab <<
1,0,0,0,0,1, 2, // u+z = 2
0,0,1,1,0,0, 4, // w+x = 4
0,1,0,1,1,1, 8; // v+x+y+z=8
SharedDiagonal constrained = noiseModel::Constrained::All(3);
// Expected result
SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(Vector3 (0,0,0));
Matrix expectedRd(3, 7);
expectedRd << 1, 0, 0, 0, 0, 1, 2, //
0, 1, 0, 1, 1, 1, 8, //
0, 0, 1, 1, 0, 0, 4; //
SharedDiagonal actual = constrained->QR(Ab);
EXPECT(assert_equal(*expectedModel,*actual,1e-6));
EXPECT(linear_dependent(expectedRd,Ab,1e-6)); // Ab was modified in place !!!
}
/* ************************************************************************* */
// This matches constraint_eliminate2 in testJacobianFactor
TEST(NoiseModel, QRNan ) TEST(NoiseModel, QRNan )
{ {
SharedDiagonal constrained = noiseModel::Constrained::All(2); SharedDiagonal constrained = noiseModel::Constrained::All(2);
Matrix Ab = (Matrix(2, 5) << 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.).finished(); Matrix Ab = (Matrix25() << 2, 4, 2, 4, 6, 2, 1, 2, 4, 4).finished();
SharedDiagonal expected = noiseModel::Constrained::All(2); SharedDiagonal expected = noiseModel::Constrained::All(2);
Matrix expectedAb = (Matrix(2, 5) << 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3).finished(); Matrix expectedAb = (Matrix25() << 1, 2, 1, 2, 3, 0, 1, 0, 0, 2.0/3).finished();
SharedDiagonal actual = constrained->QR(Ab); SharedDiagonal actual = constrained->QR(Ab);
EXPECT(assert_equal(*expected,*actual)); EXPECT(assert_equal(*expected,*actual));
EXPECT(assert_equal(expectedAb,Ab)); EXPECT(linear_dependent(expectedAb,Ab));
} }
/* ************************************************************************* */ /* ************************************************************************* */

28
gtsam/navigation/GPSFactor.cpp
View File

@ -25,7 +25,7 @@ namespace gtsam {
//*************************************************************************** //***************************************************************************
void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor on " << keyFormatter(key()) << "\n"; cout << s << "GPSFactor on " << keyFormatter(key()) << "\n";
cout << " prior mean: " << nT_ << "\n"; cout << " GPS measurement: " << nT_ << "\n";
noiseModel_->print(" noise model: "); noiseModel_->print(" noise model: ");
} }
@ -38,12 +38,7 @@ bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
//*************************************************************************** //***************************************************************************
Vector GPSFactor::evaluateError(const Pose3& p, Vector GPSFactor::evaluateError(const Pose3& p,
boost::optional<Matrix&> H) const { boost::optional<Matrix&> H) const {
if (H) { return p.translation(H) -nT_;
H->resize(3, 6);
H->block < 3, 3 > (0, 0) << zeros(3, 3);
H->block < 3, 3 > (0, 3) << p.rotation().matrix();
}
return p.translation() -nT_;
} }
//*************************************************************************** //***************************************************************************
@ -68,6 +63,25 @@ pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,
return make_pair(nTb, nV); return make_pair(nTb, nV);
} }
//***************************************************************************
void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n";
nT_.print(" GPS measurement: ");
noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol);
}
//***************************************************************************
Vector GPSFactor2::evaluateError(const NavState& p,
boost::optional<Matrix&> H) const {
return p.position(H).vector() -nT_.vector();
}
//*************************************************************************** //***************************************************************************
}/// namespace gtsam }/// namespace gtsam

88
gtsam/navigation/GPSFactor.h
View File

@ -18,6 +18,7 @@
#pragma once #pragma once
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
namespace gtsam { namespace gtsam {
@ -37,7 +38,7 @@ private:
typedef NoiseModelFactor1<Pose3> Base; typedef NoiseModelFactor1<Pose3> Base;
Point3 nT_; ///< Position measurement in Point3 nT_; ///< Position measurement in cartesian coordinates
public: public:
@ -50,14 +51,13 @@ public:
/** default constructor - only use for serialization */ /** default constructor - only use for serialization */
GPSFactor(): nT_(0, 0, 0) {} GPSFactor(): nT_(0, 0, 0) {}
virtual ~GPSFactor() { virtual ~GPSFactor() {}
}
/** /**
* @brief Constructor from a measurement in a Cartesian frame. * @brief Constructor from a measurement in a Cartesian frame.
* Use GeographicLib to convert from geographic (latitude and longitude) coordinates * Use GeographicLib to convert from geographic (latitude and longitude) coordinates
* @param key of the Pose3 variable that will be constrained * @param key of the Pose3 variable that will be constrained
* @param gpsIn measurement already in coordinates * @param gpsIn measurement already in correct coordinates
* @param model Gaussian noise model * @param model Gaussian noise model
*/ */
GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
@ -70,18 +70,14 @@ public:
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
/** implement functions needed for Testable */ /// print
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const; DefaultKeyFormatter) const;
/** equals */ /// equals
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/** implement functions needed to derive from Factor */ /// vector of errors
/** vector of errors */
Vector evaluateError(const Pose3& p, Vector evaluateError(const Pose3& p,
boost::optional<Matrix&> H = boost::none) const; boost::optional<Matrix&> H = boost::none) const;
@ -89,8 +85,8 @@ public:
return nT_; return nT_;
} }
/* /**
* Convenience funcion to estimate state at time t, given two GPS * Convenience function to estimate state at time t, given two GPS
* readings (in local NED Cartesian frame) bracketing t * readings (in local NED Cartesian frame) bracketing t
* Assumes roll is zero, calculates yaw and pitch from NED1->NED2 vector. * Assumes roll is zero, calculates yaw and pitch from NED1->NED2 vector.
*/ */
@ -99,7 +95,71 @@ public:
private: private:
/** Serialization function */ /// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar
& boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(nT_);
}
};
/**
* Version of GPSFactor for NavState
* @addtogroup Navigation
*/
class GTSAM_EXPORT GPSFactor2: public NoiseModelFactor1<NavState> {
private:
typedef NoiseModelFactor1<NavState> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GPSFactor2> shared_ptr;
/// Typedef to this class
typedef GPSFactor2 This;
/// default constructor - only use for serialization
GPSFactor2():nT_(0, 0, 0) {}
virtual ~GPSFactor2() {}
/// Constructor from a measurement in a Cartesian frame.
GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
Base(model, key), nT_(gpsIn) {
}
/// @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)));
}
/// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/// equals
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/// vector of errors
Vector evaluateError(const NavState& p,
boost::optional<Matrix&> H = boost::none) const;
inline const Point3 & measurementIn() const {
return nT_;
}
private:
/// Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE & ar, const unsigned int /*version*/) {

7
gtsam/navigation/ImuBias.h
View File

@ -44,7 +44,7 @@ public:
biasAcc_(biasAcc), biasGyro_(biasGyro) { biasAcc_(biasAcc), biasGyro_(biasGyro) {
} }
ConstantBias(const Vector6& v) : explicit ConstantBias(const Vector6& v) :
biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) { biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) {
} }
@ -114,6 +114,11 @@ public:
return ConstantBias(-biasAcc_, -biasGyro_); return ConstantBias(-biasAcc_, -biasGyro_);
} }
/** addition of vector on right */
ConstantBias operator+(const Vector6& v) const {
return ConstantBias(biasAcc_ + v.head<3>(), biasGyro_ + v.tail<3>());
}
/** addition */ /** addition */
ConstantBias operator+(const ConstantBias& b) const { ConstantBias operator+(const ConstantBias& b) const {
return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_);

78
gtsam/navigation/ImuFactor.cpp
View File

@ -72,9 +72,30 @@ void PreintegratedImuMeasurements::integrateMeasurement(
preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose(); preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose();
preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose(); preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();
// NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt) // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3
static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished(); preintMeasCov_.block<3,3>(3,3).noalias() += iCov * dt;
preintMeasCov_.noalias() += Gi * (iCov * dt) * Gi.transpose(); }
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredAccs,
const Matrix& measuredOmegas,
const Matrix& dts) {
assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1);
assert(dts.cols() >= 1);
assert(measuredAccs.cols() == dts.cols());
assert(measuredOmegas.cols() == dts.cols());
size_t n = static_cast<size_t>(dts.cols());
for (size_t j = 0; j < n; j++) {
integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0,j));
}
}
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, //
Matrix9* H1, Matrix9* H2) {
PreintegrationBase::mergeWith(pim12, H1, H2);
preintMeasCov_ =
*H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose();
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -120,9 +141,7 @@ NonlinearFactor::shared_ptr ImuFactor::clone() const {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
std::ostream& operator<<(std::ostream& os, const ImuFactor& f) { std::ostream& operator<<(std::ostream& os, const ImuFactor& f) {
os << " preintegrated measurements:\n" << f._PIM_; f._PIM_.print("preintegrated measurements:\n");
;
// Print standard deviations on covariance only
os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose(); os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
return os; return os;
} }
@ -172,9 +191,6 @@ PreintegratedImuMeasurements ImuFactor::Merge(
Matrix9 H1, H2; Matrix9 H1, H2;
pim02.mergeWith(pim12, &H1, &H2); pim02.mergeWith(pim12, &H1, &H2);
pim02.preintMeasCov_ = H1 * pim01.preintMeasCov_ * H1.transpose() +
H2 * pim12.preintMeasCov_ * H2.transpose();
return pim02; return pim02;
} }
@ -229,6 +245,50 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
vel_j = pvb.velocity; vel_j = pvb.velocity;
} }
#endif #endif
//------------------------------------------------------------------------------
// ImuFactor2 methods
//------------------------------------------------------------------------------
ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, const PreintegratedImuMeasurements& pim)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, bias),
_PIM_(pim) {}
//------------------------------------------------------------------------------
NonlinearFactor::shared_ptr ImuFactor2::clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this)));
}
//------------------------------------------------------------------------------
std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) {
f._PIM_.print("preintegrated measurements:\n");
os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
return os;
}
//------------------------------------------------------------------------------
void ImuFactor2::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2())
<< "," << keyFormatter(this->key3()) << ")\n";
cout << *this << endl;
}
//------------------------------------------------------------------------------
bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const {
const This *e = dynamic_cast<const This*>(&other);
const bool base = Base::equals(*e, tol);
const bool pim = _PIM_.equals(e->_PIM_, tol);
return e != nullptr && base && pim;
}
//------------------------------------------------------------------------------
Vector ImuFactor2::evaluateError(const NavState& state_i, const NavState& state_j,
const imuBias::ConstantBias& bias_i, //
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3);
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
} }

86
gtsam/navigation/ImuFactor.h
View File

@ -64,6 +64,7 @@ namespace gtsam {
class PreintegratedImuMeasurements: public PreintegrationBase { class PreintegratedImuMeasurements: public PreintegrationBase {
friend class ImuFactor; friend class ImuFactor;
friend class ImuFactor2;
protected: protected:
@ -114,12 +115,18 @@ public:
* @param measuredOmega Measured angular velocity (as given by the sensor) * @param measuredOmega Measured angular velocity (as given by the sensor)
* @param dt Time interval between this and the last IMU measurement * @param dt Time interval between this and the last IMU measurement
*/ */
void integrateMeasurement(const Vector3& measuredAcc, void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt);
const Vector3& measuredOmega, double dt);
/// Add multiple measurements, in matrix columns
void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas,
const Matrix& dts);
/// Return pre-integrated measurement covariance /// Return pre-integrated measurement covariance
Matrix preintMeasCov() const { return preintMeasCov_; } Matrix preintMeasCov() const { return preintMeasCov_; }
/// Merge in a different set of measurements and update bias derivatives accordingly
void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @deprecated constructor /// @deprecated constructor
/// NOTE(frank): assumes Z-Down convention, only second order integration supported /// NOTE(frank): assumes Z-Down convention, only second order integration supported
@ -162,8 +169,6 @@ private:
*/ */
class ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3, class ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias> { imuBias::ConstantBias> {
public:
private: private:
typedef ImuFactor This; typedef ImuFactor This;
@ -220,7 +225,7 @@ public:
/// vector of errors /// vector of errors
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias, boost::optional<Matrix&> H1 = const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none, boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 = boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
boost::none, boost::optional<Matrix&> H5 = boost::none) const; boost::none, boost::optional<Matrix&> H5 = boost::none) const;
@ -265,10 +270,81 @@ private:
}; };
// class ImuFactor // class ImuFactor
/**
* ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
* @addtogroup SLAM
*/
class ImuFactor2 : public NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> {
private:
typedef ImuFactor2 This;
typedef NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> Base;
PreintegratedImuMeasurements _PIM_;
public:
/** Default constructor - only use for serialization */
ImuFactor2() {}
/**
* Constructor
* @param state_i Previous state key
* @param state_j Current state key
* @param bias Previous bias key
*/
ImuFactor2(Key state_i, Key state_j, Key bias,
const PreintegratedImuMeasurements& preintegratedMeasurements);
virtual ~ImuFactor2() {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
/// @name Testable
/// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/// @}
/** Access the preintegrated measurements. */
const PreintegratedImuMeasurements& preintegratedMeasurements() const {
return _PIM_;
}
/** implement functions needed to derive from Factor */
/// vector of errors
Vector evaluateError(const NavState& state_i, const NavState& state_j,
const imuBias::ConstantBias& bias_i, //
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_);
}
};
// class ImuFactor2
template <> template <>
struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {}; struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
template <> template <>
struct traits<ImuFactor> : public Testable<ImuFactor> {}; struct traits<ImuFactor> : public Testable<ImuFactor> {};
template <>
struct traits<ImuFactor2> : public Testable<ImuFactor2> {};
} /// namespace gtsam } /// namespace gtsam

2
gtsam/navigation/NavState.cpp
View File

@ -239,6 +239,7 @@ Matrix7 NavState::wedge(const Vector9& xi) {
#define D_v_v(H) (H)->block<3,3>(6,6) #define D_v_v(H) (H)->block<3,3>(6,6)
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const { OptionalJacobian<9, 3> G2) const {
@ -281,6 +282,7 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
} }
return newState; return newState;
} }
#endif
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,

2
gtsam/navigation/NavState.h
View File

@ -203,11 +203,13 @@ public:
/// @name Dynamics /// @name Dynamics
/// @{ /// @{
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// Integrate forward in time given angular velocity and acceleration in body frame /// Integrate forward in time given angular velocity and acceleration in body frame
/// Uses second order integration for position, returns derivatives except dt. /// Uses second order integration for position, returns derivatives except dt.
NavState update(const Vector3& b_acceleration, const Vector3& b_omega, NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const; OptionalJacobian<9, 3> G2) const;
#endif
/// Compute tangent space contribution due to Coriolis forces /// Compute tangent space contribution due to Coriolis forces
Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,

3
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -63,7 +63,8 @@ void PreintegratedRotation::print(const string& s) const {
bool PreintegratedRotation::equals(const PreintegratedRotation& other, bool PreintegratedRotation::equals(const PreintegratedRotation& other,
double tol) const { double tol) const {
return p_->equals(*other.p_,tol) && deltaRij_.equals(other.deltaRij_, tol) return this->matchesParamsWith(other)
&& deltaRij_.equals(other.deltaRij_, tol)
&& fabs(deltaTij_ - other.deltaTij_) < tol && fabs(deltaTij_ - other.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
} }

43
gtsam/navigation/PreintegrationBase.cpp
View File

@ -61,8 +61,8 @@ void PreintegrationBase::print(const string& s) const {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool PreintegrationBase::equals(const PreintegrationBase& other, bool PreintegrationBase::equals(const PreintegrationBase& other,
double tol) const { double tol) const {
const bool params_match = p_->equals(*other.p_, tol); return p_->equals(*other.p_, tol)
return params_match && fabs(deltaTij_ - other.deltaTij_) < tol && fabs(deltaTij_ - other.deltaTij_) < tol
&& biasHat_.equals(other.biasHat_, tol) && biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol)
&& equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol) && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol)
@ -72,9 +72,9 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose( pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
const Vector3& unbiasedAcc, const Vector3& unbiasedOmega, const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
OptionalJacobian<3, 3> D_correctedAcc_unbiasedAcc, OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc,
OptionalJacobian<3, 3> D_correctedAcc_unbiasedOmega, OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega,
OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega) const { OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega) const {
assert(p().body_P_sensor); assert(p().body_P_sensor);
// Compensate for sensor-body displacement if needed: we express the quantities // Compensate for sensor-body displacement if needed: we express the quantities
@ -89,9 +89,9 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
const Vector3 correctedOmega = bRs * unbiasedOmega; const Vector3 correctedOmega = bRs * unbiasedOmega;
// Jacobians // Jacobians
if (D_correctedAcc_unbiasedAcc) *D_correctedAcc_unbiasedAcc = bRs; if (correctedAcc_H_unbiasedAcc) *correctedAcc_H_unbiasedAcc = bRs;
if (D_correctedAcc_unbiasedOmega) *D_correctedAcc_unbiasedOmega = Z_3x3; if (correctedAcc_H_unbiasedOmega) *correctedAcc_H_unbiasedOmega = Z_3x3;
if (D_correctedOmega_unbiasedOmega) *D_correctedOmega_unbiasedOmega = bRs; if (correctedOmega_H_unbiasedOmega) *correctedOmega_H_unbiasedOmega = bRs;
// Centrifugal acceleration // Centrifugal acceleration
const Vector3 b_arm = p().body_P_sensor->translation(); const Vector3 b_arm = p().body_P_sensor->translation();
@ -103,9 +103,9 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
correctedAcc -= body_Omega_body * b_velocity_bs; correctedAcc -= body_Omega_body * b_velocity_bs;
// Update derivative: centrifugal causes the correlation between acc and omega!!! // Update derivative: centrifugal causes the correlation between acc and omega!!!
if (D_correctedAcc_unbiasedOmega) { if (correctedAcc_H_unbiasedOmega) {
double wdp = correctedOmega.dot(b_arm); double wdp = correctedOmega.dot(b_arm);
*D_correctedAcc_unbiasedOmega = -(diag(Vector3::Constant(wdp)) *correctedAcc_H_unbiasedOmega = -(diag(Vector3::Constant(wdp))
+ correctedOmega * b_arm.transpose()) * bRs.matrix() + correctedOmega * b_arm.transpose()) * bRs.matrix()
+ 2 * b_arm * unbiasedOmega.transpose(); + 2 * b_arm * unbiasedOmega.transpose();
} }
@ -366,27 +366,26 @@ Vector9 PreintegrationBase::Compose(const Vector9& zeta01,
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1, void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1,
Matrix9* H2) { Matrix9* H2) {
if (!matchesParamsWith(pim12)) if (!matchesParamsWith(pim12)) {
throw std::domain_error( throw std::domain_error("Cannot merge pre-integrated measurements with different params");
"Cannot merge pre-integrated measurements with different params"); }
if (params()->body_P_sensor) if (params()->body_P_sensor) {
throw std::domain_error( throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet");
"Cannot merge pre-integrated measurements with sensor pose yet"); }
const double& t01 = deltaTij(); const double t01 = deltaTij();
const double& t12 = pim12.deltaTij(); const double t12 = pim12.deltaTij();
deltaTij_ = t01 + t12; deltaTij_ = t01 + t12;
Vector9 zeta01 = preintegrated(); const Vector9 zeta01 = preintegrated();
Vector9 zeta12 = pim12.preintegrated(); Vector9 zeta12 = pim12.preintegrated(); // will be modified.
// TODO(frank): adjust zeta12 due to bias difference
const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat(); const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat();
zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope() zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope()
+ pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer(); + pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer();
preintegrated_ << PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2); preintegrated_ = PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2);
preintegrated_H_biasAcc_ = preintegrated_H_biasAcc_ =
(*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_; (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_;

18
gtsam/navigation/PreintegrationBase.h
View File

@ -77,6 +77,7 @@ class PreintegrationBase {
/** /**
* Preintegrated navigation state, from frame i to frame j * Preintegrated navigation state, from frame i to frame j
* Order is: theta, position, velocity
* Note: relative position does not take into account velocity at time i, see deltap+, in [2] * Note: relative position does not take into account velocity at time i, see deltap+, in [2]
* Note: velocity is now also in frame i, as opposed to deltaVij in [2] * Note: velocity is now also in frame i, as opposed to deltaVij in [2]
*/ */
@ -94,6 +95,9 @@ public:
/// @name Constructors /// @name Constructors
/// @{ /// @{
/// @name Constructors
/// @{
/** /**
* Constructor, initializes the variables in the base class * Constructor, initializes the variables in the base class
* @param p Parameters, typically fixed in a single application * @param p Parameters, typically fixed in a single application
@ -111,7 +115,7 @@ public:
/// check parameters equality: checks whether shared pointer points to same Params object. /// check parameters equality: checks whether shared pointer points to same Params object.
bool matchesParamsWith(const PreintegrationBase& other) const { bool matchesParamsWith(const PreintegrationBase& other) const {
return p_ == other.p_; return p_.get() == other.p_.get();
} }
/// shared pointer to params /// shared pointer to params
@ -134,7 +138,7 @@ public:
/// @name Instance variables access /// @name Instance variables access
/// @{ /// @{
const imuBias::ConstantBias& biasHat() const { return biasHat_; } const imuBias::ConstantBias& biasHat() const { return biasHat_; }
const double& deltaTij() const { return deltaTij_; } double deltaTij() const { return deltaTij_; }
const Vector9& preintegrated() const { return preintegrated_; } const Vector9& preintegrated() const { return preintegrated_; }
@ -167,9 +171,9 @@ public:
/// Ignore D_correctedOmega_measuredAcc as it is trivially zero /// Ignore D_correctedOmega_measuredAcc as it is trivially zero
std::pair<Vector3, Vector3> correctMeasurementsBySensorPose( std::pair<Vector3, Vector3> correctMeasurementsBySensorPose(
const Vector3& unbiasedAcc, const Vector3& unbiasedOmega, const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
OptionalJacobian<3, 3> D_correctedAcc_unbiasedAcc = boost::none, OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none,
OptionalJacobian<3, 3> D_correctedAcc_unbiasedOmega = boost::none, OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega = boost::none) const; OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
// Update integrated vector on tangent manifold preintegrated with acceleration // Update integrated vector on tangent manifold preintegrated with acceleration
// Static, functional version. // Static, functional version.
@ -215,7 +219,7 @@ public:
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
// Compose the two preintegrated vectors // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives
static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
double deltaT12, double deltaT12,
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H1 = boost::none,
@ -244,6 +248,8 @@ public:
/// @} /// @}
#endif #endif
/// @}
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;

2
gtsam/navigation/tests/imuFactorTesting.h
View File

@ -28,8 +28,8 @@ using symbol_shorthand::X;
using symbol_shorthand::V; using symbol_shorthand::V;
using symbol_shorthand::B; using symbol_shorthand::B;
static const Vector3 kZero = Z_3x1;
typedef imuBias::ConstantBias Bias; typedef imuBias::ConstantBias Bias;
static const Vector3 Z_3x1 = Vector3::Zero();
static const Bias kZeroBiasHat, kZeroBias; static const Bias kZeroBiasHat, kZeroBias;
static const Vector3 kZeroOmegaCoriolis(0, 0, 0); static const Vector3 kZeroOmegaCoriolis(0, 0, 0);

7
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -136,8 +136,7 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
auto p = testing::Params(); auto p = testing::Params();
testing::SomeMeasurements measurements; testing::SomeMeasurements measurements;
boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated = auto preintegrated = [=](const Vector3& a, const Vector3& w) {
[=](const Vector3& a, const Vector3& w) {
PreintegratedImuMeasurements pim(p, Bias(a, w)); PreintegratedImuMeasurements pim(p, Bias(a, w));
testing::integrateMeasurements(measurements, &pim); testing::integrateMeasurements(measurements, &pim);
return pim.preintegrated(); return pim.preintegrated();
@ -147,9 +146,9 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
PreintegratedCombinedMeasurements pim(p); PreintegratedCombinedMeasurements pim(p);
testing::integrateMeasurements(measurements, &pim); testing::integrateMeasurements(measurements, &pim);
EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1), EXPECT(assert_equal(numericalDerivative21<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1),
pim.preintegrated_H_biasAcc())); pim.preintegrated_H_biasAcc()));
EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1),
pim.preintegrated_H_biasOmega(), 1e-3)); pim.preintegrated_H_biasOmega(), 1e-3));
} }

50
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -29,19 +29,24 @@ using namespace gtsam;
using namespace GeographicLib; using namespace GeographicLib;
// ************************************************************************* // *************************************************************************
TEST( GPSFactor, Constructors ) { namespace example {
// ENU Origin is where the plane was in hold next to runway
const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
// Convert from GPS to ENU // Convert from GPS to ENU
// ENU Origin is where the plane was in hold next to runway LocalCartesian origin_ENU(lat0, lon0, h0, Geocentric::WGS84);
const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
LocalCartesian enu(lat0, lon0, h0, Geocentric::WGS84);
// Dekalb-Peachtree Airport runway 2L // Dekalb-Peachtree Airport runway 2L
const double lat = 33.87071, lon = -84.30482, h = 274; const double lat = 33.87071, lon = -84.30482, h = 274;
}
// *************************************************************************
TEST( GPSFactor, Constructor ) {
using namespace example;
// From lat-lon to geocentric // From lat-lon to geocentric
double E, N, U; double E, N, U;
enu.Forward(lat, lon, h, E, N, U); origin_ENU.Forward(lat, lon, h, E, N, U);
EXPECT_DOUBLES_EQUAL(133.24, E, 1e-2); EXPECT_DOUBLES_EQUAL(133.24, E, 1e-2);
EXPECT_DOUBLES_EQUAL(80.98, N, 1e-2); EXPECT_DOUBLES_EQUAL(80.98, N, 1e-2);
EXPECT_DOUBLES_EQUAL(0, U, 1e-2); EXPECT_DOUBLES_EQUAL(0, U, 1e-2);
@ -67,6 +72,35 @@ TEST( GPSFactor, Constructors ) {
EXPECT(assert_equal(expectedH, actualH, 1e-8)); EXPECT(assert_equal(expectedH, actualH, 1e-8));
} }
// *************************************************************************
TEST( GPSFactor2, Constructor ) {
using namespace example;
// From lat-lon to geocentric
double E, N, U;
origin_ENU.Forward(lat, lon, h, E, N, U);
// Factor
Key key(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactor2 factor(key, Point3(E, N, U), model);
// Create a linearization point at zero error
NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero());
EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,NavState>(
boost::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T);
// Use the factor to calculate the derivative
Matrix actualH;
factor.evaluateError(T, actualH);
// Verify we get the expected error
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
//*************************************************************************** //***************************************************************************
TEST(GPSData, init) { TEST(GPSData, init) {

54
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -94,9 +94,9 @@ TEST(ImuFactor, PreintegratedMeasurements) {
// Actual pre-integrated values // Actual pre-integrated values
PreintegratedImuMeasurements actual(testing::Params()); PreintegratedImuMeasurements actual(testing::Params());
EXPECT(assert_equal(Z_3x1, actual.theta())); EXPECT(assert_equal(kZero, actual.theta()));
EXPECT(assert_equal(Z_3x1, actual.deltaPij())); EXPECT(assert_equal(kZero, actual.deltaPij()));
EXPECT(assert_equal(Z_3x1, actual.deltaVij())); EXPECT(assert_equal(kZero, actual.deltaVij()));
DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9); DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9);
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -185,8 +185,25 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none,
boost::none), kZeroBias); boost::none), kZeroBias);
EXPECT(assert_equal(eH2, aH2)); EXPECT(assert_equal(eH2, aH2));
return; }
/* ************************************************************************* */
TEST(ImuFactor, MultipleMeasurements) {
using namespace common;
PreintegratedImuMeasurements expected(testing::Params(), kZeroBiasHat);
expected.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
expected.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
Matrix32 acc,gyro;
Matrix12 dts;
acc << measuredAcc, measuredAcc;
gyro << measuredOmega, measuredOmega;
dts << deltaT, deltaT;
PreintegratedImuMeasurements actual(testing::Params(), kZeroBiasHat);
actual.integrateMeasurements(acc,gyro,dts);
EXPECT(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -436,9 +453,9 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
PreintegratedImuMeasurements pim(testing::Params()); PreintegratedImuMeasurements pim(testing::Params());
testing::integrateMeasurements(measurements, &pim); testing::integrateMeasurements(measurements, &pim);
EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1), EXPECT(assert_equal(numericalDerivative21(preintegrated, kZero, kZero),
pim.preintegrated_H_biasAcc())); pim.preintegrated_H_biasAcc()));
EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), EXPECT(assert_equal(numericalDerivative22(preintegrated, kZero, kZero),
pim.preintegrated_H_biasOmega(), 1e-3)); pim.preintegrated_H_biasOmega(), 1e-3));
} }
@ -780,7 +797,7 @@ struct ImuFactorMergeTest {
ImuFactorMergeTest() ImuFactorMergeTest()
: p_(PreintegratedImuMeasurements::Params::MakeSharedU(kGravity)), : p_(PreintegratedImuMeasurements::Params::MakeSharedU(kGravity)),
forward_(Z_3x1, Vector3(kVelocity, 0, 0)), forward_(kZero, Vector3(kVelocity, 0, 0)),
loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) { loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) {
// arbitrary noise values // arbitrary noise values
p_->gyroscopeCovariance = I_3x3 * 0.01; p_->gyroscopeCovariance = I_3x3 * 0.01;
@ -867,6 +884,29 @@ TEST(ImuFactor, MergeWithCoriolis) {
mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4); mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4);
} }
/* ************************************************************************* */
// Same values as pre-integration test but now testing covariance
TEST(ImuFactor, CheckCovariance) {
// Measurements
Vector3 measuredAcc(0.1, 0.0, 0.0);
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
double deltaT = 0.5;
PreintegratedImuMeasurements actual(testing::Params());
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
Matrix9 expected;
expected << 1.0577e-08, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 1.0577e-08, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 1.0577e-08, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, 0, 0, //
0, 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, 0, //
0, 0, 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, //
0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06, 0, 0, //
0, 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06, 0, //
0, 0, 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06;
EXPECT(assert_equal(expected, actual.preintMeasCov()));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

2
gtsam/navigation/tests/testNavState.cpp
View File

@ -193,6 +193,7 @@ TEST( NavState, Lie ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
TEST(NavState, Update) { TEST(NavState, Update) {
Vector3 omega(M_PI / 100.0, 0.0, 0.0); Vector3 omega(M_PI / 100.0, 0.0, 0.0);
Vector3 acc(0.1, 0.0, 0.0); Vector3 acc(0.1, 0.0, 0.0);
@ -220,6 +221,7 @@ TEST(NavState, Update) {
EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7)); EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7));
EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7)); EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7));
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
static const double dt = 2.0; static const double dt = 2.0;

9
gtsam/navigation/tests/testPreintegrationBase.cpp
View File

@ -120,11 +120,10 @@ TEST(PreintegrationBase, Compose) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(PreintegrationBase, MergedBiasDerivatives) { TEST(PreintegrationBase, MergedBiasDerivatives) {
testing::SomeMeasurements measurements; testing::SomeMeasurements measurements;
boost::function<Vector9(const Vector3&, const Vector3&)> f = auto f = [=](const Vector3& a, const Vector3& w) {
[=](const Vector3& a, const Vector3& w) {
PreintegrationBase pim02(testing::Params(), Bias(a, w)); PreintegrationBase pim02(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim02); testing::integrateMeasurements(measurements, &pim02);
testing::integrateMeasurements(measurements, &pim02); testing::integrateMeasurements(measurements, &pim02);
@ -136,9 +135,9 @@ TEST(PreintegrationBase, Compose) {
testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02);
testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02);
EXPECT(assert_equal(numericalDerivative21(f, Z_3x1, Z_3x1), EXPECT(assert_equal(numericalDerivative21<Vector9, Vector3, Vector3>(f, Z_3x1, Z_3x1),
expected_pim02.preintegrated_H_biasAcc())); expected_pim02.preintegrated_H_biasAcc()));
EXPECT(assert_equal(numericalDerivative22(f, Z_3x1, Z_3x1), EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(f, Z_3x1, Z_3x1),
expected_pim02.preintegrated_H_biasOmega(), 1e-7)); expected_pim02.preintegrated_H_biasOmega(), 1e-7));
} }

2
gtsam/navigation/tests/testScenarios.cpp
View File

@ -75,7 +75,7 @@ TEST(ScenarioRunner, Spin) {
/* ************************************************************************* */ /* ************************************************************************* */
namespace forward { namespace forward {
const double v = 2; // m/s const double v = 2; // m/s
ConstantTwistScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Forward) { TEST(ScenarioRunner, Forward) {

38
gtsam/nonlinear/Expression-inl.h
View File

@ -27,11 +27,6 @@
namespace gtsam { namespace gtsam {
template<typename T>
void Expression<T>::print(const std::string& s) const {
root_->print(s);
}
template<typename T> template<typename T>
Expression<T>::Expression(const T& value) : Expression<T>::Expression(const T& value) :
root_(new internal::ConstantExpression<T>(value)) { root_(new internal::ConstantExpression<T>(value)) {
@ -48,7 +43,7 @@ Expression<T>::Expression(const Symbol& symbol) :
} }
template<typename T> template<typename T>
Expression<T>::Expression(unsigned char c, size_t j) : Expression<T>::Expression(unsigned char c, std::uint64_t j) :
root_(new internal::LeafExpression<T>(Symbol(c, j))) { root_(new internal::LeafExpression<T>(Symbol(c, j))) {
} }
@ -128,6 +123,11 @@ void Expression<T>::dims(std::map<Key, int>& map) const {
root_->dims(map); root_->dims(map);
} }
template<typename T>
void Expression<T>::print(const std::string& s) const {
root_->print(s);
}
template<typename T> template<typename T>
T Expression<T>::value(const Values& values, T Expression<T>::value(const Values& values,
boost::optional<std::vector<Matrix>&> H) const { boost::optional<std::vector<Matrix>&> H) const {
@ -259,4 +259,30 @@ std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start) {
return unknowns; return unknowns;
} }
template <typename T>
ScalarMultiplyExpression<T>::ScalarMultiplyExpression(double s, const Expression<T>& e)
: Expression<T>(boost::make_shared<internal::ScalarMultiplyNode<T>>(s, e)) {}
template <typename T>
SumExpression<T>::SumExpression(const std::vector<Expression<T>>& expressions)
: Expression<T>(boost::make_shared<internal::SumExpressionNode<T>>(expressions)) {}
template <typename T>
SumExpression<T> SumExpression<T>::operator+(const Expression<T>& e) const {
SumExpression<T> copy = *this;
boost::static_pointer_cast<internal::SumExpressionNode<T>>(copy.root_)->add(e);
return copy;
}
template <typename T>
SumExpression<T>& SumExpression<T>::operator+=(const Expression<T>& e) {
boost::static_pointer_cast<internal::SumExpressionNode<T>>(this->root_)->add(e);
return *this;
}
template <typename T>
size_t SumExpression<T>::nrTerms() const {
return boost::static_pointer_cast<internal::SumExpressionNode<T>>(this->root_)->nrTerms();
}
} // namespace gtsam } // namespace gtsam

95
gtsam/nonlinear/Expression.h
View File

@ -22,6 +22,7 @@
#include <gtsam/nonlinear/internal/JacobianMap.h> #include <gtsam/nonlinear/internal/JacobianMap.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/base/OptionalJacobian.h> #include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/VectorSpace.h>
#include <boost/bind.hpp> #include <boost/bind.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
@ -52,11 +53,14 @@ public:
/// Define type so we can apply it as a meta-function /// Define type so we can apply it as a meta-function
typedef Expression<T> type; typedef Expression<T> type;
private: protected:
// Paul's trick shared pointer, polymorphic root of entire expression tree // Paul's trick shared pointer, polymorphic root of entire expression tree
boost::shared_ptr<internal::ExpressionNode<T> > root_; boost::shared_ptr<internal::ExpressionNode<T> > root_;
/// Construct with a custom root
Expression(const boost::shared_ptr<internal::ExpressionNode<T> >& root) : root_(root) {}
public: public:
// Expressions wrap trees of functions that can evaluate their own derivatives. // Expressions wrap trees of functions that can evaluate their own derivatives.
@ -85,9 +89,6 @@ public:
typename MakeOptionalJacobian<T, A3>::type)> type; typename MakeOptionalJacobian<T, A3>::type)> type;
}; };
/// Print
void print(const std::string& s) const;
/// Construct a constant expression /// Construct a constant expression
Expression(const T& value); Expression(const T& value);
@ -98,7 +99,7 @@ public:
Expression(const Symbol& symbol); Expression(const Symbol& symbol);
/// Construct a leaf expression, creating Symbol /// Construct a leaf expression, creating Symbol
Expression(unsigned char c, size_t j); Expression(unsigned char c, std::uint64_t j);
/// Construct a unary function expression /// Construct a unary function expression
template<typename A> template<typename A>
@ -147,6 +148,9 @@ public:
/// Return dimensions for each argument, as a map /// Return dimensions for each argument, as a map
void dims(std::map<Key, int>& map) const; void dims(std::map<Key, int>& map) const;
/// Print
void print(const std::string& s) const;
/** /**
* @brief Return value and optional derivatives, reverse AD version * @brief Return value and optional derivatives, reverse AD version
* Notes: this is not terribly efficient, and H should have correct size. * Notes: this is not terribly efficient, and H should have correct size.
@ -170,7 +174,7 @@ public:
/// Return size needed for memory buffer in traceExecution /// Return size needed for memory buffer in traceExecution
size_t traceSize() const; size_t traceSize() const;
private: protected:
/// Default constructor, for serialization /// Default constructor, for serialization
Expression() {} Expression() {}
@ -199,6 +203,85 @@ private:
friend class ::ExpressionFactorShallowTest; friend class ::ExpressionFactorShallowTest;
}; };
/**
* A ScalarMultiplyExpression is a specialization of Expression that multiplies with a scalar
* It optimizes the Jacobian calculation for this specific case
*/
template <typename T>
class ScalarMultiplyExpression : public Expression<T> {
// Check that T is a vector space
BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>));
public:
explicit ScalarMultiplyExpression(double s, const Expression<T>& e);
};
/**
* A SumExpression is a specialization of Expression that just sums the arguments
* It optimizes the Jacobian calculation for this specific case
*/
template <typename T>
class SumExpression : public Expression<T> {
// Check that T is a vector space
BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>));
public:
explicit SumExpression(const std::vector<Expression<T>>& expressions);
// Syntactic sugar to allow e1 + e2 + e3...
SumExpression operator+(const Expression<T>& e) const;
SumExpression& operator+=(const Expression<T>& e);
size_t nrTerms() const;
};
/**
* Create an expression out of a linear function f:T->A with (constant) Jacobian dTdA
* TODO(frank): create a more efficient version like ScalarMultiplyExpression. This version still
* does a malloc every linearize.
*/
template <typename T, typename A>
Expression<T> linearExpression(
const boost::function<T(A)>& f, const Expression<A>& expression,
const Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension>& dTdA) {
// Use lambda to endow f with a linear Jacobian
typename Expression<T>::template UnaryFunction<A>::type g =
[=](const A& value, typename MakeOptionalJacobian<T, A>::type H) {
if (H)
*H << dTdA;
return f(value);
};
return Expression<T>(g, expression);
}
/**
* Construct an expression that executes the scalar multiplication with an input expression
* The type T must be a vector space
* Example:
* Expression<Point2> a(0), b = 12 * a;
*/
template <typename T>
ScalarMultiplyExpression<T> operator*(double s, const Expression<T>& e) {
return ScalarMultiplyExpression<T>(s, e);
}
/**
* Construct an expression that sums two input expressions of the same type T
* The type T must be a vector space
* Example:
* Expression<Point2> a(0), b(1), c = a + b;
*/
template <typename T>
SumExpression<T> operator+(const Expression<T>& e1, const Expression<T>& e2) {
return SumExpression<T>({e1, e2});
}
/// Construct an expression that subtracts one expression from another
template <typename T>
SumExpression<T> operator-(const Expression<T>& e1, const Expression<T>& e2) {
return e1 + (-1.0) * e2;
}
/** /**
* Construct a product expression, assumes T::compose(T) -> T * Construct a product expression, assumes T::compose(T) -> T
* Example: * Example:

7
gtsam/nonlinear/ExpressionFactor.h
View File

@ -43,8 +43,8 @@ protected:
Expression<T> expression_; ///< the expression that is AD enabled Expression<T> expression_; ///< the expression that is AD enabled
FastVector<int> dims_; ///< dimensions of the Jacobian matrices FastVector<int> dims_; ///< dimensions of the Jacobian matrices
public:
public:
typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr; typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
/// Constructor /// Constructor
@ -61,9 +61,10 @@ public:
const T& measured() const { return measured_; } const T& measured() const { return measured_; }
/// print relies on Testable traits being defined for T /// print relies on Testable traits being defined for T
void print(const std::string& s, const KeyFormatter& keyFormatter) const { void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
NoiseModelFactor::print(s, keyFormatter); NoiseModelFactor::print(s, keyFormatter);
traits<T>::Print(measured_, s + ".measured_"); traits<T>::Print(measured_, "ExpressionFactor with measurement: ");
} }
/// equals relies on Testable traits being defined for T /// equals relies on Testable traits being defined for T

53
gtsam/nonlinear/ExtendedKalmanFilter-inl.h
View File

@ -29,8 +29,8 @@ namespace gtsam {
template<class VALUE> template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_( typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_(
const GaussianFactorGraph& linearFactorGraph, const GaussianFactorGraph& linearFactorGraph,
const Values& linearizationPoints, Key lastKey, const Values& linearizationPoint, Key lastKey,
JacobianFactor::shared_ptr& newPrior) const JacobianFactor::shared_ptr* newPrior)
{ {
// Compute the marginal on the last key // Compute the marginal on the last key
// Solve the linear factor graph, converting it into a linear Bayes Network // Solve the linear factor graph, converting it into a linear Bayes Network
@ -42,7 +42,7 @@ namespace gtsam {
// Extract the current estimate of x1,P1 // Extract the current estimate of x1,P1
VectorValues result = marginal->solve(VectorValues()); VectorValues result = marginal->solve(VectorValues());
const T& current = linearizationPoints.at<T>(lastKey); const T& current = linearizationPoint.at<T>(lastKey);
T x = traits<T>::Retract(current, result[lastKey]); T x = traits<T>::Retract(current, result[lastKey]);
// Create a Jacobian Factor from the root node of the produced Bayes Net. // Create a Jacobian Factor from the root node of the produced Bayes Net.
@ -51,7 +51,7 @@ namespace gtsam {
// and the key/index needs to be reset to 0, the first key in the next iteration. // and the key/index needs to be reset to 0, the first key in the next iteration.
assert(marginal->nrFrontals() == 1); assert(marginal->nrFrontals() == 1);
assert(marginal->nrParents() == 0); assert(marginal->nrParents() == 0);
newPrior = boost::make_shared<JacobianFactor>( *newPrior = boost::make_shared<JacobianFactor>(
marginal->keys().front(), marginal->keys().front(),
marginal->getA(marginal->begin()), marginal->getA(marginal->begin()),
marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey], marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey],
@ -80,20 +80,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUE> template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::predict( typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::predict(
const MotionFactor& motionFactor) { const NoiseModelFactor& motionFactor) {
const auto keys = motionFactor.keys();
// TODO: This implementation largely ignores the actual factor symbols.
// Calling predict() then update() with drastically
// different keys will still compute as if a common key-set was used
// Create Keys
Key x0 = motionFactor.key1();
Key x1 = motionFactor.key2();
// Create a set of linearization points
Values linearizationPoints;
linearizationPoints.insert(x0, x_);
linearizationPoints.insert(x1, x_); // TODO should this really be x_ ?
// Create a Gaussian Factor Graph // Create a Gaussian Factor Graph
GaussianFactorGraph linearFactorGraph; GaussianFactorGraph linearFactorGraph;
@ -102,12 +90,14 @@ namespace gtsam {
linearFactorGraph.push_back(priorFactor_); linearFactorGraph.push_back(priorFactor_);
// Linearize motion model and add it to the Kalman Filter graph // Linearize motion model and add it to the Kalman Filter graph
linearFactorGraph.push_back( Values linearizationPoint;
motionFactor.linearize(linearizationPoints)); linearizationPoint.insert(keys[0], x_);
linearizationPoint.insert(keys[1], x_); // TODO should this really be x_ ?
linearFactorGraph.push_back(motionFactor.linearize(linearizationPoint));
// Solve the factor graph and update the current state estimate // Solve the factor graph and update the current state estimate
// and the posterior for the next iteration. // and the posterior for the next iteration.
x_ = solve_(linearFactorGraph, linearizationPoints, x1, priorFactor_); x_ = solve_(linearFactorGraph, linearizationPoint, keys[1], &priorFactor_);
return x_; return x_;
} }
@ -115,18 +105,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUE> template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::update( typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::update(
const MeasurementFactor& measurementFactor) { const NoiseModelFactor& measurementFactor) {
const auto keys = measurementFactor.keys();
// TODO: This implementation largely ignores the actual factor symbols.
// Calling predict() then update() with drastically
// different keys will still compute as if a common key-set was used
// Create Keys
Key x0 = measurementFactor.key();
// Create a set of linearization points
Values linearizationPoints;
linearizationPoints.insert(x0, x_);
// Create a Gaussian Factor Graph // Create a Gaussian Factor Graph
GaussianFactorGraph linearFactorGraph; GaussianFactorGraph linearFactorGraph;
@ -135,12 +115,13 @@ namespace gtsam {
linearFactorGraph.push_back(priorFactor_); linearFactorGraph.push_back(priorFactor_);
// Linearize measurement factor and add it to the Kalman Filter graph // Linearize measurement factor and add it to the Kalman Filter graph
linearFactorGraph.push_back( Values linearizationPoint;
measurementFactor.linearize(linearizationPoints)); linearizationPoint.insert(keys[0], x_);
linearFactorGraph.push_back(measurementFactor.linearize(linearizationPoint));
// Solve the factor graph and update the current state estimate // Solve the factor graph and update the current state estimate
// and the prior factor for the next iteration // and the prior factor for the next iteration
x_ = solve_(linearFactorGraph, linearizationPoints, x0, priorFactor_); x_ = solve_(linearFactorGraph, linearizationPoint, keys[0], &priorFactor_);
return x_; return x_;
} }

53
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -24,9 +24,10 @@
namespace gtsam { namespace gtsam {
/** /**
* This is a generic Extended Kalman Filter class implemented using nonlinear factors. GTSAM * This is a generic Extended Kalman Filter class implemented using nonlinear factors. GTSAM
* basically does SRIF with Cholesky to solve the filter problem, making this an efficient, numerically * basically does SRIF with Cholesky to solve the filter problem, making this an efficient,
*numerically
* stable Kalman Filter implementation. * stable Kalman Filter implementation.
* *
* The Kalman Filter relies on two models: a motion model that predicts the next state using * The Kalman Filter relies on two models: a motion model that predicts the next state using
@ -40,59 +41,67 @@ namespace gtsam {
* \nosubgrouping * \nosubgrouping
*/ */
template<class VALUE> template <class VALUE>
class ExtendedKalmanFilter { class ExtendedKalmanFilter {
// Check that VALUE type is a testable Manifold // Check that VALUE type is a testable Manifold
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>)); BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
BOOST_CONCEPT_ASSERT((IsManifold<VALUE>)); BOOST_CONCEPT_ASSERT((IsManifold<VALUE>));
public: public:
typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr; typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
typedef VALUE T; typedef VALUE T;
//@deprecated: any NoiseModelFactor will do, as long as they have the right keys
typedef NoiseModelFactor2<VALUE, VALUE> MotionFactor; typedef NoiseModelFactor2<VALUE, VALUE> MotionFactor;
typedef NoiseModelFactor1<VALUE> MeasurementFactor; typedef NoiseModelFactor1<VALUE> MeasurementFactor;
protected: protected:
T x_; // linearization point T x_; // linearization point
JacobianFactor::shared_ptr priorFactor_; // density JacobianFactor::shared_ptr priorFactor_; // Gaussian density on x_
T solve_(const GaussianFactorGraph& linearFactorGraph, static T solve_(const GaussianFactorGraph& linearFactorGraph, const Values& linearizationPoints,
const Values& linearizationPoints, Key x, JacobianFactor::shared_ptr* newPrior);
Key x, JacobianFactor::shared_ptr& newPrior) const;
public: public:
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
ExtendedKalmanFilter(Key key_initial, T x_initial, ExtendedKalmanFilter(Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial);
noiseModel::Gaussian::shared_ptr P_initial);
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
/// print /// print
void print(const std::string& s="") const { void print(const std::string& s = "") const {
std::cout << s << "\n"; std::cout << s << "\n";
x_.print(s+"x"); x_.print(s + "x");
priorFactor_->print(s+"density"); priorFactor_->print(s + "density");
} }
/// @} /// @}
/// @name Advanced Interface /// @name Interface
/// @{ /// @{
///TODO: comment /**
T predict(const MotionFactor& motionFactor); * Calculate predictive density P(x_) ~ \int P(x_min) P(x_min, x_)
* The motion model should be given as a factor with key1 for x_min and key2_ for x
*/
T predict(const NoiseModelFactor& motionFactor);
///TODO: comment /**
T update(const MeasurementFactor& measurementFactor); * Calculate posterior density P(x_) ~ L(z|x) P(x)
* The likelihood L(z|x) should be given as a unary factor on x
*/
T update(const NoiseModelFactor& measurementFactor);
/// Return current predictive (if called after predict)/posterior (if called after update)
const JacobianFactor::shared_ptr Density() const {
return priorFactor_;
}
/// @} /// @}
}; };
} // namespace } // namespace

5
gtsam/nonlinear/GaussNewtonOptimizer.cpp
View File

@ -26,14 +26,19 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void GaussNewtonOptimizer::iterate() { void GaussNewtonOptimizer::iterate() {
gttic(GaussNewtonOptimizer_Iterate);
const NonlinearOptimizerState& current = state_; const NonlinearOptimizerState& current = state_;
// Linearize graph // Linearize graph
gttic(GaussNewtonOptimizer_Linearize);
GaussianFactorGraph::shared_ptr linear = graph_.linearize(current.values); GaussianFactorGraph::shared_ptr linear = graph_.linearize(current.values);
gttoc(GaussNewtonOptimizer_Linearize);
// Solve Factor Graph // Solve Factor Graph
gttic(GaussNewtonOptimizer_Solve);
const VectorValues delta = solve(*linear, current.values, params_); const VectorValues delta = solve(*linear, current.values, params_);
gttoc(GaussNewtonOptimizer_Solve);
// Maybe show output // Maybe show output
if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta.print("delta"); if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta.print("delta");

21
gtsam/nonlinear/ISAM2.cpp
View File

@ -171,14 +171,14 @@ bool ISAM2::equals(const ISAM2& other, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
FastSet<size_t> ISAM2::getAffectedFactors(const FastList<Key>& keys) const { KeySet ISAM2::getAffectedFactors(const KeyList& keys) const {
static const bool debug = false; static const bool debug = false;
if(debug) cout << "Getting affected factors for "; if(debug) cout << "Getting affected factors for ";
if(debug) { BOOST_FOREACH(const Key key, keys) { cout << key << " "; } } if(debug) { BOOST_FOREACH(const Key key, keys) { cout << key << " "; } }
if(debug) cout << endl; if(debug) cout << endl;
NonlinearFactorGraph allAffected; NonlinearFactorGraph allAffected;
FastSet<size_t> indices; KeySet indices;
BOOST_FOREACH(const Key key, keys) { BOOST_FOREACH(const Key key, keys) {
const VariableIndex::Factors& factors(variableIndex_[key]); const VariableIndex::Factors& factors(variableIndex_[key]);
indices.insert(factors.begin(), factors.end()); indices.insert(factors.begin(), factors.end());
@ -197,7 +197,7 @@ GaussianFactorGraph::shared_ptr
ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySet& relinKeys) const ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySet& relinKeys) const
{ {
gttic(getAffectedFactors); gttic(getAffectedFactors);
FastSet<size_t> candidates = getAffectedFactors(affectedKeys); KeySet candidates = getAffectedFactors(affectedKeys);
gttoc(getAffectedFactors); gttoc(getAffectedFactors);
NonlinearFactorGraph nonlinearAffectedFactors; NonlinearFactorGraph nonlinearAffectedFactors;
@ -210,7 +210,7 @@ ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySe
gttic(check_candidates_and_linearize); gttic(check_candidates_and_linearize);
GaussianFactorGraph::shared_ptr linearized = boost::make_shared<GaussianFactorGraph>(); GaussianFactorGraph::shared_ptr linearized = boost::make_shared<GaussianFactorGraph>();
BOOST_FOREACH(size_t idx, candidates) { BOOST_FOREACH(Key idx, candidates) {
bool inside = true; bool inside = true;
bool useCachedLinear = params_.cacheLinearizedFactors; bool useCachedLinear = params_.cacheLinearizedFactors;
BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) { BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) {
@ -464,9 +464,8 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
constraintGroups = *constrainKeys; constraintGroups = *constrainKeys;
} else { } else {
constraintGroups = FastMap<Key,int>(); constraintGroups = FastMap<Key,int>();
const int group = observedKeys.size() < affectedFactorsVarIndex.size() const int group = observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0;
? 1 : 0; BOOST_FOREACH (Key var, observedKeys)
BOOST_FOREACH(Key var, observedKeys)
constraintGroups.insert(make_pair(var, group)); constraintGroups.insert(make_pair(var, group));
} }
@ -766,7 +765,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
map<Key, vector<GaussianFactor::shared_ptr> > marginalFactors; map<Key, vector<GaussianFactor::shared_ptr> > marginalFactors;
// Keep track of factors that get summarized by removing cliques // Keep track of factors that get summarized by removing cliques
FastSet<size_t> factorIndicesToRemove; KeySet factorIndicesToRemove;
// Keep track of variables removed in subtrees // Keep track of variables removed in subtrees
KeySet leafKeysRemoved; KeySet leafKeysRemoved;
@ -831,7 +830,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
// Add child marginals and remove marginalized subtrees // Add child marginals and remove marginalized subtrees
GaussianFactorGraph graph; GaussianFactorGraph graph;
FastSet<size_t> factorsInSubtreeRoot; KeySet factorsInSubtreeRoot;
Cliques subtreesToRemove; Cliques subtreesToRemove;
BOOST_FOREACH(const sharedClique& child, clique->children) { BOOST_FOREACH(const sharedClique& child, clique->children) {
// Remove subtree if child depends on any marginalized keys // Remove subtree if child depends on any marginalized keys
@ -867,14 +866,14 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
// These are the factors that involve *marginalized* frontal variables in this clique // These are the factors that involve *marginalized* frontal variables in this clique
// but do not involve frontal variables of any of its children. // but do not involve frontal variables of any of its children.
// TODO: reuse cached linear factors // TODO: reuse cached linear factors
FastSet<size_t> factorsFromMarginalizedInClique_step1; KeySet factorsFromMarginalizedInClique_step1;
BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) {
if(leafKeys.exists(frontal)) if(leafKeys.exists(frontal))
factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); } factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); }
// Remove any factors in subtrees that we're removing at this step // Remove any factors in subtrees that we're removing at this step
BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) { BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) {
BOOST_FOREACH(Key indexInClique, removedChild->conditional()->frontals()) { BOOST_FOREACH(Key indexInClique, removedChild->conditional()->frontals()) {
BOOST_FOREACH(size_t factorInvolving, variableIndex_[indexInClique]) { BOOST_FOREACH(Key factorInvolving, variableIndex_[indexInClique]) {
factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } } factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } }
// Create factor graph from factor indices // Create factor graph from factor indices
BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique_step1) { BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique_step1) {

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