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

36
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_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_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
# TODO: Check for matlab mex binary before handling building of binaries
@ -190,10 +191,10 @@ if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL
include_directories(${MKL_INCLUDE_DIR})
list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES})
# --no-as-needed is required with gcc according to the MKL link advisor
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed")
endif()
else()
set(GTSAM_USE_EIGEN_MKL 0)
@ -224,17 +225,17 @@ option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', us
if(GTSAM_USE_SYSTEM_EIGEN)
find_package(Eigen3 REQUIRED)
include_directories(AFTER "${EIGEN3_INCLUDE_DIR}")
# Use generic Eigen include paths e.g. <Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}")
# check if MKL is also enabled - can have one or the other, but not both!
# Note: Eigen >= v3.2.5 includes our patches
if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5))
message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL")
endif()
else()
# Use bundled Eigen include path.
# Use bundled Eigen include path.
# Clear any variables set by FindEigen3
if(EIGEN3_INCLUDE_DIR)
set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
@ -242,11 +243,11 @@ else()
# Add the bundled version of eigen to the include path so that it can still be included
# with #include <Eigen/Core>
include_directories(BEFORE "gtsam/3rdparty/Eigen/")
# set full path to be used by external projects
# this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in
set(GTSAM_EIGEN_INCLUDE_PREFIX "${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/")
endif()
###############################################################################
@ -289,18 +290,24 @@ endif()
# Include boost - use 'BEFORE' so that a specific boost specified to CMake
# takes precedence over a system-installed one.
# Use 'SYSTEM' to ignore compiler warnings in Boost headers
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
# paths so that the compiler uses GTSAM headers in our source directory instead
# of any previously installed GTSAM headers.
include_directories(BEFORE
gtsam/3rdparty/UFconfig
gtsam/3rdparty/CCOLAMD/Include
gtsam/3rdparty/metis/include
gtsam/3rdparty/metis/libmetis
gtsam/3rdparty/metis/GKlib
${METIS_INCLUDE_DIRECTORIES}
${PROJECT_SOURCE_DIR}
${PROJECT_BINARY_DIR} # So we can include generated config header files
CppUnitLite)
@ -362,9 +369,9 @@ if (GTSAM_BUILD_PYTHON)
# NOTE: The automatic generation of python wrapper from the gtsampy.h interface is
# not working yet, so we're using a handwritten wrapper files on python/handwritten.
# Once the python wrapping from the interface file is working, you can _swap_ the
# Once the python wrapping from the interface file is working, you can _swap_ the
# comments on the next lines
# wrap_and_install_python(gtsampy.h "${GTSAM_ADDITIONAL_LIBRARIES}" "")
add_subdirectory(python)
@ -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_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_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")

8
GTSAM-Concepts.md
View File

@ -18,14 +18,14 @@ 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.
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 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 that the following items are defined in the traits object (although, not all are needed for optimization):
* 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.
* types:
* types:
* `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix<double,n,1>`.
* `ChartJacobian`, a typedef for `OptionalJacobian<dimension, dimension>`.
* `ChartJacobian`, a typedef for `OptionalJacobian<dimension, dimension>`.
* `ManifoldType`, a pointer back to the type.
* `structure_category`, a tag type that defines what requirements the type fulfills, and therefore what requirements this traits class must fulfill. It should be defined to be one of the following:
* `gtsam::traits::manifold_tag` -- Everything in this list is expected

135
cmake/GtsamBuildTypes.cmake
View File

@ -3,56 +3,76 @@
list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}")
# Default to Release mode
if(NOT FIRST_PASS_DONE AND NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
set(CMAKE_BUILD_TYPE "Release" CACHE STRING
"Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo."
FORCE)
if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING
"Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo.")
set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE})
endif()
# Add option for using build type postfixes to allow installing multiple build modes
option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON)
# Add debugging flags but only on the first pass
if(NOT FIRST_PASS_DONE)
if(MSVC)
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)
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(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(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(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(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(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_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
set(CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_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 CMAKE_MODULE_LINKER_FLAGS_TIMING)
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(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(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_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_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 CMAKE_MODULE_LINKER_FLAGS_PROFILING)
else()
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(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(CMAKE_C_FLAGS_RELWITHDEBINFO "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
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()
# Set custom compilation flags.
# NOTE: We set all the CACHE variables with a GTSAM prefix, and then set a normal local variable below
# so that we don't "pollute" the global variable namespace in the cmake cache.
if(MSVC)
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(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(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(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(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(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(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /Zi" CACHE STRING "Flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /Zi" CACHE STRING "Flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.")
set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.")
else()
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(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(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "-std=c11 -Wall -g -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.")
set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "-std=c++11 -Wall -g -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.")
set(GTSAM_CMAKE_C_FLAGS_RELEASE "-std=c11 -Wall -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during release builds.")
set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "-std=c++11 -Wall -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during release builds.")
set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE}" CACHE STRING "Flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE}" CACHE STRING "Flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.")
set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.")
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
if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
# Apple Clang before 5.0 does not support -ftemplate-depth.
@ -63,10 +83,10 @@ endif()
# Set up build type library postfixes
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel)
string(TOUPPER "${build_type}" build_type_toupper)
set(CMAKE_${build_type_toupper}_POSTFIX ${build_type})
endforeach()
foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel)
string(TOUPPER "${build_type}" build_type_toupper)
set(CMAKE_${build_type_toupper}_POSTFIX ${build_type})
endforeach()
endif()
# Make common binary output directory when on Windows
@ -78,17 +98,16 @@ endif()
# Set up build type list for cmake-gui
if(NOT "${CMAKE_BUILD_TYPE}" STREQUAL "")
if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8)
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS None Debug Release Timing Profiling RelWithDebInfo MinSizeRel)
endif()
if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8)
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS None Debug Release Timing Profiling RelWithDebInfo MinSizeRel)
endif()
endif()
# Set up build types for MSVC and XCode
if(NOT FIRST_PASS_DONE)
set(CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel
CACHE STRING "Build types available to MSVC and XCode" FORCE)
mark_as_advanced(FORCE CMAKE_CONFIGURATION_TYPES)
endif()
set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel
CACHE STRING "Build types available to MSVC and XCode")
mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES)
set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES})
# Check build types
string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower)
@ -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 "relwithdebinfo"
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()
# 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
set_property(GLOBAL PROPERTY USE_FOLDERS On)

6
cmake/GtsamTesting.cmake
View File

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

3
gtsam.h
View File

@ -1742,7 +1742,8 @@ virtual class NonlinearFactor {
virtual class NoiseModelFactor: gtsam::NonlinearFactor {
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 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()
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)

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

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

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

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

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

@ -20,7 +20,7 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
endif()
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)
set(METIS_INSTALL FALSE)
@ -29,11 +29,11 @@ else()
endif()
# Configure libmetis library.
if(SHARED)
if(METIS_SHARED)
set(METIS_LIBRARY_TYPE SHARED)
else()
set(METIS_LIBRARY_TYPE STATIC)
endif(SHARED)
endif(METIS_SHARED)
include(${GKLIB_PATH}/GKlibSystem.cmake)
# 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")
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
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));
}
/**
* 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
/**

52
gtsam/base/Matrix.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -46,29 +46,29 @@ typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> M
// Create handy typedefs and constants for square-size matrices
// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9
#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX) \
typedef Eigen::Matrix<double, SIZE, SIZE> Matrix##SUFFIX; \
typedef Eigen::Matrix<double, 1, SIZE> Matrix1##SUFFIX; \
typedef Eigen::Matrix<double, 2, SIZE> Matrix2##SUFFIX; \
typedef Eigen::Matrix<double, 3, SIZE> Matrix3##SUFFIX; \
typedef Eigen::Matrix<double, 4, SIZE> Matrix4##SUFFIX; \
typedef Eigen::Matrix<double, 5, SIZE> Matrix5##SUFFIX; \
typedef Eigen::Matrix<double, 6, SIZE> Matrix6##SUFFIX; \
typedef Eigen::Matrix<double, 7, SIZE> Matrix7##SUFFIX; \
typedef Eigen::Matrix<double, 8, SIZE> Matrix8##SUFFIX; \
typedef Eigen::Matrix<double, 9, SIZE> Matrix9##SUFFIX; \
static const Eigen::MatrixBase<Matrix##SUFFIX>::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \
static const Eigen::MatrixBase<Matrix##SUFFIX>::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero();
#define GTSAM_MAKE_MATRIX_DEFS(N) \
typedef Eigen::Matrix<double, N, N> Matrix##N; \
typedef Eigen::Matrix<double, 1, N> Matrix1##N; \
typedef Eigen::Matrix<double, 2, N> Matrix2##N; \
typedef Eigen::Matrix<double, 3, N> Matrix3##N; \
typedef Eigen::Matrix<double, 4, N> Matrix4##N; \
typedef Eigen::Matrix<double, 5, N> Matrix5##N; \
typedef Eigen::Matrix<double, 6, N> Matrix6##N; \
typedef Eigen::Matrix<double, 7, N> Matrix7##N; \
typedef Eigen::Matrix<double, 8, N> Matrix8##N; \
typedef Eigen::Matrix<double, 9, N> Matrix9##N; \
static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Zero();
GTSAM_MAKE_TYPEDEFS(1,1);
GTSAM_MAKE_TYPEDEFS(2,2);
GTSAM_MAKE_TYPEDEFS(3,3);
GTSAM_MAKE_TYPEDEFS(4,4);
GTSAM_MAKE_TYPEDEFS(5,5);
GTSAM_MAKE_TYPEDEFS(6,6);
GTSAM_MAKE_TYPEDEFS(7,7);
GTSAM_MAKE_TYPEDEFS(8,8);
GTSAM_MAKE_TYPEDEFS(9,9);
GTSAM_MAKE_MATRIX_DEFS(1);
GTSAM_MAKE_MATRIX_DEFS(2);
GTSAM_MAKE_MATRIX_DEFS(3);
GTSAM_MAKE_MATRIX_DEFS(4);
GTSAM_MAKE_MATRIX_DEFS(5);
GTSAM_MAKE_MATRIX_DEFS(6);
GTSAM_MAKE_MATRIX_DEFS(7);
GTSAM_MAKE_MATRIX_DEFS(8);
GTSAM_MAKE_MATRIX_DEFS(9);
// Matrix expressions for accessing parts of matrices
typedef Eigen::Block<Matrix> SubMatrix;
@ -135,7 +135,7 @@ inline bool operator==(const Matrix& A, const Matrix& B) {
}
/**
* inequality
* inequality
*/
inline bool operator!=(const Matrix& A, const Matrix& B) {
return !(A==B);
@ -371,7 +371,7 @@ GTSAM_EXPORT Matrix inverse(const Matrix& A);
* m*n matrix -> m*m Q, m*n R
* @param A a matrix
* @return <Q,R> rotation matrix Q, upper triangular R
*/
*/
GTSAM_EXPORT std::pair<Matrix,Matrix> qr(const Matrix& A);
/**
@ -434,7 +434,7 @@ GTSAM_EXPORT Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool u
* @param b an RHS vector
* @param unit, set true if unit triangular
* @return the solution x of L*x=b
*/
*/
GTSAM_EXPORT Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit=false);
/**

12
gtsam/base/Testable.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -13,20 +13,20 @@
* @file Testable.h
* @brief Concept check for values that can be used in unit tests
* @author Frank Dellaert
*
*
* The necessary functions to implement for Testable are defined
* below with additional details as to the interface.
* The concept checking function will check whether or not
* the function exists in derived class and throw compile-time errors.
*
*
* print with optional string naming the object
* void print(const std::string& name) const = 0;
*
*
* equality up to tolerance
* tricky to implement, see NoiseModelFactor1 for an example
* equals is not supposed to print out *anything*, just return true|false
* bool equals(const Derived& expected, double tol) const = 0;
*
*
*/
// \callgraph
@ -123,7 +123,7 @@ namespace gtsam {
double tol_;
equals_star(double tol = 1e-9) : tol_(tol) {}
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_);
}
};

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
// pseudoinverse, we enforce the constraint by turning
// 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;
}
}

30
gtsam/base/Vector.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -41,13 +41,25 @@ typedef Eigen::VectorXd Vector;
typedef Eigen::Matrix<double, 1, 1> Vector1;
typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Matrix<double, 4, 1> Vector4;
typedef Eigen::Matrix<double, 5, 1> Vector5;
typedef Eigen::Matrix<double, 6, 1> Vector6;
typedef Eigen::Matrix<double, 7, 1> Vector7;
typedef Eigen::Matrix<double, 8, 1> Vector8;
typedef Eigen::Matrix<double, 9, 1> Vector9;
typedef Eigen::Matrix<double, 10, 1> Vector10;
static const Eigen::MatrixBase<Vector2>::ConstantReturnType Z_2x1 = Vector2::Zero();
static const Eigen::MatrixBase<Vector3>::ConstantReturnType Z_3x1 = Vector3::Zero();
// Create handy typedefs and constants for vectors with N>3
// VectorN and Z_Nx1, for N=1..9
#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<const Vector> ConstSubVector;
@ -89,7 +101,7 @@ inline Vector zero(size_t n) { return Vector::Zero(n);}
* @param n size
*/
inline Vector ones(size_t n) { return Vector::Ones(n); }
/**
* check if all zero
*/

37
gtsam/base/VectorSpace.h
View File

@ -43,7 +43,7 @@ struct VectorSpaceImpl {
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = 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) {
if (H1) *H1 = 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.
/// To use this for your gtsam type, define:
/// template<> struct traits<Type> : public VectorSpaceTraits<Type> { };
/// Requirements on type to pass it to Manifold template below
template<class Class>
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>
struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
// Check that Class has the necessary machinery
BOOST_CONCEPT_ASSERT((HasVectorSpacePrereqs<Class>));
typedef vector_space_tag structure_category;
/// @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>
struct VectorSpace: Testable<Class>, VectorSpaceTraits<Class> {};
/// A helper that implements the traits interface for GTSAM lie groups.
/// To use this for your gtsam type, define:
/// A helper that implements the traits interface for scalar vector spaces. Usage:
/// template<> struct traits<Type> : public ScalarTraits<Type> { };
template<typename Scalar>
struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {

18
gtsam/base/VerticalBlockMatrix.h
View File

@ -165,24 +165,24 @@ namespace gtsam {
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 */
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 */
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 */
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()) */
const Matrix& matrix() const { return matrix_; }

14
gtsam/base/numericalDerivative.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -131,11 +131,11 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
typedef typename TraitsX::TangentVector TangentX;
// 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
TangentY zeroY = TraitsY::Local(hx, hx);
size_t m = zeroY.size();
const TangentY zeroY = TraitsY::Local(hx, hx);
const size_t m = zeroY.size();
// Prepare a tangent vector to perturb x with, only works for fixed size
TangentX dx;
@ -143,12 +143,12 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
// Fill in Jacobian H
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++) {
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;
TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
dx(j) = 0;
H.col(j) << (dy1 - dy2) * factor;
}

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

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

5
gtsam/base/types.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -25,6 +25,7 @@
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <cstddef>
#include <cstdint>
#ifdef GTSAM_USE_TBB
#include <tbb/task_scheduler_init.h>
@ -53,7 +54,7 @@
namespace gtsam {
/// Integer nonlinear key type
typedef size_t Key;
typedef std::uint64_t Key;
/// The index type for Eigen objects
typedef ptrdiff_t DenseIndex;

9
gtsam/config.h.in
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -32,11 +32,11 @@
#cmakedefine GTSAM_USE_QUATERNIONS
// Whether GTSAM is compiled to use Pose3::EXPMAP as the default coordinates mode for Pose3's retract and localCoordinates (otherwise, Pose3::FIRST_ORDER will be used)
#cmakedefine GTSAM_POSE3_EXPMAP
#cmakedefine GTSAM_POSE3_EXPMAP
// Whether GTSAM is compiled to use Rot3::EXPMAP as the default coordinates mode for Rot3's retract and localCoordinates (otherwise, Pose3::CAYLEY will be used)
#ifndef GTSAM_USE_QUATERNIONS
#cmakedefine GTSAM_ROT3_EXPMAP
#cmakedefine GTSAM_ROT3_EXPMAP
#endif
// Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake)
@ -65,3 +65,6 @@
// Publish flag about Eigen typedef
#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");
dot(fDiscreteKey, "conversion-f1");
std::map<size_t, size_t> ordering;
ordering[0] = 5;
ordering[1] = 2;
std::map<Key, Key> keyMap;
keyMap[0] = 5;
keyMap[1] = 2;
AlgebraicDecisionTree<size_t> fIndexKey(fDiscreteKey, ordering);
AlgebraicDecisionTree<Key> fIndexKey(fDiscreteKey, keyMap);
// f1.print("f1");
// f2.print("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;
x01[5] = 0, x01[2] = 1;
x10[5] = 1, x10[2] = 0;

5
gtsam/geometry/Cal3_S2.h
View File

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

11
gtsam/geometry/EssentialMatrix.cpp
View File

@ -12,6 +12,17 @@ using namespace std;
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,
OptionalJacobian<5, 6> H) {

5
gtsam/geometry/EssentialMatrix.h
View File

@ -52,6 +52,11 @@ public:
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)
static EssentialMatrix FromPose3(const Pose3& _1P2_,
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);
}
/* ************************************************************************* */
Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const {
Vector2 n_error = -n_.localCoordinates(plane.n_);
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 {
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;
/** 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
inline static size_t Dim() {
return 3;

5
gtsam/geometry/PinholePose.h
View File

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

6
gtsam/geometry/Point2.cpp
View File

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

11
gtsam/geometry/Point2.h
View File

@ -50,7 +50,7 @@ public:
/// @{
/// construct from 2D vector
Point2(const Vector2& v) {
explicit Point2(const Vector2& v) {
x_ = v(0);
y_ = v(1);
}
@ -112,6 +112,11 @@ public:
/// inverse
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
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
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,
OptionalJacobian<3,3> H2) const {
if (H1) *H1 = I_3x3;
if (H2) *H2 = I_3x3;
if (H2) *H2 = -I_3x3;
return *this - q;
}
#endif

14
gtsam/geometry/Point3.h
View File

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

4
gtsam/geometry/Pose2.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -216,7 +216,7 @@ Point2 Pose2::transform_from(const Point2& point,
OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0);
OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2);
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_;
}

84
gtsam/geometry/Pose3.cpp
View File

@ -36,6 +36,14 @@ Pose3::Pose3(const Pose2& pose2) :
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 {
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)
*/
static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
const Vector3 w = xi.head<3>();
const Vector3 v = xi.tail<3>();
const auto w = xi.head<3>();
const auto v = xi.tail<3>();
const Matrix3 V = skewSymmetric(v);
const Matrix3 W = skewSymmetric(w);
@ -260,9 +268,18 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const {
return t_;
}
/* ************************************************************************* */
const Rot3& Pose3::rotation(OptionalJacobian<3, 6> H) const {
if (H) {
*H << I_3x3, Z_3x3;
}
return R_;
}
/* ************************************************************************* */
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;
mat << R_.matrix(), t_, A14;
return mat;
@ -275,6 +292,14 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
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,
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) {
const size_t n = pairs.size();
boost::optional<Pose3> Pose3::Align(const std::vector<Point3Pair>& abPointPairs) {
const size_t n = abPointPairs.size();
if (n < 3)
return boost::none; // we need at least three pairs
return boost::none; // we need at least three pairs
// calculate centroids
Point3 cp(0,0,0), cq(0,0,0);
BOOST_FOREACH(const Point3Pair& pair, pairs) {
cp += pair.first;
cq += pair.second;
Point3 aCentroid(0,0,0), bCentroid(0,0,0);
for(const Point3Pair& abPair: abPointPairs) {
aCentroid += abPair.first;
bCentroid += abPair.second;
}
double f = 1.0 / n;
cp *= f;
cq *= f;
aCentroid *= f;
bCentroid *= f;
// Add to form H matrix
Matrix3 H = Z_3x3;
BOOST_FOREACH(const Point3Pair& pair, pairs) {
Point3 dp = pair.first - cp;
Point3 dq = pair.second - cq;
H += dp * dq.transpose();
for(const Point3Pair& abPair: abPointPairs) {
Point3 da = abPair.first - aCentroid;
Point3 db = abPair.second - bCentroid;
H += db * da.transpose();
}
// Compute SVD
Matrix U, V;
Vector S;
svd(H, U, S, V);
// Compute SVD
Eigen::JacobiSVD<Matrix> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
Matrix U = svd.matrixU();
Vector S = svd.singularValues();
Matrix V = svd.matrixV();
// Check rank
if (S[1] < 1e-10)
return boost::none;
// Recover transform with correction from Eggert97machinevisionandapplications
Matrix3 UVtranspose = U * V.transpose();
Matrix3 detWeighting = I_3x3;
detWeighting(2, 2) = UVtranspose.determinant();
Rot3 R(Matrix3(V * detWeighting * U.transpose()));
Point3 t = Point3(cq) - R * Point3(cp);
return Pose3(R, t);
Rot3 aRb(Matrix(V * detWeighting * U.transpose()));
Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid);
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)) {
}
/// 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
/// @{
@ -213,9 +226,7 @@ public:
/// @{
/// get rotation
const Rot3& rotation() const {
return R_;
}
const Rot3& rotation(OptionalJacobian<3, 6> H = boost::none) const;
/// get translation
const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const;
@ -238,9 +249,15 @@ public:
/** convert to 4*4 matrix */
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;
/** 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
* @param point 3D location of landmark
@ -291,7 +308,7 @@ public:
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
private:
private:
/** Serialization function */
friend class boost::serialization::access;
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)
* where q = Pose3::transform_from(p) = t + R*p
* Calculate pose between a vector of 3D point correspondences (b_point, a_point)
* 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>& pairs);
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& baPointPairs);
// For MATLAB wrapper
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);
}
/* ************************************************************************* */
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 {
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 {
// amazingly simple in GTSAM :-)
assert(t>=0 && t<=1);
Vector3 omega = Logmap(between(other));
return compose(Expmap(t * omega));
return interpolate(*this, other, t);
}
/* ************************************************************************* */

10
gtsam/geometry/Rot3.h
View File

@ -215,6 +215,13 @@ namespace gtsam {
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
/// @{
@ -456,9 +463,6 @@ namespace gtsam {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @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 Unit3& axis, double angle) { return AxisAngle(axis, angle); }
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) {
rot_.col(0) = (Vector3)col1;
rot_.col(1) = (Vector3)col2;
rot_.col(2) = (Vector3)col3;
rot_.col(0) = col1;
rot_.col(1) = col2;
rot_.col(2) = col3;
}
/* ************************************************************************* */

13
gtsam/geometry/SO3.cpp
View File

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

7
gtsam/geometry/StereoPoint2.h
View File

@ -50,7 +50,7 @@ public:
}
/// construct from 3D vector
StereoPoint2(const Vector3& v) :
explicit StereoPoint2(const Vector3& v) :
uL_(v(0)), uR_(v(1)), v_(v(2)) {}
/// @}
@ -80,6 +80,11 @@ public:
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
inline StereoPoint2 operator +(const StereoPoint2& b) const {
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).
boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> > generator(
rng, randomDirection);
vector<double> d = generator();
const vector<double> d = generator();
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
// the chosen axis.
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|.
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.
// No need to normalize this, p and b1 are orthogonal unit vectors.
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.
B_.reset(Matrix32());
@ -118,8 +118,8 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
if (H) {
// Chain rule tomfoolery to compute the derivative.
const Matrix32& H_n_p = *B_;
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_b1_p = H_b1_B1 * H_B1_n * H_n_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.
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 {
// Get the unit vectors of each, and the derivative.
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;
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.
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) {
(*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 {
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
Matrix23 Bt = basis().transpose();
Vector2 xi = Bt * q.p_;
const Vector2 xi = basis().transpose() * q.p_;
if (H_q) {
*H_q = Bt * q.basis();
*H_q = basis().transpose() * q.basis();
}
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 {
// Get the point3 of this, and the derivative.
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).
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;
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);
// Derivatives of the two entries of xi wrt the basis vectors.
Matrix13 H_xi1_b1 = qn.transpose();
Matrix13 H_xi2_b2 = qn.transpose();
const Matrix13 H_xi1_b1 = qn.transpose();
const Matrix13 H_xi2_b2 = qn.transpose();
// Assemble dxi/dp = dxi/dB * dB/dp.
Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p;
Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p;
const Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p;
const Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p;
*H_p << H_xi1_p, H_xi2_p;
}
if (H_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;
}
@ -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 {
Matrix2 H_;
Vector2 xi = error(q, H_);
double theta = xi.norm();
Matrix2 H_xi_q;
const Vector2 xi = error(q, H ? &H_xi_q : nullptr);
const double theta = xi.norm();
if (H)
*H = (xi.transpose() / theta) * H_;
*H = (xi.transpose() / theta) * H_xi_q;
return theta;
}
/* ************************************************************************* */
Unit3 Unit3::retract(const Vector2& v) const {
// Compute the 3D xi_hat vector
Vector3 xi_hat = basis() * v;
double theta = xi_hat.norm();
const Vector3 xi_hat = basis() * v;
const double theta = xi_hat.norm();
// Treat case of very small v differently
if (theta < std::numeric_limits<double>::epsilon()) {
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);
}

7
gtsam/geometry/Unit3.h
View File

@ -65,14 +65,7 @@ public:
p_(1.0, 0.0, 0.0) {
}
#ifndef GTSAM_USE_VECTOR3_POINTS
/// Construct from point
explicit Unit3(const Point3& p) :
p_(p.vector().normalized()) {
}
#endif
/// Construct from a vector3
explicit Unit3(const Vector3& p) :
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
Rot3 c1Rc2 = Rot3::Yaw(M_PI_2);
Point3 c1Tc2(0.1, 0, 0);
EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2));
Rot3 trueRotation = Rot3::Yaw(M_PI_2);
Point3 trueTranslation(0.1, 0, 0);
Unit3 trueDirection(trueTranslation);
EssentialMatrix trueE(trueRotation, trueDirection);
//*************************************************************************
TEST (EssentialMatrix, equality) {
EssentialMatrix actual(c1Rc2, Unit3(c1Tc2)), expected(c1Rc2, Unit3(c1Tc2));
EssentialMatrix actual(trueRotation, trueDirection), expected(trueRotation, trueDirection);
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) {
EssentialMatrix expected(c1Rc2, Unit3(c1Tc2));
Pose3 pose(c1Rc2, c1Tc2);
EssentialMatrix expected(trueRotation, trueDirection);
Pose3 pose(trueRotation, trueTranslation);
EssentialMatrix actual = EssentialMatrix::FromPose3(pose);
EXPECT(assert_equal(expected, actual));
}
@ -50,7 +71,7 @@ TEST(EssentialMatrix, localCoordinates0) {
TEST (EssentialMatrix, localCoordinates) {
// Pose between two cameras
Pose3 pose(c1Rc2, c1Tc2);
Pose3 pose(trueRotation, trueTranslation);
EssentialMatrix hx = EssentialMatrix::FromPose3(pose);
Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose));
EXPECT(assert_equal(zero(5), actual, 1e-8));
@ -70,15 +91,15 @@ TEST (EssentialMatrix, retract0) {
//*************************************************************************
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());
EXPECT(assert_equal(expected, actual));
}
//*************************************************************************
TEST (EssentialMatrix, retract2) {
EssentialMatrix expected(c1Rc2,
Unit3(c1Tc2).retract(Vector2(0.1, 0)));
EssentialMatrix expected(trueRotation,
trueDirection.retract(Vector2(0.1, 0)));
EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0).finished());
EXPECT(assert_equal(expected, actual));
}
@ -124,8 +145,8 @@ TEST (EssentialMatrix, rotate) {
Rot3 bRc(bX, bZ, -bY), cRb = bRc.inverse();
// Let's compute the ground truth E in body frame:
Rot3 b1Rb2 = bRc * c1Rc2 * cRb;
Point3 b1Tb2 = bRc * c1Tc2;
Rot3 b1Rb2 = bRc * trueRotation * cRb;
Point3 b1Tb2 = bRc * trueTranslation;
EssentialMatrix bodyE(b1Rb2, Unit3(b1Tb2));
EXPECT(assert_equal(bodyE, bRc * trueE, 1e-8));
EXPECT(assert_equal(bodyE, trueE.rotate(bRc), 1e-8));
@ -149,7 +170,7 @@ TEST (EssentialMatrix, rotate) {
//*************************************************************************
TEST (EssentialMatrix, FromPose3_a) {
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));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
@ -171,7 +192,7 @@ TEST (EssentialMatrix, FromPose3_b) {
//*************************************************************************
TEST (EssentialMatrix, streaming) {
EssentialMatrix expected(c1Rc2, Unit3(c1Tc2)), actual;
EssentialMatrix expected(trueRotation, trueDirection), actual;
stringstream ss;
ss << expected;
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() {
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);
CHECK(origin.dot(Point3(1, 1, 0)) == 0);
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));
}
/* ************************************************************************* */
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() {
TestResult tr;

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

@ -31,12 +31,13 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Pose3)
GTSAM_CONCEPT_LIE_INST(Pose3)
static Point3 P(0.2,0.7,-2);
static Rot3 R = Rot3::Rodrigues(0.3,0,0);
static Pose3 T(R,Point3(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 Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3));
const double tol=1e-5;
static const Point3 P(0.2,0.7,-2);
static const Rot3 R = Rot3::Rodrigues(0.3,0,0);
static const Point3 P2(3.5,-8.2,4.2);
static const Pose3 T(R,P2);
static const Pose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),P2);
static const Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3));
static const double tol=1e-5;
/* ************************************************************************* */
TEST( Pose3, equals)
@ -197,6 +198,17 @@ TEST(Pose3, translation) {
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)
{
@ -403,6 +415,41 @@ TEST( Pose3, transform_to)
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)
{
@ -476,7 +523,7 @@ TEST(Pose3, Retract_LocalCoordinates)
{
Vector6 d;
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);
EXPECT(assert_equal(d, Pose3::LocalCoordinates(t)));
}
@ -506,6 +553,8 @@ TEST(Pose3, retract_localCoordinates2)
EXPECT(assert_equal(t2, t1.retract(d12)));
Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21)));
// TODO(hayk): This currently fails!
// EXPECT(assert_equal(d12, -d21));
}
/* ************************************************************************* */
TEST(Pose3, manifold_expmap)
@ -637,16 +686,26 @@ TEST( Pose3, range_pose )
Unit3 bearing_proxy(const Pose3& pose, const Point3& point) {
return pose.bearing(point);
}
TEST( Pose3, bearing )
{
TEST(Pose3, Bearing) {
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
expectedH1 = numericalDerivative21(bearing_proxy, x1, l1);
expectedH2 = numericalDerivative22(bearing_proxy, x1, l1);
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2));
EXPECT(assert_equal(expectedH1, actualH1));
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));
vector<Point3Pair> correspondences;
Point3Pair pq1(make_pair(Point3(0,0,0), Point3(10,10,0)));
Point3Pair pq2(make_pair(Point3(20,10,0), Point3(30,20,0)));
Point3Pair pq3(make_pair(Point3(10,20,0), Point3(20,30,0)));
correspondences += pq1, pq2, pq3;
Point3Pair ab1(make_pair(Point3(10,10,0), Point3(0,0,0)));
Point3Pair ab2(make_pair(Point3(30,20,0), Point3(20,10,0)));
Point3Pair ab3(make_pair(Point3(20,30,0), Point3(10,20,0)));
correspondences += ab1, ab2, ab3;
boost::optional<Pose3> actual = align(correspondences);
boost::optional<Pose3> actual = Pose3::Align(correspondences);
EXPECT(assert_equal(expected, *actual));
}
/* ************************************************************************* */
TEST(Pose3, align_2) {
TEST(Pose3, Align2) {
Point3 t(20,10,5);
Rot3 R = Rot3::RzRyRx(0.3, 0.2, 0.1);
Pose3 expected(R, t);
@ -695,12 +754,12 @@ TEST(Pose3, align_2) {
Point3 q1 = expected.transform_from(p1),
q2 = expected.transform_from(p2),
q3 = expected.transform_from(p3);
Point3Pair pq1(make_pair(p1, q1));
Point3Pair pq2(make_pair(p2, q2));
Point3Pair pq3(make_pair(p3, q3));
correspondences += pq1, pq2, pq3;
Point3Pair ab1(make_pair(q1, p1));
Point3Pair ab2(make_pair(q2, p2));
Point3Pair ab3(make_pair(q3, p3));
correspondences += ab1, ab2, ab3;
boost::optional<Pose3> actual = align(correspondences);
boost::optional<Pose3> actual = Pose3::Align(correspondences);
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() {
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
double angle = 3.14 / 4.0;
Rot3 actual = Rot3::AxisAngle(axis, angle);
Rot3 expected(0.707388, 0, 0.706825,
0, 1, 0,
-0.706825, 0, 0.707388);
Rot3 actual = Rot3::AxisAngle(axis, angle);
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)));
Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21)));
EXPECT(assert_equal(d12, -d21));
}
/* ************************************************************************* */
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) {
Matrix32 B = p.basis(H);
Vector6 B_vec;

8
gtsam/geometry/triangulation.cpp
View File

@ -56,11 +56,10 @@ Vector4 triangulateHomogeneousDLT(
Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol) {
Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
rank_tol);
Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
// Create 3D point from homogeneous coordinates
return Point3(sub((v / v(3)), 0, 3));
return Point3(v.head<3>() / v[3]);
}
///
@ -91,5 +90,4 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,
return result.at<Point3>(landmarkKey);
}
} // \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
// for no variable index first so that it's always computed if we need to call COLAMD because
// no Ordering is provided.
return eliminateSequential(ordering, function, VariableIndex(asDerived()), orderingType);
VariableIndex computedVariableIndex(asDerived());
return eliminateSequential(ordering, function, computedVariableIndex, orderingType);
}
else /*if(!ordering)*/ {
// 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'
// block.
if (orderingType == Ordering::METIS)
return eliminateSequential(Ordering::Metis(asDerived()), function, variableIndex, orderingType);
else
return eliminateSequential(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType);
if (orderingType == Ordering::METIS) {
Ordering computedOrdering = Ordering::Metis(asDerived());
return eliminateSequential(computedOrdering, 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
// for no variable index first so that it's always computed if we need to call COLAMD because
// no Ordering is provided.
return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType);
VariableIndex computedVariableIndex(asDerived());
return eliminateMultifrontal(ordering, function, computedVariableIndex, orderingType);
}
else /*if(!ordering)*/ {
// 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'
// block.
if (orderingType == Ordering::METIS)
return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType);
else
return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType);
if (orderingType == Ordering::METIS) {
Ordering computedOrdering = Ordering::Metis(asDerived());
return eliminateMultifrontal(computedOrdering, 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);
} else {
// 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);
} else {
// 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);
} else {
// 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);
} else {
// 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 {
// 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
{
// 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);
}
}

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

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -74,11 +74,26 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
KeySet FactorGraph<FACTOR>::keys() const {
KeySet allKeys;
BOOST_FOREACH(const sharedFactor& factor, factors_)
KeySet keys;
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)
allKeys.insert(factor->begin(), factor->end());
return allKeys;
keys.insert(keys.end(), factor->begin(), factor->end());
std::sort(keys.begin(), keys.end());
auto last = std::unique(keys.begin(), keys.end());
keys.erase(last, keys.end());
return keys;
}
/* ************************************************************************* */

13
gtsam/inference/FactorGraph.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -114,7 +114,7 @@ namespace gtsam {
/// @}
/// @name Advanced Constructors
/// @{
// TODO: are these needed?
///**
@ -319,10 +319,10 @@ namespace gtsam {
void replace(size_t index, sharedFactor factor) { at(index) = factor; }
/** 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 */
void erase(iterator first, iterator last) { factors_.erase(first, last); }
iterator erase(iterator first, iterator last) { return factors_.erase(first, last); }
/// @}
/// @name Advanced Interface
@ -331,9 +331,12 @@ namespace gtsam {
/** return the number of non-null factors */
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;
/** Potentially slow function to return all keys involved, sorted, as a vector */
KeyVector keyVector() const;
/** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */
inline bool exists(size_t idx) const { return idx < size() && at(idx); }

19
gtsam/inference/Key.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -11,7 +11,7 @@
/**
* @file Key.h
* @brief
* @brief
* @author Richard Roberts
* @date Feb 20, 2012
*/
@ -78,14 +78,19 @@ GTSAM_EXPORT void PrintKeySet(const KeySet& keys, const std::string& s = "",
// Define Key to be Testable by specializing gtsam::traits
template<typename T> struct traits;
template<> struct traits<Key> {
static void Print(const Key& key, const std::string& str = "") {
PrintKey(key, str);
template <>
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) {
return key1 == key2;
static bool Equals(const Key& val1, const Key& val2, double tol = 1e-8) {
return val1 == val2;
}
};
} // 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_) {}
/* ************************************************************************* */
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) {}
/* ************************************************************************* */

6
gtsam/inference/LabeledSymbol.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -34,7 +34,7 @@ namespace gtsam {
class GTSAM_EXPORT LabeledSymbol {
protected:
unsigned char c_, label_;
size_t j_;
std::uint64_t j_;
public:
/** Default constructor */
@ -44,7 +44,7 @@ public:
LabeledSymbol(const LabeledSymbol& key);
/** 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 */
LabeledSymbol(gtsam::Key key);

10
gtsam/inference/Ordering.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -23,7 +23,10 @@
#include <gtsam/inference/Ordering.h>
#include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
#ifdef GTSAM_SUPPORT_NESTED_DISSECTION
#include <gtsam/3rdparty/metis/include/metis.h>
#endif
using namespace std;
@ -198,6 +201,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
/* ************************************************************************* */
Ordering Ordering::Metis(const MetisIndex& met) {
#ifdef GTSAM_SUPPORT_NESTED_DISSECTION
gttic(Ordering_METIS);
vector<idx_t> xadj = met.xadj();
@ -227,6 +231,10 @@ Ordering Ordering::Metis(const MetisIndex& met) {
result[j] = met.intToKey(perm[j]);
}
return result;
#else
throw runtime_error("GTSAM was built without support for Metis-based "
"nested dissection");
#endif
}
/* ************************************************************************* */

8
gtsam/inference/Symbol.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -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 indexMask = ~chrMask;
Symbol::Symbol(Key key) {
c_ = (unsigned char) ((key & chrMask) >> indexBits);
j_ = key & indexMask;
Symbol::Symbol(Key key) :
c_((unsigned char) ((key & chrMask) >> indexBits)),
j_ (key & indexMask) {
}
Key Symbol::key() const {

67
gtsam/inference/Symbol.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -21,6 +21,7 @@
#include <gtsam/inference/Key.h>
#include <gtsam/base/Testable.h>
#include <boost/serialization/nvp.hpp>
#include <cstdint>
namespace gtsam {
@ -32,8 +33,8 @@ namespace gtsam {
*/
class GTSAM_EXPORT Symbol {
protected:
unsigned char c_;
size_t j_;
const unsigned char c_;
const std::uint64_t j_;
public:
@ -48,7 +49,7 @@ public:
}
/** Constructor */
Symbol(unsigned char c, size_t j) :
Symbol(unsigned char c, std::uint64_t j) :
c_(c), j_(j) {
}
@ -73,7 +74,7 @@ public:
}
/** Retrieve key index */
size_t index() const {
std::uint64_t index() const {
return j_;
}
@ -124,41 +125,41 @@ private:
};
/** 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. */
inline unsigned char symbolChr(Key key) { return Symbol(key).chr(); }
/** 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 {
inline Key A(size_t j) { return Symbol('a', j); }
inline Key B(size_t j) { return Symbol('b', j); }
inline Key C(size_t j) { return Symbol('c', j); }
inline Key D(size_t j) { return Symbol('d', j); }
inline Key E(size_t j) { return Symbol('e', j); }
inline Key F(size_t j) { return Symbol('f', j); }
inline Key G(size_t j) { return Symbol('g', j); }
inline Key H(size_t j) { return Symbol('h', j); }
inline Key I(size_t j) { return Symbol('i', j); }
inline Key J(size_t j) { return Symbol('j', j); }
inline Key K(size_t j) { return Symbol('k', j); }
inline Key L(size_t j) { return Symbol('l', j); }
inline Key M(size_t j) { return Symbol('m', j); }
inline Key N(size_t j) { return Symbol('n', j); }
inline Key O(size_t j) { return Symbol('o', j); }
inline Key P(size_t j) { return Symbol('p', j); }
inline Key Q(size_t j) { return Symbol('q', j); }
inline Key R(size_t j) { return Symbol('r', j); }
inline Key S(size_t j) { return Symbol('s', j); }
inline Key T(size_t j) { return Symbol('t', j); }
inline Key U(size_t j) { return Symbol('u', j); }
inline Key V(size_t j) { return Symbol('v', j); }
inline Key W(size_t j) { return Symbol('w', j); }
inline Key X(size_t j) { return Symbol('x', j); }
inline Key Y(size_t j) { return Symbol('y', j); }
inline Key Z(size_t j) { return Symbol('z', j); }
inline Key A(std::uint64_t j) { return Symbol('a', j); }
inline Key B(std::uint64_t j) { return Symbol('b', j); }
inline Key C(std::uint64_t j) { return Symbol('c', j); }
inline Key D(std::uint64_t j) { return Symbol('d', j); }
inline Key E(std::uint64_t j) { return Symbol('e', j); }
inline Key F(std::uint64_t j) { return Symbol('f', j); }
inline Key G(std::uint64_t j) { return Symbol('g', j); }
inline Key H(std::uint64_t j) { return Symbol('h', j); }
inline Key I(std::uint64_t j) { return Symbol('i', j); }
inline Key J(std::uint64_t j) { return Symbol('j', j); }
inline Key K(std::uint64_t j) { return Symbol('k', j); }
inline Key L(std::uint64_t j) { return Symbol('l', j); }
inline Key M(std::uint64_t j) { return Symbol('m', j); }
inline Key N(std::uint64_t j) { return Symbol('n', j); }
inline Key O(std::uint64_t j) { return Symbol('o', j); }
inline Key P(std::uint64_t j) { return Symbol('p', j); }
inline Key Q(std::uint64_t j) { return Symbol('q', j); }
inline Key R(std::uint64_t j) { return Symbol('r', j); }
inline Key S(std::uint64_t j) { return Symbol('s', j); }
inline Key T(std::uint64_t j) { return Symbol('t', j); }
inline Key U(std::uint64_t j) { return Symbol('u', j); }
inline Key V(std::uint64_t j) { return Symbol('v', j); }
inline Key W(std::uint64_t j) { return Symbol('w', j); }
inline Key X(std::uint64_t j) { return Symbol('x', j); }
inline Key Y(std::uint64_t j) { return Symbol('y', j); }
inline Key Z(std::uint64_t j) { return Symbol('z', j); }
}
/// traits

8
gtsam/inference/VariableIndex.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -32,7 +32,7 @@ namespace gtsam {
/**
* The VariableIndex class computes and stores the block column structure of a
* factor graph. The factor graph stores a collection of factors, each of
* factor graph. The factor graph stores a collection of factors, each of
* which involves a set of variables. In contrast, the VariableIndex is built
* from a factor graph prior to elimination, and stores the list of factors
* that involve each variable. This information is stored as a deque of
@ -45,7 +45,7 @@ public:
typedef boost::shared_ptr<VariableIndex> shared_ptr;
typedef FastVector<size_t> Factors;
typedef Factors::iterator Factor_iterator;
typedef Factors::const_iterator Factor_const_iterator;
typedef Factors::const_iterator Factor_const_iterator;
protected:
typedef FastMap<Key,Factors> KeyMap;
@ -81,7 +81,7 @@ public:
* The number of variable entries. This is one greater than the variable
* 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 */
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
typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors);
if(!childFactor->empty())
if(childFactor && !childFactor->empty())
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();
// constrained version - push one set to the end
FastMap<size_t, int> constraints;
FastMap<Key, int> constraints;
constraints[2] = 1;
constraints[4] = 1;
constraints[5] = 2;

15
gtsam/linear/GaussianBayesNet.cpp
View File

@ -86,6 +86,8 @@ namespace gtsam {
VectorValues GaussianBayesNet::backSubstitute(const VectorValues& rhs) const
{
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) {
result.insert(cg->solveOtherRHS(result, rhs));
}
@ -138,17 +140,18 @@ namespace gtsam {
// return grad;
//}
/* ************************************************************************* */
pair<Matrix,Vector> GaussianBayesNet::matrix() const
{
/* ************************************************************************* */
pair<Matrix, Vector> GaussianBayesNet::matrix() const {
GaussianFactorGraph factorGraph(*this);
KeySet keys = factorGraph.keys();
// add frontal keys in order
Ordering ordering;
BOOST_FOREACH (const sharedConditional& cg, *this)
BOOST_FOREACH (Key key, cg->frontals()) {
ordering.push_back(key);
keys.erase(key);
if (cg) {
BOOST_FOREACH (Key key, cg->frontals()) {
ordering.push_back(key);
keys.erase(key);
}
}
// add remaining keys in case Bayes net is incomplete
BOOST_FOREACH (Key key, keys)

20
gtsam/linear/GaussianConditional.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -71,7 +71,7 @@ namespace gtsam {
model_->print(" Noise model: ");
else
cout << " No noise model" << endl;
}
}
/* ************************************************************************* */
bool GaussianConditional::equals(const GaussianFactor& f, double tol) const
@ -120,24 +120,24 @@ namespace gtsam {
VectorValues GaussianConditional::solve(const VectorValues& x) const
{
// 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
xS = getb() - get_S() * xS;
const Vector rhs = get_d() - get_S() * xS;
// 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
if(soln.hasNaN()) throw IndeterminantLinearSystemException(keys().front());
// TODO, do we not have to scale by sigmas here? Copy/paste bug
if (solution.hasNaN()) {
throw IndeterminantLinearSystemException(keys().front());
}
// Insert solution into a VectorValues
VectorValues result;
DenseIndex vectorPosition = 0;
for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal)));
for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
result.insert(*frontal, solution.segment(vectorPosition, getDim(frontal)));
vectorPosition += getDim(frontal);
}

28
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -30,6 +30,8 @@
#include <gtsam/base/timing.h>
#include <gtsam/base/cholesky.h>
#include <boost/foreach.hpp>
using namespace std;
using namespace gtsam;
@ -68,27 +70,6 @@ namespace gtsam {
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 {
gtsam::GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
@ -228,7 +209,8 @@ namespace gtsam {
Matrix GaussianFactorGraph::augmentedHessian(
boost::optional<const Ordering&> optionalOrdering) const {
// 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();
// Fill in lower-triangular part of Hessian
result.triangularView<Eigen::StrictlyLower>() = result.transpose();

6
gtsam/linear/GaussianFactorGraph.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -17,7 +17,7 @@
* @author Alireza Fathi
* @author Richard Roberts
* @author Frank Dellaert
*/
*/
#pragma once
@ -141,8 +141,6 @@ namespace gtsam {
/* return a map of (Key, dimension) */
std::map<Key, size_t> getKeyDimMap() const;
std::vector<size_t> getkeydim() const;
/** unnormalized error */
double error(const VectorValues& x) const {
double total_error = 0.;

4
gtsam/linear/HessianFactor.cpp
View File

@ -455,8 +455,8 @@ std::pair<boost::shared_ptr<GaussianConditional>,
// Build joint factor
HessianFactor::shared_ptr jointFactor;
try {
jointFactor = boost::make_shared<HessianFactor>(factors,
Scatter(factors, keys));
Scatter scatter(factors, keys);
jointFactor = boost::make_shared<HessianFactor>(factors, scatter);
} catch (std::invalid_argument&) {
throw InvalidDenseElimination(
"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
* 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:

101
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
vector<DenseIndex> slots(n+1);
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]);
slots[j] = J;
// Fill off-diagonal blocks with Ai'*Aj
for (DenseIndex i = 0; i < j; ++i) {
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
(*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>,
boost::shared_ptr<JacobianFactor> > JacobianFactor::eliminate(
std::pair<GaussianConditional::shared_ptr,
JacobianFactor::shared_ptr> JacobianFactor::eliminate(
const Ordering& keys) {
GaussianFactorGraph graph;
graph.add(*this);
@ -710,8 +711,7 @@ void JacobianFactor::setModel(bool anyConstrained, const Vector& sigmas) {
}
/* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>,
boost::shared_ptr<JacobianFactor> > EliminateQR(
std::pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr> EliminateQR(
const GaussianFactorGraph& factors, const Ordering& keys) {
gttic(EliminateQR);
// Combine and sort variable blocks in elimination order
@ -721,77 +721,84 @@ std::pair<boost::shared_ptr<GaussianConditional>,
} catch (std::invalid_argument&) {
throw InvalidDenseElimination(
"EliminateQR was called with a request to eliminate variables that are not\n"
"involved in the provided factors.");
"involved in the provided factors.");
}
// Do dense elimination
// The following QR variants eliminate to fully triangular or trapezoidal
SharedDiagonal noiseModel;
if (jointFactor->model_)
jointFactor->model_ = jointFactor->model_->QR(jointFactor->Ab_.matrix());
else
inplace_QR(jointFactor->Ab_.matrix());
// Zero below the diagonal
jointFactor->Ab_.matrix().triangularView<Eigen::StrictlyLower>().setZero();
VerticalBlockMatrix& Ab = jointFactor->Ab_;
if (jointFactor->model_) {
// The noiseModel QR can, in the case of constraints, yield a "staggered" QR where
// some rows have more leading zeros than in an upper triangular matrix.
// In either case, QR will put zeros below the "diagonal".
jointFactor->model_ = jointFactor->model_->QR(Ab.matrix());
} 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
GaussianConditional::shared_ptr conditional = //
GaussianConditional::shared_ptr conditional = //
jointFactor->splitConditional(keys.size());
return make_pair(conditional, jointFactor);
}
/* ************************************************************************* */
GaussianConditional::shared_ptr JacobianFactor::splitConditional(
size_t nrFrontals) {
GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFrontals) {
gttic(JacobianFactor_splitConditional);
if (nrFrontals > size())
if (!model_) {
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();
// 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
gttic(cond_Rd);
const DenseIndex originalRowEnd = Ab_.rowEnd();
Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
SharedDiagonal conditionalNoiseModel;
if (model_) {
if ((DenseIndex) model_->dim() < frontalDim)
throw IndeterminantLinearSystemException(this->keys().front());
conditionalNoiseModel = noiseModel::Diagonal::Sigmas(
model_->sigmas().segment(Ab_.rowStart(), Ab_.rows()));
}
GaussianConditional::shared_ptr conditional = boost::make_shared<
GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);
const DenseIndex maxRemainingRows = std::min(Ab_.cols(), originalRowEnd)
- Ab_.rowStart() - frontalDim;
const DenseIndex remainingRows =
model_ ? std::min(model_->sigmas().size() - frontalDim,
maxRemainingRows) :
maxRemainingRows;
conditionalNoiseModel =
noiseModel::Diagonal::Sigmas(model_->sigmas().segment(Ab_.rowStart(), Ab_.rows()));
GaussianConditional::shared_ptr conditional =
boost::make_shared<GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);
const DenseIndex maxRemainingRows =
std::min(Ab_.cols(), originalRowEnd) - Ab_.rowStart() - frontalDim;
const DenseIndex remainingRows = std::min(model_->sigmas().size() - frontalDim, maxRemainingRows);
Ab_.rowStart() += frontalDim;
Ab_.rowEnd() = Ab_.rowStart() + remainingRows;
Ab_.firstBlock() += nrFrontals;
gttoc(cond_Rd);
// Take lower-right block of Ab to get the new factor
gttic(remaining_factor);
keys_.erase(begin(), begin() + nrFrontals);
// Set sigmas with the right model
if (model_) {
if (model_->isConstrained())
model_ = noiseModel::Constrained::MixedSigmas(
model_->sigmas().tail(remainingRows));
else
model_ = noiseModel::Diagonal::Sigmas(
model_->sigmas().tail(remainingRows));
assert(model_->dim() == (size_t)Ab_.rows());
}
gttoc(remaining_factor);
if (model_->isConstrained())
model_ = noiseModel::Constrained::MixedSigmas(model_->sigmas().tail(remainingRows));
else
model_ = noiseModel::Diagonal::Sigmas(model_->sigmas().tail(remainingRows));
assert(model_->dim() == (size_t)Ab_.rows());
return conditional;
}
}
} // namespace gtsam

6
gtsam/linear/JacobianFactor.h
View File

@ -314,7 +314,7 @@ namespace gtsam {
JacobianFactor whiten() const;
/** 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);
/** set noiseModel correctly */
@ -331,13 +331,15 @@ namespace gtsam {
* @return The conditional and remaining factor
*
* \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);
/**
* splits a pre-factorized factor into a conditional, and changes the current
* factor to be the remaining component. Performs same operation as eliminate(),
* 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);

257
gtsam/linear/NoiseModel.cpp
View File

@ -21,6 +21,7 @@
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
@ -58,7 +59,7 @@ boost::optional<Vector> checkIfDiagonal(const Matrix M) {
for (i = 0; i < m; i++)
if (!full)
for (j = i + 1; j < n; j++)
if (fabs(M(i, j)) > 1e-9) {
if (std::abs(M(i, j)) > 1e-9) {
full = true;
break;
}
@ -87,7 +88,7 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
if (diagonal)
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));
}
@ -190,8 +191,8 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const {
// get size(A) and maxRank
// TODO: really no rank problems ?
// size_t m = Ab.rows(), n = Ab.cols()-1;
// size_t maxRank = min(m,n);
size_t m = Ab.rows(), n = Ab.cols()-1;
size_t maxRank = min(m,n);
// pre-whiten everything (cheaply if possible)
WhitenInPlace(Ab);
@ -200,12 +201,13 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const {
// Eigen QR - much faster than older householder approach
inplace_QR(Ab);
Ab.triangularView<Eigen::StrictlyLower>().setZero();
// hand-coded householder implementation
// TODO: necessary to isolate last column?
// householder(Ab, maxRank);
return SharedDiagonal();
return noiseModel::Unit::Create(maxRank);
}
void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const {
@ -414,81 +416,141 @@ Constrained::shared_ptr Constrained::unit() const {
// Special version of QR for Constrained calls slower but smarter code
// that deals with possibly zero sigmas
// 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 {
bool verbose = false;
if (verbose) cout << "\nStarting Constrained::QR" << endl;
static const double kInfinity = std::numeric_limits<double>::infinity();
// get size(A) and maxRank
size_t m = Ab.rows(), n = Ab.cols()-1;
size_t maxRank = min(m,n);
size_t m = Ab.rows();
const size_t n = Ab.cols() - 1;
const size_t maxRank = min(m, n);
// create storage for [R d]
typedef boost::tuple<size_t, Vector, double> Triple;
typedef boost::tuple<size_t, Matrix, double> Triple;
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 weights = invsigmas.array().square(); // calculate weights once
Vector weights = invsigmas.array().square(); // calculate weights once
// We loop over all columns, because the columns that can be eliminated
// are not necessarily contiguous. For each one, estimate the corresponding
// 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.
for (size_t j=0; j<n; ++j) {
for (size_t j = 0; j < n; ++j) {
// extract the first column of A
Vector a = Ab.col(j);
Eigen::Block<Matrix> a = Ab.block(0, j, m, 1);
// Calculate weighted pseudo-inverse and corresponding precision
gttic(constrained_QR_weightedPseudoinverse);
double precision = weightedPseudoinverse(a, weights, pseudo);
gttoc(constrained_QR_weightedPseudoinverse);
// Check whether we need to handle as a constraint
boost::optional<size_t> constraint_row = check_if_constraint(a, invsigmas, m);
// 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
if (precision < 1e-8) continue;
if (constraint_row) {
// Handle this as a constraint, as the i^th row has zero sigma with non-zero entry A(i,j)
gttic(constrained_QR_create_rd);
// create solution [r d], rhs is automatically r(n)
Vector rd(n+1); // uninitialized !
rd(j)=1.0; // put 1 on diagonal
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);
// 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, precision));
// Construct solution (r, d, sigma)
Rd.push_back(boost::make_tuple(j, rd, kInfinity));
// exit after rank exhausted
if (Rd.size()>=maxRank) break;
// exit after rank exhausted
if (Rd.size() >= maxRank)
break;
// update Ab, expensive, using outer product
gttic(constrained_QR_update_Ab);
Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose();
gttoc(constrained_QR_update_Ab);
// 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
// Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma)
// For diagonal Sigma, inv(Sigma) = diag(precisions)
double precision = 0;
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;
// create solution [r d], rhs is automatically r(n)
rd(0, 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);
// construct solution (r, d, sigma)
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
if (Rd.size() >= maxRank)
break;
// Rank-1 down-date of Ab, expensive, using outer product
Ab.block(0, j + 1, m, n - j).noalias() -= a * rd.middleCols(j + 1, n - j);
}
}
// Create storage for precisions
Vector precisions(Rd.size());
gttic(constrained_QR_write_back_into_Ab);
// 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;
BOOST_FOREACH(const Triple& t, Rd) {
const size_t& j = t.get<0>();
const Vector& rd = t.get<1>();
precisions(i) = t.get<2>();
if (constrained(i)) mixed = true;
for (size_t j2=0; j2<j; ++j2)
Ab(i,j2) = 0.0; // fill in zeros below diagonal anway
for (size_t j2=j; j2<n+1; ++j2)
Ab(i,j2) = rd(j2);
i+=1;
Ab.setZero(); // make sure we don't look below
BOOST_FOREACH (const Triple& t, Rd) {
const size_t& j = t.get<0>();
const Matrix& rd = t.get<1>();
precisions(i) = t.get<2>();
if (std::isinf(precisions(i)))
mixed = true;
Ab.block(i, j, 1, n + 1 - j) = rd.block(0, j, 1, n + 1 - j);
i += 1;
}
gttoc(constrained_QR_write_back_into_Ab);
// Must include mu, as the defaults might be higher, resulting in non-convergence
return mixed ? Constrained::MixedPrecisions(mu_, precisions) : Diagonal::Precisions(precisions);
@ -498,13 +560,13 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
// Isotropic
/* ************************************************************************* */
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));
}
/* ************************************************************************* */
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)));
}
@ -561,21 +623,19 @@ void Unit::print(const std::string& name) const {
namespace mEstimator {
/** produce a weight vector according to an error vector and the implemented
* robust function */
Vector Base::weight(const Vector &error) const {
Vector Base::weight(const Vector& error) const {
const size_t n = error.rows();
Vector w(n);
for ( size_t i = 0 ; i < n ; ++i )
for (size_t i = 0; i < n; ++i)
w(i) = weight(error(i));
return w;
}
/** The following three functions reweight block matrices and a vector
* according to their weight implementation */
// The following three functions re-weight block matrices and a vector
// according to their weight implementation
void Base::reweight(Vector& error) const {
if(reweight_ == Block) {
if (reweight_ == Block) {
const double w = sqrtWeight(error.norm());
error *= w;
} 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 {
if ( reweight_ == Block ) {
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 {
if ( reweight_ == Block ) {
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 {
if ( reweight_ == Block ) {
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 {
if ( reweight_ == Block ) {
const double w = sqrtWeight(error.norm());
@ -659,11 +719,9 @@ void Null::print(const std::string &s="") const
Null::shared_ptr Null::Create()
{ return shared_ptr(new Null()); }
Fair::Fair(double c, const ReweightScheme reweight)
: Base(reweight), c_(c) {
if ( c_ <= 0 ) {
cout << "mEstimator Fair takes only positive double in constructor. forced to 1.0" << endl;
c_ = 1.0;
Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) {
if (c_ <= 0) {
throw runtime_error("mEstimator Fair takes only positive double in constructor.");
}
}
@ -671,16 +729,13 @@ Fair::Fair(double c, const ReweightScheme reweight)
// Fair
/* ************************************************************************* */
double Fair::weight(double error) const
{ return 1.0 / (1.0 + fabs(error)/c_); }
void Fair::print(const std::string &s="") const
{ cout << s << "fair (" << c_ << ")" << endl; }
bool Fair::equals(const Base &expected, double tol) const {
const Fair* p = dynamic_cast<const Fair*> (&expected);
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)
@ -690,18 +745,12 @@ Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight)
// Huber
/* ************************************************************************* */
Huber::Huber(double k, const ReweightScheme reweight)
: Base(reweight), k_(k) {
if ( k_ <= 0 ) {
cout << "mEstimator Huber takes only positive double in constructor. forced to 1.0" << endl;
k_ = 1.0;
Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) {
if (k_ <= 0) {
throw runtime_error("mEstimator Huber takes only positive double in constructor.");
}
}
double Huber::weight(double error) const {
return (fabs(error) > k_) ? k_ / fabs(error) : 1.0;
}
void Huber::print(const std::string &s="") const {
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 {
const Huber* p = dynamic_cast<const Huber*>(&expected);
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) {
@ -720,18 +769,12 @@ Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) {
// Cauchy
/* ************************************************************************* */
Cauchy::Cauchy(double k, const ReweightScheme reweight)
: Base(reweight), k_(k) {
if ( k_ <= 0 ) {
cout << "mEstimator Cauchy takes only positive double in constructor. forced to 1.0" << endl;
k_ = 1.0;
Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), ksquared_(k * k) {
if (k <= 0) {
throw runtime_error("mEstimator Cauchy takes only positive double in constructor.");
}
}
double Cauchy::weight(double error) const {
return k_*k_ / (k_*k_ + error*error);
}
void Cauchy::print(const std::string &s="") const {
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 {
const Cauchy* p = dynamic_cast<const Cauchy*>(&expected);
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) {
@ -749,18 +792,7 @@ Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) {
/* ************************************************************************* */
// Tukey
/* ************************************************************************* */
Tukey::Tukey(double c, const ReweightScheme reweight)
: 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;
}
Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
void Tukey::print(const std::string &s="") const {
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 {
const Tukey* p = dynamic_cast<const Tukey*>(&expected);
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) {
@ -779,14 +811,7 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) {
/* ************************************************************************* */
// Welsh
/* ************************************************************************* */
Welsh::Welsh(double c, const ReweightScheme reweight)
: Base(reweight), c_(c) {
}
double Welsh::weight(double error) const {
double xc2 = (error/c_)*(error/c_);
return std::exp(-xc2);
}
Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
void Welsh::print(const std::string &s="") const {
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 {
const Welsh* p = dynamic_cast<const Welsh*>(&expected);
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) {

130
gtsam/linear/NoiseModel.h
View File

@ -232,9 +232,11 @@ namespace gtsam {
/**
* Apply appropriately weighted QR factorization to the system [A b]
* 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]
* @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;
@ -482,6 +484,12 @@ namespace gtsam {
/**
* 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;
@ -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 {
//---------------------------------------------------------------------------------------
@ -696,19 +720,20 @@ namespace gtsam {
/// Fair implements the "Fair" robust error model (Zhang97ivc)
class GTSAM_EXPORT Fair : public Base {
protected:
double c_;
public:
typedef boost::shared_ptr<Fair> shared_ptr;
Fair(double c = 1.3998, const ReweightScheme reweight = Block);
virtual ~Fair() {}
virtual double weight(double error) const;
virtual void print(const std::string &s) const;
virtual bool equals(const Base& expected, double tol=1e-8) const;
double weight(double error) const {
return 1.0 / (1.0 + fabs(error) / c_);
}
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) ;
protected:
double c_;
private:
/** Serialization function */
friend class boost::serialization::access;
@ -721,19 +746,20 @@ namespace gtsam {
/// Huber implements the "Huber" robust error model (Zhang97ivc)
class GTSAM_EXPORT Huber : public Base {
protected:
double k_;
public:
typedef boost::shared_ptr<Huber> shared_ptr;
virtual ~Huber() {}
Huber(double k = 1.345, const ReweightScheme reweight = Block);
virtual double weight(double error) const;
virtual void print(const std::string &s) const;
virtual bool equals(const Base& expected, double tol=1e-8) const;
double weight(double error) const {
return (error < k_) ? (1.0) : (k_ / fabs(error));
}
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) ;
protected:
double k_;
private:
/** Serialization function */
friend class boost::serialization::access;
@ -750,19 +776,20 @@ namespace gtsam {
/// oberlaender@fzi.de
/// Thanks Jan!
class GTSAM_EXPORT Cauchy : public Base {
protected:
double k_, ksquared_;
public:
typedef boost::shared_ptr<Cauchy> shared_ptr;
virtual ~Cauchy() {}
Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
virtual double weight(double error) const;
virtual void print(const std::string &s) const;
virtual bool equals(const Base& expected, double tol=1e-8) const;
double weight(double error) const {
return ksquared_ / (ksquared_ + error*error);
}
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) ;
protected:
double k_;
private:
/** Serialization function */
friend class boost::serialization::access;
@ -775,19 +802,24 @@ namespace gtsam {
/// Tukey implements the "Tukey" robust error model (Zhang97ivc)
class GTSAM_EXPORT Tukey : public Base {
protected:
double c_, csquared_;
public:
typedef boost::shared_ptr<Tukey> shared_ptr;
Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
virtual ~Tukey() {}
virtual double weight(double error) const;
virtual void print(const std::string &s) const;
virtual bool equals(const Base& expected, double tol=1e-8) const;
double weight(double error) const {
if (std::fabs(error) <= c_) {
double xc2 = error*error/csquared_;
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) ;
protected:
double c_;
private:
/** Serialization function */
friend class boost::serialization::access;
@ -800,19 +832,21 @@ namespace gtsam {
/// Welsh implements the "Welsh" robust error model (Zhang97ivc)
class GTSAM_EXPORT Welsh : public Base {
protected:
double c_, csquared_;
public:
typedef boost::shared_ptr<Welsh> shared_ptr;
Welsh(double c = 2.9846, const ReweightScheme reweight = Block);
virtual ~Welsh() {}
virtual double weight(double error) const;
virtual void print(const std::string &s) const;
virtual bool equals(const Base& expected, double tol=1e-8) const;
double weight(double error) const {
double xc2 = (error*error)/csquared_;
return std::exp(-xc2);
}
void print(const std::string &s) const;
bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
protected:
double c_;
private:
/** Serialization function */
friend class boost::serialization::access;
@ -884,7 +918,23 @@ namespace gtsam {
} ///\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 {
public:
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 {
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];
}
// Add A2 contribution
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();
VectorValues y = zero();
for ( Key i = 0 ; i < y.size() ; ++i, ++it )
y[i] = *it ;
transposeMultiplyAdd2(1.0,it,e.end(),y);
for (size_t i = 0; i < y.size(); ++i, ++it)
y[i] = *it;
transposeMultiplyAdd2(1.0, it, e.end(), y);
return y;
}
@ -535,7 +534,7 @@ void SubgraphPreconditioner::transposeMultiplyAdd
(double alpha, const Errors& e, VectorValues& y) const {
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;
axpy(alpha, ei, y[i]);
}

4
gtsam/linear/VectorValues.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -135,7 +135,7 @@ namespace gtsam {
/// @{
/** Number of variables stored. */
Key size() const { return values_.size(); }
size_t size() const { return values_.size(); }
/** Return the dimension of variable \c j. */
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);
GaussianConditional::shared_ptr expectedConditional;
JacobianFactor::shared_ptr expectedFactor;
boost::tie(expectedConditional, expectedFactor) = //
jacobian.eliminate(ordering);
boost::tie(expectedConditional, expectedFactor) = jacobian.eliminate(ordering);
CHECK(expectedFactor);
// Eliminate
GaussianConditional::shared_ptr actualConditional;
HessianFactor::shared_ptr actualHessian;
boost::tie(actualConditional, actualHessian) = //
EliminateCholesky(gfg, ordering);
actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison
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(
assert_equal(expectedFactor->augmentedInformation(),
actualHessian->augmentedInformation(), 1e-9));
EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
}
#endif
}
/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate2) {
@ -381,8 +385,11 @@ TEST(HessianFactor, CombineAndEliminate2) {
HessianFactor::shared_ptr actualHessian;
boost::tie(actualConditional, actualHessian) = //
EliminateCholesky(gfg, ordering);
actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison
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;
v.insert(1, Vector3(1, 2, 3));
EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
@ -390,6 +397,7 @@ TEST(HessianFactor, CombineAndEliminate2) {
assert_equal(expectedFactor->augmentedInformation(),
actualHessian->augmentedInformation(), 1e-9));
EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
#endif
}
/* ************************************************************************* */

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

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -19,6 +19,7 @@
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h>
@ -161,37 +162,46 @@ TEST(JabobianFactor, Hessian_conversion) {
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)
{
GaussianFactorGraph factors;
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)));
using namespace simple_graph;
Matrix A1(6,2); A1 << A11, A21, Matrix::Zero(2,2);
Matrix A2(6,2); A2 << Matrix::Zero(2,2), A22, A32;
Matrix A3(6,2); A3 << Matrix::Zero(4,2), A33;
Vector b(6); b << b1, b2, b3;
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
JacobianFactor actual(factors, Ordering(list_of(10)(8)(12)));
JacobianFactor actual(factors, ordering);
EXPECT(assert_equal(expected, actual));
}
@ -466,7 +476,7 @@ TEST(JacobianFactor, eliminate2 )
+0.00,-0.20,+0.00,-0.80
).finished()/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));
@ -478,7 +488,7 @@ TEST(JacobianFactor, eliminate2 )
0.00, 1.00, +0.00, -1.00
).finished()/sigma;
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));
}
@ -516,7 +526,7 @@ TEST(JacobianFactor, EliminateQR)
EXPECT(assert_equal(2.0 * Ab, actualDense));
// 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,
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,
@ -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., 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., 0., -5.7095, -0.0090,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635).finished();
0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090).finished();
GaussianConditional expectedFragment(
list_of(3)(5)(7)(9)(11), 3, VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), R));
GaussianConditional expectedFragment(list_of(3)(5)(7)(9)(11), 3,
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 !!!!
GaussianFactorGraph::EliminationResult actual = EliminateQR(factors, list_of(3)(5)(7));
const JacobianFactor &actualJF = dynamic_cast<const JacobianFactor&>(*actual.second);
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(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, 8, 5, 2)), actualJF.getA(actualJF.begin()+1), 0.001));
EXPECT(assert_equal(Vector(R.col(10).segment(6, 5)), actualJF.getb(), 0.001));
EXPECT(!actualJF.get_model());
EXPECT(assert_equal(Matrix(R.block(6, 6, 4, 2)), actualJF.getA(actualJF.begin()), 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, 4)), actualJF.getb(), 0.001));
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>
actual = lc.eliminate(list_of(1));
// verify linear factor
EXPECT(actual.second->size() == 0);
// verify linear factor is a null ptr
EXPECT(actual.second->empty());
// verify conditional Gaussian
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;
// A1 - invertible
Matrix A1(2,2);
A1(0,0) = 1.0 ; A1(0,1) = 2.0;
A1(1,0) = 2.0 ; A1(1,1) = 1.0;
Matrix2 A1; A1 << 2,4, 2,1;
// A2 - not invertible
Matrix A2(2,2);
A2(0,0) = 1.0 ; A2(0,1) = 2.0;
A2(1,0) = 2.0 ; A2(1,1) = 4.0;
Matrix2 A2; A2 << 2,4, 2,4;
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));
// verify CG
Matrix R = (Matrix(2, 2) <<
1.0, 2.0,
0.0, 1.0).finished();
Matrix S = (Matrix(2, 2) <<
1.0, 2.0,
0.0, 0.0).finished();
Matrix2 R; R << 1,2, 0,1;
Matrix2 S; S << 1,2, 0,0;
Vector d = Vector2(3.0, 0.6666);
Vector sigmas = Vector2(0.0, 0.0);
GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas));
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);}
/* ************************************************************************* */

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

@ -222,45 +222,172 @@ namespace exampleQR {
SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
}
/* ************************************************************************* */
TEST( NoiseModel, QR )
{
Matrix Ab1 = exampleQR::Ab;
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
SharedDiagonal actual1 = exampleQR::diagonal->QR(Ab1);
EXPECT(!actual1);
EXPECT(actual1->isUnit());
EXPECT(linear_dependent(exampleQR::Rd,Ab1,1e-4)); // Ab was modified in place !!!
// Call Constrained version
SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(exampleQR::sigmas);
SharedDiagonal actual2 = constrained->QR(Ab2);
SharedDiagonal expectedModel2 = noiseModel::Diagonal::Sigmas(expectedSigmas);
EXPECT(assert_equal(*expectedModel2,*actual2,1e-6));
// Expected result for constrained version
Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished();
SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
Matrix expectedRd2 = (Matrix(4, 7) <<
1., 0., -0.2, 0., -0.8, 0., 0.2,
0., 1., 0.,-0.2, 0., -0.8,-0.14,
0., 0., 1., 0., -1., 0., 0.0,
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 )
{
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);
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);
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 {
cout << s << "GPSFactor on " << keyFormatter(key()) << "\n";
cout << " prior mean: " << nT_ << "\n";
cout << " GPS measurement: " << nT_ << "\n";
noiseModel_->print(" noise model: ");
}
@ -38,12 +38,7 @@ bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
//***************************************************************************
Vector GPSFactor::evaluateError(const Pose3& p,
boost::optional<Matrix&> H) const {
if (H) {
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_;
return p.translation(H) -nT_;
}
//***************************************************************************
@ -68,6 +63,25 @@ pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,
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

88
gtsam/navigation/GPSFactor.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/geometry/Pose3.h>
namespace gtsam {
@ -37,7 +38,7 @@ private:
typedef NoiseModelFactor1<Pose3> Base;
Point3 nT_; ///< Position measurement in
Point3 nT_; ///< Position measurement in cartesian coordinates
public:
@ -50,14 +51,13 @@ public:
/** default constructor - only use for serialization */
GPSFactor(): nT_(0, 0, 0) {}
virtual ~GPSFactor() {
}
virtual ~GPSFactor() {}
/**
* @brief Constructor from a measurement in a Cartesian frame.
* Use GeographicLib to convert from geographic (latitude and longitude) coordinates
* @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
*/
GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
@ -70,18 +70,14 @@ public:
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** implement functions needed for Testable */
/** print */
/// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/** equals */
/// equals
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,
boost::optional<Matrix&> H = boost::none) const;
@ -89,8 +85,8 @@ public:
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
* Assumes roll is zero, calculates yaw and pitch from NED1->NED2 vector.
*/
@ -99,7 +95,71 @@ public:
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;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {

7
gtsam/navigation/ImuBias.h
View File

@ -44,7 +44,7 @@ public:
biasAcc_(biasAcc), biasGyro_(biasGyro) {
}
ConstantBias(const Vector6& v) :
explicit ConstantBias(const Vector6& v) :
biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) {
}
@ -114,6 +114,11 @@ public:
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 */
ConstantBias operator+(const ConstantBias& b) const {
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() += C * (wCov / dt) * C.transpose();
// NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt)
static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished();
preintMeasCov_.noalias() += Gi * (iCov * dt) * Gi.transpose();
// NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3
preintMeasCov_.block<3,3>(3,3).noalias() += iCov * dt;
}
//------------------------------------------------------------------------------
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) {
os << " preintegrated measurements:\n" << f._PIM_;
;
// Print standard deviations on covariance only
f._PIM_.print("preintegrated measurements:\n");
os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
return os;
}
@ -172,9 +191,6 @@ PreintegratedImuMeasurements ImuFactor::Merge(
Matrix9 H1, H2;
pim02.mergeWith(pim12, &H1, &H2);
pim02.preintMeasCov_ = H1 * pim01.preintMeasCov_ * H1.transpose() +
H2 * pim12.preintMeasCov_ * H2.transpose();
return pim02;
}
@ -229,6 +245,50 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
vel_j = pvb.velocity;
}
#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 {
friend class ImuFactor;
friend class ImuFactor2;
protected:
@ -114,12 +115,18 @@ public:
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param dt Time interval between this and the last IMU measurement
*/
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double dt);
void integrateMeasurement(const Vector3& measuredAcc, 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
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
/// @deprecated constructor
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
@ -162,8 +169,6 @@ private:
*/
class ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias> {
public:
private:
typedef ImuFactor This;
@ -220,7 +225,7 @@ public:
/// vector of errors
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
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::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
boost::none, boost::optional<Matrix&> H5 = boost::none) const;
@ -265,10 +270,81 @@ private:
};
// 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 <>
struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
template <>
struct traits<ImuFactor> : public Testable<ImuFactor> {};
template <>
struct traits<ImuFactor2> : public Testable<ImuFactor2> {};
} /// 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)
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const {
@ -281,6 +282,7 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
}
return newState;
}
#endif
//------------------------------------------------------------------------------
Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,

2
gtsam/navigation/NavState.h
View File

@ -203,11 +203,13 @@ public:
/// @name Dynamics
/// @{
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// Integrate forward in time given angular velocity and acceleration in body frame
/// Uses second order integration for position, returns derivatives except dt.
NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const;
#endif
/// Compute tangent space contribution due to Coriolis forces
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,
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
&& 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,
double tol) const {
const bool params_match = p_->equals(*other.p_, tol);
return params_match && fabs(deltaTij_ - other.deltaTij_) < tol
return p_->equals(*other.p_, tol)
&& fabs(deltaTij_ - other.deltaTij_) < tol
&& biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(preintegrated_, other.preintegrated_, 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(
const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
OptionalJacobian<3, 3> D_correctedAcc_unbiasedAcc,
OptionalJacobian<3, 3> D_correctedAcc_unbiasedOmega,
OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega) const {
OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc,
OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega,
OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega) const {
assert(p().body_P_sensor);
// 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;
// Jacobians
if (D_correctedAcc_unbiasedAcc) *D_correctedAcc_unbiasedAcc = bRs;
if (D_correctedAcc_unbiasedOmega) *D_correctedAcc_unbiasedOmega = Z_3x3;
if (D_correctedOmega_unbiasedOmega) *D_correctedOmega_unbiasedOmega = bRs;
if (correctedAcc_H_unbiasedAcc) *correctedAcc_H_unbiasedAcc = bRs;
if (correctedAcc_H_unbiasedOmega) *correctedAcc_H_unbiasedOmega = Z_3x3;
if (correctedOmega_H_unbiasedOmega) *correctedOmega_H_unbiasedOmega = bRs;
// Centrifugal acceleration
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;
// Update derivative: centrifugal causes the correlation between acc and omega!!!
if (D_correctedAcc_unbiasedOmega) {
if (correctedAcc_H_unbiasedOmega) {
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()
+ 2 * b_arm * unbiasedOmega.transpose();
}
@ -366,27 +366,26 @@ Vector9 PreintegrationBase::Compose(const Vector9& zeta01,
//------------------------------------------------------------------------------
void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1,
Matrix9* H2) {
if (!matchesParamsWith(pim12))
throw std::domain_error(
"Cannot merge pre-integrated measurements with different params");
if (!matchesParamsWith(pim12)) {
throw std::domain_error("Cannot merge pre-integrated measurements with different params");
}
if (params()->body_P_sensor)
throw std::domain_error(
"Cannot merge pre-integrated measurements with sensor pose yet");
if (params()->body_P_sensor) {
throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet");
}
const double& t01 = deltaTij();
const double& t12 = pim12.deltaTij();
const double t01 = deltaTij();
const double t12 = pim12.deltaTij();
deltaTij_ = t01 + t12;
Vector9 zeta01 = preintegrated();
Vector9 zeta12 = pim12.preintegrated();
const Vector9 zeta01 = 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();
zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope()
+ 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_ =
(*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
* Order is: theta, position, velocity
* 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]
*/
@ -94,6 +95,9 @@ public:
/// @name Constructors
/// @{
/// @name Constructors
/// @{
/**
* Constructor, initializes the variables in the base class
* @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.
bool matchesParamsWith(const PreintegrationBase& other) const {
return p_ == other.p_;
return p_.get() == other.p_.get();
}
/// shared pointer to params
@ -134,7 +138,7 @@ public:
/// @name Instance variables access
/// @{
const imuBias::ConstantBias& biasHat() const { return biasHat_; }
const double& deltaTij() const { return deltaTij_; }
double deltaTij() const { return deltaTij_; }
const Vector9& preintegrated() const { return preintegrated_; }
@ -167,9 +171,9 @@ public:
/// Ignore D_correctedOmega_measuredAcc as it is trivially zero
std::pair<Vector3, Vector3> correctMeasurementsBySensorPose(
const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
OptionalJacobian<3, 3> D_correctedAcc_unbiasedAcc = boost::none,
OptionalJacobian<3, 3> D_correctedAcc_unbiasedOmega = boost::none,
OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega = boost::none) const;
OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none,
OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
// Update integrated vector on tangent manifold preintegrated with acceleration
// Static, functional version.
@ -215,7 +219,7 @@ public:
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
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,
double deltaT12,
OptionalJacobian<9, 9> H1 = boost::none,
@ -244,6 +248,8 @@ public:
/// @}
#endif
/// @}
private:
/** Serialization function */
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::B;
static const Vector3 kZero = Z_3x1;
typedef imuBias::ConstantBias Bias;
static const Vector3 Z_3x1 = Vector3::Zero();
static const Bias kZeroBiasHat, kZeroBias;
static const Vector3 kZeroOmegaCoriolis(0, 0, 0);

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

@ -136,20 +136,19 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
auto p = testing::Params();
testing::SomeMeasurements measurements;
boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
[=](const Vector3& a, const Vector3& w) {
PreintegratedImuMeasurements pim(p, Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.preintegrated();
};
auto preintegrated = [=](const Vector3& a, const Vector3& w) {
PreintegratedImuMeasurements pim(p, Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.preintegrated();
};
// Actual pre-integrated values
PreintegratedCombinedMeasurements pim(p);
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()));
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));
}

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

@ -29,19 +29,24 @@ using namespace gtsam;
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
// ENU Origin is where the plane was in hold next to runway
const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
LocalCartesian enu(lat0, lon0, h0, Geocentric::WGS84);
// Convert from GPS to ENU
LocalCartesian origin_ENU(lat0, lon0, h0, Geocentric::WGS84);
// Dekalb-Peachtree Airport runway 2L
const double lat = 33.87071, lon = -84.30482, h = 274;
// Dekalb-Peachtree Airport runway 2L
const double lat = 33.87071, lon = -84.30482, h = 274;
}
// *************************************************************************
TEST( GPSFactor, Constructor ) {
using namespace example;
// From lat-lon to geocentric
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(80.98, N, 1e-2);
EXPECT_DOUBLES_EQUAL(0, U, 1e-2);
@ -67,6 +72,35 @@ TEST( GPSFactor, Constructors ) {
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) {

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

@ -94,9 +94,9 @@ TEST(ImuFactor, PreintegratedMeasurements) {
// Actual pre-integrated values
PreintegratedImuMeasurements actual(testing::Params());
EXPECT(assert_equal(Z_3x1, actual.theta()));
EXPECT(assert_equal(Z_3x1, actual.deltaPij()));
EXPECT(assert_equal(Z_3x1, actual.deltaVij()));
EXPECT(assert_equal(kZero, actual.theta()));
EXPECT(assert_equal(kZero, actual.deltaPij()));
EXPECT(assert_equal(kZero, actual.deltaVij()));
DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9);
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -185,8 +185,25 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none,
boost::none), kZeroBias);
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());
testing::integrateMeasurements(measurements, &pim);
EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1),
EXPECT(assert_equal(numericalDerivative21(preintegrated, kZero, kZero),
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));
}
@ -780,7 +797,7 @@ struct ImuFactorMergeTest {
ImuFactorMergeTest()
: 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)) {
// arbitrary noise values
p_->gyroscopeCovariance = I_3x3 * 0.01;
@ -867,6 +884,29 @@ TEST(ImuFactor, MergeWithCoriolis) {
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() {
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) {
Vector3 omega(M_PI / 100.0, 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(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7));
}
#endif
/* ************************************************************************* */
static const double dt = 2.0;

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

@ -120,25 +120,24 @@ TEST(PreintegrationBase, Compose) {
}
/* ************************************************************************* */
TEST(PreintegrationBase, MergedBiasDerivatives) {
TEST(PreintegrationBase, MergedBiasDerivatives) {
testing::SomeMeasurements measurements;
boost::function<Vector9(const Vector3&, const Vector3&)> f =
[=](const Vector3& a, const Vector3& w) {
PreintegrationBase pim02(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim02);
testing::integrateMeasurements(measurements, &pim02);
return pim02.preintegrated();
};
auto f = [=](const Vector3& a, const Vector3& w) {
PreintegrationBase pim02(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim02);
testing::integrateMeasurements(measurements, &pim02);
return pim02.preintegrated();
};
// Expected merge result
PreintegrationBase expected_pim02(testing::Params());
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()));
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));
}

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

@ -75,7 +75,7 @@ TEST(ScenarioRunner, Spin) {
/* ************************************************************************* */
namespace forward {
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) {

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

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -27,11 +27,6 @@
namespace gtsam {
template<typename T>
void Expression<T>::print(const std::string& s) const {
root_->print(s);
}
template<typename T>
Expression<T>::Expression(const T& value) :
root_(new internal::ConstantExpression<T>(value)) {
@ -48,7 +43,7 @@ Expression<T>::Expression(const Symbol& symbol) :
}
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))) {
}
@ -128,6 +123,11 @@ void Expression<T>::dims(std::map<Key, int>& map) const {
root_->dims(map);
}
template<typename T>
void Expression<T>::print(const std::string& s) const {
root_->print(s);
}
template<typename T>
T Expression<T>::value(const Values& values,
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;
}
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

97
gtsam/nonlinear/Expression.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -22,6 +22,7 @@
#include <gtsam/nonlinear/internal/JacobianMap.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/VectorSpace.h>
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
@ -52,11 +53,14 @@ public:
/// Define type so we can apply it as a meta-function
typedef Expression<T> type;
private:
protected:
// Paul's trick shared pointer, polymorphic root of entire expression tree
boost::shared_ptr<internal::ExpressionNode<T> > root_;
/// Construct with a custom root
Expression(const boost::shared_ptr<internal::ExpressionNode<T> >& root) : root_(root) {}
public:
// Expressions wrap trees of functions that can evaluate their own derivatives.
@ -85,9 +89,6 @@ public:
typename MakeOptionalJacobian<T, A3>::type)> type;
};
/// Print
void print(const std::string& s) const;
/// Construct a constant expression
Expression(const T& value);
@ -98,7 +99,7 @@ public:
Expression(const Symbol& 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
template<typename A>
@ -147,6 +148,9 @@ public:
/// Return dimensions for each argument, as a map
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
* 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
size_t traceSize() const;
private:
protected:
/// Default constructor, for serialization
Expression() {}
@ -199,6 +203,85 @@ private:
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
* Example:

9
gtsam/nonlinear/ExpressionFactor.h
View File

@ -43,8 +43,8 @@ protected:
Expression<T> expression_; ///< the expression that is AD enabled
FastVector<int> dims_; ///< dimensions of the Jacobian matrices
public:
public:
typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
/// Constructor
@ -61,9 +61,10 @@ public:
const T& measured() const { return measured_; }
/// 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);
traits<T>::Print(measured_, s + ".measured_");
traits<T>::Print(measured_, "ExpressionFactor with measurement: ");
}
/// equals relies on Testable traits being defined for T
@ -80,7 +81,7 @@ public:
* both the function evaluation and its derivative(s) in H.
*/
virtual Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const {
boost::optional<std::vector<Matrix>&> H = boost::none) const {
if (H) {
const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
// NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here

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

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -29,8 +29,8 @@ namespace gtsam {
template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_(
const GaussianFactorGraph& linearFactorGraph,
const Values& linearizationPoints, Key lastKey,
JacobianFactor::shared_ptr& newPrior) const
const Values& linearizationPoint, Key lastKey,
JacobianFactor::shared_ptr* newPrior)
{
// Compute the marginal on the last key
// 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
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]);
// 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.
assert(marginal->nrFrontals() == 1);
assert(marginal->nrParents() == 0);
newPrior = boost::make_shared<JacobianFactor>(
*newPrior = boost::make_shared<JacobianFactor>(
marginal->keys().front(),
marginal->getA(marginal->begin()),
marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey],
@ -80,20 +80,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::predict(
const MotionFactor& motionFactor) {
// 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_ ?
const NoiseModelFactor& motionFactor) {
const auto keys = motionFactor.keys();
// Create a Gaussian Factor Graph
GaussianFactorGraph linearFactorGraph;
@ -102,12 +90,14 @@ namespace gtsam {
linearFactorGraph.push_back(priorFactor_);
// Linearize motion model and add it to the Kalman Filter graph
linearFactorGraph.push_back(
motionFactor.linearize(linearizationPoints));
Values linearizationPoint;
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
// and the posterior for the next iteration.
x_ = solve_(linearFactorGraph, linearizationPoints, x1, priorFactor_);
x_ = solve_(linearFactorGraph, linearizationPoint, keys[1], &priorFactor_);
return x_;
}
@ -115,18 +105,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::update(
const MeasurementFactor& measurementFactor) {
// 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_);
const NoiseModelFactor& measurementFactor) {
const auto keys = measurementFactor.keys();
// Create a Gaussian Factor Graph
GaussianFactorGraph linearFactorGraph;
@ -135,12 +115,13 @@ namespace gtsam {
linearFactorGraph.push_back(priorFactor_);
// Linearize measurement factor and add it to the Kalman Filter graph
linearFactorGraph.push_back(
measurementFactor.linearize(linearizationPoints));
Values linearizationPoint;
linearizationPoint.insert(keys[0], x_);
linearFactorGraph.push_back(measurementFactor.linearize(linearizationPoint));
// Solve the factor graph and update the current state estimate
// and the prior factor for the next iteration
x_ = solve_(linearFactorGraph, linearizationPoints, x0, priorFactor_);
x_ = solve_(linearFactorGraph, linearizationPoint, keys[0], &priorFactor_);
return x_;
}

141
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -24,76 +24,85 @@
namespace 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
* stable Kalman Filter implementation.
*
* The Kalman Filter relies on two models: a motion model that predicts the next state using
* the current state, and a measurement model that predicts the measurement value at a given
* state. Because these two models are situation-dependent, base classes for each have been
* provided above, from which the user must derive a class and incorporate the actual modeling
* equations.
*
* The class provides a "predict" and "update" function to perform these steps independently.
* TODO: a "predictAndUpdate" that combines both steps for some computational savings.
* \nosubgrouping
*/
template <class VALUE>
class ExtendedKalmanFilter {
// Check that VALUE type is a testable Manifold
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
BOOST_CONCEPT_ASSERT((IsManifold<VALUE>));
public:
typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
typedef VALUE T;
//@deprecated: any NoiseModelFactor will do, as long as they have the right keys
typedef NoiseModelFactor2<VALUE, VALUE> MotionFactor;
typedef NoiseModelFactor1<VALUE> MeasurementFactor;
protected:
T x_; // linearization point
JacobianFactor::shared_ptr priorFactor_; // Gaussian density on x_
static T solve_(const GaussianFactorGraph& linearFactorGraph, const Values& linearizationPoints,
Key x, JacobianFactor::shared_ptr* newPrior);
public:
/// @name Standard Constructors
/// @{
ExtendedKalmanFilter(Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial);
/// @}
/// @name Testable
/// @{
/// print
void print(const std::string& s = "") const {
std::cout << s << "\n";
x_.print(s + "x");
priorFactor_->print(s + "density");
}
/// @}
/// @name Interface
/// @{
/**
* 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
* stable Kalman Filter implementation.
*
* The Kalman Filter relies on two models: a motion model that predicts the next state using
* the current state, and a measurement model that predicts the measurement value at a given
* state. Because these two models are situation-dependent, base classes for each have been
* provided above, from which the user must derive a class and incorporate the actual modeling
* equations.
*
* The class provides a "predict" and "update" function to perform these steps independently.
* TODO: a "predictAndUpdate" that combines both steps for some computational savings.
* \nosubgrouping
* 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);
template<class VALUE>
class ExtendedKalmanFilter {
/**
* 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);
// Check that VALUE type is a testable Manifold
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
BOOST_CONCEPT_ASSERT((IsManifold<VALUE>));
/// Return current predictive (if called after predict)/posterior (if called after update)
const JacobianFactor::shared_ptr Density() const {
return priorFactor_;
}
public:
/// @}
};
typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
typedef VALUE T;
typedef NoiseModelFactor2<VALUE, VALUE> MotionFactor;
typedef NoiseModelFactor1<VALUE> MeasurementFactor;
protected:
T x_; // linearization point
JacobianFactor::shared_ptr priorFactor_; // density
T solve_(const GaussianFactorGraph& linearFactorGraph,
const Values& linearizationPoints,
Key x, JacobianFactor::shared_ptr& newPrior) const;
public:
/// @name Standard Constructors
/// @{
ExtendedKalmanFilter(Key key_initial, T x_initial,
noiseModel::Gaussian::shared_ptr P_initial);
/// @}
/// @name Testable
/// @{
/// print
void print(const std::string& s="") const {
std::cout << s << "\n";
x_.print(s+"x");
priorFactor_->print(s+"density");
}
/// @}
/// @name Advanced Interface
/// @{
///TODO: comment
T predict(const MotionFactor& motionFactor);
///TODO: comment
T update(const MeasurementFactor& measurementFactor);
/// @}
};
} // namespace
} // namespace
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>

7
gtsam/nonlinear/GaussNewtonOptimizer.cpp
View File

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

33
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;
if(debug) cout << "Getting affected factors for ";
if(debug) { BOOST_FOREACH(const Key key, keys) { cout << key << " "; } }
if(debug) cout << endl;
NonlinearFactorGraph allAffected;
FastSet<size_t> indices;
KeySet indices;
BOOST_FOREACH(const Key key, keys) {
const VariableIndex::Factors& factors(variableIndex_[key]);
indices.insert(factors.begin(), factors.end());
@ -197,7 +197,7 @@ GaussianFactorGraph::shared_ptr
ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySet& relinKeys) const
{
gttic(getAffectedFactors);
FastSet<size_t> candidates = getAffectedFactors(affectedKeys);
KeySet candidates = getAffectedFactors(affectedKeys);
gttoc(getAffectedFactors);
NonlinearFactorGraph nonlinearAffectedFactors;
@ -210,7 +210,7 @@ ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySe
gttic(check_candidates_and_linearize);
GaussianFactorGraph::shared_ptr linearized = boost::make_shared<GaussianFactorGraph>();
BOOST_FOREACH(size_t idx, candidates) {
BOOST_FOREACH(Key idx, candidates) {
bool inside = true;
bool useCachedLinear = params_.cacheLinearizedFactors;
BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) {
@ -464,9 +464,8 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
constraintGroups = *constrainKeys;
} else {
constraintGroups = FastMap<Key,int>();
const int group = observedKeys.size() < affectedFactorsVarIndex.size()
? 1 : 0;
BOOST_FOREACH(Key var, observedKeys)
const int group = observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0;
BOOST_FOREACH (Key var, observedKeys)
constraintGroups.insert(make_pair(var, group));
}
@ -766,7 +765,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
map<Key, vector<GaussianFactor::shared_ptr> > marginalFactors;
// Keep track of factors that get summarized by removing cliques
FastSet<size_t> factorIndicesToRemove;
KeySet factorIndicesToRemove;
// Keep track of variables removed in subtrees
KeySet leafKeysRemoved;
@ -828,10 +827,10 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
// get the childrens' marginals from any existing children, plus
// the marginals from the marginalFactors multimap, which come from any
// subtrees already marginalized out.
// Add child marginals and remove marginalized subtrees
GaussianFactorGraph graph;
FastSet<size_t> factorsInSubtreeRoot;
KeySet factorsInSubtreeRoot;
Cliques subtreesToRemove;
BOOST_FOREACH(const sharedClique& child, clique->children) {
// 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
// but do not involve frontal variables of any of its children.
// TODO: reuse cached linear factors
FastSet<size_t> factorsFromMarginalizedInClique_step1;
KeySet factorsFromMarginalizedInClique_step1;
BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) {
if(leafKeys.exists(frontal))
factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); }
// Remove any factors in subtrees that we're removing at this step
BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) {
BOOST_FOREACH(Key indexInClique, removedChild->conditional()->frontals()) {
BOOST_FOREACH(size_t factorInvolving, variableIndex_[indexInClique]) {
BOOST_FOREACH(Key factorInvolving, variableIndex_[indexInClique]) {
factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } }
// Create factor graph from factor indices
BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique_step1) {
@ -989,15 +988,15 @@ void ISAM2::updateDelta(bool forceFullSolve) const
gttic(Wildfire_update);
lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_, deltaNewton_, effectiveWildfireThreshold);
gttoc(Wildfire_update);
// Compute steepest descent step
const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient
Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, RgProd_); // Update RgProd
const VectorValues dx_u = Impl::ComputeGradientSearch(gradAtZero, RgProd_); // Compute gradient search point
// Clear replaced keys mask because now we've updated deltaNewton_ and RgProd_
deltaReplacedMask_.clear();
// Compute dogleg point
DoglegOptimizerImpl::IterationResult doglegResult(DoglegOptimizerImpl::Iterate(
*doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_, *this, nonlinearFactors_,
@ -1076,11 +1075,11 @@ VectorValues ISAM2::gradientAtZero() const
{
// Create result
VectorValues g;
// Sum up contributions for each clique
BOOST_FOREACH(const ISAM2::sharedClique& root, this->roots())
gradientAtZeroTreeAdder(root, g);
return g;
}

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