diff --git a/.gitignore b/.gitignore index d46bddd10..04e8e76d1 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,5 @@ /examples/Data/pose3example-rewritten.txt *.txt.user *.txt.user.6d59f0c +/python-build/ +*.pydevproject diff --git a/CMakeLists.txt b/CMakeLists.txt index fd11a6733..75c24f76d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,6 +64,8 @@ option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TB option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON) option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) +option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" ON) +option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries @@ -82,6 +84,10 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY) message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_BUILD_STATIC_LIBRARY are both enabled. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of GTSAM_BUILD_STATIC_LIBRARY.") endif() +if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4) + message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.") +endif() + # Flags for choosing default packaging tools set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator") set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") @@ -131,7 +137,7 @@ endif() if(NOT (${Boost_VERSION} LESS 105600)) message("Ignoring Boost restriction on optional lvalue assignment from rvalues") - add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES) + add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES -DBOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) endif() ############################################################################### @@ -277,7 +283,8 @@ endif() # Include boost - use 'BEFORE' so that a specific boost specified to CMake # takes precedence over a system-installed one. -include_directories(BEFORE ${Boost_INCLUDE_DIR}) +# Use 'SYSTEM' to ignore compiler warnings in Boost headers +include_directories(BEFORE SYSTEM ${Boost_INCLUDE_DIR}) # Add includes for source directories 'BEFORE' boost and any system include # paths so that the compiler uses GTSAM headers in our source directory instead @@ -304,10 +311,21 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") endif() endif() +# As of XCode 7, clang also complains about this +if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") + if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7.0) + add_definitions(-Wno-unused-local-typedefs) + endif() +endif() + if(GTSAM_ENABLE_CONSISTENCY_CHECKS) add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS) endif() +if(GTSAM_ALLOW_DEPRECATED_SINCE_V4) + add_definitions(-DGTSAM_ALLOW_DEPRECATED_SINCE_V4) +endif() + ############################################################################### # Add components @@ -336,6 +354,20 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX) add_subdirectory(matlab) endif() +# Python wrap +if (GTSAM_BUILD_PYTHON) + include(GtsamPythonWrap) + + # 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 + # comments on the next lines + + # wrap_and_install_python(gtsampy.h "${GTSAM_ADDITIONAL_LIBRARIES}" "") + add_subdirectory(python) + +endif() + # Build gtsam_unstable if (GTSAM_BUILD_UNSTABLE) add_subdirectory(gtsam_unstable) @@ -386,6 +418,8 @@ set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= # Print configuration variables message(STATUS "===============================================================") message(STATUS "================ Configuration Options ======================") +message(STATUS " CMAKE_CXX_COMPILER_ID type : ${CMAKE_CXX_COMPILER_ID}") +message(STATUS " CMAKE_CXX_COMPILER_VERSION : ${CMAKE_CXX_COMPILER_VERSION}") message(STATUS "Build flags ") print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ") print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all' ") @@ -446,10 +480,22 @@ print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default R print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") +print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") message(STATUS "MATLAB toolbox flags ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") + +message(STATUS "Python module flags ") + +if(GTSAM_PYTHON_WARNINGS) + message(STATUS " Build python module : No - dependencies missing") +else() + print_config_flag(${GTSAM_BUILD_PYTHON} "Build python module ") +endif() +if(GTSAM_BUILD_PYTHON) + message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}") +endif() message(STATUS "===============================================================") # Print warnings at the end @@ -462,6 +508,9 @@ endif() if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND) message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.") endif() +if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS) + message(WARNING "${GTSAM_PYTHON_WARNINGS}") +endif() # Include CPack *after* all flags include(CPack) diff --git a/THANKS b/THANKS index 9c06a5d28..f84cfa185 100644 --- a/THANKS +++ b/THANKS @@ -38,6 +38,10 @@ at Uni Zurich: * Christian Forster +at LAAS-CNRS + +* Ellon Paiva + Many thanks for your hard work!!!! Frank Dellaert diff --git a/cmake/FindNumPy.cmake b/cmake/FindNumPy.cmake new file mode 100644 index 000000000..eafed165e --- /dev/null +++ b/cmake/FindNumPy.cmake @@ -0,0 +1,102 @@ +# - Find the NumPy libraries +# This module finds if NumPy is installed, and sets the following variables +# indicating where it is. +# +# TODO: Update to provide the libraries and paths for linking npymath lib. +# +# NUMPY_FOUND - was NumPy found +# NUMPY_VERSION - the version of NumPy found as a string +# NUMPY_VERSION_MAJOR - the major version number of NumPy +# NUMPY_VERSION_MINOR - the minor version number of NumPy +# NUMPY_VERSION_PATCH - the patch version number of NumPy +# NUMPY_VERSION_DECIMAL - e.g. version 1.6.1 is 10601 +# NUMPY_INCLUDE_DIRS - path to the NumPy include files + +#============================================================================ +# Copyright 2012 Continuum Analytics, Inc. +# +# MIT License +# +# Permission is hereby granted, free of charge, to any person obtaining +# a copy of this software and associated documentation files +# (the "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to +# the following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR +# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +# OTHER DEALINGS IN THE SOFTWARE. +# +#============================================================================ + +# Finding NumPy involves calling the Python interpreter +if(NumPy_FIND_REQUIRED) + find_package(PythonInterp REQUIRED) +else() + find_package(PythonInterp) +endif() + +if(NOT PYTHONINTERP_FOUND) + set(NUMPY_FOUND FALSE) + return() +endif() + +execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c" + "import numpy as n; print(n.__version__); print(n.get_include());" + RESULT_VARIABLE _NUMPY_SEARCH_SUCCESS + OUTPUT_VARIABLE _NUMPY_VALUES_OUTPUT + ERROR_VARIABLE _NUMPY_ERROR_VALUE + OUTPUT_STRIP_TRAILING_WHITESPACE) + +if(NOT _NUMPY_SEARCH_SUCCESS MATCHES 0) + if(NumPy_FIND_REQUIRED) + message(FATAL_ERROR + "NumPy import failure:\n${_NUMPY_ERROR_VALUE}") + endif() + set(NUMPY_FOUND FALSE) + return() +endif() + +# Convert the process output into a list +string(REGEX REPLACE ";" "\\\\;" _NUMPY_VALUES ${_NUMPY_VALUES_OUTPUT}) +string(REGEX REPLACE "\n" ";" _NUMPY_VALUES ${_NUMPY_VALUES}) +# Just in case there is unexpected output from the Python command. +list(GET _NUMPY_VALUES -2 NUMPY_VERSION) +list(GET _NUMPY_VALUES -1 NUMPY_INCLUDE_DIRS) + +string(REGEX MATCH "^[0-9]+\\.[0-9]+\\.[0-9]+" _VER_CHECK "${NUMPY_VERSION}") +if("${_VER_CHECK}" STREQUAL "") + # The output from Python was unexpected. Raise an error always + # here, because we found NumPy, but it appears to be corrupted somehow. + message(FATAL_ERROR + "Requested version and include path from NumPy, got instead:\n${_NUMPY_VALUES_OUTPUT}\n") + return() +endif() + +# Make sure all directory separators are '/' +string(REGEX REPLACE "\\\\" "/" NUMPY_INCLUDE_DIRS ${NUMPY_INCLUDE_DIRS}) + +# Get the major and minor version numbers +string(REGEX REPLACE "\\." ";" _NUMPY_VERSION_LIST ${NUMPY_VERSION}) +list(GET _NUMPY_VERSION_LIST 0 NUMPY_VERSION_MAJOR) +list(GET _NUMPY_VERSION_LIST 1 NUMPY_VERSION_MINOR) +list(GET _NUMPY_VERSION_LIST 2 NUMPY_VERSION_PATCH) +string(REGEX MATCH "[0-9]*" NUMPY_VERSION_PATCH ${NUMPY_VERSION_PATCH}) +math(EXPR NUMPY_VERSION_DECIMAL + "(${NUMPY_VERSION_MAJOR} * 10000) + (${NUMPY_VERSION_MINOR} * 100) + ${NUMPY_VERSION_PATCH}") + +find_package_message(NUMPY + "Found NumPy: version \"${NUMPY_VERSION}\" ${NUMPY_INCLUDE_DIRS}" + "${NUMPY_INCLUDE_DIRS}${NUMPY_VERSION}") + +set(NUMPY_FOUND TRUE) + diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index c2cd7b449..43ae36929 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -34,19 +34,19 @@ if(NOT FIRST_PASS_DONE) 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} -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} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_C_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) - set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) - set(CMAKE_C_FLAGS_RELEASE "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) - set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) + 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 "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) - set(CMAKE_CXX_FLAGS_PROFILING "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) + 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) diff --git a/cmake/GtsamPythonWrap.cmake b/cmake/GtsamPythonWrap.cmake index d065a7828..714e37488 100644 --- a/cmake/GtsamPythonWrap.cmake +++ b/cmake/GtsamPythonWrap.cmake @@ -1,5 +1,5 @@ #Setup cache options -option(GTSAM_BUILD_PYTHON "Build Python wrapper statically (increases build time)" OFF) +set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version") set(GTSAM_BUILD_PYTHON_FLAGS "" CACHE STRING "Extra flags for running Matlab PYTHON compilation") set(GTSAM_PYTHON_INSTALL_PATH "" CACHE PATH "Python toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/borg/python") if(NOT GTSAM_PYTHON_INSTALL_PATH) @@ -8,13 +8,13 @@ endif() #Author: Paul Furgale Modified by Andrew Melim function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY) - # Boost - find_package(Boost COMPONENTS python filesystem system REQUIRED) - include_directories(${Boost_INCLUDE_DIRS}) + # # Boost + # find_package(Boost COMPONENTS python filesystem system REQUIRED) + # include_directories(${Boost_INCLUDE_DIRS}) - # Find Python - FIND_PACKAGE(PythonLibs 2.7 REQUIRED) - INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS}) + # # Find Python + # FIND_PACKAGE(PythonLibs 2.7 REQUIRED) + # INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS}) IF(APPLE) # The apple framework headers don't include the numpy headers for some reason. @@ -36,23 +36,46 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY) ENDIF() ENDIF(APPLE) + if(MSVC) + add_library(${moduleName}_python MODULE ${ARGN}) + set_target_properties(${moduleName}_python PROPERTIES + OUTPUT_NAME ${moduleName}_python + CLEAN_DIRECT_OUTPUT 1 + VERSION 1 + SOVERSION 0 + SUFFIX ".pyd") + target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp - # Create a static library version - add_library(${TARGET_NAME} SHARED ${ARGN}) + set(PYLIB_OUTPUT_FILE $) + message(${PYLIB_OUTPUT_FILE}) + get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE) + set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.pyd) - target_link_libraries(${TARGET_NAME} ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} gtsam-shared) - set_target_properties(${TARGET_NAME} PROPERTIES - OUTPUT_NAME ${TARGET_NAME} - CLEAN_DIRECT_OUTPUT 1 - VERSION 1 - SOVERSION 0) + ELSE() + # Create a shared library + add_library(${moduleName}_python SHARED ${generated_cpp_file}) + set_target_properties(${moduleName}_python PROPERTIES + OUTPUT_NAME ${moduleName}_python + CLEAN_DIRECT_OUTPUT 1) + target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp + # On OSX and Linux, the python library must end in the extension .so. Build this + # filename here. + get_property(PYLIB_OUTPUT_FILE TARGET ${moduleName}_python PROPERTY LOCATION) + set(PYLIB_OUTPUT_FILE $) + message(${PYLIB_OUTPUT_FILE}) + get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE) + set(PYLIB_SO_NAME lib${moduleName}_python.so) + ENDIF(MSVC) - # On OSX and Linux, the python library must end in the extension .so. Build this - # filename here. - get_property(PYLIB_OUTPUT_FILE TARGET ${TARGET_NAME} PROPERTY LOCATION) - get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE) - set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.so) + # Installs the library in the gtsam folder, which is used by setup.py to create the gtsam package + set(PYTHON_MODULE_DIRECTORY ${CMAKE_SOURCE_DIR}/python/gtsam) + # Cause the library to be output in the correct directory. + add_custom_command(TARGET ${moduleName}_python + POST_BUILD + COMMAND cp -v ${PYLIB_OUTPUT_FILE} ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME} + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + COMMENT "Copying library files to python directory" ) # Cause the library to be output in the correct directory. add_custom_command(TARGET ${TARGET_NAME} @@ -64,4 +87,16 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY) get_directory_property(AMCF ADDITIONAL_MAKE_CLEAN_FILES) list(APPEND AMCF ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME}) set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${AMCF}") -endfunction(wrap_python) \ No newline at end of file +endfunction(wrap_python) + +# Macro to get list of subdirectories +macro(SUBDIRLIST result curdir) + file(GLOB children RELATIVE ${curdir} ${curdir}/*) + set(dirlist "") + foreach(child ${children}) + if(IS_DIRECTORY ${curdir}/${child}) + list(APPEND dirlist ${child}) + endif() + endforeach() + set(${result} ${dirlist}) +endmacro() diff --git a/doc/.gitignore b/doc/.gitignore index ac7af2e80..8a3139177 100644 --- a/doc/.gitignore +++ b/doc/.gitignore @@ -1 +1,3 @@ /html/ +*.lyx~ +*.bib~ diff --git a/doc/ImuFactor.lyx b/doc/ImuFactor.lyx new file mode 100644 index 000000000..d9cd35584 --- /dev/null +++ b/doc/ImuFactor.lyx @@ -0,0 +1,1399 @@ +#LyX 2.0 created this file. For more info see http://www.lyx.org/ +\lyxformat 413 +\begin_document +\begin_header +\textclass article +\use_default_options true +\maintain_unincluded_children false +\language english +\language_package default +\inputencoding auto +\fontencoding global +\font_roman default +\font_sans default +\font_typewriter default +\font_default_family default +\use_non_tex_fonts false +\font_sc false +\font_osf false +\font_sf_scale 100 +\font_tt_scale 100 + +\graphics default +\default_output_format default +\output_sync 0 +\bibtex_command default +\index_command default +\paperfontsize 11 +\spacing single +\use_hyperref false +\papersize default +\use_geometry true +\use_amsmath 1 +\use_esint 1 +\use_mhchem 1 +\use_mathdots 1 +\cite_engine basic +\use_bibtopic false +\use_indices false +\paperorientation portrait +\suppress_date false +\use_refstyle 1 +\index Index +\shortcut idx +\color #008000 +\end_index +\leftmargin 3cm +\topmargin 3cm +\rightmargin 3cm +\bottommargin 3cm +\secnumdepth 3 +\tocdepth 3 +\paragraph_separation indent +\paragraph_indentation default +\quotes_language english +\papercolumns 1 +\papersides 1 +\paperpagestyle default +\tracking_changes false +\output_changes false +\html_math_output 0 +\html_css_as_file 0 +\html_be_strict false +\end_header + +\begin_body + +\begin_layout Title +The new IMU Factor +\end_layout + +\begin_layout Author +Frank Dellaert +\end_layout + +\begin_layout Standard +\begin_inset CommandInset include +LatexCommand include +filename "macros.lyx" + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\renewcommand{\sothree}{\mathfrak{so(3)}} +{\mathfrak{so(3)}} +\end_inset + + +\end_layout + +\begin_layout Subsubsection* +Navigation States +\end_layout + +\begin_layout Standard +Let us assume a setup where frames with image and/or laser measurements + are processed at some fairly low rate, e.g., 10 Hz. +\end_layout + +\begin_layout Standard +We define the state of the vehicle at those times as attitude, position, + and velocity. + These three quantities are jointly referred to as a NavState +\begin_inset Formula $X_{b}^{n}\define\left\{ R_{b}^{n},P_{b}^{n},V_{b}^{n}\right\} $ +\end_inset + +, where the superscript +\begin_inset Formula $n$ +\end_inset + + denotes the +\emph on +navigation frame +\emph default +, and +\begin_inset Formula $b$ +\end_inset + + the +\emph on +body frame +\emph default +. + For simplicity, we drop these indices below where clear from context. +\end_layout + +\begin_layout Subsubsection* +Vector Fields and Differential Equations +\end_layout + +\begin_layout Standard +We need a way to describe the evolution of a NavState over time. + The NavState lives in a 9-dimensional manifold +\begin_inset Formula $M$ +\end_inset + +, defined by the orthonormality constraints on +\begin_inset Formula $\Rone$ +\end_inset + +. + For a NavState +\begin_inset Formula $X$ +\end_inset + + evolving over time we can write down a differential equation +\begin_inset Formula +\begin{equation} +\dot{X}(t)=F(t,X)\label{eq:diffeqM} +\end{equation} + +\end_inset + +where +\begin_inset Formula $F$ +\end_inset + + is a time-varying +\series bold +vector field +\series default + on +\begin_inset Formula $M$ +\end_inset + +, defined as a mapping from +\begin_inset Formula $\Rone\times M$ +\end_inset + + to tangent vectors at +\begin_inset Formula $X$ +\end_inset + +. + A +\series bold +tangent vector +\series default + at +\begin_inset Formula $X$ +\end_inset + + is defined as the derivative of a trajectory at +\begin_inset Formula $X$ +\end_inset + +, and for the NavState manifold this will be a triplet +\begin_inset Formula +\[ +\left[\dot{R}(t,X),\dot{P}(t,X),\dot{V}(t,X)\right]\in\sothree\times\Rthree\times\Rthree +\] + +\end_inset + +where we use square brackets to indicate a tangent vector. + The space of all tangent vectors at +\begin_inset Formula $X$ +\end_inset + + is denoted by +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $T_{X}M$ +\end_inset + + +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\uuline default +\uwave default +\noun default +\color inherit +, and hence +\begin_inset Formula $F(t,X)\in T_{X}M$ +\end_inset + +. + For example, if the state evolves along a constant velocity trajectory +\begin_inset Formula +\[ +X(t)=\left\{ R_{0},P_{0}+V_{0}t,V_{0}\right\} +\] + +\end_inset + +then the differential equation describing the trajectory is +\begin_inset Formula +\[ +\dot{X}(t)=\left[0_{3x3},V_{0},0_{3x1}\right],\,\,\,\,\, X(0)=\left\{ R_{0},P_{0},V_{0}\right\} +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +Valid vector fields on a NavState manifold are special, in that the attitude + and velocity derivatives can be arbitrary functions of X and t, but the + derivative of position is constrained to be equal to the current velocity + +\begin_inset Formula $V(t)$ +\end_inset + +: +\begin_inset Formula +\begin{equation} +\dot{X}(t)=\left[\dot{R}(X,t),V(t),\dot{V}(X,t)\right]\label{eq:validField} +\end{equation} + +\end_inset + +Suppose we are given the +\series bold +body angular velocity +\series default + +\begin_inset Formula $\omega^{b}(t)$ +\end_inset + + and non-gravity +\series bold +acceleration +\series default + +\begin_inset Formula $a^{b}(t)$ +\end_inset + + in the body frame. + We know (from Murray84book) that the derivative of +\begin_inset Formula $R$ +\end_inset + + can be written as +\begin_inset Formula +\[ +\dot{R}(X,t)=R(t)\Skew{\omega^{b}(t)} +\] + +\end_inset + +where +\begin_inset Formula $\Skew{\theta}\in so(3)$ +\end_inset + + is the skew-symmetric matrix corresponding to +\begin_inset Formula $\theta$ +\end_inset + +, and hence the resulting exact vector field is +\begin_inset Formula +\begin{equation} +\dot{X}(t)=\left[\dot{R}(X,t),V(t),\dot{V}(X,t)\right]=\left[R(t)\Skew{\omega^{b}(t)},V(t),g+R(t)a^{b}(t)\right]\label{eq:bodyField} +\end{equation} + +\end_inset + + +\end_layout + +\begin_layout Subsubsection* +Local Coordinates +\end_layout + +\begin_layout Standard +Optimization on manifolds relies crucially on the concept of +\series bold +local coordinates +\series default +. + For example, when optimizing over the rotations +\begin_inset Formula $\SOthree$ +\end_inset + + starting from an initial estimate +\begin_inset Formula $R_{0}$ +\end_inset + +, we define a local map +\begin_inset Formula $\Phi_{R_{0}}$ +\end_inset + + from +\begin_inset Formula $\theta\in\Rthree$ +\end_inset + + to a neighborhood of +\begin_inset Formula $\SOthree$ +\end_inset + + centered around +\begin_inset Formula $R_{0}$ +\end_inset + +, +\begin_inset Formula +\[ +\Phi_{R_{0}}(\theta)=R_{0}\exp\left(\Skew{\theta}\right) +\] + +\end_inset + +where +\begin_inset Formula $\exp$ +\end_inset + + is the matrix exponential, given by +\begin_inset Formula +\begin{equation} +\exp\left(\Skew{\theta}\right)=\sum_{k=0}^{\infty}\frac{1}{k!}\Skew{\theta}^{k}\label{eq:expm} +\end{equation} + +\end_inset + +which for +\begin_inset Formula $\SOthree$ +\end_inset + + can be efficiently computed in closed form. +\end_layout + +\begin_layout Standard +The local coordinates +\begin_inset Formula $\theta$ +\end_inset + + are isomorphic to tangent vectors at +\emph on + +\begin_inset Formula $R_{0}$ +\end_inset + + +\emph default +. + To see this, define +\begin_inset Formula $\theta=\omega t$ +\end_inset + + and note that +\begin_inset Formula +\[ +\frac{d\Phi_{R_{0}}\left(\omega t\right)}{dt}\biggr\vert_{t=0}=\frac{dR_{0}\exp\left(\Skew{\omega t}\right)}{dt}\biggr\vert_{t=0}=R_{0}\Skew{\omega} +\] + +\end_inset + +Hence, the 3-vector +\begin_inset Formula $\omega$ +\end_inset + + defines a direction of travel on the +\begin_inset Formula $\SOthree$ +\end_inset + + manifold, but does so in the local coordinate frame define by +\begin_inset Formula $R_{0}$ +\end_inset + +. +\end_layout + +\begin_layout Standard +A similar story holds in +\begin_inset Formula $\SEthree$ +\end_inset + +: we define local coordinates +\begin_inset Formula $\xi=\left[\omega t,vt\right]\in\Rsix$ +\end_inset + + and a mapping +\begin_inset Formula +\[ +\Phi_{T_{0}}(\xi)=T_{0}\exp\xihat +\] + +\end_inset + +where +\begin_inset Formula $\xihat\in\sethree$ +\end_inset + + is defined as +\begin_inset Formula +\[ +\xihat=\left[\begin{array}{cc} +\Skew{\omega} & v\\ +0 & 0 +\end{array}\right]t +\] + +\end_inset + +and the 6-vectors +\begin_inset Formula $\xi$ +\end_inset + + are mapped to tangent vectors +\begin_inset Formula $T_{0}\xihat$ +\end_inset + + at +\begin_inset Formula $T_{0}$ +\end_inset + +. +\end_layout + +\begin_layout Subsubsection* +Derivative of The Local Coordinate Mapping +\end_layout + +\begin_layout Standard +For the local coordinate mapping +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $\Phi_{R_{0}}\left(\theta\right)$ +\end_inset + + +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\uuline default +\uwave default +\noun default +\color inherit + in +\begin_inset Formula $\SOthree$ +\end_inset + + we can define a +\begin_inset Formula $3\times3$ +\end_inset + + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none +Jacobian +\begin_inset Formula $H(\theta)$ +\end_inset + + that models the effect of an incremental change +\begin_inset Formula $\delta$ +\end_inset + + to the local coordinates: +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\uuline default +\uwave default +\noun default +\color inherit + +\begin_inset Formula +\begin{equation} +\Phi_{R_{0}}\left(\theta+\delta\right)\approx\Phi_{R_{0}}\left(\theta\right)\,\exp\left(\Skew{H(\theta)\delta}\right)=\Phi_{\Phi_{R_{0}}\left(\theta\right)}\left(H(\theta)\delta\right)\label{eq:push_exp} +\end{equation} + +\end_inset + +This Jacobian depends only on +\begin_inset Formula $\theta$ +\end_inset + + and, for the case of +\begin_inset Formula $\SOthree$ +\end_inset + +, is given by a formula similar to the matrix exponential map, +\begin_inset Formula +\[ +H(\theta)=\sum_{k=0}^{\infty}\frac{(-1)^{k}}{(k+1)!}\Skew{\theta}^{k} +\] + +\end_inset + +which can also be computed in closed form. + In particular, +\begin_inset Formula $H(0)=I_{3\times3}$ +\end_inset + + at the base +\begin_inset Formula $R_{0}$ +\end_inset + +. +\end_layout + +\begin_layout Subsubsection* +Numerical Integration in Local Coordinates +\end_layout + +\begin_layout Standard +Inspired by the paper +\begin_inset Quotes eld +\end_inset + +Lie Group Methods +\begin_inset Quotes erd +\end_inset + + by Iserles et al. + +\begin_inset CommandInset citation +LatexCommand cite +key "Iserles00an" + +\end_inset + +, when we have a differential equation on +\begin_inset Formula $\SOthree$ +\end_inset + +, +\begin_inset Formula +\begin{equation} +\dot{R}(t)=F(R,t),\,\,\,\, R(0)=R_{0}\label{eq:diffSo3} +\end{equation} + +\end_inset + +we can transfer it to a differential equation in the 3-dimensional local + coordinate space. + To do so, we model the solution to +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:diffSo3" + +\end_inset + + as +\begin_inset Formula +\[ +R(t)=\Phi_{R_{0}}(\theta(t)) +\] + +\end_inset + +To find an expression for +\begin_inset Formula $\dot{\theta}(t)$ +\end_inset + +, create a trajectory +\begin_inset Formula $\gamma(\delta)$ +\end_inset + + that passes through +\begin_inset Formula $R(t)$ +\end_inset + + for +\begin_inset Formula $\delta=0$ +\end_inset + +, and moves +\begin_inset Formula $\theta(t)$ +\end_inset + + along the direction +\begin_inset Formula $\dot{\theta}(t)$ +\end_inset + +: +\begin_inset Formula +\[ +\gamma(\delta)=R(t+\delta)=\Phi_{R_{0}}\left(\theta(t)+\dot{\theta}(t)\delta\right)\approx\Phi_{R(t)}\left(H(\theta)\dot{\theta}(t)\delta\right) +\] + +\end_inset + +Taking the derivative for +\begin_inset Formula $\delta=0$ +\end_inset + + we obtain +\begin_inset Formula +\[ +\dot{R}(t)=\frac{d\gamma(\delta)}{d\delta}\biggr\vert_{\delta=0}=\frac{d\Phi_{R(t)}\left(H(\theta)\dot{\theta}(t)\delta\right)}{d\delta}\biggr\vert_{\delta=0}=R(t)\Skew{H(\theta)\dot{\theta}(t)} +\] + +\end_inset + +Comparing this to +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:diffSo3" + +\end_inset + + we obtain a differential equation for +\begin_inset Formula $\theta(t)$ +\end_inset + +: +\begin_inset Formula +\[ +\dot{\theta}(t)=H(\theta)^{-1}\left\{ R(t)^{T}F(R,t)\right\} \check{},\,\,\,\,\theta(0)=0_{3\times1} +\] + +\end_inset + +In other words, the vector field +\begin_inset Formula $F(R,t)$ +\end_inset + + is rotated to the local frame, the inverse hat operator is applied to get + a 3-vector, which is then corrected by +\begin_inset Formula $H(\theta)^{-1}$ +\end_inset + + away from +\begin_inset Formula $\theta=0$ +\end_inset + +. +\end_layout + +\begin_layout Subsubsection* +Retractions +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}} +{\mathfrak{\mathbb{R}^{9}}} +\end_inset + + +\end_layout + +\begin_layout Standard +Note that the use of the exponential map in local coordinate mappings is + not obligatory, even in the context of Lie groups. + Often it is computationally expedient to use mappings that are easier to + compute, but yet induce the same tangent vector at +\begin_inset Formula $T_{0}.$ +\end_inset + + Mappings that satisfy this constraint are collectively known as +\series bold +retractions +\series default +. + For example, for +\begin_inset Formula $\SEthree$ +\end_inset + + one could use the retraction +\begin_inset Formula $\mathcal{R}_{T_{0}}:\Rsix\rightarrow\SEthree$ +\end_inset + + +\begin_inset Formula +\[ +\mathcal{R}_{T_{0}}\left(\xi\right)=T_{0}\left\{ \exp\left(\Skew{\omega t}\right),vt\right\} =\left\{ \Phi_{R_{0}}\left(\omega t\right),P_{0}+R_{0}vt\right\} +\] + +\end_inset + +This trajectory describes a linear path in position while the frame rotates, + as opposed to the helical path traced out by the exponential map. + The tangent vector at +\begin_inset Formula $T_{0}$ +\end_inset + + can be computed as +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +\frac{d\mathcal{R}_{T_{0}}\left(\xi\right)}{dt}\biggr\vert_{t=0}=\left[R_{0}\Skew{\omega},R_{0}v\right] +\] + +\end_inset + +which is identical to the one induced by +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $\Phi_{T_{0}}(\xi)=T_{0}\exp\xihat$ +\end_inset + +. +\end_layout + +\begin_layout Standard +The NavState manifold is not a Lie group like +\begin_inset Formula $\SEthree$ +\end_inset + +, but we can easily define a retraction that behaves similarly to the one + for +\begin_inset Formula $\SEthree$ +\end_inset + +, while treating velocities the same way as positions: +\begin_inset Formula +\[ +\mathcal{R}_{X_{0}}(\zeta)=\left\{ \Phi_{R_{0}}\left(\omega t\right),P_{0}+R_{0}vt,V_{0}+R_{0}at\right\} +\] + +\end_inset + +Here +\begin_inset Formula $\zeta=\left[\omega t,vt,at\right]$ +\end_inset + + is a 9-vector, with respectively angular, position, and velocity components. + The tangent vector at +\begin_inset Formula $X_{0}$ +\end_inset + + is +\begin_inset Formula +\[ +\frac{d\mathcal{R}_{X_{0}}(\zeta)}{dt}\biggr\vert_{t=0}=\left[R_{0}\Skew{\omega},R_{0}v,R_{0}a\right] +\] + +\end_inset + +and the isomorphism between +\begin_inset Formula $\Rnine$ +\end_inset + + and +\begin_inset Formula $T_{X_{0}}M$ +\end_inset + + is +\begin_inset Formula $\zeta\rightarrow\left[R_{0}\Skew{\omega t},R_{0}vt,R_{0}at\right]$ +\end_inset + +. +\end_layout + +\begin_layout Subsubsection* +Integration in Local Coordinates +\end_layout + +\begin_layout Standard +We now proceed exactly as before to describe the evolution of the NavState + in local coordinates. + Let us model the solution of the differential equation +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:diffeqM" + +\end_inset + + as a trajectory +\begin_inset Formula $\zeta(t)=\left[\theta(t),p(t),v(t)\right]$ +\end_inset + +, with +\begin_inset Formula $\zeta(0)=0$ +\end_inset + +, in the local coordinate frame anchored at +\begin_inset Formula $X_{0}$ +\end_inset + +. + Note that this trajectory evolves away from +\begin_inset Formula $X_{0}$ +\end_inset + +, and we use the symbols +\begin_inset Formula $\theta$ +\end_inset + +, +\begin_inset Formula $p$ +\end_inset + +, and +\begin_inset Formula $v$ +\end_inset + + to indicate that these are integrated rather than differential quantities. + With that, we have +\begin_inset Formula +\begin{equation} +X(t)=\mathcal{R}_{X_{0}}(\zeta(t))=\left\{ \Phi_{R_{0}}\left(\theta(t)\right),P_{0}+R_{0}p(t),V_{0}+R_{0}v(t)\right\} \label{eq:scheme1} +\end{equation} + +\end_inset + +We can create a trajectory +\begin_inset Formula $\gamma(\delta)$ +\end_inset + + that passes through +\begin_inset Formula $X(t)$ +\end_inset + + for +\begin_inset Formula $\delta=0$ +\end_inset + + +\begin_inset Formula +\[ +\gamma(\delta)=X(t+\delta)=\left\{ \Phi_{R_{0}}\left(\theta(t)+\dot{\theta}(t)\delta\right),P_{0}+R_{0}\left\{ p(t)+\dot{p}(t)\delta\right\} ,V_{0}+R_{0}\left\{ v(t)+\dot{v}(t)\delta\right\} \right\} +\] + +\end_inset + +and taking the derivative for +\begin_inset Formula $\delta=0$ +\end_inset + + we obtain +\begin_inset Formula +\[ +\dot{X}(t)=\frac{d\gamma(\delta)}{d\delta}\biggr\vert_{\delta=0}=\left[R(t)\Skew{H(\theta)\dot{\theta}(t)},R_{0}\,\dot{p}(t),R_{0}\,\dot{v}(t)\right] +\] + +\end_inset + +Comparing that with the vector field +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:bodyField" + +\end_inset + +, we have exact integration iff +\begin_inset Formula +\[ +\left[R(t)\Skew{H(\theta)\dot{\theta}(t)},R_{0}\,\dot{p}(t),R_{0}\,\dot{v}(t)\right]=\left[R(t)\Skew{\omega^{b}(t)},V(t),g+R(t)a^{b}(t)\right] +\] + +\end_inset + +Or, as another way to state this, if we solve the differential equations + for +\begin_inset Formula $\theta(t)$ +\end_inset + +, +\begin_inset Formula $p(t)$ +\end_inset + +, and +\begin_inset Formula $v(t)$ +\end_inset + + such that +\begin_inset Formula +\begin{eqnarray*} +\dot{\theta}(t) & = & H(\theta)^{-1}\,\omega^{b}(t)\\ +\dot{p}(t) & = & R_{0}^{T}\, V_{0}+v(t)\\ +\dot{v}(t) & = & R_{0}^{T}\, g+R_{b}^{0}(t)a^{b}(t) +\end{eqnarray*} + +\end_inset + +where +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $R_{b}^{0}(t)=R_{0}^{T}R(t)$ +\end_inset + + is the rotation of the body frame with respect to +\begin_inset Formula $R_{0}$ +\end_inset + +, and we have used +\begin_inset Formula $V(t)=V_{0}+R_{0}v(t)$ +\end_inset + +. +\end_layout + +\begin_layout Subsubsection* +Application: The New IMU Factor +\end_layout + +\begin_layout Standard +In the IMU factor, we need to predict the NavState +\begin_inset Formula $X_{j}$ +\end_inset + + from the current NavState +\begin_inset Formula $X_{i}$ +\end_inset + + and the IMU measurements in-between. + The above scheme suffers from a problem, which is that +\begin_inset Formula $X_{i}$ +\end_inset + + needs to be known in order to compensate properly for the initial velocity + and rotated gravity vector. + Hence, the idea of Lupton was to split up +\begin_inset Formula $v(t)$ +\end_inset + + into a gravity-induced part and an accelerometer part +\begin_inset Formula +\[ +v(t)=v_{g}(t)+v_{a}(t) +\] + +\end_inset + +evolving as +\begin_inset Formula +\begin{eqnarray*} +\dot{v}_{g}(t) & = & R_{i}^{T}\, g\\ +\dot{v}_{a}(t) & = & R_{b}^{i}(t)a^{b}(t) +\end{eqnarray*} + +\end_inset + +The solution for the first equation is simply +\begin_inset Formula $v_{g}(t)=R_{i}^{T}gt$ +\end_inset + +. + Similarly, we split the position +\begin_inset Formula $p(t)$ +\end_inset + + up in three parts +\begin_inset Formula +\[ +p(t)=p_{i}(t)+p_{g}(t)+p_{v}(t) +\] + +\end_inset + +evolving as +\begin_inset Formula +\begin{eqnarray*} +\dot{p}_{i}(t) & = & R_{i}^{T}\, V_{i}\\ +\dot{p}_{g}(t) & = & v_{g}(t)=R_{i}^{T}gt\\ +\dot{p}_{v}(t) & = & v_{a}(t) +\end{eqnarray*} + +\end_inset + +Here the solutions for the two first equations are simply +\begin_inset Formula +\begin{eqnarray*} +p_{i}(t) & = & R_{i}^{T}V_{i}t\\ +p_{g}(t) & = & R_{i}^{T}\frac{gt^{2}}{2} +\end{eqnarray*} + +\end_inset + +The recipe for the IMU factor is then, in summary. + Solve the ordinary differential equations +\begin_inset Formula +\begin{eqnarray*} +\dot{\theta}(t) & = & H(\theta(t))^{-1}\,\omega^{b}(t)\\ +\dot{p}_{v}(t) & = & v_{a}(t)\\ +\dot{v}_{a}(t) & = & R_{b}^{i}(t)a^{b}(t) +\end{eqnarray*} + +\end_inset + +starting from zero, up to time +\begin_inset Formula $t_{ij}$ +\end_inset + +, where +\begin_inset Formula $R_{b}^{i}(t)=\exp\Skew{\theta(t)}$ +\end_inset + + at all times. + Form the local coordinate vector as +\begin_inset Formula +\[ +\zeta(t_{ij})=\left[\theta(t_{ij}),p(t_{ij}),v(t_{ij})\right]=\left[\theta(t_{ij}),R_{i}^{T}V_{i}t_{ij}+R_{i}^{T}\frac{gt_{ij}^{2}}{2}+p_{v}(t_{ij}),R_{i}^{T}gt_{ij}+v_{a}(t_{ij})\right] +\] + +\end_inset + +Predict the NavState +\begin_inset Formula $X_{j}$ +\end_inset + + at time +\begin_inset Formula $t_{j}$ +\end_inset + + from +\begin_inset Formula +\[ +X_{j}=\mathcal{R}_{X_{i}}(\zeta(t_{ij}))=\left\{ \Phi_{R_{0}}\left(\theta(t_{ij})\right),P_{i}+V_{i}t_{ij}+\frac{gt_{ij}^{2}}{2}+R_{i}\, p_{v}(t_{ij}),V_{i}+gt_{ij}+R_{i}\, v_{a}(t_{ij})\right\} +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +Note that the predicted NavState +\begin_inset Formula $X_{j}$ +\end_inset + + depends on +\begin_inset Formula $X_{i}$ +\end_inset + +, but the integrated quantities +\begin_inset Formula $\theta(t)$ +\end_inset + +, +\begin_inset Formula $p_{v}(t)$ +\end_inset + +, and +\begin_inset Formula $v_{a}(t)$ +\end_inset + + do not. +\end_layout + +\begin_layout Subsubsection* +A Simple Euler Scheme +\end_layout + +\begin_layout Standard +To solve the differential equation we can use a simple Euler scheme: +\begin_inset Formula +\begin{eqnarray} +\theta_{k+1}=\theta_{k}+\dot{\theta}(t_{k})\Delta_{t} & = & \theta_{k}+H(\theta_{k})^{-1}\,\omega_{k}^{b}\Delta_{t}\label{eq:euler_theta-1}\\ +p_{k+1}=p_{k}+\dot{p}_{v}(t_{k})\Delta_{t} & = & p_{k}+v_{k}\Delta_{t}\label{eq:euler_p-1}\\ +v_{k+1}=v_{k}+\dot{v}_{a}(t_{k})\Delta_{t} & = & v_{k}+\exp\left(\Skew{\theta_{k}}\right)a_{k}^{b}\Delta_{t}\label{eq:euler_v-1} +\end{eqnarray} + +\end_inset + +where +\begin_inset Formula $\theta_{k}\define\theta(t_{k})$ +\end_inset + +, +\begin_inset Formula $p_{k}\define p_{v}(t_{k})$ +\end_inset + +, and +\begin_inset Formula $v_{k}\define v_{a}(t_{k})$ +\end_inset + +. + However, the position propagation can be done more accurately, by using + exact integration of the zero-order hold acceleration +\begin_inset Formula $a_{k}^{b}$ +\end_inset + +: +\begin_inset Formula +\begin{eqnarray} +\theta_{k+1} & = & \theta_{k}+H(\theta_{k})^{-1}\,\omega_{k}^{b}\Delta_{t}\label{eq:euler_theta}\\ +p_{k+1} & = & p_{k}+v_{k}\Delta_{t}+R_{k}a_{k}^{b}\frac{\Delta_{t}^{2}}{2}\label{eq:euler_p}\\ +v_{k+1} & = & v_{k}+R_{k}a_{k}^{b}\Delta_{t}\label{eq:euler_v} +\end{eqnarray} + +\end_inset + +where we defined the rotation matrix +\begin_inset Formula $R_{k}=\exp\left(\Skew{\theta_{k}}\right)$ +\end_inset + +. +\end_layout + +\begin_layout Subsubsection* +Noise Propagation +\end_layout + +\begin_layout Standard +Even when we assume uncorrelated noise on +\begin_inset Formula $\omega^{b}$ +\end_inset + + and +\begin_inset Formula $a^{b}$ +\end_inset + +, the noise on the final computed quantities will have a non-trivial covariance + structure, because the intermediate quantities +\begin_inset Formula $\theta_{k}$ +\end_inset + + and +\begin_inset Formula $v_{k}$ +\end_inset + + appear in multiple places. + To model the noise propagation, let us define +\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k}]$ +\end_inset + + and rewrite Eqns. + ( +\begin_inset CommandInset ref +LatexCommand ref +reference "eq:euler_theta" + +\end_inset + +- +\begin_inset CommandInset ref +LatexCommand ref +reference "eq:euler_v" + +\end_inset + +) as the non-linear function +\begin_inset Formula $f$ +\end_inset + + +\begin_inset Formula +\[ +\zeta_{k+1}=f\left(\zeta_{k},a_{k}^{b},\omega_{k}^{b}\right) +\] + +\end_inset + +Then the noise on +\begin_inset Formula $\zeta_{k+1}$ +\end_inset + + propagates as +\begin_inset Formula +\begin{equation} +\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}+C_{k}\Sigma_{\eta}^{gd}C_{k}\label{eq:prop} +\end{equation} + +\end_inset + +where +\begin_inset Formula $A_{k}$ +\end_inset + + is the +\begin_inset Formula $9\times9$ +\end_inset + + partial derivative of +\begin_inset Formula $f$ +\end_inset + + wrpt +\begin_inset Formula $\zeta$ +\end_inset + +, and +\begin_inset Formula $B_{k}$ +\end_inset + + and +\begin_inset Formula $C_{k}$ +\end_inset + + the respective +\begin_inset Formula $9\times3$ +\end_inset + + partial derivatives with respect to the measured quantities +\begin_inset Formula $a^{b}$ +\end_inset + + and +\begin_inset Formula $\omega^{b}$ +\end_inset + +. +\end_layout + +\begin_layout Standard +We start with the noise propagation on +\begin_inset Formula $\theta$ +\end_inset + +, which is independent of the other quantities. + Taking the derivative, we have +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t} +\] + +\end_inset + +It can be shown that for small +\begin_inset Formula $\theta_{k}$ +\end_inset + + we have +\begin_inset Formula +\[ +\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}-\frac{\Delta t}{2}\Skew{\omega_{k}^{b}} +\] + +\end_inset + +For the derivatives of +\begin_inset Formula $p_{k+1}$ +\end_inset + + and +\begin_inset Formula $v_{k+1}$ +\end_inset + + we need the derivative +\begin_inset Formula +\[ +\deriv{R_{k}a_{k}^{b}}{\theta_{k}}=R_{k}\Skew{-a_{k}^{b}}\deriv{R_{k}}{\theta_{k}}=R_{k}\Skew{-a_{k}^{b}}H(\theta_{k}) +\] + +\end_inset + +where we used +\begin_inset Formula +\[ +\deriv{\left(Ra\right)}R\approx R\Skew{-a} +\] + +\end_inset + +and the fact that the dependence of the rotation +\begin_inset Formula $R_{k}$ +\end_inset + + on +\begin_inset Formula $\theta_{k}$ +\end_inset + + is the already computed +\begin_inset Formula $H(\theta_{k})$ +\end_inset + +. + +\end_layout + +\begin_layout Standard +Putting all this together, we finally obtain +\begin_inset Formula +\[ +A_{k}\approx\left[\begin{array}{ccc} +I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}\\ +R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t}\\ +R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & & I_{3\times3} +\end{array}\right] +\] + +\end_inset + +The other partial derivatives are simply +\begin_inset Formula +\[ +B_{k}=\left[\begin{array}{c} +0_{3\times3}\\ +R_{k}\frac{\Delta_{t}}{2}^{2}\\ +R_{k}\Delta_{t} +\end{array}\right],\,\,\,\, C_{k}=\left[\begin{array}{c} +H(\theta_{k})^{-1}\Delta_{t}\\ +0_{3\times3}\\ +0_{3\times3} +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset CommandInset bibtex +LatexCommand bibtex +bibfiles "refs" +options "plain" + +\end_inset + + +\end_layout + +\end_body +\end_document diff --git a/doc/ImuFactor.pdf b/doc/ImuFactor.pdf new file mode 100644 index 000000000..f5a25f54f Binary files /dev/null and b/doc/ImuFactor.pdf differ diff --git a/doc/LieGroups.lyx b/doc/LieGroups.lyx index adf62314c..0d194b31d 100644 --- a/doc/LieGroups.lyx +++ b/doc/LieGroups.lyx @@ -76,335 +76,10 @@ Frank Dellaert \end_layout \begin_layout Standard -\begin_inset Note Comment -status open +\begin_inset CommandInset include +LatexCommand include +filename "macros.lyx" -\begin_layout Plain Layout -Derivatives -\end_layout - -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset FormulaMacro -\newcommand{\deriv}[2]{\frac{\partial#1}{\partial#2}} -{\frac{\partial#1}{\partial#2}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\at}[2]{#1\biggr\rvert_{#2}} -{#1\biggr\rvert_{#2}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} } -{\at{\deriv{#1}{#2}}{#3}} -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset Note Comment -status open - -\begin_layout Plain Layout -Lie Groups -\end_layout - -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset FormulaMacro -\newcommand{\xhat}{\hat{x}} -{\hat{x}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\yhat}{\hat{y}} -{\hat{y}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\Ad}[1]{Ad_{#1}} -{Ad_{#1}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\AAdd}[1]{\mathbf{\mathop{Ad}}{}_{#1}} -{\mathbf{\mathop{Ad}}{}_{#1}} -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset FormulaMacro -\newcommand{\define}{\stackrel{\Delta}{=}} -{\stackrel{\Delta}{=}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\gg}{\mathfrak{g}} -{\mathfrak{g}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\Rn}{\mathbb{R}^{n}} -{\mathbb{R}^{n}} -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset Note Comment -status open - -\begin_layout Plain Layout -SO(2), 1 -\end_layout - -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset FormulaMacro -\newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}} -{\mathfrak{\mathbb{R}^{2}}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\SOtwo}{SO(2)} -{SO(2)} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\sotwo}{\mathfrak{so(2)}} -{\mathfrak{so(2)}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\that}{\hat{\theta}} -{\hat{\theta}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\skew}[1]{[#1]_{+}} -{[#1]_{+}} -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset Note Comment -status open - -\begin_layout Plain Layout -SE(2), 3 -\end_layout - -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset FormulaMacro -\newcommand{\SEtwo}{SE(2)} -{SE(2)} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\setwo}{\mathfrak{se(2)}} -{\mathfrak{se(2)}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\Skew}[1]{[#1]_{\times}} -{[#1]_{\times}} -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset Note Comment -status open - -\begin_layout Plain Layout -SO(3), 3 -\end_layout - -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset FormulaMacro -\newcommand{\Rthree}{\mathfrak{\mathbb{R}^{3}}} -{\mathfrak{\mathbb{R}^{3}}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\SOthree}{SO(3)} -{SO(3)} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\sothree}{\mathfrak{so(3)}} -{\mathfrak{so(3)}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\what}{\hat{\omega}} -{\hat{\omega}} -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset Note Comment -status open - -\begin_layout Plain Layout -SE(3),6 -\end_layout - -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset FormulaMacro -\newcommand{\Rsix}{\mathfrak{\mathbb{R}^{6}}} -{\mathfrak{\mathbb{R}^{6}}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\SEthree}{SE(3)} -{SE(3)} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\sethree}{\mathfrak{se(3)}} -{\mathfrak{se(3)}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\xihat}{\hat{\xi}} -{\hat{\xi}} -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset Note Comment -status open - -\begin_layout Plain Layout -Aff(2),6 -\end_layout - -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset FormulaMacro -\newcommand{\Afftwo}{Aff(2)} -{Aff(2)} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\afftwo}{\mathfrak{aff(2)}} -{\mathfrak{aff(2)}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\aa}{a} -{a} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\ahat}{\hat{a}} -{\hat{a}} -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset Note Comment -status collapsed - -\begin_layout Plain Layout -SL(3),8 -\end_layout - -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset FormulaMacro -\newcommand{\SLthree}{SL(3)} -{SL(3)} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\slthree}{\mathfrak{sl(3)}} -{\mathfrak{sl(3)}} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\hh}{h} -{h} -\end_inset - - -\begin_inset FormulaMacro -\newcommand{\hhat}{\hat{h}} -{\hat{h}} \end_inset diff --git a/doc/macros.lyx b/doc/macros.lyx index 1e57e1675..0d8429c4a 100644 --- a/doc/macros.lyx +++ b/doc/macros.lyx @@ -1,42 +1,60 @@ -#LyX 1.6.5 created this file. For more info see http://www.lyx.org/ -\lyxformat 345 +#LyX 2.0 created this file. For more info see http://www.lyx.org/ +\lyxformat 413 \begin_document \begin_header \textclass article \use_default_options true +\maintain_unincluded_children false \language english +\language_package default \inputencoding auto +\fontencoding global \font_roman default \font_sans default \font_typewriter default \font_default_family default +\use_non_tex_fonts false \font_sc false \font_osf false \font_sf_scale 100 \font_tt_scale 100 \graphics default +\default_output_format default +\output_sync 0 +\bibtex_command default +\index_command default \paperfontsize default \use_hyperref false \papersize default \use_geometry false \use_amsmath 1 \use_esint 1 +\use_mhchem 1 +\use_mathdots 0 \cite_engine basic \use_bibtopic false +\use_indices false \paperorientation portrait +\suppress_date false +\use_refstyle 0 +\index Index +\shortcut idx +\color #008000 +\end_index \secnumdepth 3 \tocdepth 3 \paragraph_separation indent -\defskip medskip +\paragraph_indentation default \quotes_language english \papercolumns 1 \papersides 1 \paperpagestyle default \tracking_changes false \output_changes false -\author "" -\author "" +\html_math_output 0 +\html_css_as_file 0 +\html_be_strict false \end_header \begin_body @@ -62,14 +80,14 @@ Derivatives \begin_inset FormulaMacro -\newcommand{\at}[2]{#1\biggr\rvert_{#2}} -{#1\biggr\rvert_{#2}} +\newcommand{\at}[1]{#1\biggr\vert_{\#2}} +{#1\biggr\vert_{\#2}} \end_inset \begin_inset FormulaMacro \newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} } -{\at{\deriv{#1}{#2}}{#3}} +{\at{\deriv{#1}{#2}}#3} \end_inset @@ -107,6 +125,15 @@ Lie Groups \end_inset +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\AAdd}[1]{\mathbf{\mathop{Ad}}{}_{#1}} +{\mathbf{\mathop{Ad}}{}_{#1}} +\end_inset + + \end_layout \begin_layout Standard @@ -144,6 +171,12 @@ SO(2) \end_layout \begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\Rone}{\mathfrak{\mathbb{R}}} +{\mathfrak{\mathbb{R}}} +\end_inset + + \begin_inset FormulaMacro \newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}} {\mathfrak{\mathbb{R}^{2}}} @@ -202,6 +235,12 @@ SE(2) \end_inset +\begin_inset FormulaMacro +\newcommand{\Skew}[1]{[#1]_{\times}} +{[#1]_{\times}} +\end_inset + + \end_layout \begin_layout Standard @@ -243,7 +282,7 @@ SO(3) \begin_inset FormulaMacro -\newcommand{\Skew}[1]{[#1]_{\times}} +\renewcommand{\Skew}[1]{[#1]_{\times}} {[#1]_{\times}} \end_inset @@ -288,6 +327,86 @@ SE(3) \end_inset +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +Aff(2),6 +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\Afftwo}{Aff(2)} +{Aff(2)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\afftwo}{\mathfrak{aff(2)}} +{\mathfrak{aff(2)}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\aa}{a} +{a} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\ahat}{\hat{a}} +{\hat{a}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status collapsed + +\begin_layout Plain Layout +SL(3),8 +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\SLthree}{SL(3)} +{SL(3)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\slthree}{\mathfrak{sl(3)}} +{\mathfrak{sl(3)}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\hh}{h} +{h} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\hhat}{\hat{h}} +{\hat{h}} +\end_inset + + \end_layout \end_body diff --git a/doc/math.lyx b/doc/math.lyx index d96f1f4c8..5571532f6 100644 --- a/doc/math.lyx +++ b/doc/math.lyx @@ -1237,21 +1237,28 @@ reference "eq:ApproximateObjective" \end_inset . - In particular, the notion of an exponential map allows us to define an - incremental transformation as tracing out a geodesic curve on the group - manifold along a certain + In particular, the notion of an exponential map allows us to define a mapping + from \series bold -tangent vector +local coordinates \series default \begin_inset Formula $\xi$ +\end_inset + + back to a neighborhood in +\begin_inset Formula $G$ +\end_inset + + around +\begin_inset Formula $a$ \end_inset , \begin_inset Formula -\[ -a\oplus\xi\define a\exp\left(\hat{\xi}\right) -\] +\begin{equation} +a\oplus\xi\define a\exp\left(\hat{\xi}\right)\label{eq:expmap} +\end{equation} \end_inset @@ -1263,11 +1270,12 @@ with \begin_inset Formula $n$ \end_inset --dimensional Lie group, +-dimensional Lie group. + Above, \begin_inset Formula $\hat{\xi}\in\mathfrak{g}$ \end_inset - the Lie algebra element corresponding to the vector + is the Lie algebra element corresponding to the vector \begin_inset Formula $\xi$ \end_inset @@ -1305,7 +1313,7 @@ For the Lie group \end_inset is denoted as -\begin_inset Formula $\omega$ +\begin_inset Formula $\omega t$ \end_inset and represents an angular displacement. @@ -1314,17 +1322,17 @@ For the Lie group \end_inset is a skew symmetric matrix denoted as -\begin_inset Formula $\Skew{\omega}\in\sothree$ +\begin_inset Formula $\Skew{\omega t}\in\sothree$ \end_inset , and is given by \begin_inset Formula \[ -\Skew{\omega}=\left[\begin{array}{ccc} +\Skew{\omega t}=\left[\begin{array}{ccc} 0 & -\omega_{z} & \omega_{y}\\ \omega_{z} & 0 & -\omega_{x}\\ -\omega_{y} & \omega_{x} & 0 -\end{array}\right] +\end{array}\right]t \] \end_inset @@ -1334,12 +1342,136 @@ Finally, the increment \end_inset corresponds to an incremental rotation -\begin_inset Formula $R\oplus\omega=Re^{\Skew{\omega}}$ +\begin_inset Formula $R\oplus\omega t=Re^{\Skew{\omega t}}$ \end_inset . \end_layout +\begin_layout Subsection +Local Coordinates vs. + Tangent Vectors +\end_layout + +\begin_layout Standard +In differential geometry, +\series bold +tangent vectors +\series default + +\begin_inset Formula $v\in T_{a}G$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + + are elements of the Lie algebra +\begin_inset Formula $\mathfrak{g}$ +\end_inset + +, and are defined as +\begin_inset Formula +\[ +v\define\Jac{\gamma(t)}t{t=0} +\] + +\end_inset + +where +\begin_inset Formula $\gamma$ +\end_inset + + is some curve that passes through +\begin_inset Formula $a$ +\end_inset + + at +\begin_inset Formula $t=0$ +\end_inset + +, i.e. + +\begin_inset Formula $\gamma(0)=a$ +\end_inset + +. + In particular, for any fixed local coordinate +\begin_inset Formula $\xi$ +\end_inset + + the map +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:expmap" + +\end_inset + + can be used to define a +\series bold +geodesic curve +\series default + on the group manifold defined by +\begin_inset Formula $\gamma:t\mapsto ae^{\widehat{t\xi}}$ +\end_inset + +, and the corresponding tangent vector is given by +\begin_inset Formula +\begin{equation} +\Jac{ae^{\widehat{t\xi}}}t{t=0}=a\xihat\label{eq:tangent-vector} +\end{equation} + +\end_inset + +This defines the mapping between local coordinates +\begin_inset Formula $\xi\in\Rn$ +\end_inset + + and actual tangent vectors +\begin_inset Formula $a\xihat\in g$ +\end_inset + +: the vector +\begin_inset Formula $\xi$ +\end_inset + + defines a direction of travel on the manifold, but does so in the local + coordinate frame +\begin_inset Formula $a$ +\end_inset + +. +\end_layout + +\begin_layout Example +Assume a rigid body's attitude is described by +\begin_inset Formula $R_{b}^{n}(t)$ +\end_inset + +, where the indices denote the navigation frame +\begin_inset Formula $N$ +\end_inset + + and body frame +\begin_inset Formula $B$ +\end_inset + +, respectively. + An extrinsically calibrated gyroscope measures the angular velocity +\begin_inset Formula $\omega^{b}$ +\end_inset + +, in the body frame, and the corresponding tangent vector is +\begin_inset Formula +\[ +\dot{R}_{b}^{n}(t)=R_{b}^{n}(t)\widehat{\omega^{b}} +\] + +\end_inset + + +\end_layout + \begin_layout Subsection Derivatives \end_layout @@ -1352,7 +1484,7 @@ reference "def:differentiable" \end_inset - to map exponential coordinates + to map local coordinates \begin_inset Formula $\xi$ \end_inset @@ -1368,7 +1500,7 @@ reference "def:differentiable" \begin_inset Formula $Df_{a}$ \end_inset - locally approximates a function + approximates the function \begin_inset Formula $f$ \end_inset @@ -1378,6 +1510,10 @@ reference "def:differentiable" to \begin_inset Formula $\Reals m$ +\end_inset + + in a neighborhood around +\begin_inset Formula $a$ \end_inset : @@ -1455,41 +1591,6 @@ derivative . \end_layout -\begin_layout Standard -Note that the vectors -\begin_inset Formula $\xi$ -\end_inset - - can be viewed as lying in the tangent space to -\begin_inset Formula $G$ -\end_inset - - at -\begin_inset Formula $a$ -\end_inset - -, but defining this rigorously would take us on a longer tour of differential - geometry. - Informally, -\begin_inset Formula $\xi$ -\end_inset - - is simply the direction, in a local coordinate frame, that is locally tangent - at -\begin_inset Formula $a$ -\end_inset - - to a geodesic curve -\begin_inset Formula $\gamma:t\mapsto ae^{\widehat{t\xi}}$ -\end_inset - - traced out by the exponential map, with -\begin_inset Formula $\gamma(0)=a$ -\end_inset - -. -\end_layout - \begin_layout Subsection Derivative of an Action \begin_inset CommandInset label @@ -3000,7 +3101,7 @@ f(ge^{\xhat})=f(ge^{\xhat}g^{-1}g)=f(e^{\Ad g\xhat}g) \end_layout \begin_layout Subsection -Derivative of the Exponential and Logarithm Map +Derivative of the Exponential Map \end_layout \begin_layout Theorem @@ -3064,17 +3165,196 @@ For \begin_inset Formula $\xi\neq0$ \end_inset -, things are not simple, see . +, things are not simple. + As with pushforwards above, we will be looking for an +\begin_inset Formula $n\times n$ +\end_inset + -\begin_inset Flex URL +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none +Jacobian +\begin_inset Formula $f'(\xi)$ +\end_inset + + such that +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\uuline default +\uwave default +\noun default +\color inherit + +\begin_inset Formula +\begin{equation} +f\left(\xi+\delta\right)\approx f\left(\xi\right)\exp\left(\widehat{f'(\xi)\delta}\right)\label{eq:push_exp} +\end{equation} + +\end_inset + +Differential geometry tells us that for any Lie algebra element +\begin_inset Formula $\xihat\in\mathfrak{g}$ +\end_inset + + there exists a +\emph on +linear +\emph default + map +\begin_inset Formula $d\exp_{\xihat}:T_{\xihat}\mathfrak{g}\rightarrow T_{\exp(\xihat)}G$ +\end_inset + +, which is given by +\begin_inset Foot status collapsed \begin_layout Plain Layout +See +\begin_inset Flex URL +status open -http://deltaepsilons.wordpress.com/2009/11/06/helgasons-formula-for-the-differenti -al-of-the-exponential/ +\begin_layout Plain Layout + +http://deltaepsilons.wordpress.com/2009/11/06/ \end_layout +\end_inset + + or +\begin_inset Flex URL +status open + +\begin_layout Plain Layout + +https://en.wikipedia.org/wiki/Derivative_of_the_exponential_map +\end_layout + +\end_inset + +. +\end_layout + +\end_inset + + +\begin_inset Formula +\begin{equation} +d\exp_{\xihat}\hat{x}=\exp(\xihat)\frac{1-\exp(-ad_{\xihat})}{ad_{\xihat}}\hat{x}\label{eq:dexp} +\end{equation} + +\end_inset + +with +\begin_inset Formula $\hat{x}\in T_{\xihat}\mathfrak{g}$ +\end_inset + + and +\begin_inset Formula $ad_{\xihat}$ +\end_inset + + itself a linear map taking +\begin_inset Formula $\hat{x}$ +\end_inset + + to +\begin_inset Formula $[\xihat,\hat{x}]$ +\end_inset + +, the Lie bracket. + The actual formula above is not really as important as the fact that the + linear map exists, although it is expressed directly in terms of tangent + vectors to +\begin_inset Formula $\mathfrak{g}$ +\end_inset + + and +\begin_inset Formula $G$ +\end_inset + +. + Equation +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:dexp" + +\end_inset + + is a tangent vector, and comparing with +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:tangent-vector" + +\end_inset + + we see that it maps to local coordinates +\begin_inset Formula $y$ +\end_inset + + as follows: +\begin_inset Formula +\[ +\yhat=\frac{1-\exp(-ad_{\xihat})}{ad_{\xihat}}\hat{x} +\] + +\end_inset + +which can be used to construct the Jacobian +\begin_inset Formula $f'(\xi)$ +\end_inset + +. +\end_layout + +\begin_layout Example +For +\begin_inset Formula $\SOthree$ +\end_inset + +, the operator +\begin_inset Formula $ad_{\xihat}$ +\end_inset + + is simply a matrix multiplication when representing +\begin_inset Formula $\sothree$ +\end_inset + + using 3-vectors, i.e., +\begin_inset Formula $ad_{\xihat}x=\xihat x$ +\end_inset + +, and the +\begin_inset Formula $3\times3$ +\end_inset + + Jacobian corresponding to +\begin_inset Formula $d\exp$ +\end_inset + + is +\begin_inset Formula +\[ +f'(\xi)=\frac{I_{3\times3}-\exp(-\xihat)}{\xihat}=\sum_{k=0}^{\infty}\frac{(-1)^{k}}{(k+1)!}\xihat^{k} +\] + +\end_inset + +which, similar to the exponential map, has a simple closed form expression + for +\begin_inset Formula $\SOthree$ \end_inset . @@ -3097,7 +3377,7 @@ Retractions \begin_layout Standard \begin_inset FormulaMacro -\newcommand{\retract}{\mathcal{R}} +\renewcommand{\retract}{\mathcal{R}} {\mathcal{R}} \end_inset @@ -6797,7 +7077,7 @@ Then \begin_layout Standard \begin_inset CommandInset bibtex LatexCommand bibtex -bibfiles "/Users/dellaert/papers/refs" +bibfiles "refs" options "plain" \end_inset diff --git a/doc/refs.bib b/doc/refs.bib new file mode 100644 index 000000000..414773483 --- /dev/null +++ b/doc/refs.bib @@ -0,0 +1,26 @@ +@article{Iserles00an, + title = {Lie-group methods}, + author = {Iserles, Arieh and Munthe-Kaas, Hans Z and + N{\o}rsett, Syvert P and Zanna, Antonella}, + journal = {Acta Numerica 2000}, + volume = {9}, + pages = {215--365}, + year = {2000}, + publisher = {Cambridge Univ Press} +} + +@book{Murray94book, + title = {A mathematical introduction to robotic manipulation}, + author = {Murray, Richard M and Li, Zexiang and Sastry, S + Shankar and Sastry, S Shankara}, + year = {1994}, + publisher = {CRC press} +} + +@book{Spivak65book, + title = {Calculus on manifolds}, + author = {Spivak, Michael}, + volume = {1}, + year = {1965}, + publisher = {WA Benjamin New York} +} \ No newline at end of file diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index bcc0b6320..010f474bf 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -35,7 +35,7 @@ void createExampleBALFile(const string& filename, const vector& P, SfM_data data; // Create two cameras - Rot3 aRb = Rot3::yaw(M_PI_2); + Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(0.1, 0, 0); Pose3 identity, aPb(aRb, aTb); data.cameras.push_back(SfM_Camera(pose1, K)); @@ -66,7 +66,7 @@ void createExampleBALFile(const string& filename, const vector& P, void create5PointExample1() { // Create two cameras poses - Rot3 aRb = Rot3::yaw(M_PI_2); + Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(0.1, 0, 0); Pose3 pose1, pose2(aRb, aTb); @@ -85,7 +85,7 @@ void create5PointExample1() { void create5PointExample2() { // Create two cameras poses - Rot3 aRb = Rot3::yaw(M_PI_2); + Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(10, 0, 0); Pose3 pose1, pose2(aRb, aTb); diff --git a/gtsam.h b/gtsam.h index 8640b1492..fff556777 100644 --- a/gtsam.h +++ b/gtsam.h @@ -434,11 +434,11 @@ class Rot3 { static gtsam::Rot3 Rz(double t); static gtsam::Rot3 RzRyRx(double x, double y, double z); static gtsam::Rot3 RzRyRx(Vector xyz); - static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading) - static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) - static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft) - static gtsam::Rot3 ypr(double y, double p, double r); - static gtsam::Rot3 quaternion(double w, double x, double y, double z); + static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) + static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) + static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) + static gtsam::Rot3 Ypr(double y, double p, double r); + static gtsam::Rot3 Quaternion(double w, double x, double y, double z); static gtsam::Rot3 Rodrigues(Vector v); // Testable @@ -2438,7 +2438,7 @@ namespace imuBias { #include class ConstantBias { - // Standard Constructor + // Constructors ConstantBias(); ConstantBias(Vector biasAcc, Vector biasGyro); @@ -2470,97 +2470,107 @@ class ConstantBias { }///\namespace imuBias +#include +class NavState { + // Constructors + NavState(); + NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); + NavState(const gtsam::Pose3& pose, Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::NavState& expected, double tol) const; + + // Access + gtsam::Rot3 attitude() const; + gtsam::Point3 position() const; + Vector velocity() const; + gtsam::Pose3 pose() const; +}; + +#include +class PreintegrationParams { + PreintegrationParams(Vector n_gravity); + // TODO(frank): add setters/getters or make this MATLAB wrapper feature +}; + +#include +virtual class PreintegrationBase { + // Constructors + PreintegrationBase(const gtsam::PreintegrationParams* params); + PreintegrationBase(const gtsam::PreintegrationParams* params, + const gtsam::imuBias::ConstantBias& bias); + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegrationBase& expected, double tol); + + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + + // Standard Interface + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; +}; + #include -class PoseVelocityBias{ - PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); - }; -class PreintegratedImuMeasurements { - // Standard Constructor - PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); - PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); - // PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& rhs); +virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase { + // Constructors + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, + const gtsam::imuBias::ConstantBias& bias); // Testable void print(string s) const; bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - Vector biasHatVector() const; - Matrix delPdelBiasAcc() const; - Matrix delPdelBiasOmega() const; - Matrix delVdelBiasAcc() const; - Matrix delVdelBiasOmega() const; - Matrix delRdelBiasOmega() const; - Matrix preintMeasCov() const; - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); - gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, - Vector gravity, Vector omegaCoriolis) const; + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + Matrix preintMeasCov() const; }; -virtual class ImuFactor : gtsam::NonlinearFactor { - ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, - const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); - ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, - const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, - const gtsam::Pose3& body_P_sensor); +virtual class ImuFactor: gtsam::NonlinearFactor { + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, + size_t bias, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); + // Standard Interface gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, + const gtsam::Pose3& pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias& bias); }; #include -class PreintegratedCombinedMeasurements { - // Standard Constructor - PreintegratedCombinedMeasurements( - const gtsam::imuBias::ConstantBias& bias, - Matrix measuredAccCovariance, - Matrix measuredOmegaCovariance, - Matrix integrationErrorCovariance, - Matrix biasAccCovariance, - Matrix biasOmegaCovariance, - Matrix biasAccOmegaInit); - PreintegratedCombinedMeasurements( - const gtsam::imuBias::ConstantBias& bias, - Matrix measuredAccCovariance, - Matrix measuredOmegaCovariance, - Matrix integrationErrorCovariance, - Matrix biasAccCovariance, - Matrix biasOmegaCovariance, - Matrix biasAccOmegaInit, - bool use2ndOrderIntegration); - // PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& rhs); - +virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase { // Testable void print(string s) const; - bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol); - - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - Vector biasHatVector() const; - Matrix delPdelBiasAcc() const; - Matrix delPdelBiasOmega() const; - Matrix delVdelBiasAcc() const; - Matrix delVdelBiasOmega() const; - Matrix delRdelBiasOmega() const; - Matrix preintMeasCov() const; + bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, + double tol); // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); - gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, - Vector gravity, Vector omegaCoriolis) const; + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + Matrix preintMeasCov() const; }; -virtual class CombinedImuFactor : gtsam::NonlinearFactor { - CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, - const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); +virtual class CombinedImuFactor: gtsam::NonlinearFactor { + CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, + size_t bias_i, size_t bias_j, + const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); + // Standard Interface gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, + const gtsam::Pose3& pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias& bias_i, + const gtsam::imuBias::ConstantBias& bias_j); }; #include diff --git a/gtsam/3rdparty/Eigen/.hg_archival.txt b/gtsam/3rdparty/Eigen/.hg_archival.txt index aea5a5515..8c88afc32 100644 --- a/gtsam/3rdparty/Eigen/.hg_archival.txt +++ b/gtsam/3rdparty/Eigen/.hg_archival.txt @@ -1,4 +1,4 @@ repo: 8a21fd850624c931e448cbcfb38168cb2717c790 -node: 10219c95fe653d4962aa9db4946f6fbea96dd740 +node: b30b87236a1b1552af32ac34075ee5696a9b5a33 branch: 3.2 -tag: 3.2.4 +tag: 3.2.7 diff --git a/gtsam/3rdparty/Eigen/.hgtags b/gtsam/3rdparty/Eigen/.hgtags index 7a6e19411..c4ccd33fa 100644 --- a/gtsam/3rdparty/Eigen/.hgtags +++ b/gtsam/3rdparty/Eigen/.hgtags @@ -27,3 +27,6 @@ ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0 6b38706d90a9fe182e66ab88477b3dbde34b9f66 3.2.1 1306d75b4a21891e59ff9bd96678882cf831e39f 3.2.2 36fd1ba04c120cfdd90f3e4cede47f43b21d19ad 3.2.3 +10219c95fe653d4962aa9db4946f6fbea96dd740 3.2.4 +bdd17ee3b1b3a166cd5ec36dcad4fc1f3faf774a 3.2.5 +c58038c56923e0fd86de3ded18e03df442e66dfb 3.2.6 diff --git a/gtsam/3rdparty/Eigen/CMakeLists.txt b/gtsam/3rdparty/Eigen/CMakeLists.txt index 76a11b9d2..ed3e67fe9 100644 --- a/gtsam/3rdparty/Eigen/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/CMakeLists.txt @@ -301,7 +301,7 @@ if(EIGEN_INCLUDE_INSTALL_DIR) ) else() set(INCLUDE_INSTALL_DIR - "${CMAKE_INSTALL_PREFIX}/include/eigen3" + "include/eigen3" CACHE INTERNAL "The directory where we install the header files (internal)" ) @@ -404,7 +404,7 @@ if(cmake_generator_tolower MATCHES "makefile") message(STATUS "make install | Install to ${CMAKE_INSTALL_PREFIX}. To change that:") message(STATUS " | cmake . -DCMAKE_INSTALL_PREFIX=yourpath") message(STATUS " | Eigen headers will then be installed to:") - message(STATUS " | ${INCLUDE_INSTALL_DIR}") + message(STATUS " | ${CMAKE_INSTALL_PREFIX}/${INCLUDE_INSTALL_DIR}") message(STATUS " | To install Eigen headers to a separate location, do:") message(STATUS " | cmake . -DEIGEN_INCLUDE_INSTALL_DIR=yourpath") message(STATUS "make doc | Generate the API documentation, requires Doxygen & LaTeX") diff --git a/gtsam/3rdparty/Eigen/Eigen/Core b/gtsam/3rdparty/Eigen/Eigen/Core index c87f99df3..509c529e1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Core +++ b/gtsam/3rdparty/Eigen/Eigen/Core @@ -123,7 +123,7 @@ #undef bool #undef vector #undef pixel - #elif defined __ARM_NEON__ + #elif defined __ARM_NEON #define EIGEN_VECTORIZE #define EIGEN_VECTORIZE_NEON #include diff --git a/gtsam/3rdparty/Eigen/Eigen/SparseCore b/gtsam/3rdparty/Eigen/Eigen/SparseCore index 9b5be5e15..24bcf0156 100644 --- a/gtsam/3rdparty/Eigen/Eigen/SparseCore +++ b/gtsam/3rdparty/Eigen/Eigen/SparseCore @@ -14,7 +14,7 @@ /** * \defgroup SparseCore_Module SparseCore module * - * This module provides a sparse matrix representation, and basic associatd matrix manipulations + * This module provides a sparse matrix representation, and basic associated matrix manipulations * and operations. * * See the \ref TutorialSparse "Sparse tutorial" diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h index 02ab93880..abd30bd91 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h @@ -235,6 +235,11 @@ template class LDLT } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } /** \internal * Used to compute and store the Cholesky decomposition A = L D L^* = U^* D U. @@ -434,6 +439,8 @@ template struct LDLT_Traits template LDLT& LDLT::compute(const MatrixType& a) { + check_template_parameters(); + eigen_assert(a.rows()==a.cols()); const Index size = a.rows(); @@ -457,7 +464,7 @@ LDLT& LDLT::compute(const MatrixType& a) */ template template -LDLT& LDLT::rankUpdate(const MatrixBase& w, const typename NumTraits::Real& sigma) +LDLT& LDLT::rankUpdate(const MatrixBase& w, const typename LDLT::RealScalar& sigma) { const Index size = w.rows(); if (m_isInitialized) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h index 2e6189f7d..7c11a2dc2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h @@ -174,6 +174,12 @@ template class LLT LLT rankUpdate(const VectorType& vec, const RealScalar& sigma = 1); protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + /** \internal * Used to compute and store L * The strict upper part is not used and even not initialized. @@ -283,7 +289,7 @@ template struct llt_inplace return k; mat.coeffRef(k,k) = x = sqrt(x); if (k>0 && rs>0) A21.noalias() -= A20 * A10.adjoint(); - if (rs>0) A21 *= RealScalar(1)/x; + if (rs>0) A21 /= x; } return -1; } @@ -384,6 +390,8 @@ template struct LLT_Traits template LLT& LLT::compute(const MatrixType& a) { + check_template_parameters(); + eigen_assert(a.rows()==a.cols()); const Index size = a.rows(); m_matrix.resize(size, size); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h index b9bcb5240..f7c365aee 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h @@ -60,7 +60,7 @@ template<> struct mkl_llt \ lda = m.outerStride(); \ \ info = LAPACKE_##MKLPREFIX##potrf( matrix_order, uplo, size, (MKLTYPE*)a, lda ); \ - info = (info==0) ? -1 : 1; \ + info = (info==0) ? -1 : info>0 ? info-1 : size; \ return info; \ } \ }; \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h index c449960de..99dbe171c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h @@ -78,7 +78,7 @@ cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat) { res.itype = CHOLMOD_INT; } - else if (internal::is_same<_Index,UF_long>::value) + else if (internal::is_same<_Index,SuiteSparse_long>::value) { res.itype = CHOLMOD_LONG; } @@ -395,7 +395,7 @@ class CholmodSimplicialLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimpl CholmodSimplicialLLT(const MatrixType& matrix) : Base() { init(); - compute(matrix); + Base::compute(matrix); } ~CholmodSimplicialLLT() {} @@ -442,7 +442,7 @@ class CholmodSimplicialLDLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimp CholmodSimplicialLDLT(const MatrixType& matrix) : Base() { init(); - compute(matrix); + Base::compute(matrix); } ~CholmodSimplicialLDLT() {} @@ -487,7 +487,7 @@ class CholmodSupernodalLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSuper CholmodSupernodalLLT(const MatrixType& matrix) : Base() { init(); - compute(matrix); + Base::compute(matrix); } ~CholmodSupernodalLLT() {} @@ -534,7 +534,7 @@ class CholmodDecomposition : public CholmodBase<_MatrixType, _UpLo, CholmodDecom CholmodDecomposition(const MatrixType& matrix) : Base() { init(); - compute(matrix); + Base::compute(matrix); } ~CholmodDecomposition() {} diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h index 0ab03eff0..0b9c38c82 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h @@ -124,6 +124,21 @@ class Array } #endif +#ifdef EIGEN_HAVE_RVALUE_REFERENCES + Array(Array&& other) + : Base(std::move(other)) + { + Base::_check_template_params(); + if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic) + Base::_set_noalias(other); + } + Array& operator=(Array&& other) + { + other.swap(*this); + return *this; + } +#endif + /** Constructs a vector or row-vector with given dimension. \only_for_vectors * * Note that this is only useful for dynamic-size vectors. For fixed-size vectors, diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h index 38852600d..33ff55371 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h @@ -46,9 +46,6 @@ template class ArrayBase typedef ArrayBase Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl; - using internal::special_scalar_op_base::Scalar, - typename NumTraits::Scalar>::Real>::operator*; - typedef typename internal::traits::StorageKind StorageKind; typedef typename internal::traits::Index Index; typedef typename internal::traits::Scalar Scalar; @@ -56,6 +53,7 @@ template class ArrayBase typedef typename NumTraits::Real RealScalar; typedef DenseBase Base; + using Base::operator*; using Base::RowsAtCompileTime; using Base::ColsAtCompileTime; using Base::SizeAtCompileTime; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h index 1dccc2f42..f48173172 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h @@ -439,19 +439,26 @@ struct assign_impl PacketTraits; + typedef typename Derived1::Scalar Scalar; + typedef packet_traits PacketTraits; enum { packetSize = PacketTraits::size, alignable = PacketTraits::AlignedOnScalar, - dstAlignment = alignable ? Aligned : int(assign_traits::DstIsAligned) , + dstIsAligned = assign_traits::DstIsAligned, + dstAlignment = alignable ? Aligned : int(dstIsAligned), srcAlignment = assign_traits::JointAlignment }; + const Scalar *dst_ptr = &dst.coeffRef(0,0); + if((!bool(dstIsAligned)) && (size_t(dst_ptr) % sizeof(Scalar))>0) + { + // the pointer is not aligend-on scalar, so alignment is not possible + return assign_impl::run(dst, src); + } const Index packetAlignedMask = packetSize - 1; const Index innerSize = dst.innerSize(); const Index outerSize = dst.outerSize(); const Index alignedStep = alignable ? (packetSize - dst.outerStride() % packetSize) & packetAlignedMask : 0; - Index alignedStart = ((!alignable) || assign_traits::DstIsAligned) ? 0 - : internal::first_aligned(&dst.coeffRef(0,0), innerSize); + Index alignedStart = ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned(dst_ptr, innerSize); for(Index outer = 0; outer < outerSize; ++outer) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h index 87bedfa46..827894443 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h @@ -66,8 +66,9 @@ struct traits > : traits::MaxColsAtCompileTime), XprTypeIsRowMajor = (int(traits::Flags)&RowMajorBit) != 0, - IsRowMajor = (MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1 - : (MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0 + IsDense = is_same::value, + IsRowMajor = (IsDense&&MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1 + : (IsDense&&MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0 : XprTypeIsRowMajor, HasSameStorageOrderAsXprType = (IsRowMajor == XprTypeIsRowMajor), InnerSize = IsRowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime), diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h.orig b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h.orig deleted file mode 100644 index a036d8c3b..000000000 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h.orig +++ /dev/null @@ -1,154 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Gael Guennebaud -// Copyright (C) 2006-2008 Benoit Jacob -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_COMMAINITIALIZER_H -#define EIGEN_COMMAINITIALIZER_H - -namespace Eigen { - -/** \class CommaInitializer - * \ingroup Core_Module - * - * \brief Helper class used by the comma initializer operator - * - * This class is internally used to implement the comma initializer feature. It is - * the return type of MatrixBase::operator<<, and most of the time this is the only - * way it is used. - * - * \sa \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished() - */ -template -struct CommaInitializer -{ - typedef typename XprType::Scalar Scalar; - typedef typename XprType::Index Index; - - inline CommaInitializer(XprType& xpr, const Scalar& s) - : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1) - { - m_xpr.coeffRef(0,0) = s; - } - - template - inline CommaInitializer(XprType& xpr, const DenseBase& other) - : m_xpr(xpr), m_row(0), m_col(other.cols()), m_currentBlockRows(other.rows()) - { - m_xpr.block(0, 0, other.rows(), other.cols()) = other; - } - - /* Copy/Move constructor which transfers ownership. This is crucial in - * absence of return value optimization to avoid assertions during destruction. */ - // FIXME in C++11 mode this could be replaced by a proper RValue constructor - inline CommaInitializer(const CommaInitializer& o) - : m_xpr(o.m_xpr), m_row(o.m_row), m_col(o.m_col), m_currentBlockRows(o.m_currentBlockRows) { - // Mark original object as finished. In absence of R-value references we need to const_cast: - const_cast(o).m_row = m_xpr.rows(); - const_cast(o).m_col = m_xpr.cols(); - const_cast(o).m_currentBlockRows = 0; - } - - /* inserts a scalar value in the target matrix */ - CommaInitializer& operator,(const Scalar& s) - { - if (m_col==m_xpr.cols()) - { - m_row+=m_currentBlockRows; - m_col = 0; - m_currentBlockRows = 1; - eigen_assert(m_row - CommaInitializer& operator,(const DenseBase& other) - { - if(other.cols()==0 || other.rows()==0) - return *this; - if (m_col==m_xpr.cols()) - { - m_row+=m_currentBlockRows; - m_col = 0; - m_currentBlockRows = other.rows(); - eigen_assert(m_row+m_currentBlockRows<=m_xpr.rows() - && "Too many rows passed to comma initializer (operator<<)"); - } - eigen_assert(m_col - (m_row, m_col) = other; - else - m_xpr.block(m_row, m_col, other.rows(), other.cols()) = other; - m_col += other.cols(); - return *this; - } - - inline ~CommaInitializer() - { - eigen_assert((m_row+m_currentBlockRows) == m_xpr.rows() - && m_col == m_xpr.cols() - && "Too few coefficients passed to comma initializer (operator<<)"); - } - - /** \returns the built matrix once all its coefficients have been set. - * Calling finished is 100% optional. Its purpose is to write expressions - * like this: - * \code - * quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished()); - * \endcode - */ - inline XprType& finished() { return m_xpr; } - - XprType& m_xpr; // target expression - Index m_row; // current row id - Index m_col; // current col id - Index m_currentBlockRows; // current block height -}; - -/** \anchor MatrixBaseCommaInitRef - * Convenient operator to set the coefficients of a matrix. - * - * The coefficients must be provided in a row major order and exactly match - * the size of the matrix. Otherwise an assertion is raised. - * - * Example: \include MatrixBase_set.cpp - * Output: \verbinclude MatrixBase_set.out - * - * \note According the c++ standard, the argument expressions of this comma initializer are evaluated in arbitrary order. - * - * \sa CommaInitializer::finished(), class CommaInitializer - */ -template -inline CommaInitializer DenseBase::operator<< (const Scalar& s) -{ - return CommaInitializer(*static_cast(this), s); -} - -/** \sa operator<<(const Scalar&) */ -template -template -inline CommaInitializer -DenseBase::operator<<(const DenseBase& other) -{ - return CommaInitializer(*static_cast(this), other); -} - -} // end namespace Eigen - -#endif // EIGEN_COMMAINITIALIZER_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h index 586f77aaf..519a866e6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h @@ -81,7 +81,8 @@ struct traits > ) ), Flags = (Flags0 & ~RowMajorBit) | (LhsFlags & RowMajorBit), - CoeffReadCost = LhsCoeffReadCost + RhsCoeffReadCost + functor_traits::Cost + Cost0 = EIGEN_ADD_COST(LhsCoeffReadCost,RhsCoeffReadCost), + CoeffReadCost = EIGEN_ADD_COST(Cost0,functor_traits::Cost) }; }; } // end namespace internal diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h index f2de749f9..f7ee60e98 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h @@ -47,7 +47,7 @@ struct traits > Flags = _XprTypeNested::Flags & ( HereditaryBits | LinearAccessBit | AlignedBit | (functor_traits::PacketAccess ? PacketAccessBit : 0)), - CoeffReadCost = _XprTypeNested::CoeffReadCost + functor_traits::Cost + CoeffReadCost = EIGEN_ADD_COST(_XprTypeNested::CoeffReadCost, functor_traits::Cost) }; }; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h index 04862f374..354c739f7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h @@ -41,14 +41,13 @@ static inline void check_DenseIndex_is_signed() { template class DenseBase #ifndef EIGEN_PARSED_BY_DOXYGEN : public internal::special_scalar_op_base::Scalar, - typename NumTraits::Scalar>::Real> + typename NumTraits::Scalar>::Real, + DenseCoeffsBase > #else : public DenseCoeffsBase #endif // not EIGEN_PARSED_BY_DOXYGEN { public: - using internal::special_scalar_op_base::Scalar, - typename NumTraits::Scalar>::Real>::operator*; class InnerIterator; @@ -63,8 +62,9 @@ template class DenseBase typedef typename internal::traits::Scalar Scalar; typedef typename internal::packet_traits::type PacketScalar; typedef typename NumTraits::Real RealScalar; + typedef internal::special_scalar_op_base > Base; - typedef DenseCoeffsBase Base; + using Base::operator*; using Base::derived; using Base::const_cast_derived; using Base::rows; @@ -183,10 +183,6 @@ template class DenseBase /** \returns the number of nonzero coefficients which is in practice the number * of stored coefficients. */ inline Index nonZeros() const { return size(); } - /** \returns true if either the number of rows or the number of columns is equal to 1. - * In other words, this function returns - * \code rows()==1 || cols()==1 \endcode - * \sa rows(), cols(), IsVectorAtCompileTime. */ /** \returns the outer size. * @@ -266,11 +262,13 @@ template class DenseBase template Derived& operator=(const ReturnByValue& func); -#ifndef EIGEN_PARSED_BY_DOXYGEN - /** Copies \a other into *this without evaluating other. \returns a reference to *this. */ + /** \internal Copies \a other into *this without evaluating other. \returns a reference to *this. */ template Derived& lazyAssign(const DenseBase& other); -#endif // not EIGEN_PARSED_BY_DOXYGEN + + /** \internal Evaluates \a other into *this. \returns a reference to *this. */ + template + Derived& lazyAssign(const ReturnByValue& other); CommaInitializer operator<< (const Scalar& s); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h index a72738e55..568493cba 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h @@ -122,33 +122,41 @@ template class DenseSt { internal::plain_array m_data; public: - inline DenseStorage() {} - inline DenseStorage(internal::constructor_without_unaligned_array_assert) + DenseStorage() {} + DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(internal::constructor_without_unaligned_array_assert()) {} - inline DenseStorage(DenseIndex,DenseIndex,DenseIndex) {} - inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); } - static inline DenseIndex rows(void) {return _Rows;} - static inline DenseIndex cols(void) {return _Cols;} - inline void conservativeResize(DenseIndex,DenseIndex,DenseIndex) {} - inline void resize(DenseIndex,DenseIndex,DenseIndex) {} - inline const T *data() const { return m_data.array; } - inline T *data() { return m_data.array; } + DenseStorage(const DenseStorage& other) : m_data(other.m_data) {} + DenseStorage& operator=(const DenseStorage& other) + { + if (this != &other) m_data = other.m_data; + return *this; + } + DenseStorage(DenseIndex,DenseIndex,DenseIndex) {} + void swap(DenseStorage& other) { std::swap(m_data,other.m_data); } + static DenseIndex rows(void) {return _Rows;} + static DenseIndex cols(void) {return _Cols;} + void conservativeResize(DenseIndex,DenseIndex,DenseIndex) {} + void resize(DenseIndex,DenseIndex,DenseIndex) {} + const T *data() const { return m_data.array; } + T *data() { return m_data.array; } }; // null matrix template class DenseStorage { public: - inline DenseStorage() {} - inline DenseStorage(internal::constructor_without_unaligned_array_assert) {} - inline DenseStorage(DenseIndex,DenseIndex,DenseIndex) {} - inline void swap(DenseStorage& ) {} - static inline DenseIndex rows(void) {return _Rows;} - static inline DenseIndex cols(void) {return _Cols;} - inline void conservativeResize(DenseIndex,DenseIndex,DenseIndex) {} - inline void resize(DenseIndex,DenseIndex,DenseIndex) {} - inline const T *data() const { return 0; } - inline T *data() { return 0; } + DenseStorage() {} + DenseStorage(internal::constructor_without_unaligned_array_assert) {} + DenseStorage(const DenseStorage&) {} + DenseStorage& operator=(const DenseStorage&) { return *this; } + DenseStorage(DenseIndex,DenseIndex,DenseIndex) {} + void swap(DenseStorage& ) {} + static DenseIndex rows(void) {return _Rows;} + static DenseIndex cols(void) {return _Cols;} + void conservativeResize(DenseIndex,DenseIndex,DenseIndex) {} + void resize(DenseIndex,DenseIndex,DenseIndex) {} + const T *data() const { return 0; } + T *data() { return 0; } }; // more specializations for null matrices; these are necessary to resolve ambiguities @@ -168,18 +176,29 @@ template class DenseStorage class DenseStorage m_data; DenseIndex m_rows; public: - inline DenseStorage() : m_rows(0) {} - inline DenseStorage(internal::constructor_without_unaligned_array_assert) + DenseStorage() : m_rows(0) {} + DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0) {} - inline DenseStorage(DenseIndex, DenseIndex nbRows, DenseIndex) : m_rows(nbRows) {} - inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } - inline DenseIndex rows(void) const {return m_rows;} - inline DenseIndex cols(void) const {return _Cols;} - inline void conservativeResize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; } - inline void resize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; } - inline const T *data() const { return m_data.array; } - inline T *data() { return m_data.array; } + DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_rows(other.m_rows) {} + DenseStorage& operator=(const DenseStorage& other) + { + if (this != &other) + { + m_data = other.m_data; + m_rows = other.m_rows; + } + return *this; + } + DenseStorage(DenseIndex, DenseIndex nbRows, DenseIndex) : m_rows(nbRows) {} + void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } + DenseIndex rows(void) const {return m_rows;} + DenseIndex cols(void) const {return _Cols;} + void conservativeResize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; } + void resize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; } + const T *data() const { return m_data.array; } + T *data() { return m_data.array; } }; // dynamic-size matrix with fixed-size storage and fixed height @@ -207,17 +236,27 @@ template class DenseStorage m_data; DenseIndex m_cols; public: - inline DenseStorage() : m_cols(0) {} - inline DenseStorage(internal::constructor_without_unaligned_array_assert) + DenseStorage() : m_cols(0) {} + DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(internal::constructor_without_unaligned_array_assert()), m_cols(0) {} - inline DenseStorage(DenseIndex, DenseIndex, DenseIndex nbCols) : m_cols(nbCols) {} - inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); } - inline DenseIndex rows(void) const {return _Rows;} - inline DenseIndex cols(void) const {return m_cols;} - inline void conservativeResize(DenseIndex, DenseIndex, DenseIndex nbCols) { m_cols = nbCols; } - inline void resize(DenseIndex, DenseIndex, DenseIndex nbCols) { m_cols = nbCols; } - inline const T *data() const { return m_data.array; } - inline T *data() { return m_data.array; } + DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_cols(other.m_cols) {} + DenseStorage& operator=(const DenseStorage& other) + { + if (this != &other) + { + m_data = other.m_data; + m_cols = other.m_cols; + } + return *this; + } + DenseStorage(DenseIndex, DenseIndex, DenseIndex nbCols) : m_cols(nbCols) {} + void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); } + DenseIndex rows(void) const {return _Rows;} + DenseIndex cols(void) const {return m_cols;} + void conservativeResize(DenseIndex, DenseIndex, DenseIndex nbCols) { m_cols = nbCols; } + void resize(DenseIndex, DenseIndex, DenseIndex nbCols) { m_cols = nbCols; } + const T *data() const { return m_data.array; } + T *data() { return m_data.array; } }; // purely dynamic matrix. @@ -227,18 +266,35 @@ template class DenseStorage(size)), m_rows(nbRows), m_cols(nbCols) { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN } - inline ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, m_rows*m_cols); } - inline void swap(DenseStorage& other) +#ifdef EIGEN_HAVE_RVALUE_REFERENCES + DenseStorage(DenseStorage&& other) + : m_data(std::move(other.m_data)) + , m_rows(std::move(other.m_rows)) + , m_cols(std::move(other.m_cols)) + { + other.m_data = nullptr; + } + DenseStorage& operator=(DenseStorage&& other) + { + using std::swap; + swap(m_data, other.m_data); + swap(m_rows, other.m_rows); + swap(m_cols, other.m_cols); + return *this; + } +#endif + ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, m_rows*m_cols); } + void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); } - inline DenseIndex rows(void) const {return m_rows;} - inline DenseIndex cols(void) const {return m_cols;} - inline void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex nbCols) + DenseIndex rows(void) const {return m_rows;} + DenseIndex cols(void) const {return m_cols;} + void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex nbCols) { m_data = internal::conditional_aligned_realloc_new_auto(m_data, size, m_rows*m_cols); m_rows = nbRows; @@ -258,8 +314,11 @@ template class DenseStorage class DenseStorage(size)), m_cols(nbCols) + DenseStorage() : m_data(0), m_cols(0) {} + DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_cols(0) {} + DenseStorage(DenseIndex size, DenseIndex, DenseIndex nbCols) : m_data(internal::conditional_aligned_new_auto(size)), m_cols(nbCols) { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN } - inline ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, _Rows*m_cols); } - inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); } - static inline DenseIndex rows(void) {return _Rows;} - inline DenseIndex cols(void) const {return m_cols;} - inline void conservativeResize(DenseIndex size, DenseIndex, DenseIndex nbCols) +#ifdef EIGEN_HAVE_RVALUE_REFERENCES + DenseStorage(DenseStorage&& other) + : m_data(std::move(other.m_data)) + , m_cols(std::move(other.m_cols)) + { + other.m_data = nullptr; + } + DenseStorage& operator=(DenseStorage&& other) + { + using std::swap; + swap(m_data, other.m_data); + swap(m_cols, other.m_cols); + return *this; + } +#endif + ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, _Rows*m_cols); } + void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); } + static DenseIndex rows(void) {return _Rows;} + DenseIndex cols(void) const {return m_cols;} + void conservativeResize(DenseIndex size, DenseIndex, DenseIndex nbCols) { m_data = internal::conditional_aligned_realloc_new_auto(m_data, size, _Rows*m_cols); m_cols = nbCols; @@ -294,8 +368,11 @@ template class DenseStorage class DenseStorage(size)), m_rows(nbRows) + DenseStorage() : m_data(0), m_rows(0) {} + DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_rows(0) {} + DenseStorage(DenseIndex size, DenseIndex nbRows, DenseIndex) : m_data(internal::conditional_aligned_new_auto(size)), m_rows(nbRows) { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN } - inline ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, _Cols*m_rows); } - inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } - inline DenseIndex rows(void) const {return m_rows;} - static inline DenseIndex cols(void) {return _Cols;} - inline void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex) +#ifdef EIGEN_HAVE_RVALUE_REFERENCES + DenseStorage(DenseStorage&& other) + : m_data(std::move(other.m_data)) + , m_rows(std::move(other.m_rows)) + { + other.m_data = nullptr; + } + DenseStorage& operator=(DenseStorage&& other) + { + using std::swap; + swap(m_data, other.m_data); + swap(m_rows, other.m_rows); + return *this; + } +#endif + ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, _Cols*m_rows); } + void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } + DenseIndex rows(void) const {return m_rows;} + static DenseIndex cols(void) {return _Cols;} + void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex) { m_data = internal::conditional_aligned_realloc_new_auto(m_data, size, m_rows*_Cols); m_rows = nbRows; @@ -330,8 +422,11 @@ template class DenseStorage > _Vectorizable = bool(int(MatrixType::Flags)&PacketAccessBit) && _SameTypes && (_ScalarAccessOnDiag || (bool(int(DiagonalType::DiagonalVectorType::Flags)&PacketAccessBit))), _LinearAccessMask = (RowsAtCompileTime==1 || ColsAtCompileTime==1) ? LinearAccessBit : 0, - Flags = ((HereditaryBits|_LinearAccessMask) & (unsigned int)(MatrixType::Flags)) | (_Vectorizable ? PacketAccessBit : 0) | AlignedBit,//(int(MatrixType::Flags)&int(DiagonalType::DiagonalVectorType::Flags)&AlignedBit), - CoeffReadCost = NumTraits::MulCost + MatrixType::CoeffReadCost + DiagonalType::DiagonalVectorType::CoeffReadCost + Flags = ((HereditaryBits|_LinearAccessMask|AlignedBit) & (unsigned int)(MatrixType::Flags)) | (_Vectorizable ? PacketAccessBit : 0),//(int(MatrixType::Flags)&int(DiagonalType::DiagonalVectorType::Flags)&AlignedBit), + Cost0 = EIGEN_ADD_COST(NumTraits::MulCost, MatrixType::CoeffReadCost), + CoeffReadCost = EIGEN_ADD_COST(Cost0,DiagonalType::DiagonalVectorType::CoeffReadCost) }; }; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h index b08b967ff..5f14c6587 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h @@ -259,6 +259,47 @@ template<> struct functor_traits { }; }; +/** \internal + * \brief Template functors for comparison of two scalars + * \todo Implement packet-comparisons + */ +template struct scalar_cmp_op; + +template +struct functor_traits > { + enum { + Cost = NumTraits::AddCost, + PacketAccess = false + }; +}; + +template +struct result_of(Scalar,Scalar)> { + typedef bool type; +}; + + +template struct scalar_cmp_op { + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a==b;} +}; +template struct scalar_cmp_op { + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a struct scalar_cmp_op { + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a<=b;} +}; +template struct scalar_cmp_op { + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return !(a<=b || b<=a);} +}; +template struct scalar_cmp_op { + EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op) + EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a!=b;} +}; + // unary functors: /** \internal diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h index 9e805a80f..0eae52990 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h @@ -257,7 +257,7 @@ template class GeneralProduct : public ProductBase, Lhs, Rhs> { - template struct IsRowMajor : internal::conditional<(int(T::Flags)&RowMajorBit), internal::true_type, internal::false_type>::type {}; + template struct is_row_major : internal::conditional<(int(T::Flags)&RowMajorBit), internal::true_type, internal::false_type>::type {}; public: EIGEN_PRODUCT_PUBLIC_INTERFACE(GeneralProduct) @@ -281,22 +281,22 @@ class GeneralProduct template inline void evalTo(Dest& dest) const { - internal::outer_product_selector_run(*this, dest, set(), IsRowMajor()); + internal::outer_product_selector_run(*this, dest, set(), is_row_major()); } template inline void addTo(Dest& dest) const { - internal::outer_product_selector_run(*this, dest, add(), IsRowMajor()); + internal::outer_product_selector_run(*this, dest, add(), is_row_major()); } template inline void subTo(Dest& dest) const { - internal::outer_product_selector_run(*this, dest, sub(), IsRowMajor()); + internal::outer_product_selector_run(*this, dest, sub(), is_row_major()); } template void scaleAndAddTo(Dest& dest, const Scalar& alpha) const { - internal::outer_product_selector_run(*this, dest, adds(alpha), IsRowMajor()); + internal::outer_product_selector_run(*this, dest, adds(alpha), is_row_major()); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h index cebed2bb6..a9828f7f4 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h @@ -123,7 +123,7 @@ template class MapBase return internal::ploadt(m_data + index * innerStride()); } - inline MapBase(PointerType dataPtr) : m_data(dataPtr), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime) + explicit inline MapBase(PointerType dataPtr) : m_data(dataPtr), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime) { EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived) checkSanity(); @@ -157,7 +157,7 @@ template class MapBase internal::inner_stride_at_compile_time::ret==1), PACKET_ACCESS_REQUIRES_TO_HAVE_INNER_STRIDE_FIXED_TO_1); eigen_assert(EIGEN_IMPLIES(internal::traits::Flags&AlignedBit, (size_t(m_data) % 16) == 0) - && "data is not aligned"); + && "input pointer is not aligned on a 16 byte boundary"); } PointerType m_data; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h index 2bfc5ebd9..adf2f9c51 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h @@ -294,7 +294,7 @@ struct hypot_impl RealScalar _x = abs(x); RealScalar _y = abs(y); RealScalar p = (max)(_x, _y); - if(p==RealScalar(0)) return 0; + if(p==RealScalar(0)) return RealScalar(0); RealScalar q = (min)(_x, _y); RealScalar qp = q/p; return p * sqrt(RealScalar(1) + qp*qp); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h index d7d0b5b9a..02be142d8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h @@ -211,6 +211,21 @@ class Matrix : Base(internal::constructor_without_unaligned_array_assert()) { Base::_check_template_params(); EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } +#ifdef EIGEN_HAVE_RVALUE_REFERENCES + Matrix(Matrix&& other) + : Base(std::move(other)) + { + Base::_check_template_params(); + if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic) + Base::_set_noalias(other); + } + Matrix& operator=(Matrix&& other) + { + other.swap(*this); + return *this; + } +#endif + /** \brief Constructs a vector or row-vector with given dimension. \only_for_vectors * * Note that this is only useful for dynamic-size vectors. For fixed-size vectors, diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h index cc3279746..e83ef4dc0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h @@ -159,13 +159,11 @@ template class MatrixBase template Derived& operator=(const ReturnByValue& other); -#ifndef EIGEN_PARSED_BY_DOXYGEN template Derived& lazyAssign(const ProductBase& other); template Derived& lazyAssign(const MatrixPowerProduct& other); -#endif // not EIGEN_PARSED_BY_DOXYGEN template Derived& operator+=(const MatrixBase& other); @@ -442,6 +440,15 @@ template class MatrixBase template void applyOnTheRight(Index p, Index q, const JacobiRotation& j); +///////// SparseCore module ///////// + + template + EIGEN_STRONG_INLINE const typename SparseMatrixBase::template CwiseProductDenseReturnType::Type + cwiseProduct(const SparseMatrixBase &other) const + { + return other.cwiseProduct(derived()); + } + ///////// MatrixFunctions module ///////// typedef typename internal::stem_function::type StemFunction; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h index f26f3e5cc..85ffae265 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h @@ -250,6 +250,35 @@ class PermutationBase : public EigenBase template friend inline PlainPermutationType operator*(const Transpose >& other, const PermutationBase& perm) { return PlainPermutationType(internal::PermPermProduct, other.eval(), perm); } + + /** \returns the determinant of the permutation matrix, which is either 1 or -1 depending on the parity of the permutation. + * + * This function is O(\c n) procedure allocating a buffer of \c n booleans. + */ + Index determinant() const + { + Index res = 1; + Index n = size(); + Matrix mask(n); + mask.fill(false); + Index r = 0; + while(r < n) + { + // search for the next seed + while(r=n) + break; + // we got one, let's follow it until we are back to the seed + Index k0 = r++; + mask.coeffRef(k0) = true; + for(Index k=indices().coeff(k0); k!=k0; k=indices().coeff(k)) + { + mask.coeffRef(k) = true; + res = -res; + } + } + return res; + } protected: diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h index dd34b59e5..a4e4af4a7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h @@ -437,6 +437,36 @@ class PlainObjectBase : public internal::dense_xpr_base::type } #endif +#ifdef EIGEN_HAVE_RVALUE_REFERENCES + PlainObjectBase(PlainObjectBase&& other) + : m_storage( std::move(other.m_storage) ) + { + } + + PlainObjectBase& operator=(PlainObjectBase&& other) + { + using std::swap; + swap(m_storage, other.m_storage); + return *this; + } +#endif + + /** Copy constructor */ + EIGEN_STRONG_INLINE PlainObjectBase(const PlainObjectBase& other) + : m_storage() + { + _check_template_params(); + lazyAssign(other); + } + + template + EIGEN_STRONG_INLINE PlainObjectBase(const DenseBase &other) + : m_storage() + { + _check_template_params(); + lazyAssign(other); + } + EIGEN_STRONG_INLINE PlainObjectBase(Index a_size, Index nbRows, Index nbCols) : m_storage(a_size, nbRows, nbCols) { @@ -573,6 +603,8 @@ class PlainObjectBase : public internal::dense_xpr_base::type : (rows() == other.rows() && cols() == other.cols()))) && "Size mismatch. Automatic resizing is disabled because EIGEN_NO_AUTOMATIC_RESIZING is defined"); EIGEN_ONLY_USED_FOR_DEBUG(other); + if(this->size()==0) + resizeLike(other); #else resizeLike(other); #endif diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h index 50548fa9a..9b8662a6f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h @@ -247,8 +247,9 @@ struct redux_impl } }; -template -struct redux_impl +// NOTE: for SliceVectorizedTraversal we simply bypass unrolling +template +struct redux_impl { typedef typename Derived::Scalar Scalar; typedef typename packet_traits::type PacketScalar; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h index df87b1af4..7a3becaf8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h @@ -108,7 +108,8 @@ struct traits > OuterStrideMatch = Derived::IsVectorAtCompileTime || int(StrideType::OuterStrideAtCompileTime)==int(Dynamic) || int(StrideType::OuterStrideAtCompileTime)==int(Derived::OuterStrideAtCompileTime), AlignmentMatch = (_Options!=Aligned) || ((PlainObjectType::Flags&AlignedBit)==0) || ((traits::Flags&AlignedBit)==AlignedBit), - MatchAtCompileTime = HasDirectAccess && StorageOrderMatch && InnerStrideMatch && OuterStrideMatch && AlignmentMatch + ScalarTypeMatch = internal::is_same::value, + MatchAtCompileTime = HasDirectAccess && StorageOrderMatch && InnerStrideMatch && OuterStrideMatch && AlignmentMatch && ScalarTypeMatch }; typedef typename internal::conditional::type type; }; @@ -187,9 +188,11 @@ protected: template class Ref : public RefBase > { + private: typedef internal::traits Traits; template - inline Ref(const PlainObjectBase& expr); + inline Ref(const PlainObjectBase& expr, + typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0); public: typedef RefBase Base; @@ -198,13 +201,15 @@ template class Ref #ifndef EIGEN_PARSED_BY_DOXYGEN template - inline Ref(PlainObjectBase& expr) + inline Ref(PlainObjectBase& expr, + typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0) { EIGEN_STATIC_ASSERT(static_cast(Traits::template match::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH); Base::construct(expr.derived()); } template - inline Ref(const DenseBase& expr) + inline Ref(const DenseBase& expr, + typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0) #else template inline Ref(DenseBase& expr) @@ -231,13 +236,23 @@ template class Ref< EIGEN_DENSE_PUBLIC_INTERFACE(Ref) template - inline Ref(const DenseBase& expr) + inline Ref(const DenseBase& expr, + typename internal::enable_if::ScalarTypeMatch),Derived>::type* = 0) { // std::cout << match_helper::HasDirectAccess << "," << match_helper::OuterStrideMatch << "," << match_helper::InnerStrideMatch << "\n"; // std::cout << int(StrideType::OuterStrideAtCompileTime) << " - " << int(Derived::OuterStrideAtCompileTime) << "\n"; // std::cout << int(StrideType::InnerStrideAtCompileTime) << " - " << int(Derived::InnerStrideAtCompileTime) << "\n"; construct(expr.derived(), typename Traits::template match::type()); } + + inline Ref(const Ref& other) : Base(other) { + // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy + } + + template + inline Ref(const RefBase& other) { + construct(other.derived(), typename Traits::template match::type()); + } protected: diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h index d66c24ba0..f635598dc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h @@ -72,6 +72,8 @@ template class ReturnByValue const Unusable& coeff(Index,Index) const { return *reinterpret_cast(this); } Unusable& coeffRef(Index) { return *reinterpret_cast(this); } Unusable& coeffRef(Index,Index) { return *reinterpret_cast(this); } + template Unusable& packet(Index) const; + template Unusable& packet(Index, Index) const; #endif }; @@ -83,6 +85,15 @@ Derived& DenseBase::operator=(const ReturnByValue& other) return derived(); } +template +template +Derived& DenseBase::lazyAssign(const ReturnByValue& other) +{ + other.evalTo(derived()); + return derived(); +} + + } // end namespace Eigen #endif // EIGEN_RETURNBYVALUE_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h index 22f3047b4..0956475af 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h @@ -180,15 +180,9 @@ inline Derived& DenseBase::operator*=(const Scalar& other) template inline Derived& DenseBase::operator/=(const Scalar& other) { - typedef typename internal::conditional::IsInteger, - internal::scalar_quotient_op, - internal::scalar_product_op >::type BinOp; typedef typename Derived::PlainObject PlainObject; - SelfCwiseBinaryOp tmp(derived()); - Scalar actual_other; - if(NumTraits::IsInteger) actual_other = other; - else actual_other = Scalar(1)/other; - tmp = PlainObject::Constant(rows(),cols(), actual_other); + SelfCwiseBinaryOp, Derived, typename PlainObject::ConstantReturnType> tmp(derived()); + tmp = PlainObject::Constant(rows(),cols(), other); return derived(); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h index 94dfab330..d49670e04 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h @@ -384,6 +384,7 @@ template<> EIGEN_STRONG_INLINE int predux_max(const Packet4i& a) a_lo = vget_low_s32(a); a_hi = vget_high_s32(a); max = vpmax_s32(a_lo, a_hi); + max = vpmax_s32(max, max); return vget_lane_s32(max, 0); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h index d16f30bb0..2b07168ae 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h @@ -126,7 +126,7 @@ Packet4f pexp(const Packet4f& _x) _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p4, 1.6666665459E-1f); _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p5, 5.0000001201E-1f); - Packet4f tmp = _mm_setzero_ps(), fx; + Packet4f tmp, fx; Packet4i emm0; // clamp x @@ -195,7 +195,7 @@ Packet2d pexp(const Packet2d& _x) _EIGEN_DECLARE_CONST_Packet2d(cephes_exp_C2, 1.42860682030941723212e-6); static const __m128i p4i_1023_0 = _mm_setr_epi32(1023, 1023, 0, 0); - Packet2d tmp = _mm_setzero_pd(), fx; + Packet2d tmp, fx; Packet4i emm0; // clamp x @@ -279,7 +279,7 @@ Packet4f psin(const Packet4f& _x) _EIGEN_DECLARE_CONST_Packet4f(coscof_p2, 4.166664568298827E-002f); _EIGEN_DECLARE_CONST_Packet4f(cephes_FOPI, 1.27323954473516f); // 4 / M_PI - Packet4f xmm1, xmm2 = _mm_setzero_ps(), xmm3, sign_bit, y; + Packet4f xmm1, xmm2, xmm3, sign_bit, y; Packet4i emm0, emm2; sign_bit = x; @@ -378,7 +378,7 @@ Packet4f pcos(const Packet4f& _x) _EIGEN_DECLARE_CONST_Packet4f(coscof_p2, 4.166664568298827E-002f); _EIGEN_DECLARE_CONST_Packet4f(cephes_FOPI, 1.27323954473516f); // 4 / M_PI - Packet4f xmm1, xmm2 = _mm_setzero_ps(), xmm3, y; + Packet4f xmm1, xmm2, xmm3, y; Packet4i emm0, emm2; x = pabs(x); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h index 421f925e1..2a9d65b94 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h @@ -134,7 +134,7 @@ class CoeffBasedProduct }; typedef internal::product_coeff_impl ScalarCoeffImpl; typedef CoeffBasedProduct LazyCoeffBasedProductType; @@ -185,7 +185,7 @@ class CoeffBasedProduct { PacketScalar res; internal::product_packet_impl ::run(row, col, m_lhs, m_rhs, res); return res; @@ -243,7 +243,17 @@ struct product_coeff_impl static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, RetScalar &res) { product_coeff_impl::run(row, col, lhs, rhs, res); - res += lhs.coeff(row, UnrollingIndex) * rhs.coeff(UnrollingIndex, col); + res += lhs.coeff(row, UnrollingIndex-1) * rhs.coeff(UnrollingIndex-1, col); + } +}; + +template +struct product_coeff_impl +{ + typedef typename Lhs::Index Index; + static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, RetScalar &res) + { + res = lhs.coeff(row, 0) * rhs.coeff(0, col); } }; @@ -251,9 +261,9 @@ template struct product_coeff_impl { typedef typename Lhs::Index Index; - static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, RetScalar &res) + static EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, RetScalar &res) { - res = lhs.coeff(row, 0) * rhs.coeff(0, col); + res = RetScalar(0); } }; @@ -293,6 +303,16 @@ struct product_coeff_vectorized_unroller<0, Lhs, Rhs, Packet> } }; +template +struct product_coeff_impl +{ + typedef typename Lhs::Index Index; + static EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, RetScalar &res) + { + res = 0; + } +}; + template struct product_coeff_impl { @@ -302,8 +322,7 @@ struct product_coeff_impl::run(row, col, lhs, rhs, pres); - product_coeff_impl::run(row, col, lhs, rhs, res); + product_coeff_vectorized_unroller::run(row, col, lhs, rhs, pres); res = predux(pres); } }; @@ -371,7 +390,7 @@ struct product_packet_impl static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet &res) { product_packet_impl::run(row, col, lhs, rhs, res); - res = pmadd(pset1(lhs.coeff(row, UnrollingIndex)), rhs.template packet(UnrollingIndex, col), res); + res = pmadd(pset1(lhs.coeff(row, UnrollingIndex-1)), rhs.template packet(UnrollingIndex-1, col), res); } }; @@ -382,12 +401,12 @@ struct product_packet_impl static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet &res) { product_packet_impl::run(row, col, lhs, rhs, res); - res = pmadd(lhs.template packet(row, UnrollingIndex), pset1(rhs.coeff(UnrollingIndex, col)), res); + res = pmadd(lhs.template packet(row, UnrollingIndex-1), pset1(rhs.coeff(UnrollingIndex-1, col)), res); } }; template -struct product_packet_impl +struct product_packet_impl { typedef typename Lhs::Index Index; static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet &res) @@ -397,7 +416,7 @@ struct product_packet_impl }; template -struct product_packet_impl +struct product_packet_impl { typedef typename Lhs::Index Index; static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet &res) @@ -406,16 +425,35 @@ struct product_packet_impl } }; +template +struct product_packet_impl +{ + typedef typename Lhs::Index Index; + static EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, Packet &res) + { + res = pset1(0); + } +}; + +template +struct product_packet_impl +{ + typedef typename Lhs::Index Index; + static EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, Packet &res) + { + res = pset1(0); + } +}; + template struct product_packet_impl { typedef typename Lhs::Index Index; static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet& res) { - eigen_assert(lhs.cols()>0 && "you are using a non initialized matrix"); - res = pmul(pset1(lhs.coeff(row, 0)),rhs.template packet(0, col)); - for(Index i = 1; i < lhs.cols(); ++i) - res = pmadd(pset1(lhs.coeff(row, i)), rhs.template packet(i, col), res); + res = pset1(0); + for(Index i = 0; i < lhs.cols(); ++i) + res = pmadd(pset1(lhs.coeff(row, i)), rhs.template packet(i, col), res); } }; @@ -425,10 +463,9 @@ struct product_packet_impl typedef typename Lhs::Index Index; static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet& res) { - eigen_assert(lhs.cols()>0 && "you are using a non initialized matrix"); - res = pmul(lhs.template packet(row, 0), pset1(rhs.coeff(0, col))); - for(Index i = 1; i < lhs.cols(); ++i) - res = pmadd(lhs.template packet(row, i), pset1(rhs.coeff(i, col)), res); + res = pset1(0); + for(Index i = 0; i < lhs.cols(); ++i) + res = pmadd(lhs.template packet(row, i), pset1(rhs.coeff(i, col)), res); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h index 5c3e9b7ac..6937ee332 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h @@ -125,19 +125,22 @@ void parallelize_gemm(const Functor& func, Index rows, Index cols, bool transpos if(transpose) std::swap(rows,cols); - Index blockCols = (cols / threads) & ~Index(0x3); - Index blockRows = (rows / threads) & ~Index(0x7); - GemmParallelInfo* info = new GemmParallelInfo[threads]; - #pragma omp parallel for schedule(static,1) num_threads(threads) - for(Index i=0; i class Rotation2D; template class AngleAxis; template class Translation; +// Sparse module: +template class SparseMatrixBase; + #ifdef EIGEN2_SUPPORT template class eigen2_RotationBase; template class eigen2_Cross; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h index 8acca9c8c..1ef3b61db 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h @@ -76,6 +76,38 @@ #include #define EIGEN_MKL_VML_THRESHOLD 128 +/* MKL_DOMAIN_BLAS, etc are defined only in 10.3 update 7 */ +/* MKL_BLAS, etc are not defined in 11.2 */ +#ifdef MKL_DOMAIN_ALL +#define EIGEN_MKL_DOMAIN_ALL MKL_DOMAIN_ALL +#else +#define EIGEN_MKL_DOMAIN_ALL MKL_ALL +#endif + +#ifdef MKL_DOMAIN_BLAS +#define EIGEN_MKL_DOMAIN_BLAS MKL_DOMAIN_BLAS +#else +#define EIGEN_MKL_DOMAIN_BLAS MKL_BLAS +#endif + +#ifdef MKL_DOMAIN_FFT +#define EIGEN_MKL_DOMAIN_FFT MKL_DOMAIN_FFT +#else +#define EIGEN_MKL_DOMAIN_FFT MKL_FFT +#endif + +#ifdef MKL_DOMAIN_VML +#define EIGEN_MKL_DOMAIN_VML MKL_DOMAIN_VML +#else +#define EIGEN_MKL_DOMAIN_VML MKL_VML +#endif + +#ifdef MKL_DOMAIN_PARDISO +#define EIGEN_MKL_DOMAIN_PARDISO MKL_DOMAIN_PARDISO +#else +#define EIGEN_MKL_DOMAIN_PARDISO MKL_PARDISO +#endif + namespace Eigen { typedef std::complex dcomplex; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h index 6d1e6c133..53fb5fae4 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h @@ -13,7 +13,7 @@ #define EIGEN_WORLD_VERSION 3 #define EIGEN_MAJOR_VERSION 2 -#define EIGEN_MINOR_VERSION 4 +#define EIGEN_MINOR_VERSION 7 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ @@ -96,6 +96,20 @@ #define EIGEN_DEFAULT_DENSE_INDEX_TYPE std::ptrdiff_t #endif +// A Clang feature extension to determine compiler features. +// We use it to determine 'cxx_rvalue_references' +#ifndef __has_feature +# define __has_feature(x) 0 +#endif + +// Do we support r-value references? +#if (__has_feature(cxx_rvalue_references) || \ + defined(__GXX_EXPERIMENTAL_CXX0X__) || \ + (defined(_MSC_VER) && _MSC_VER >= 1600)) + #define EIGEN_HAVE_RVALUE_REFERENCES +#endif + + // Cross compiler wrapper around LLVM's __has_builtin #ifdef __has_builtin # define EIGEN_HAS_BUILTIN(x) __has_builtin(x) @@ -278,6 +292,7 @@ namespace Eigen { #error Please tell me what is the equivalent of __attribute__((aligned(n))) for your compiler #endif +#define EIGEN_ALIGN8 EIGEN_ALIGN_TO_BOUNDARY(8) #define EIGEN_ALIGN16 EIGEN_ALIGN_TO_BOUNDARY(16) #if EIGEN_ALIGN_STATICALLY @@ -332,8 +347,11 @@ namespace Eigen { } #endif -#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \ - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) +/** \internal + * \brief Macro to manually inherit assignment operators. + * This is necessary, because the implicitly defined assignment operator gets deleted when a custom operator= is defined. + */ +#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) /** * Just a side note. Commenting within defines works only by documenting @@ -405,6 +423,8 @@ namespace Eigen { #define EIGEN_SIZE_MAX(a,b) (((int)a == Dynamic || (int)b == Dynamic) ? Dynamic \ : ((int)a >= (int)b) ? (int)a : (int)b) +#define EIGEN_ADD_COST(a,b) int(a)==Dynamic || int(b)==Dynamic ? Dynamic : int(a)+int(b) + #define EIGEN_LOGICAL_XOR(a,b) (((a) || (b)) && !((a) && (b))) #define EIGEN_IMPLIES(a,b) (!(a) || (b)) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h index 41dd7db06..bc1ea69ed 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h @@ -523,7 +523,7 @@ template struct smart_copy_helper { // you can overwrite Eigen's default behavior regarding alloca by defining EIGEN_ALLOCA // to the appropriate stack allocation function #ifndef EIGEN_ALLOCA - #if (defined __linux__) + #if (defined __linux__) || (defined __APPLE__) || (defined alloca) #define EIGEN_ALLOCA alloca #elif defined(_MSC_VER) #define EIGEN_ALLOCA _alloca diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h index 781965d2c..d05f8e5f6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h @@ -366,17 +366,17 @@ struct dense_xpr_base /** \internal Helper base class to add a scalar multiple operator * overloads for complex types */ -template::value > -struct special_scalar_op_base : public DenseCoeffsBase +struct special_scalar_op_base : public BaseType { // dummy operator* so that the // "using special_scalar_op_base::operator*" compiles void operator*() const; }; -template -struct special_scalar_op_base : public DenseCoeffsBase +template +struct special_scalar_op_base : public BaseType { const CwiseUnaryOp, Derived> operator*(const OtherScalar& scalar) const diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h index af434bc9b..417c72944 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h @@ -234,6 +234,12 @@ template class ComplexEigenSolver } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + EigenvectorType m_eivec; EigenvalueType m_eivalues; ComplexSchur m_schur; @@ -251,6 +257,8 @@ template ComplexEigenSolver& ComplexEigenSolver::compute(const MatrixType& matrix, bool computeEigenvectors) { + check_template_parameters(); + // this code is inspired from Jampack eigen_assert(matrix.cols() == matrix.rows()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h index 6e7150685..20c59a7a2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h @@ -298,6 +298,13 @@ template class EigenSolver void doComputeEigenvectors(); protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + EIGEN_STATIC_ASSERT(!NumTraits::IsComplex, NUMERIC_TYPE_MUST_BE_REAL); + } + MatrixType m_eivec; EigenvalueType m_eivalues; bool m_isInitialized; @@ -364,6 +371,8 @@ template EigenSolver& EigenSolver::compute(const MatrixType& matrix, bool computeEigenvectors) { + check_template_parameters(); + using std::sqrt; using std::abs; eigen_assert(matrix.cols() == matrix.rows()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h index dc240e13e..956e80d9e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h @@ -263,6 +263,13 @@ template class GeneralizedEigenSolver } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + EIGEN_STATIC_ASSERT(!NumTraits::IsComplex, NUMERIC_TYPE_MUST_BE_REAL); + } + MatrixType m_eivec; ComplexVectorType m_alphas; VectorType m_betas; @@ -290,6 +297,8 @@ template GeneralizedEigenSolver& GeneralizedEigenSolver::compute(const MatrixType& A, const MatrixType& B, bool computeEigenvectors) { + check_template_parameters(); + using std::sqrt; using std::abs; eigen_assert(A.cols() == A.rows() && B.cols() == A.rows() && B.cols() == B.rows()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h index 4f36091db..aa3833eba 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h @@ -240,10 +240,10 @@ namespace Eigen { m_S.coeffRef(i,j) = Scalar(0.0); m_S.rightCols(dim-j-1).applyOnTheLeft(i-1,i,G.adjoint()); m_T.rightCols(dim-i+1).applyOnTheLeft(i-1,i,G.adjoint()); + // update Q + if (m_computeQZ) + m_Q.applyOnTheRight(i-1,i,G); } - // update Q - if (m_computeQZ) - m_Q.applyOnTheRight(i-1,i,G); // kill T(i,i-1) if(m_T.coeff(i,i-1)!=Scalar(0)) { @@ -251,10 +251,10 @@ namespace Eigen { m_T.coeffRef(i,i-1) = Scalar(0.0); m_S.applyOnTheRight(i,i-1,G); m_T.topRows(i).applyOnTheRight(i,i-1,G); + // update Z + if (m_computeQZ) + m_Z.applyOnTheLeft(i,i-1,G.adjoint()); } - // update Z - if (m_computeQZ) - m_Z.applyOnTheLeft(i,i-1,G.adjoint()); } } } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h index 64d136341..16d387537 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h @@ -234,7 +234,7 @@ template class RealSchur typedef Matrix Vector3s; Scalar computeNormOfT(); - Index findSmallSubdiagEntry(Index iu, const Scalar& norm); + Index findSmallSubdiagEntry(Index iu); void splitOffTwoRows(Index iu, bool computeU, const Scalar& exshift); void computeShift(Index iu, Index iter, Scalar& exshift, Vector3s& shiftInfo); void initFrancisQRStep(Index il, Index iu, const Vector3s& shiftInfo, Index& im, Vector3s& firstHouseholderVector); @@ -286,7 +286,7 @@ RealSchur& RealSchur::computeFromHessenberg(const HessMa { while (iu >= 0) { - Index il = findSmallSubdiagEntry(iu, norm); + Index il = findSmallSubdiagEntry(iu); // Check for convergence if (il == iu) // One root found @@ -343,16 +343,14 @@ inline typename MatrixType::Scalar RealSchur::computeNormOfT() /** \internal Look for single small sub-diagonal element and returns its index */ template -inline typename MatrixType::Index RealSchur::findSmallSubdiagEntry(Index iu, const Scalar& norm) +inline typename MatrixType::Index RealSchur::findSmallSubdiagEntry(Index iu) { using std::abs; Index res = iu; while (res > 0) { Scalar s = abs(m_matT.coeff(res-1,res-1)) + abs(m_matT.coeff(res,res)); - if (s == 0.0) - s = norm; - if (abs(m_matT.coeff(res,res-1)) < NumTraits::epsilon() * s) + if (abs(m_matT.coeff(res,res-1)) <= NumTraits::epsilon() * s) break; res--; } @@ -457,9 +455,7 @@ inline void RealSchur::initFrancisQRStep(Index il, Index iu, const V const Scalar lhs = m_matT.coeff(im,im-1) * (abs(v.coeff(1)) + abs(v.coeff(2))); const Scalar rhs = v.coeff(0) * (abs(m_matT.coeff(im-1,im-1)) + abs(Tmm) + abs(m_matT.coeff(im+1,im+1))); if (abs(lhs) < NumTraits::epsilon() * rhs) - { break; - } } } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h index be89de4a9..1131c8af8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h @@ -80,6 +80,8 @@ template class SelfAdjointEigenSolver /** \brief Scalar type for matrices of type \p _MatrixType. */ typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::Index Index; + + typedef Matrix EigenvectorsType; /** \brief Real scalar type for \p _MatrixType. * @@ -225,7 +227,7 @@ template class SelfAdjointEigenSolver * * \sa eigenvalues() */ - const MatrixType& eigenvectors() const + const EigenvectorsType& eigenvectors() const { eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); @@ -351,7 +353,12 @@ template class SelfAdjointEigenSolver #endif // EIGEN2_SUPPORT protected: - MatrixType m_eivec; + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + + EigenvectorsType m_eivec; RealVectorType m_eivalues; typename TridiagonalizationType::SubDiagonalType m_subdiag; ComputationInfo m_info; @@ -376,7 +383,7 @@ template class SelfAdjointEigenSolver * "implicit symmetric QR step with Wilkinson shift" */ namespace internal { -template +template static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n); } @@ -384,6 +391,8 @@ template SelfAdjointEigenSolver& SelfAdjointEigenSolver ::compute(const MatrixType& matrix, int options) { + check_template_parameters(); + using std::abs; eigen_assert(matrix.cols() == matrix.rows()); eigen_assert((options&~(EigVecMask|GenEigMask))==0 @@ -406,7 +415,7 @@ SelfAdjointEigenSolver& SelfAdjointEigenSolver // declare some aliases RealVectorType& diag = m_eivalues; - MatrixType& mat = m_eivec; + EigenvectorsType& mat = m_eivec; // map the matrix coefficients to [-1:1] to avoid over- and underflow. mat = matrix.template triangularView(); @@ -442,7 +451,7 @@ SelfAdjointEigenSolver& SelfAdjointEigenSolver while (start>0 && m_subdiag[start-1]!=0) start--; - internal::tridiagonal_qr_step(diag.data(), m_subdiag.data(), start, end, computeEigenvectors ? m_eivec.data() : (Scalar*)0, n); + internal::tridiagonal_qr_step(diag.data(), m_subdiag.data(), start, end, computeEigenvectors ? m_eivec.data() : (Scalar*)0, n); } if (iter <= m_maxIterations * n) @@ -490,7 +499,13 @@ template struct direct_selfadjoint_eigenvalues struct direct_selfadjoint_eigenvalues Scalar(0)) + Scalar a_over_3 = (c2*c2_over_3 - c1)*s_inv3; + if(a_over_3 Scalar(0)) + Scalar q = a_over_3*a_over_3*a_over_3 - half_b*half_b; + if(q 0, atan2 is in [0, pi] and theta is in [0, pi/3] Scalar cos_theta = cos(theta); Scalar sin_theta = sin(theta); - roots(0) = c2_over_3 + Scalar(2)*rho*cos_theta; - roots(1) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta); - roots(2) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta); - - // Sort in increasing order. - if (roots(0) >= roots(1)) - std::swap(roots(0),roots(1)); - if (roots(1) >= roots(2)) - { - std::swap(roots(1),roots(2)); - if (roots(0) >= roots(1)) - std::swap(roots(0),roots(1)); - } + // roots are already sorted, since cos is monotonically decreasing on [0, pi] + roots(0) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta); // == 2*rho*cos(theta+2pi/3) + roots(1) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta); // == 2*rho*cos(theta+ pi/3) + roots(2) = c2_over_3 + Scalar(2)*rho*cos_theta; } - + + static inline bool extract_kernel(MatrixType& mat, Ref res, Ref representative) + { + using std::abs; + Index i0; + // Find non-zero column i0 (by construction, there must exist a non zero coefficient on the diagonal): + mat.diagonal().cwiseAbs().maxCoeff(&i0); + // mat.col(i0) is a good candidate for an orthogonal vector to the current eigenvector, + // so let's save it: + representative = mat.col(i0); + Scalar n0, n1; + VectorType c0, c1; + n0 = (c0 = representative.cross(mat.col((i0+1)%3))).squaredNorm(); + n1 = (c1 = representative.cross(mat.col((i0+2)%3))).squaredNorm(); + if(n0>n1) res = c0/std::sqrt(n0); + else res = c1/std::sqrt(n1); + + return true; + } + static inline void run(SolverType& solver, const MatrixType& mat, int options) { - using std::sqrt; eigen_assert(mat.cols() == 3 && mat.cols() == mat.rows()); eigen_assert((options&~(EigVecMask|GenEigMask))==0 && (options&EigVecMask)!=EigVecMask && "invalid option parameter"); bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors; - MatrixType& eivecs = solver.m_eivec; + EigenvectorsType& eivecs = solver.m_eivec; VectorType& eivals = solver.m_eivalues; - // map the matrix coefficients to [-1:1] to avoid over- and underflow. - Scalar scale = mat.cwiseAbs().maxCoeff(); - MatrixType scaledMat = mat / scale; + // Shift the matrix to the mean eigenvalue and map the matrix coefficients to [-1:1] to avoid over- and underflow. + Scalar shift = mat.trace() / Scalar(3); + // TODO Avoid this copy. Currently it is necessary to suppress bogus values when determining maxCoeff and for computing the eigenvectors later + MatrixType scaledMat = mat.template selfadjointView(); + scaledMat.diagonal().array() -= shift; + Scalar scale = scaledMat.cwiseAbs().maxCoeff(); + if(scale > 0) scaledMat /= scale; // TODO for scale==0 we could save the remaining operations // compute the eigenvalues computeRoots(scaledMat,eivals); - // compute the eigen vectors + // compute the eigenvectors if(computeEigenvectors) { - Scalar safeNorm2 = Eigen::NumTraits::epsilon(); if((eivals(2)-eivals(0))<=Eigen::NumTraits::epsilon()) { + // All three eigenvalues are numerically the same eivecs.setIdentity(); } else { - scaledMat = scaledMat.template selfadjointView(); MatrixType tmp; tmp = scaledMat; + // Compute the eigenvector of the most distinct eigenvalue Scalar d0 = eivals(2) - eivals(1); Scalar d1 = eivals(1) - eivals(0); - int k = d0 > d1 ? 2 : 0; - d0 = d0 > d1 ? d0 : d1; - - tmp.diagonal().array () -= eivals(k); - VectorType cross; - Scalar n; - n = (cross = tmp.row(0).cross(tmp.row(1))).squaredNorm(); - - if(n>safeNorm2) + Index k(0), l(2); + if(d0 > d1) { - eivecs.col(k) = cross / sqrt(n); + std::swap(k,l); + d0 = d1; + } + + // Compute the eigenvector of index k + { + tmp.diagonal().array () -= eivals(k); + // By construction, 'tmp' is of rank 2, and its kernel corresponds to the respective eigenvector. + extract_kernel(tmp, eivecs.col(k), eivecs.col(l)); + } + + // Compute eigenvector of index l + if(d0<=2*Eigen::NumTraits::epsilon()*d1) + { + // If d0 is too small, then the two other eigenvalues are numerically the same, + // and thus we only have to ortho-normalize the near orthogonal vector we saved above. + eivecs.col(l) -= eivecs.col(k).dot(eivecs.col(l))*eivecs.col(l); + eivecs.col(l).normalize(); } else { - n = (cross = tmp.row(0).cross(tmp.row(2))).squaredNorm(); + tmp = scaledMat; + tmp.diagonal().array () -= eivals(l); - if(n>safeNorm2) - { - eivecs.col(k) = cross / sqrt(n); - } - else - { - n = (cross = tmp.row(1).cross(tmp.row(2))).squaredNorm(); - - if(n>safeNorm2) - { - eivecs.col(k) = cross / sqrt(n); - } - else - { - // the input matrix and/or the eigenvaues probably contains some inf/NaN, - // => exit - // scale back to the original size. - eivals *= scale; - - solver.m_info = NumericalIssue; - solver.m_isInitialized = true; - solver.m_eigenvectorsOk = computeEigenvectors; - return; - } - } + VectorType dummy; + extract_kernel(tmp, eivecs.col(l), dummy); } - tmp = scaledMat; - tmp.diagonal().array() -= eivals(1); - - if(d0<=Eigen::NumTraits::epsilon()) - { - eivecs.col(1) = eivecs.col(k).unitOrthogonal(); - } - else - { - n = (cross = eivecs.col(k).cross(tmp.row(0))).squaredNorm(); - if(n>safeNorm2) - { - eivecs.col(1) = cross / sqrt(n); - } - else - { - n = (cross = eivecs.col(k).cross(tmp.row(1))).squaredNorm(); - if(n>safeNorm2) - eivecs.col(1) = cross / sqrt(n); - else - { - n = (cross = eivecs.col(k).cross(tmp.row(2))).squaredNorm(); - if(n>safeNorm2) - eivecs.col(1) = cross / sqrt(n); - else - { - // we should never reach this point, - // if so the last two eigenvalues are likely to be very close to each other - eivecs.col(1) = eivecs.col(k).unitOrthogonal(); - } - } - } - - // make sure that eivecs[1] is orthogonal to eivecs[2] - // FIXME: this step should not be needed - Scalar d = eivecs.col(1).dot(eivecs.col(k)); - eivecs.col(1) = (eivecs.col(1) - d * eivecs.col(k)).normalized(); - } - - eivecs.col(k==2 ? 0 : 2) = eivecs.col(k).cross(eivecs.col(1)).normalized(); + // Compute last eigenvector from the other two + eivecs.col(1) = eivecs.col(2).cross(eivecs.col(0)).normalized(); } } + // Rescale back to the original size. eivals *= scale; + eivals.array() += shift; solver.m_info = Success; solver.m_isInitialized = true; @@ -675,11 +655,12 @@ template struct direct_selfadjoint_eigenvalues struct direct_selfadjoint_eigenvalues struct direct_selfadjoint_eigenvaluesc2) + if((eivals(1)-eivals(0))<=abs(eivals(1))*Eigen::NumTraits::epsilon()) { - eivecs.col(1) << -scaledMat(1,0), scaledMat(0,0); - eivecs.col(1) /= sqrt(a2+b2); + eivecs.setIdentity(); } else { - eivecs.col(1) << -scaledMat(1,1), scaledMat(1,0); - eivecs.col(1) /= sqrt(c2+b2); - } + scaledMat.diagonal().array () -= eivals(1); + Scalar a2 = numext::abs2(scaledMat(0,0)); + Scalar c2 = numext::abs2(scaledMat(1,1)); + Scalar b2 = numext::abs2(scaledMat(1,0)); + if(a2>c2) + { + eivecs.col(1) << -scaledMat(1,0), scaledMat(0,0); + eivecs.col(1) /= sqrt(a2+b2); + } + else + { + eivecs.col(1) << -scaledMat(1,1), scaledMat(1,0); + eivecs.col(1) /= sqrt(c2+b2); + } - eivecs.col(0) << eivecs.col(1).unitOrthogonal(); + eivecs.col(0) << eivecs.col(1).unitOrthogonal(); + } } // Rescale back to the original size. @@ -746,7 +736,7 @@ SelfAdjointEigenSolver& SelfAdjointEigenSolver } namespace internal { -template +template static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n) { using std::abs; @@ -798,8 +788,7 @@ static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index sta // apply the givens rotation to the unit matrix Q = Q * G if (matrixQ) { - // FIXME if StorageOrder == RowMajor this operation is not very efficient - Map > q(matrixQ,n,n); + Map > q(matrixQ,n,n); q.applyOnTheRight(k,k+1,rot); } } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h index 8e186d57a..7e1cd9eb7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h @@ -19,10 +19,12 @@ namespace Eigen { * * \brief An axis aligned box * - * \param _Scalar the type of the scalar coefficients - * \param _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic. + * \tparam _Scalar the type of the scalar coefficients + * \tparam _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic. * * This class represents an axis aligned box as a pair of the minimal and maximal corners. + * \warning The result of most methods is undefined when applied to an empty box. You can check for empty boxes using isEmpty(). + * \sa alignedboxtypedefs */ template class AlignedBox @@ -40,18 +42,21 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** Define constants to name the corners of a 1D, 2D or 3D axis aligned bounding box */ enum CornerType { - /** 1D names */ + /** 1D names @{ */ Min=0, Max=1, + /** @} */ - /** Added names for 2D */ + /** Identifier for 2D corner @{ */ BottomLeft=0, BottomRight=1, TopLeft=2, TopRight=3, + /** @} */ - /** Added names for 3D */ + /** Identifier for 3D corner @{ */ BottomLeftFloor=0, BottomRightFloor=1, TopLeftFloor=2, TopRightFloor=3, BottomLeftCeil=4, BottomRightCeil=5, TopLeftCeil=6, TopRightCeil=7 + /** @} */ }; @@ -63,34 +68,33 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) inline explicit AlignedBox(Index _dim) : m_min(_dim), m_max(_dim) { setEmpty(); } - /** Constructs a box with extremities \a _min and \a _max. */ + /** Constructs a box with extremities \a _min and \a _max. + * \warning If either component of \a _min is larger than the same component of \a _max, the constructed box is empty. */ template inline AlignedBox(const OtherVectorType1& _min, const OtherVectorType2& _max) : m_min(_min), m_max(_max) {} /** Constructs a box containing a single point \a p. */ template - inline explicit AlignedBox(const MatrixBase& a_p) - { - typename internal::nested::type p(a_p.derived()); - m_min = p; - m_max = p; - } + inline explicit AlignedBox(const MatrixBase& p) : m_min(p), m_max(m_min) + { } ~AlignedBox() {} /** \returns the dimension in which the box holds */ inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); } - /** \deprecated use isEmpty */ + /** \deprecated use isEmpty() */ inline bool isNull() const { return isEmpty(); } - /** \deprecated use setEmpty */ + /** \deprecated use setEmpty() */ inline void setNull() { setEmpty(); } - /** \returns true if the box is empty. */ + /** \returns true if the box is empty. + * \sa setEmpty */ inline bool isEmpty() const { return (m_min.array() > m_max.array()).any(); } - /** Makes \c *this an empty box. */ + /** Makes \c *this an empty box. + * \sa isEmpty */ inline void setEmpty() { m_min.setConstant( ScalarTraits::highest() ); @@ -159,7 +163,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) * a uniform distribution */ inline VectorType sample() const { - VectorType r; + VectorType r(dim()); for(Index d=0; d - inline bool contains(const MatrixBase& a_p) const + inline bool contains(const MatrixBase& p) const { - typename internal::nested::type p(a_p.derived()); - return (m_min.array()<=p.array()).all() && (p.array()<=m_max.array()).all(); + typename internal::nested::type p_n(p.derived()); + return (m_min.array()<=p_n.array()).all() && (p_n.array()<=m_max.array()).all(); } /** \returns true if the box \a b is entirely inside the box \c *this. */ inline bool contains(const AlignedBox& b) const { return (m_min.array()<=(b.min)().array()).all() && ((b.max)().array()<=m_max.array()).all(); } - /** Extends \c *this such that it contains the point \a p and returns a reference to \c *this. */ + /** \returns true if the box \a b is intersecting the box \c *this. + * \sa intersection, clamp */ + inline bool intersects(const AlignedBox& b) const + { return (m_min.array()<=(b.max)().array()).all() && ((b.min)().array()<=m_max.array()).all(); } + + /** Extends \c *this such that it contains the point \a p and returns a reference to \c *this. + * \sa extend(const AlignedBox&) */ template - inline AlignedBox& extend(const MatrixBase& a_p) + inline AlignedBox& extend(const MatrixBase& p) { - typename internal::nested::type p(a_p.derived()); - m_min = m_min.cwiseMin(p); - m_max = m_max.cwiseMax(p); + typename internal::nested::type p_n(p.derived()); + m_min = m_min.cwiseMin(p_n); + m_max = m_max.cwiseMax(p_n); return *this; } - /** Extends \c *this such that it contains the box \a b and returns a reference to \c *this. */ + /** Extends \c *this such that it contains the box \a b and returns a reference to \c *this. + * \sa merged, extend(const MatrixBase&) */ inline AlignedBox& extend(const AlignedBox& b) { m_min = m_min.cwiseMin(b.m_min); @@ -203,7 +214,9 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) return *this; } - /** Clamps \c *this by the box \a b and returns a reference to \c *this. */ + /** Clamps \c *this by the box \a b and returns a reference to \c *this. + * \note If the boxes don't intersect, the resulting box is empty. + * \sa intersection(), intersects() */ inline AlignedBox& clamp(const AlignedBox& b) { m_min = m_min.cwiseMax(b.m_min); @@ -211,11 +224,15 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) return *this; } - /** Returns an AlignedBox that is the intersection of \a b and \c *this */ + /** Returns an AlignedBox that is the intersection of \a b and \c *this + * \note If the boxes don't intersect, the resulting box is empty. + * \sa intersects(), clamp, contains() */ inline AlignedBox intersection(const AlignedBox& b) const {return AlignedBox(m_min.cwiseMax(b.m_min), m_max.cwiseMin(b.m_max)); } - /** Returns an AlignedBox that is the union of \a b and \c *this */ + /** Returns an AlignedBox that is the union of \a b and \c *this. + * \note Merging with an empty box may result in a box bigger than \c *this. + * \sa extend(const AlignedBox&) */ inline AlignedBox merged(const AlignedBox& b) const { return AlignedBox(m_min.cwiseMin(b.m_min), m_max.cwiseMax(b.m_max)); } @@ -231,20 +248,20 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** \returns the squared distance between the point \a p and the box \c *this, * and zero if \a p is inside the box. - * \sa exteriorDistance() + * \sa exteriorDistance(const MatrixBase&), squaredExteriorDistance(const AlignedBox&) */ template - inline Scalar squaredExteriorDistance(const MatrixBase& a_p) const; + inline Scalar squaredExteriorDistance(const MatrixBase& p) const; /** \returns the squared distance between the boxes \a b and \c *this, * and zero if the boxes intersect. - * \sa exteriorDistance() + * \sa exteriorDistance(const AlignedBox&), squaredExteriorDistance(const MatrixBase&) */ inline Scalar squaredExteriorDistance(const AlignedBox& b) const; /** \returns the distance between the point \a p and the box \c *this, * and zero if \a p is inside the box. - * \sa squaredExteriorDistance() + * \sa squaredExteriorDistance(const MatrixBase&), exteriorDistance(const AlignedBox&) */ template inline NonInteger exteriorDistance(const MatrixBase& p) const @@ -252,7 +269,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** \returns the distance between the boxes \a b and \c *this, * and zero if the boxes intersect. - * \sa squaredExteriorDistance() + * \sa squaredExteriorDistance(const AlignedBox&), exteriorDistance(const MatrixBase&) */ inline NonInteger exteriorDistance(const AlignedBox& b) const { using std::sqrt; return sqrt(NonInteger(squaredExteriorDistance(b))); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h index 553d38c74..bbf6a7ed8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h @@ -131,7 +131,7 @@ public: m_angle = Scalar(other.angle()); } - static inline const AngleAxis Identity() { return AngleAxis(0, Vector3::UnitX()); } + static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); } /** \returns \c true if \c *this is approximately equal to \a other, within the precision * determined by \a prec. @@ -165,8 +165,8 @@ AngleAxis& AngleAxis::operator=(const QuaternionBase::dummy_precision()*NumTraits::dummy_precision()) { - m_angle = 0; - m_axis << 1, 0, 0; + m_angle = Scalar(0); + m_axis << Scalar(1), Scalar(0), Scalar(0); } else { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h index 00e71d190..372e422b9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h @@ -79,7 +79,7 @@ template class Homogeneous { if( (int(Direction)==Vertical && row==m_matrix.rows()) || (int(Direction)==Horizontal && col==m_matrix.cols())) - return 1; + return Scalar(1); return m_matrix.coeff(row, col); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h index f06653f1c..25ed17bb6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h @@ -102,11 +102,11 @@ public: /** \returns a quaternion representing an identity rotation * \sa MatrixBase::Identity() */ - static inline Quaternion Identity() { return Quaternion(1, 0, 0, 0); } + static inline Quaternion Identity() { return Quaternion(Scalar(1), Scalar(0), Scalar(0), Scalar(0)); } /** \sa QuaternionBase::Identity(), MatrixBase::setIdentity() */ - inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; } + inline QuaternionBase& setIdentity() { coeffs() << Scalar(0), Scalar(0), Scalar(0), Scalar(1); return *this; } /** \returns the squared norm of the quaternion's coefficients * \sa QuaternionBase::norm(), MatrixBase::squaredNorm() @@ -161,7 +161,7 @@ public: { return coeffs().isApprox(other.coeffs(), prec); } /** return the result vector of \a v through the rotation*/ - EIGEN_STRONG_INLINE Vector3 _transformVector(Vector3 v) const; + EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const; /** \returns \c *this with scalar type casted to \a NewScalarType * @@ -231,7 +231,7 @@ class Quaternion : public QuaternionBase > public: typedef _Scalar Scalar; - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Quaternion) using Base::operator*=; typedef typename internal::traits::Coefficients Coefficients; @@ -341,7 +341,7 @@ class Map, _Options > public: typedef _Scalar Scalar; typedef typename internal::traits::Coefficients Coefficients; - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) using Base::operator*=; /** Constructs a Mapped Quaternion object from the pointer \a coeffs @@ -378,7 +378,7 @@ class Map, _Options > public: typedef _Scalar Scalar; typedef typename internal::traits::Coefficients Coefficients; - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) using Base::operator*=; /** Constructs a Mapped Quaternion object from the pointer \a coeffs @@ -461,7 +461,7 @@ EIGEN_STRONG_INLINE Derived& QuaternionBase::operator*= (const Quaterni */ template EIGEN_STRONG_INLINE typename QuaternionBase::Vector3 -QuaternionBase::_transformVector(Vector3 v) const +QuaternionBase::_transformVector(const Vector3& v) const { // Note that this algorithm comes from the optimization by hand // of the conversion to a Matrix followed by a Matrix/Vector product. @@ -637,7 +637,7 @@ inline Quaternion::Scalar> QuaternionBasesquaredNorm(); - if (n2 > 0) + if (n2 > Scalar(0)) return Quaternion(conjugate().coeffs() / n2); else { @@ -667,12 +667,10 @@ template inline typename internal::traits::Scalar QuaternionBase::angularDistance(const QuaternionBase& other) const { - using std::acos; + using std::atan2; using std::abs; - Scalar d = abs(this->dot(other)); - if (d>=Scalar(1)) - return Scalar(0); - return Scalar(2) * acos(d); + Quaternion d = (*this) * other.conjugate(); + return Scalar(2) * atan2( d.vec().norm(), abs(d.w()) ); } @@ -712,7 +710,7 @@ QuaternionBase::slerp(const Scalar& t, const QuaternionBase(scale0 * coeffs() + scale1 * other.coeffs()); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h index 73ca9bfde..1f3c060d0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h @@ -65,10 +65,10 @@ class DiagonalPreconditioner { typename MatType::InnerIterator it(mat,j); while(it && it.index()!=j) ++it; - if(it && it.index()==j) + if(it && it.index()==j && it.value()!=Scalar(0)) m_invdiag(j) = Scalar(1)/it.value(); else - m_invdiag(j) = 0; + m_invdiag(j) = Scalar(1); } m_isInitialized = true; return *this; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h index dd135c21f..551221907 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h @@ -151,20 +151,7 @@ struct traits > * \endcode * * By default the iterations start with x=0 as an initial guess of the solution. - * One can control the start using the solveWithGuess() method. Here is a step by - * step execution example starting with a random guess and printing the evolution - * of the estimated error: - * * \code - * x = VectorXd::Random(n); - * solver.setMaxIterations(1); - * int i = 0; - * do { - * x = solver.solveWithGuess(b,x); - * std::cout << i << " : " << solver.error() << std::endl; - * ++i; - * } while (solver.info()!=Success && i<100); - * \endcode - * Note that such a step by step excution is slightly slower. + * One can control the start using the solveWithGuess() method. * * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner */ @@ -199,7 +186,8 @@ public: * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - BiCGSTAB(const MatrixType& A) : Base(A) {} + template + explicit BiCGSTAB(const EigenBase& A) : Base(A.derived()) {} ~BiCGSTAB() {} diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h index a74a8155e..1a7e569c8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h @@ -112,9 +112,9 @@ struct traits > * This class allows to solve for A.x = b sparse linear problems using a conjugate gradient algorithm. * The sparse matrix A must be selfadjoint. The vectors x and b can be either dense or sparse. * - * \tparam _MatrixType the type of the sparse matrix A, can be a dense or a sparse matrix. - * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower - * or Upper. Default is Lower. + * \tparam _MatrixType the type of the matrix A, can be a dense or a sparse matrix. + * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower, + * Upper, or Lower|Upper in which the full matrix entries will be considered. Default is Lower. * \tparam _Preconditioner the type of the preconditioner. Default is DiagonalPreconditioner * * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations() @@ -137,20 +137,7 @@ struct traits > * \endcode * * By default the iterations start with x=0 as an initial guess of the solution. - * One can control the start using the solveWithGuess() method. Here is a step by - * step execution example starting with a random guess and printing the evolution - * of the estimated error: - * * \code - * x = VectorXd::Random(n); - * cg.setMaxIterations(1); - * int i = 0; - * do { - * x = cg.solveWithGuess(b,x); - * std::cout << i << " : " << cg.error() << std::endl; - * ++i; - * } while (cg.info()!=Success && i<100); - * \endcode - * Note that such a step by step excution is slightly slower. + * One can control the start using the solveWithGuess() method. * * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner */ @@ -189,7 +176,8 @@ public: * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - ConjugateGradient(const MatrixType& A) : Base(A) {} + template + explicit ConjugateGradient(const EigenBase& A) : Base(A.derived()) {} ~ConjugateGradient() {} @@ -213,6 +201,10 @@ public: template void _solveWithGuess(const Rhs& b, Dest& x) const { + typedef typename internal::conditional + >::type MatrixWrapperType; m_iterations = Base::maxIterations(); m_error = Base::m_tolerance; @@ -222,8 +214,7 @@ public: m_error = Base::m_tolerance; typename Dest::ColXpr xj(x,j); - internal::conjugate_gradient(mp_matrix->template selfadjointView(), b.col(j), xj, - Base::m_preconditioner, m_iterations, m_error); + internal::conjugate_gradient(MatrixWrapperType(*mp_matrix), b.col(j), xj, Base::m_preconditioner, m_iterations, m_error); } m_isInitialized = true; @@ -234,7 +225,7 @@ public: template void _solve(const Rhs& b, Dest& x) const { - x.setOnes(); + x.setZero(); _solveWithGuess(b,x); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h index b55afc136..d3f37fea2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h @@ -150,7 +150,6 @@ class IncompleteLUT : internal::noncopyable { analyzePattern(amat); factorize(amat); - m_isInitialized = m_factorizationIsOk; return *this; } @@ -160,7 +159,7 @@ class IncompleteLUT : internal::noncopyable template void _solve(const Rhs& b, Dest& x) const { - x = m_Pinv * b; + x = m_Pinv * b; x = m_lu.template triangularView().solve(x); x = m_lu.template triangularView().solve(x); x = m_P * x; @@ -223,18 +222,29 @@ template void IncompleteLUT::analyzePattern(const _MatrixType& amat) { // Compute the Fill-reducing permutation + // Since ILUT does not perform any numerical pivoting, + // it is highly preferable to keep the diagonal through symmetric permutations. +#ifndef EIGEN_MPL2_ONLY + // To this end, let's symmetrize the pattern and perform AMD on it. SparseMatrix mat1 = amat; SparseMatrix mat2 = amat.transpose(); - // Symmetrize the pattern // FIXME for a matrix with nearly symmetric pattern, mat2+mat1 is the appropriate choice. // on the other hand for a really non-symmetric pattern, mat2*mat1 should be prefered... SparseMatrix AtA = mat2 + mat1; - AtA.prune(keep_diag()); - internal::minimum_degree_ordering(AtA, m_P); // Then compute the AMD ordering... - - m_Pinv = m_P.inverse(); // ... and the inverse permutation + AMDOrdering ordering; + ordering(AtA,m_P); + m_Pinv = m_P.inverse(); // cache the inverse permutation +#else + // If AMD is not available, (MPL2-only), then let's use the slower COLAMD routine. + SparseMatrix mat1 = amat; + COLAMDOrdering ordering; + ordering(mat1,m_Pinv); + m_P = m_Pinv.inverse(); +#endif m_analysisIsOk = true; + m_factorizationIsOk = false; + m_isInitialized = false; } template @@ -442,6 +452,7 @@ void IncompleteLUT::factorize(const _MatrixType& amat) m_lu.makeCompressed(); m_factorizationIsOk = true; + m_isInitialized = m_factorizationIsOk; m_info = Success; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h index 2036922d6..501ef2f8d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h @@ -49,10 +49,11 @@ public: * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - IterativeSolverBase(const MatrixType& A) + template + IterativeSolverBase(const EigenBase& A) { init(); - compute(A); + compute(A.derived()); } ~IterativeSolverBase() {} @@ -62,9 +63,11 @@ public: * Currently, this function mostly call analyzePattern on the preconditioner. In the future * we might, for instance, implement column reodering for faster matrix vector products. */ - Derived& analyzePattern(const MatrixType& A) + template + Derived& analyzePattern(const EigenBase& A) { - m_preconditioner.analyzePattern(A); + grabInput(A.derived()); + m_preconditioner.analyzePattern(*mp_matrix); m_isInitialized = true; m_analysisIsOk = true; m_info = Success; @@ -80,11 +83,12 @@ public: * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - Derived& factorize(const MatrixType& A) + template + Derived& factorize(const EigenBase& A) { + grabInput(A.derived()); eigen_assert(m_analysisIsOk && "You must first call analyzePattern()"); - mp_matrix = &A; - m_preconditioner.factorize(A); + m_preconditioner.factorize(*mp_matrix); m_factorizationIsOk = true; m_info = Success; return derived(); @@ -100,10 +104,11 @@ public: * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - Derived& compute(const MatrixType& A) + template + Derived& compute(const EigenBase& A) { - mp_matrix = &A; - m_preconditioner.compute(A); + grabInput(A.derived()); + m_preconditioner.compute(*mp_matrix); m_isInitialized = true; m_analysisIsOk = true; m_factorizationIsOk = true; @@ -212,6 +217,28 @@ public: } protected: + + template + void grabInput(const EigenBase& A) + { + // we const cast to prevent the creation of a MatrixType temporary by the compiler. + grabInput_impl(A.const_cast_derived()); + } + + template + void grabInput_impl(const EigenBase& A) + { + m_copyMatrix = A; + mp_matrix = &m_copyMatrix; + } + + void grabInput_impl(MatrixType& A) + { + if(MatrixType::RowsAtCompileTime==Dynamic && MatrixType::ColsAtCompileTime==Dynamic) + m_copyMatrix.resize(0,0); + mp_matrix = &A; + } + void init() { m_isInitialized = false; @@ -220,6 +247,7 @@ protected: m_maxIterations = -1; m_tolerance = NumTraits::epsilon(); } + MatrixType m_copyMatrix; const MatrixType* mp_matrix; Preconditioner m_preconditioner; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h index 79ab6a8c8..26bc71447 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h @@ -374,6 +374,12 @@ template class FullPivLU inline Index cols() const { return m_lu.cols(); } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + MatrixType m_lu; PermutationPType m_p; PermutationQType m_q; @@ -418,6 +424,8 @@ FullPivLU::FullPivLU(const MatrixType& matrix) template FullPivLU& FullPivLU::compute(const MatrixType& matrix) { + check_template_parameters(); + // the permutations are stored as int indices, so just to be sure: eigen_assert(matrix.rows()<=NumTraits::highest() && matrix.cols()<=NumTraits::highest()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h index 740ee694c..7d1db948c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h @@ -171,6 +171,12 @@ template class PartialPivLU inline Index cols() const { return m_lu.cols(); } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + MatrixType m_lu; PermutationType m_p; TranspositionType m_rowsTranspositions; @@ -386,6 +392,8 @@ void partial_lu_inplace(MatrixType& lu, TranspositionType& row_transpositions, t template PartialPivLU& PartialPivLU::compute(const MatrixType& matrix) { + check_template_parameters(); + // the row permutation is stored as int indices, so just to be sure: eigen_assert(matrix.rows()::highest()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h b/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h index 41b4fd7e3..70550b8a9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h @@ -137,22 +137,27 @@ void minimum_degree_ordering(SparseMatrix& C, Permutation degree[i] = len[i]; // degree of node i } mark = internal::cs_wclear(0, 0, w, n); /* clear w */ - elen[n] = -2; /* n is a dead element */ - Cp[n] = -1; /* n is a root of assembly tree */ - w[n] = 0; /* n is a dead element */ /* --- Initialize degree lists ------------------------------------------ */ for(i = 0; i < n; i++) { + bool has_diag = false; + for(p = Cp[i]; p dense) /* node i is dense */ + else if(d > dense || !has_diag) /* node i is dense or has no structural diagonal element */ { nv[i] = 0; /* absorb i into element n */ elen[i] = -1; /* node i is dead */ @@ -168,6 +173,10 @@ void minimum_degree_ordering(SparseMatrix& C, Permutation } } + elen[n] = -2; /* n is a dead element */ + Cp[n] = -1; /* n is a root of assembly tree */ + w[n] = 0; /* n is a dead element */ + while (nel < n) /* while (selecting pivots) do */ { /* --- Select node of minimum approximate degree -------------------- */ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h index 773d1df8f..567eab7cd 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h @@ -384,6 +384,12 @@ template class ColPivHouseholderQR } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + MatrixType m_qr; HCoeffsType m_hCoeffs; PermutationType m_colsPermutation; @@ -422,6 +428,8 @@ typename MatrixType::RealScalar ColPivHouseholderQR::logAbsDetermina template ColPivHouseholderQR& ColPivHouseholderQR::compute(const MatrixType& matrix) { + check_template_parameters(); + using std::abs; Index rows = matrix.rows(); Index cols = matrix.cols(); @@ -463,20 +471,10 @@ ColPivHouseholderQR& ColPivHouseholderQR::compute(const // we store that back into our table: it can't hurt to correct our table. m_colSqNorms.coeffRef(biggest_col_index) = biggest_col_sq_norm; - // if the current biggest column is smaller than epsilon times the initial biggest column, - // terminate to avoid generating nan/inf values. - // Note that here, if we test instead for "biggest == 0", we get a failure every 1000 (or so) - // repetitions of the unit test, with the result of solve() filled with large values of the order - // of 1/(size*epsilon). - if(biggest_col_sq_norm < threshold_helper * RealScalar(rows-k)) - { + // Track the number of meaningful pivots but do not stop the decomposition to make + // sure that the initial matrix is properly reproduced. See bug 941. + if(m_nonzero_pivots==size && biggest_col_sq_norm < threshold_helper * RealScalar(rows-k)) m_nonzero_pivots = k; - m_hCoeffs.tail(size-k).setZero(); - m_qr.bottomRightCorner(rows-k,cols-k) - .template triangularView() - .setZero(); - break; - } // apply the transposition to the columns m_colsTranspositions.coeffRef(k) = biggest_col_index; @@ -505,7 +503,7 @@ ColPivHouseholderQR& ColPivHouseholderQR::compute(const } m_colsPermutation.setIdentity(PermIndexType(cols)); - for(PermIndexType k = 0; k < m_nonzero_pivots; ++k) + for(PermIndexType k = 0; k < size/*m_nonzero_pivots*/; ++k) m_colsPermutation.applyTranspositionOnTheRight(k, PermIndexType(m_colsTranspositions.coeff(k))); m_det_pq = (number_of_transpositions%2) ? -1 : 1; @@ -555,13 +553,15 @@ struct solve_retval, Rhs> } // end namespace internal -/** \returns the matrix Q as a sequence of householder transformations */ +/** \returns the matrix Q as a sequence of householder transformations. + * You can extract the meaningful part only by using: + * \code qr.householderQ().setLength(qr.nonzeroPivots()) \endcode*/ template typename ColPivHouseholderQR::HouseholderSequenceType ColPivHouseholderQR ::householderQ() const { eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized."); - return HouseholderSequenceType(m_qr, m_hCoeffs.conjugate()).setLength(m_nonzero_pivots); + return HouseholderSequenceType(m_qr, m_hCoeffs.conjugate()); } /** \return the column-pivoting Householder QR decomposition of \c *this. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h index e66196b1d..b1332be5e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h @@ -63,12 +63,12 @@ ColPivHouseholderQR class FullPivHouseholderQR RealScalar maxPivot() const { return m_maxpivot; } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + MatrixType m_qr; HCoeffsType m_hCoeffs; IntDiagSizeVectorType m_rows_transpositions; @@ -407,6 +413,8 @@ typename MatrixType::RealScalar FullPivHouseholderQR::logAbsDetermin template FullPivHouseholderQR& FullPivHouseholderQR::compute(const MatrixType& matrix) { + check_template_parameters(); + using std::abs; Index rows = matrix.rows(); Index cols = matrix.cols(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h index 3a94a3c34..343a66499 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h @@ -189,6 +189,12 @@ template class HouseholderQR const HCoeffsType& hCoeffs() const { return m_hCoeffs; } protected: + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } + MatrixType m_qr; HCoeffsType m_hCoeffs; RowVectorType m_temp; @@ -349,6 +355,8 @@ struct solve_retval, Rhs> template HouseholderQR& HouseholderQR::compute(const MatrixType& matrix) { + check_template_parameters(); + Index rows = matrix.rows(); Index cols = matrix.cols(); Index size = (std::min)(rows,cols); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h index a2cc2a9e2..36138101d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h @@ -47,7 +47,7 @@ namespace Eigen { * You can then apply it to a vector. * * R is the sparse triangular factor. Use matrixQR() to get it as SparseMatrix. - * NOTE : The Index type of R is always UF_long. You can get it with SPQR::Index + * NOTE : The Index type of R is always SuiteSparse_long. You can get it with SPQR::Index * * \tparam _MatrixType The type of the sparse matrix A, must be a column-major SparseMatrix<> * NOTE @@ -59,24 +59,18 @@ class SPQR public: typedef typename _MatrixType::Scalar Scalar; typedef typename _MatrixType::RealScalar RealScalar; - typedef UF_long Index ; + typedef SuiteSparse_long Index ; typedef SparseMatrix MatrixType; typedef PermutationMatrix PermutationType; public: SPQR() - : m_isInitialized(false), - m_ordering(SPQR_ORDERING_DEFAULT), - m_allow_tol(SPQR_DEFAULT_TOL), - m_tolerance (NumTraits::epsilon()) + : m_isInitialized(false), m_ordering(SPQR_ORDERING_DEFAULT), m_allow_tol(SPQR_DEFAULT_TOL), m_tolerance (NumTraits::epsilon()), m_useDefaultThreshold(true) { cholmod_l_start(&m_cc); } - SPQR(const _MatrixType& matrix) - : m_isInitialized(false), - m_ordering(SPQR_ORDERING_DEFAULT), - m_allow_tol(SPQR_DEFAULT_TOL), - m_tolerance (NumTraits::epsilon()) + SPQR(const _MatrixType& matrix) + : m_isInitialized(false), m_ordering(SPQR_ORDERING_DEFAULT), m_allow_tol(SPQR_DEFAULT_TOL), m_tolerance (NumTraits::epsilon()), m_useDefaultThreshold(true) { cholmod_l_start(&m_cc); compute(matrix); @@ -101,10 +95,26 @@ class SPQR if(m_isInitialized) SPQR_free(); MatrixType mat(matrix); + + /* Compute the default threshold as in MatLab, see: + * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing + * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011, Page 8:3 + */ + RealScalar pivotThreshold = m_tolerance; + if(m_useDefaultThreshold) + { + using std::max; + RealScalar max2Norm = 0.0; + for (int j = 0; j < mat.cols(); j++) max2Norm = (max)(max2Norm, mat.col(j).norm()); + if(max2Norm==RealScalar(0)) + max2Norm = RealScalar(1); + pivotThreshold = 20 * (mat.rows() + mat.cols()) * max2Norm * NumTraits::epsilon(); + } + cholmod_sparse A; A = viewAsCholmod(mat); Index col = matrix.cols(); - m_rank = SuiteSparseQR(m_ordering, m_tolerance, col, &A, + m_rank = SuiteSparseQR(m_ordering, pivotThreshold, col, &A, &m_cR, &m_E, &m_H, &m_HPinv, &m_HTau, &m_cc); if (!m_cR) @@ -120,7 +130,7 @@ class SPQR /** * Get the number of rows of the input matrix and the Q matrix */ - inline Index rows() const {return m_H->nrow; } + inline Index rows() const {return m_cR->nrow; } /** * Get the number of columns of the input matrix. @@ -145,16 +155,25 @@ class SPQR { eigen_assert(m_isInitialized && " The QR factorization should be computed first, call compute()"); eigen_assert(b.cols()==1 && "This method is for vectors only"); - + //Compute Q^T * b - typename Dest::PlainObject y; + typename Dest::PlainObject y, y2; y = matrixQ().transpose() * b; - // Solves with the triangular matrix R + + // Solves with the triangular matrix R Index rk = this->rank(); - y.topRows(rk) = this->matrixR().topLeftCorner(rk, rk).template triangularView().solve(y.topRows(rk)); - y.bottomRows(cols()-rk).setZero(); + y2 = y; + y.resize((std::max)(cols(),Index(y.rows())),y.cols()); + y.topRows(rk) = this->matrixR().topLeftCorner(rk, rk).template triangularView().solve(y2.topRows(rk)); + // Apply the column permutation - dest.topRows(cols()) = colsPermutation() * y.topRows(cols()); + // colsPermutation() performs a copy of the permutation, + // so let's apply it manually: + for(Index i = 0; i < rk; ++i) dest.row(m_E[i]) = y.row(i); + for(Index i = rk; i < cols(); ++i) dest.row(m_E[i]).setZero(); + +// y.bottomRows(y.rows()-rk).setZero(); +// dest = colsPermutation() * y.topRows(cols()); m_info = Success; } @@ -197,7 +216,11 @@ class SPQR /// Set the fill-reducing ordering method to be used void setSPQROrdering(int ord) { m_ordering = ord;} /// Set the tolerance tol to treat columns with 2-norm < =tol as zero - void setPivotThreshold(const RealScalar& tol) { m_tolerance = tol; } + void setPivotThreshold(const RealScalar& tol) + { + m_useDefaultThreshold = false; + m_tolerance = tol; + } /** \returns a pointer to the SPQR workspace */ cholmod_common *cholmodCommon() const { return &m_cc; } @@ -230,6 +253,7 @@ class SPQR mutable cholmod_dense *m_HTau; // The Householder coefficients mutable Index m_rank; // The rank of the matrix mutable cholmod_common m_cc; // Workspace and parameters + bool m_useDefaultThreshold; // Use default threshold template friend struct SPQR_QProduct; }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h index c57f2974c..1b2977419 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h @@ -742,6 +742,11 @@ template class JacobiSVD private: void allocate(Index rows, Index cols, unsigned int computationOptions); + + static void check_template_parameters() + { + EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); + } protected: MatrixUType m_matrixU; @@ -818,6 +823,8 @@ template JacobiSVD& JacobiSVD::compute(const MatrixType& matrix, unsigned int computationOptions) { + check_template_parameters(); + using std::abs; allocate(matrix.rows(), matrix.cols(), computationOptions); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h index 0ede034ba..0c90bafbe 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h @@ -57,6 +57,16 @@ public: inline BlockImpl(const XprType& xpr, int startRow, int startCol, int blockRows, int blockCols) : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) {} + + inline const Scalar coeff(int row, int col) const + { + return m_matrix.coeff(row + IsRowMajor ? m_outerStart : 0, col +IsRowMajor ? 0 : m_outerStart); + } + + inline const Scalar coeff(int index) const + { + return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); + } EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } @@ -226,6 +236,21 @@ public: else return Map >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum(); } + + inline Scalar& coeffRef(int row, int col) + { + return m_matrix.const_cast_derived().coeffRef(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); + } + + inline const Scalar coeff(int row, int col) const + { + return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); + } + + inline const Scalar coeff(int index) const + { + return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); + } const Scalar& lastCoeff() const { @@ -313,6 +338,16 @@ public: else return Map >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum(); } + + inline const Scalar coeff(int row, int col) const + { + return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); + } + + inline const Scalar coeff(int index) const + { + return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); + } const Scalar& lastCoeff() const { @@ -355,7 +390,8 @@ const typename SparseMatrixBase::ConstInnerVectorReturnType SparseMatri * is col-major (resp. row-major). */ template -Block SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) +typename SparseMatrixBase::InnerVectorsReturnType +SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) { return Block(derived(), IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart, @@ -367,7 +403,8 @@ Block SparseMatrixBase::innerVectors(Inde * is col-major (resp. row-major). Read-only. */ template -const Block SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) const +const typename SparseMatrixBase::ConstInnerVectorsReturnType +SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) const { return Block(derived(), IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart, @@ -394,8 +431,8 @@ public: : m_matrix(xpr), m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? i : 0), m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? i : 0), - m_blockRows(xpr.rows()), - m_blockCols(xpr.cols()) + m_blockRows(BlockRows==1 ? 1 : xpr.rows()), + m_blockCols(BlockCols==1 ? 1 : xpr.cols()) {} /** Dynamic-size constructor @@ -497,3 +534,4 @@ public: } // end namespace Eigen #endif // EIGEN_SPARSE_BLOCK_H + diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h index 60ca7690c..4ca912833 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h @@ -314,10 +314,10 @@ SparseMatrixBase::operator+=(const SparseMatrixBase& othe template template -EIGEN_STRONG_INLINE const EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE +EIGEN_STRONG_INLINE const typename SparseMatrixBase::template CwiseProductDenseReturnType::Type SparseMatrixBase::cwiseProduct(const MatrixBase &other) const { - return EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE(derived(), other.derived()); + return typename CwiseProductDenseReturnType::Type(derived(), other.derived()); } } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h index a9084192e..ccb6ae7b7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h @@ -180,7 +180,7 @@ struct sparse_time_dense_product_impl >(&this->m_data.index(0), rows()).setLinSpaced(0, rows()-1); Eigen::Map >(&this->m_data.value(0), rows()).setOnes(); Eigen::Map >(this->m_outerIndex, rows()+1).setLinSpaced(0, rows()); + std::free(m_innerNonZeros); + m_innerNonZeros = 0; } inline SparseMatrix& operator=(const SparseMatrix& other) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h index 485e85bec..9341d9ad2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h @@ -23,7 +23,14 @@ namespace Eigen { * This class can be extended with the help of the plugin mechanism described on the page * \ref TopicCustomizingEigen by defining the preprocessor symbol \c EIGEN_SPARSEMATRIXBASE_PLUGIN. */ -template class SparseMatrixBase : public EigenBase +template class SparseMatrixBase +#ifndef EIGEN_PARSED_BY_DOXYGEN + : public internal::special_scalar_op_base::Scalar, + typename NumTraits::Scalar>::Real, + EigenBase > +#else + : public EigenBase +#endif // not EIGEN_PARSED_BY_DOXYGEN { public: @@ -36,7 +43,6 @@ template class SparseMatrixBase : public EigenBase >::type PacketReturnType; typedef SparseMatrixBase StorageBaseType; - typedef EigenBase Base; template Derived& operator=(const EigenBase &other) @@ -132,6 +138,9 @@ template class SparseMatrixBase : public EigenBase inline Derived& derived() { return *static_cast(this); } inline Derived& const_cast_derived() const { return *static_cast(const_cast(this)); } + + typedef internal::special_scalar_op_base > Base; + using Base::operator*; #endif // not EIGEN_PARSED_BY_DOXYGEN #define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::SparseMatrixBase @@ -317,20 +326,18 @@ template class SparseMatrixBase : public EigenBase Derived& operator*=(const Scalar& other); Derived& operator/=(const Scalar& other); - #define EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE \ - CwiseBinaryOp< \ - internal::scalar_product_op< \ - typename internal::scalar_product_traits< \ - typename internal::traits::Scalar, \ - typename internal::traits::Scalar \ - >::ReturnType \ - >, \ - const Derived, \ - const OtherDerived \ - > + template struct CwiseProductDenseReturnType { + typedef CwiseBinaryOp::Scalar, + typename internal::traits::Scalar + >::ReturnType>, + const Derived, + const OtherDerived + > Type; + }; template - EIGEN_STRONG_INLINE const EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE + EIGEN_STRONG_INLINE const typename CwiseProductDenseReturnType::Type cwiseProduct(const MatrixBase &other) const; // sparse * sparse @@ -404,8 +411,10 @@ template class SparseMatrixBase : public EigenBase const ConstInnerVectorReturnType innerVector(Index outer) const; // set of inner-vectors - Block innerVectors(Index outerStart, Index outerSize); - const Block innerVectors(Index outerStart, Index outerSize) const; + typedef Block InnerVectorsReturnType; + typedef Block ConstInnerVectorsReturnType; + InnerVectorsReturnType innerVectors(Index outerStart, Index outerSize); + const ConstInnerVectorsReturnType innerVectors(Index outerStart, Index outerSize) const; /** \internal use operator= */ template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h index 0ba471320..d627546de 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h @@ -67,7 +67,6 @@ const int InnerRandomAccessPattern = 0x2 | CoherentAccessPattern; const int OuterRandomAccessPattern = 0x4 | CoherentAccessPattern; const int RandomAccessPattern = 0x8 | OuterRandomAccessPattern | InnerRandomAccessPattern; -template class SparseMatrixBase; template class SparseMatrix; template class DynamicSparseMatrix; template class SparseVector; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseVector.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseVector.h index 7e15c814b..49865d0e7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseVector.h @@ -158,6 +158,7 @@ class SparseVector Index inner = IsColVector ? row : col; Index outer = IsColVector ? col : row; + EIGEN_ONLY_USED_FOR_DEBUG(outer); eigen_assert(outer==0); return insert(inner); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h index cb8ad82b4..ccc12af79 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h @@ -69,7 +69,7 @@ struct sparse_solve_triangular_selector for(int i=lhs.rows()-1 ; i>=0 ; --i) { Scalar tmp = other.coeff(i,col); - Scalar l_ii = 0; + Scalar l_ii(0); typename Lhs::InnerIterator it(lhs, i); while(it && it.index()cols(); ++j) + { + for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) + { + if(it.index() == j) + { + if(it.value()<0) + det = -det; + else if(it.value()==0) + return 0; + break; + } + } + } + return det * m_detPermR * m_detPermC; + } + + /** \returns The determinant of the matrix. + * + * \sa absDeterminant(), logAbsDeterminant() + */ + Scalar determinant() + { + eigen_assert(m_factorizationIsOk && "The matrix should be factorized first."); + // Initialize with the determinant of the row matrix + Scalar det = Scalar(1.); + // Note that the diagonal blocks of U are stored in supernodes, + // which are available in the L part :) + for (Index j = 0; j < this->cols(); ++j) + { + for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it) + { + if(it.index() == j) + { + det *= it.value(); + break; + } + } + } + return det * Scalar(m_detPermR * m_detPermC); + } protected: // Functions void initperfvalues() { - m_perfv.panel_size = 1; + m_perfv.panel_size = 16; m_perfv.relax = 1; m_perfv.maxsuper = 128; m_perfv.rowblk = 16; @@ -345,8 +390,8 @@ class SparseLU : public internal::SparseLUImpl m_perfv; RealScalar m_diagpivotthresh; // Specifies the threshold used for a diagonal entry to be an acceptable pivot - Index m_nnzL, m_nnzU; // Nonzeros in L and U factors - Index m_detPermR; // Determinant of the coefficient matrix + Index m_nnzL, m_nnzU; // Nonzeros in L and U factors + Index m_detPermR, m_detPermC; // Determinants of the permutation matrices private: // Disable copy constructor SparseLU (const SparseLU& ); @@ -622,7 +667,8 @@ void SparseLU::factorize(const MatrixType& matrix) } // Update the determinant of the row permutation matrix - if (pivrow != jj) m_detPermR *= -1; + // FIXME: the following test is not correct, we should probably take iperm_c into account and pivrow is not directly the row pivot. + if (pivrow != jj) m_detPermR = -m_detPermR; // Prune columns (0:jj-1) using column jj Base::pruneL(jj, m_perm_r.indices(), pivrow, nseg, segrep, repfnz_k, xprune, m_glu); @@ -637,10 +683,13 @@ void SparseLU::factorize(const MatrixType& matrix) jcol += panel_size; // Move to the next panel } // end for -- end elimination + m_detPermR = m_perm_r.determinant(); + m_detPermC = m_perm_c.determinant(); + // Count the number of nonzeros in factors Base::countnz(n, m_nnzL, m_nnzU, m_glu); // Apply permutation to the L subscripts - Base::fixupL(n, m_perm_r.indices(), m_glu); + Base::fixupL(n, m_perm_r.indices(), m_glu); // Create supernode matrix L m_Lstore.setInfos(m, n, m_glu.lusup, m_glu.xlusup, m_glu.lsub, m_glu.xlsub, m_glu.supno, m_glu.xsup); @@ -700,8 +749,8 @@ struct SparseLUMatrixUReturnType : internal::no_assignment_operator } else { - Map, 0, OuterStride<> > A( &(m_mapL.valuePtr()[luptr]), nsupc, nsupc, OuterStride<>(lda) ); - Map< Matrix, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) ); + Map, 0, OuterStride<> > A( &(m_mapL.valuePtr()[luptr]), nsupc, nsupc, OuterStride<>(lda) ); + Map< Matrix, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) ); U = A.template triangularView().solve(U); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLUImpl.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLUImpl.h index 14d70897d..99d651e40 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLUImpl.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLUImpl.h @@ -21,6 +21,8 @@ class SparseLUImpl { public: typedef Matrix ScalarVector; + typedef Matrix ScalarMatrix; + typedef Map > MappedMatrixBlock; typedef Matrix IndexVector; typedef typename ScalarVector::RealScalar RealScalar; typedef Ref > BlockScalarVector; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h index 1ffa7d54e..45f96d16a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h @@ -153,8 +153,8 @@ Index SparseLUImpl::memInit(Index m, Index n, Index annz, Index lw { Index& num_expansions = glu.num_expansions; //No memory expansions so far num_expansions = 0; - glu.nzumax = glu.nzlumax = (std::min)(fillratio * annz / n, m) * n; // estimated number of nonzeros in U - glu.nzlmax = (std::max)(Index(4), fillratio) * annz / 4; // estimated nnz in L factor + glu.nzumax = glu.nzlumax = (std::min)(fillratio * (annz+1) / n, m) * n; // estimated number of nonzeros in U + glu.nzlmax = (std::max)(Index(4), fillratio) * (annz+1) / 4; // estimated nnz in L factor // Return the estimated size to the user if necessary Index tempSpace; tempSpace = (2*panel_size + 4 + LUNoMarker) * m * sizeof(Index) + (panel_size + 1) * m * sizeof(Scalar); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h index b16afd6a4..54a569408 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h @@ -236,7 +236,7 @@ void MappedSuperNodalMatrix::solveInPlace( MatrixBase&X) con Index n = X.rows(); Index nrhs = X.cols(); const Scalar * Lval = valuePtr(); // Nonzero values - Matrix work(n, nrhs); // working vector + Matrix work(n, nrhs); // working vector work.setZero(); for (Index k = 0; k <= nsuper(); k ++) { @@ -267,12 +267,12 @@ void MappedSuperNodalMatrix::solveInPlace( MatrixBase&X) con Index lda = colIndexPtr()[fsupc+1] - luptr; // Triangular solve - Map, 0, OuterStride<> > A( &(Lval[luptr]), nsupc, nsupc, OuterStride<>(lda) ); - Map< Matrix, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) ); + Map, 0, OuterStride<> > A( &(Lval[luptr]), nsupc, nsupc, OuterStride<>(lda) ); + Map< Matrix, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) ); U = A.template triangularView().solve(U); // Matrix-vector product - new (&A) Map, 0, OuterStride<> > ( &(Lval[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) ); + new (&A) Map, 0, OuterStride<> > ( &(Lval[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) ); work.block(0, 0, nrow, nrhs) = A * U; //Begin Scatter diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_column_bmod.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_column_bmod.h index f24bd87d3..cacc7e987 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_column_bmod.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_column_bmod.h @@ -162,11 +162,11 @@ Index SparseLUImpl::column_bmod(const Index jcol, const Index nseg // points to the beginning of jcol in snode L\U(jsupno) ufirst = glu.xlusup(jcol) + d_fsupc; Index lda = glu.xlusup(jcol+1) - glu.xlusup(jcol); - Map, 0, OuterStride<> > A( &(glu.lusup.data()[luptr]), nsupc, nsupc, OuterStride<>(lda) ); + MappedMatrixBlock A( &(glu.lusup.data()[luptr]), nsupc, nsupc, OuterStride<>(lda) ); VectorBlock u(glu.lusup, ufirst, nsupc); u = A.template triangularView().solve(u); - new (&A) Map, 0, OuterStride<> > ( &(glu.lusup.data()[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) ); + new (&A) MappedMatrixBlock ( &(glu.lusup.data()[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) ); VectorBlock l(glu.lusup, ufirst+nsupc, nrow); l.noalias() -= A * u; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_kernel_bmod.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_kernel_bmod.h index 0d0283b13..6af026754 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_kernel_bmod.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_kernel_bmod.h @@ -56,7 +56,7 @@ EIGEN_DONT_INLINE void LU_kernel_bmod::run(const int segsi // Dense triangular solve -- start effective triangle luptr += lda * no_zeros + no_zeros; // Form Eigen matrix and vector - Map, 0, OuterStride<> > A( &(lusup.data()[luptr]), segsize, segsize, OuterStride<>(lda) ); + Map, 0, OuterStride<> > A( &(lusup.data()[luptr]), segsize, segsize, OuterStride<>(lda) ); Map > u(tempv.data(), segsize); u = A.template triangularView().solve(u); @@ -65,7 +65,7 @@ EIGEN_DONT_INLINE void LU_kernel_bmod::run(const int segsi luptr += segsize; const Index PacketSize = internal::packet_traits::size; Index ldl = internal::first_multiple(nrow, PacketSize); - Map, 0, OuterStride<> > B( &(lusup.data()[luptr]), nrow, segsize, OuterStride<>(lda) ); + Map, 0, OuterStride<> > B( &(lusup.data()[luptr]), nrow, segsize, OuterStride<>(lda) ); Index aligned_offset = internal::first_aligned(tempv.data()+segsize, PacketSize); Index aligned_with_B_offset = (PacketSize-internal::first_aligned(B.data(), PacketSize))%PacketSize; Map, 0, OuterStride<> > l(tempv.data()+segsize+aligned_offset+aligned_with_B_offset, nrow, OuterStride<>(ldl) ); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_panel_bmod.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_panel_bmod.h index da0e0fc3c..9d2ff2906 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_panel_bmod.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_panel_bmod.h @@ -102,7 +102,7 @@ void SparseLUImpl::panel_bmod(const Index m, const Index w, const if(nsupc >= 2) { Index ldu = internal::first_multiple(u_rows, PacketSize); - Map, Aligned, OuterStride<> > U(tempv.data(), u_rows, u_cols, OuterStride<>(ldu)); + Map > U(tempv.data(), u_rows, u_cols, OuterStride<>(ldu)); // gather U Index u_col = 0; @@ -136,17 +136,17 @@ void SparseLUImpl::panel_bmod(const Index m, const Index w, const Index lda = glu.xlusup(fsupc+1) - glu.xlusup(fsupc); no_zeros = (krep - u_rows + 1) - fsupc; luptr += lda * no_zeros + no_zeros; - Map, 0, OuterStride<> > A(glu.lusup.data()+luptr, u_rows, u_rows, OuterStride<>(lda) ); + MappedMatrixBlock A(glu.lusup.data()+luptr, u_rows, u_rows, OuterStride<>(lda) ); U = A.template triangularView().solve(U); // update luptr += u_rows; - Map, 0, OuterStride<> > B(glu.lusup.data()+luptr, nrow, u_rows, OuterStride<>(lda) ); + MappedMatrixBlock B(glu.lusup.data()+luptr, nrow, u_rows, OuterStride<>(lda) ); eigen_assert(tempv.size()>w*ldu + nrow*w + 1); Index ldl = internal::first_multiple(nrow, PacketSize); Index offset = (PacketSize-internal::first_aligned(B.data(), PacketSize)) % PacketSize; - Map, 0, OuterStride<> > L(tempv.data()+w*ldu+offset, nrow, u_cols, OuterStride<>(ldl)); + MappedMatrixBlock L(tempv.data()+w*ldu+offset, nrow, u_cols, OuterStride<>(ldl)); L.setZero(); internal::sparselu_gemm(L.rows(), L.cols(), B.cols(), B.data(), B.outerStride(), U.data(), U.outerStride(), L.data(), L.outerStride()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_pivotL.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_pivotL.h index ddcd4ec98..2e49ef667 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_pivotL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_pivotL.h @@ -71,13 +71,14 @@ Index SparseLUImpl::pivotL(const Index jcol, const RealScalar& dia // Determine the largest abs numerical value for partial pivoting Index diagind = iperm_c(jcol); // diagonal index - RealScalar pivmax = 0.0; + RealScalar pivmax(-1.0); Index pivptr = nsupc; Index diag = emptyIdxLU; RealScalar rtemp; Index isub, icol, itemp, k; for (isub = nsupc; isub < nsupr; ++isub) { - rtemp = std::abs(lu_col_ptr[isub]); + using std::abs; + rtemp = abs(lu_col_ptr[isub]); if (rtemp > pivmax) { pivmax = rtemp; pivptr = isub; @@ -86,8 +87,9 @@ Index SparseLUImpl::pivotL(const Index jcol, const RealScalar& dia } // Test for singularity - if ( pivmax == 0.0 ) { - pivrow = lsub_ptr[pivptr]; + if ( pivmax <= RealScalar(0.0) ) { + // if pivmax == -1, the column is structurally empty, otherwise it is only numerically zero + pivrow = pivmax < RealScalar(0.0) ? diagind : lsub_ptr[pivptr]; perm_r(pivrow) = jcol; return (jcol+1); } @@ -101,7 +103,8 @@ Index SparseLUImpl::pivotL(const Index jcol, const RealScalar& dia if (diag >= 0 ) { // Diagonal element exists - rtemp = std::abs(lu_col_ptr[diag]); + using std::abs; + rtemp = abs(lu_col_ptr[diag]); if (rtemp != 0.0 && rtemp >= thresh) pivptr = diag; } pivrow = lsub_ptr[pivptr]; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h b/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h index 5c8c476ee..1951286f3 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h @@ -70,6 +70,43 @@ max return (max)(Derived::PlainObject::Constant(rows(), cols(), other)); } + +#define EIGEN_MAKE_CWISE_COMP_OP(OP, COMPARATOR) \ +template \ +EIGEN_STRONG_INLINE const CwiseBinaryOp, const Derived, const OtherDerived> \ +OP(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const \ +{ \ + return CwiseBinaryOp, const Derived, const OtherDerived>(derived(), other.derived()); \ +}\ +typedef CwiseBinaryOp, const Derived, const CwiseNullaryOp, PlainObject> > Cmp ## COMPARATOR ## ReturnType; \ +typedef CwiseBinaryOp, const CwiseNullaryOp, PlainObject>, const Derived > RCmp ## COMPARATOR ## ReturnType; \ +EIGEN_STRONG_INLINE const Cmp ## COMPARATOR ## ReturnType \ +OP(const Scalar& s) const { \ + return this->OP(Derived::PlainObject::Constant(rows(), cols(), s)); \ +} \ +friend EIGEN_STRONG_INLINE const RCmp ## COMPARATOR ## ReturnType \ +OP(const Scalar& s, const Derived& d) { \ + return Derived::PlainObject::Constant(d.rows(), d.cols(), s).OP(d); \ +} + +#define EIGEN_MAKE_CWISE_COMP_R_OP(OP, R_OP, RCOMPARATOR) \ +template \ +EIGEN_STRONG_INLINE const CwiseBinaryOp, const OtherDerived, const Derived> \ +OP(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const \ +{ \ + return CwiseBinaryOp, const OtherDerived, const Derived>(other.derived(), derived()); \ +} \ +\ +inline const RCmp ## RCOMPARATOR ## ReturnType \ +OP(const Scalar& s) const { \ + return Derived::PlainObject::Constant(rows(), cols(), s).R_OP(*this); \ +} \ +friend inline const Cmp ## RCOMPARATOR ## ReturnType \ +OP(const Scalar& s, const Derived& d) { \ + return d.R_OP(Derived::PlainObject::Constant(d.rows(), d.cols(), s)); \ +} + + /** \returns an expression of the coefficient-wise \< operator of *this and \a other * * Example: \include Cwise_less.cpp @@ -77,7 +114,7 @@ max * * \sa all(), any(), operator>(), operator<=() */ -EIGEN_MAKE_CWISE_BINARY_OP(operator<,std::less) +EIGEN_MAKE_CWISE_COMP_OP(operator<, LT) /** \returns an expression of the coefficient-wise \<= operator of *this and \a other * @@ -86,7 +123,7 @@ EIGEN_MAKE_CWISE_BINARY_OP(operator<,std::less) * * \sa all(), any(), operator>=(), operator<() */ -EIGEN_MAKE_CWISE_BINARY_OP(operator<=,std::less_equal) +EIGEN_MAKE_CWISE_COMP_OP(operator<=, LE) /** \returns an expression of the coefficient-wise \> operator of *this and \a other * @@ -95,7 +132,7 @@ EIGEN_MAKE_CWISE_BINARY_OP(operator<=,std::less_equal) * * \sa all(), any(), operator>=(), operator<() */ -EIGEN_MAKE_CWISE_BINARY_OP(operator>,std::greater) +EIGEN_MAKE_CWISE_COMP_R_OP(operator>, operator<, LT) /** \returns an expression of the coefficient-wise \>= operator of *this and \a other * @@ -104,7 +141,7 @@ EIGEN_MAKE_CWISE_BINARY_OP(operator>,std::greater) * * \sa all(), any(), operator>(), operator<=() */ -EIGEN_MAKE_CWISE_BINARY_OP(operator>=,std::greater_equal) +EIGEN_MAKE_CWISE_COMP_R_OP(operator>=, operator<=, LE) /** \returns an expression of the coefficient-wise == operator of *this and \a other * @@ -118,7 +155,7 @@ EIGEN_MAKE_CWISE_BINARY_OP(operator>=,std::greater_equal) * * \sa all(), any(), isApprox(), isMuchSmallerThan() */ -EIGEN_MAKE_CWISE_BINARY_OP(operator==,std::equal_to) +EIGEN_MAKE_CWISE_COMP_OP(operator==, EQ) /** \returns an expression of the coefficient-wise != operator of *this and \a other * @@ -132,7 +169,10 @@ EIGEN_MAKE_CWISE_BINARY_OP(operator==,std::equal_to) * * \sa all(), any(), isApprox(), isMuchSmallerThan() */ -EIGEN_MAKE_CWISE_BINARY_OP(operator!=,std::not_equal_to) +EIGEN_MAKE_CWISE_COMP_OP(operator!=, NEQ) + +#undef EIGEN_MAKE_CWISE_COMP_OP +#undef EIGEN_MAKE_CWISE_COMP_R_OP // scalar addition @@ -209,3 +249,5 @@ operator||(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_OF_BOOL); return CwiseBinaryOp(derived(),other.derived()); } + + diff --git a/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseUnaryOps.h b/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseUnaryOps.h index a59636790..1c3ed3fcd 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseUnaryOps.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseUnaryOps.h @@ -185,19 +185,3 @@ cube() const { return derived(); } - -#define EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(METHOD_NAME,FUNCTOR) \ - inline const CwiseUnaryOp >, const Derived> \ - METHOD_NAME(const Scalar& s) const { \ - return CwiseUnaryOp >, const Derived> \ - (derived(), std::bind2nd(FUNCTOR(), s)); \ - } - -EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator==, std::equal_to) -EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator!=, std::not_equal_to) -EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator<, std::less) -EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator<=, std::less_equal) -EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator>, std::greater) -EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator>=, std::greater_equal) - - diff --git a/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h b/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h index 7f62149e0..c4a042b70 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h @@ -124,3 +124,20 @@ cwiseQuotient(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const { return CwiseBinaryOp, const Derived, const OtherDerived>(derived(), other.derived()); } + +typedef CwiseBinaryOp, const Derived, const ConstantReturnType> CwiseScalarEqualReturnType; + +/** \returns an expression of the coefficient-wise == operator of \c *this and a scalar \a s + * + * \warning this performs an exact comparison, which is generally a bad idea with floating-point types. + * In order to check for equality between two vectors or matrices with floating-point coefficients, it is + * generally a far better idea to use a fuzzy comparison as provided by isApprox() and + * isMuchSmallerThan(). + * + * \sa cwiseEqual(const MatrixBase &) const + */ +inline const CwiseScalarEqualReturnType +cwiseEqual(const Scalar& s) const +{ + return CwiseScalarEqualReturnType(derived(), Derived::Constant(rows(), cols(), s), internal::scalar_cmp_op()); +} diff --git a/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseUnaryOps.h b/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseUnaryOps.h index 0cf0640ba..8de10935d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseUnaryOps.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseUnaryOps.h @@ -50,18 +50,3 @@ cwiseSqrt() const { return derived(); } inline const CwiseUnaryOp, const Derived> cwiseInverse() const { return derived(); } -/** \returns an expression of the coefficient-wise == operator of \c *this and a scalar \a s - * - * \warning this performs an exact comparison, which is generally a bad idea with floating-point types. - * In order to check for equality between two vectors or matrices with floating-point coefficients, it is - * generally a far better idea to use a fuzzy comparison as provided by isApprox() and - * isMuchSmallerThan(). - * - * \sa cwiseEqual(const MatrixBase &) const - */ -inline const CwiseUnaryOp >, const Derived> -cwiseEqual(const Scalar& s) const -{ - return CwiseUnaryOp >,const Derived> - (derived(), std::bind1st(std::equal_to(), s)); -} diff --git a/gtsam/3rdparty/Eigen/blas/xerbla.cpp b/gtsam/3rdparty/Eigen/blas/xerbla.cpp index 0d57710fe..dd39a5244 100644 --- a/gtsam/3rdparty/Eigen/blas/xerbla.cpp +++ b/gtsam/3rdparty/Eigen/blas/xerbla.cpp @@ -1,7 +1,7 @@ #include -#if (defined __GNUC__) +#if (defined __GNUC__) && (!defined __MINGW32__) && (!defined __CYGWIN__) #define EIGEN_WEAK_LINKING __attribute__ ((weak)) #else #define EIGEN_WEAK_LINKING diff --git a/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake b/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake index 11ecc9585..2b11d8360 100644 --- a/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake +++ b/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake @@ -26,29 +26,18 @@ include(CTest) set(EIGEN_TEST_BUILD_FLAGS " " CACHE STRING "Options passed to the build command of unit tests") -# overwrite default DartConfiguration.tcl -# The worarounds are different for each version of the MSVC IDE -if(MSVC_IDE) - if(CMAKE_MAKE_PROGRAM_SAVE MATCHES "devenv") # devenv - set(EIGEN_MAKECOMMAND_PLACEHOLDER "${CMAKE_MAKE_PROGRAM_SAVE} Eigen.sln /build \"Release\" /project buildtests ${EIGEN_TEST_BUILD_FLAGS} \n# ") - else() # msbuild - set(EIGEN_MAKECOMMAND_PLACEHOLDER "${CMAKE_MAKE_PROGRAM_SAVE} buildtests.vcxproj /p:Configuration=\${CTEST_CONFIGURATION_TYPE} ${EIGEN_TEST_BUILD_FLAGS}\n# ") - endif() -else() - # for make and nmake - set(EIGEN_MAKECOMMAND_PLACEHOLDER "${CMAKE_MAKE_PROGRAM_SAVE} buildtests ${EIGEN_TEST_BUILD_FLAGS}") +# Overwrite default DartConfiguration.tcl such that ctest can build our unit tests. +# Recall that our unit tests are not in the "all" target, so we have to explicitely ask ctest to build our custom 'buildtests' target. +# At this stage, we can also add custom flags to the build tool through the user defined EIGEN_TEST_BUILD_FLAGS variable. +file(READ "${CMAKE_CURRENT_BINARY_DIR}/DartConfiguration.tcl" EIGEN_DART_CONFIG_FILE) +# try to grab the default flags +string(REGEX MATCH "MakeCommand:.*-- (.*)\nDefaultCTestConfigurationType" EIGEN_DUMMY ${EIGEN_DART_CONFIG_FILE}) +if(NOT CMAKE_MATCH_1) +string(REGEX MATCH "MakeCommand:.*[^c]make (.*)\nDefaultCTestConfigurationType" EIGEN_DUMMY ${EIGEN_DART_CONFIG_FILE}) endif() - -# copy ctest properties, which currently -# o raise the warning levels -configure_file(${CMAKE_CURRENT_BINARY_DIR}/DartConfiguration.tcl ${CMAKE_BINARY_DIR}/DartConfiguration.tcl) - -# restore default CMAKE_MAKE_PROGRAM -set(CMAKE_MAKE_PROGRAM ${CMAKE_MAKE_PROGRAM_SAVE}) -# un-set temporary variables so that it is like they never existed. -# CMake 2.6.3 introduces the more logical unset() syntax for this. -set(CMAKE_MAKE_PROGRAM_SAVE) -set(EIGEN_MAKECOMMAND_PLACEHOLDER) +string(REGEX REPLACE "MakeCommand:.*DefaultCTestConfigurationType" "MakeCommand: ${CMAKE_COMMAND} --build . --target buildtests --config \"\${CTEST_CONFIGURATION_TYPE}\" -- ${CMAKE_MATCH_1} ${EIGEN_TEST_BUILD_FLAGS}\nDefaultCTestConfigurationType" + EIGEN_DART_CONFIG_FILE2 ${EIGEN_DART_CONFIG_FILE}) +file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/DartConfiguration.tcl" ${EIGEN_DART_CONFIG_FILE2}) configure_file(${CMAKE_CURRENT_SOURCE_DIR}/CTestCustom.cmake.in ${CMAKE_BINARY_DIR}/CTestCustom.cmake) diff --git a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake index 80b2841df..cbe12d51b 100644 --- a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake +++ b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake @@ -322,22 +322,21 @@ macro(ei_get_compilerver VAR) endif() else() # on all other system we rely on ${CMAKE_CXX_COMPILER} - # supporting a "--version" flag + # supporting a "--version" or "/version" flag - # check whether the head command exists - find_program(HEAD_EXE head NO_CMAKE_ENVIRONMENT_PATH NO_CMAKE_PATH NO_CMAKE_SYSTEM_PATH) - if(HEAD_EXE) - execute_process(COMMAND ${CMAKE_CXX_COMPILER} --version - COMMAND head -n 1 - OUTPUT_VARIABLE eigen_cxx_compiler_version_string OUTPUT_STRIP_TRAILING_WHITESPACE) + if(WIN32 AND ${CMAKE_CXX_COMPILER_ID} EQUAL "Intel") + set(EIGEN_CXX_FLAG_VERSION "/version") else() - execute_process(COMMAND ${CMAKE_CXX_COMPILER} --version - OUTPUT_VARIABLE eigen_cxx_compiler_version_string OUTPUT_STRIP_TRAILING_WHITESPACE) - string(REGEX REPLACE "[\n\r].*" "" eigen_cxx_compiler_version_string ${eigen_cxx_compiler_version_string}) + set(EIGEN_CXX_FLAG_VERSION "--version") endif() + execute_process(COMMAND ${CMAKE_CXX_COMPILER} ${EIGEN_CXX_FLAG_VERSION} + OUTPUT_VARIABLE eigen_cxx_compiler_version_string OUTPUT_STRIP_TRAILING_WHITESPACE) + string(REGEX REPLACE "[\n\r].*" "" eigen_cxx_compiler_version_string ${eigen_cxx_compiler_version_string}) + ei_get_compilerver_from_cxx_version_string("${eigen_cxx_compiler_version_string}" CNAME CVER) set(${VAR} "${CNAME}-${CVER}") + endif() endmacro(ei_get_compilerver) diff --git a/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake b/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake index e0040d320..6a0ce790c 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake @@ -26,7 +26,7 @@ macro(_metis_check_version) string(REGEX MATCH "define[ \t]+METIS_VER_SUBMINOR[ \t]+([0-9]+)" _metis_subminor_version_match "${_metis_version_header}") set(METIS_SUBMINOR_VERSION "${CMAKE_MATCH_1}") if(NOT METIS_MAJOR_VERSION) - message(WARNING "Could not determine Metis version. Assuming version 4.0.0") + message(STATUS "Could not determine Metis version. Assuming version 4.0.0") set(METIS_VERSION 4.0.0) else() set(METIS_VERSION ${METIS_MAJOR_VERSION}.${METIS_MINOR_VERSION}.${METIS_SUBMINOR_VERSION}) diff --git a/gtsam/3rdparty/Eigen/cmake/FindSPQR.cmake b/gtsam/3rdparty/Eigen/cmake/FindSPQR.cmake index 794c212af..1e958c3c1 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindSPQR.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindSPQR.cmake @@ -26,7 +26,12 @@ if(SPQR_LIBRARIES) find_library(SUITESPARSE_LIBRARY SuiteSparse PATHS $ENV{SPQRDIR} ${LIB_INSTALL_DIR}) if (SUITESPARSE_LIBRARY) set(SPQR_LIBRARIES ${SPQR_LIBRARIES} ${SUITESPARSE_LIBRARY}) - endif (SUITESPARSE_LIBRARY) + endif() + + find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{UMFPACK_LIBDIR} $ENV{UMFPACKDIR} ${LIB_INSTALL_DIR}) + if(CHOLMOD_LIBRARY) + set(SPQR_LIBRARIES ${SPQR_LIBRARIES} ${CHOLMOD_LIBRARY}) + endif() endif(SPQR_LIBRARIES) diff --git a/gtsam/3rdparty/Eigen/cmake/FindUmfpack.cmake b/gtsam/3rdparty/Eigen/cmake/FindUmfpack.cmake index 16b046cd6..53cf0b49b 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindUmfpack.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindUmfpack.cmake @@ -20,24 +20,29 @@ find_library(UMFPACK_LIBRARIES umfpack PATHS $ENV{UMFPACKDIR} ${LIB_INSTALL_DIR} if(UMFPACK_LIBRARIES) - if (NOT UMFPACK_LIBDIR) + if(NOT UMFPACK_LIBDIR) get_filename_component(UMFPACK_LIBDIR ${UMFPACK_LIBRARIES} PATH) endif(NOT UMFPACK_LIBDIR) find_library(COLAMD_LIBRARY colamd PATHS ${UMFPACK_LIBDIR} $ENV{UMFPACKDIR} ${LIB_INSTALL_DIR}) - if (COLAMD_LIBRARY) + if(COLAMD_LIBRARY) set(UMFPACK_LIBRARIES ${UMFPACK_LIBRARIES} ${COLAMD_LIBRARY}) - endif (COLAMD_LIBRARY) + endif () find_library(AMD_LIBRARY amd PATHS ${UMFPACK_LIBDIR} $ENV{UMFPACKDIR} ${LIB_INSTALL_DIR}) - if (AMD_LIBRARY) + if(AMD_LIBRARY) set(UMFPACK_LIBRARIES ${UMFPACK_LIBRARIES} ${AMD_LIBRARY}) - endif (AMD_LIBRARY) + endif () find_library(SUITESPARSE_LIBRARY SuiteSparse PATHS ${UMFPACK_LIBDIR} $ENV{UMFPACKDIR} ${LIB_INSTALL_DIR}) - if (SUITESPARSE_LIBRARY) + if(SUITESPARSE_LIBRARY) set(UMFPACK_LIBRARIES ${UMFPACK_LIBRARIES} ${SUITESPARSE_LIBRARY}) - endif (SUITESPARSE_LIBRARY) + endif () + + find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{UMFPACK_LIBDIR} $ENV{UMFPACKDIR} ${LIB_INSTALL_DIR}) + if(CHOLMOD_LIBRARY) + set(UMFPACK_LIBRARIES ${UMFPACK_LIBRARIES} ${CHOLMOD_LIBRARY}) + endif() endif(UMFPACK_LIBRARIES) @@ -45,4 +50,4 @@ include(FindPackageHandleStandardArgs) find_package_handle_standard_args(UMFPACK DEFAULT_MSG UMFPACK_INCLUDES UMFPACK_LIBRARIES) -mark_as_advanced(UMFPACK_INCLUDES UMFPACK_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY SUITESPARSE_LIBRARY) +mark_as_advanced(UMFPACK_INCLUDES UMFPACK_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY CHOLMOD_LIBRARY SUITESPARSE_LIBRARY) diff --git a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt index 8a493031c..2fc2a0dfc 100644 --- a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt @@ -91,7 +91,8 @@ add_custom_target(doc ALL COMMAND doxygen Doxyfile-unsupported COMMAND ${CMAKE_COMMAND} -E rename html eigen-doc COMMAND ${CMAKE_COMMAND} -E remove eigen-doc/eigen-doc.tgz - COMMAND ${CMAKE_COMMAND} -E tar cfz eigen-doc/eigen-doc.tgz eigen-doc + COMMAND ${CMAKE_COMMAND} -E tar cfz eigen-doc.tgz eigen-doc + COMMAND ${CMAKE_COMMAND} -E rename eigen-doc.tgz eigen-doc/eigen-doc.tgz COMMAND ${CMAKE_COMMAND} -E rename eigen-doc html WORKING_DIRECTORY ${Eigen_BINARY_DIR}/doc) diff --git a/gtsam/3rdparty/Eigen/doc/Doxyfile.in b/gtsam/3rdparty/Eigen/doc/Doxyfile.in index 1a2603b04..696dd2af1 100644 --- a/gtsam/3rdparty/Eigen/doc/Doxyfile.in +++ b/gtsam/3rdparty/Eigen/doc/Doxyfile.in @@ -222,7 +222,7 @@ ALIASES = "only_for_vectors=This is only for vectors (either row- "note_about_using_kernel_to_study_multiple_solutions=If you need a complete analysis of the space of solutions, take the one solution obtained by this method and add to it elements of the kernel, as determined by kernel()." \ "note_about_checking_solutions=This method just tries to find as good a solution as possible. If you want to check whether a solution exists or if it is accurate, just call this function to get a result and then compute the error of this result, or use MatrixBase::isApprox() directly, for instance like this: \code bool a_solution_exists = (A*result).isApprox(b, precision); \endcode This method avoids dividing by zero, so that the non-existence of a solution doesn't by itself mean that you'll get \c inf or \c nan values." \ "note_try_to_help_rvo=This function returns the result by value. In order to make that efficient, it is implemented as just a return statement using a special constructor, hopefully allowing the compiler to perform a RVO (return value optimization)." \ - "nonstableyet=\warning This is not considered to be part of the stable public API yet. Changes may happen in future releases. See \ref Experimental \"Experimental parts of Eigen\" + "nonstableyet=\warning This is not considered to be part of the stable public API yet. Changes may happen in future releases. See \ref Experimental \"Experimental parts of Eigen\"" ALIASES += "eigenAutoToc= " ALIASES += "eigenManualPage=\defgroup" @@ -315,7 +315,7 @@ IDL_PROPERTY_SUPPORT = YES # member in the group (if any) for the other members of the group. By default # all members of a group must be documented explicitly. -DISTRIBUTE_GROUP_DOC = NO +DISTRIBUTE_GROUP_DOC = YES # Set the SUBGROUPING tag to YES (the default) to allow class member groups of # the same type (for instance a group of public functions) to be put as a @@ -365,7 +365,7 @@ TYPEDEF_HIDES_STRUCT = NO # 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, # corresponding to a cache size of 2^16 = 65536 symbols. -SYMBOL_CACHE_SIZE = 0 +# SYMBOL_CACHE_SIZE = 0 # Similar to the SYMBOL_CACHE_SIZE the size of the symbol lookup cache can be # set using LOOKUP_CACHE_SIZE. This cache is used to resolve symbols given @@ -562,7 +562,7 @@ GENERATE_BUGLIST = NO # disable (NO) the deprecated list. This list is created by putting # \deprecated commands in the documentation. -GENERATE_DEPRECATEDLIST= NO +GENERATE_DEPRECATEDLIST= YES # The ENABLED_SECTIONS tag can be used to enable conditional # documentation sections, marked by \if sectionname ... \endif. @@ -1465,13 +1465,13 @@ XML_OUTPUT = xml # which can be used by a validating XML parser to check the # syntax of the XML files. -XML_SCHEMA = +# XML_SCHEMA = # The XML_DTD tag can be used to specify an XML DTD, # which can be used by a validating XML parser to check the # syntax of the XML files. -XML_DTD = +# XML_DTD = # If the XML_PROGRAMLISTING tag is set to YES Doxygen will # dump the program listings (including syntax highlighting @@ -1699,7 +1699,7 @@ DOT_NUM_THREADS = 0 # the DOTFONTPATH environment variable or by setting DOT_FONTPATH to the # directory containing the font. -DOT_FONTNAME = FreeSans +DOT_FONTNAME = # The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs. # The default size is 10pt. diff --git a/gtsam/3rdparty/Eigen/doc/Manual.dox b/gtsam/3rdparty/Eigen/doc/Manual.dox index 3367982ca..55057d213 100644 --- a/gtsam/3rdparty/Eigen/doc/Manual.dox +++ b/gtsam/3rdparty/Eigen/doc/Manual.dox @@ -11,6 +11,7 @@ namespace Eigen { - \subpage TopicCustomizingEigen - \subpage TopicMultiThreading - \subpage TopicUsingIntelMKL + - \subpage TopicPitfalls - \subpage TopicTemplateKeyword - \subpage UserManual_UnderstandingEigen */ diff --git a/gtsam/3rdparty/Eigen/doc/Pitfalls.dox b/gtsam/3rdparty/Eigen/doc/Pitfalls.dox new file mode 100644 index 000000000..cf42effef --- /dev/null +++ b/gtsam/3rdparty/Eigen/doc/Pitfalls.dox @@ -0,0 +1,38 @@ +namespace Eigen { + +/** \page TopicPitfalls Common pitfalls + +\section TopicPitfalls_template_keyword Compilation error with template methods + +See this \link TopicTemplateKeyword page \endlink. + +\section TopicPitfalls_auto_keyword C++11 and the auto keyword + +In short: do not use the auto keywords with Eigen's expressions, unless you are 100% sure about what you are doing. In particular, do not use the auto keyword as a replacement for a Matrix<> type. Here is an example: + +\code +MatrixXd A, B; +auto C = A*B; +for(...) { ... w = C * v; ...} +\endcode + +In this example, the type of C is not a MatrixXd but an abstract expression representing a matrix product and storing references to A and B. Therefore, the product of A*B will be carried out multiple times, once per iteration of the for loop. Moreover, if the coefficients of A or B change during the iteration, then C will evaluate to different values. + +Here is another example leading to a segfault: +\code +auto C = ((A+B).eval()).transpose(); +// do something with C +\endcode +The problem is that eval() returns a temporary object (in this case a MatrixXd) which is then referenced by the Transpose<> expression. However, this temporary is deleted right after the first line, and there the C expression reference a dead object. The same issue might occur when sub expressions are automatically evaluated by Eigen as in the following example: +\code +VectorXd u, v; +auto C = u + (A*v).normalized(); +// do something with C +\endcode +where the normalized() method has to evaluate the expensive product A*v to avoid evaluating it twice. On the other hand, the following example is perfectly fine: +\code +auto C = (u + (A*v).normalized()).eval(); +\endcode +In this case, C will be a regular VectorXd object. +*/ +} diff --git a/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox b/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox index f7d082668..7db2b0e07 100644 --- a/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox +++ b/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox @@ -17,7 +17,7 @@ You can control the number of thread that will be used using either the OpenMP A Unless setNbThreads has been called, Eigen uses the number of threads specified by OpenMP. You can restore this bahavior by calling \code setNbThreads(0); \endcode You can query the number of threads that will be used with: \code -n = Eigen::nbThreads(n); +n = Eigen::nbThreads( ); \endcode You can disable Eigen's multi threading at compile time by defining the EIGEN_DONT_PARALLELIZE preprocessor token. diff --git a/gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox b/gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox index 4b624a156..84db992b6 100644 --- a/gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox +++ b/gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox @@ -40,7 +40,8 @@ Since Eigen version 3.1 and later, users can benefit from built-in Intel MKL opt Intel MKL provides highly optimized multi-threaded mathematical routines for x86-compatible architectures. Intel MKL is available on Linux, Mac and Windows for both Intel64 and IA32 architectures. -\warning Be aware that Intel® MKL is a proprietary software. It is the responsibility of the users to buy MKL licenses for their products. Moreover, the license of the user product has to allow linking to proprietary software that excludes any unmodified versions of the GPL. +\note +Intel® MKL is a proprietary software and it is the responsibility of users to buy or register for community (free) Intel MKL licenses for their products. Moreover, the license of the user product has to allow linking to proprietary software that excludes any unmodified versions of the GPL. Using Intel MKL through Eigen is easy: -# define the \c EIGEN_USE_MKL_ALL macro before including any Eigen's header diff --git a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt index f8a0148c8..3ab75dbfe 100644 --- a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt @@ -10,9 +10,10 @@ if(QT4_FOUND) target_link_libraries(Tutorial_sparse_example ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}) add_custom_command( - TARGET Tutorial_sparse_example - POST_BUILD - COMMAND Tutorial_sparse_example ARGS ${CMAKE_CURRENT_BINARY_DIR}/../html/Tutorial_sparse_example.jpeg + TARGET Tutorial_sparse_example + POST_BUILD + COMMAND ${CMAKE_COMMAND} -E make_directory ${CMAKE_CURRENT_BINARY_DIR}/../html/ + COMMAND Tutorial_sparse_example ARGS ${CMAKE_CURRENT_BINARY_DIR}/../html/Tutorial_sparse_example.jpeg ) add_dependencies(all_examples Tutorial_sparse_example) diff --git a/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt b/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt index 5c4860237..cadc6a255 100644 --- a/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt @@ -32,6 +32,17 @@ ei_add_failtest("ref_3") ei_add_failtest("ref_4") ei_add_failtest("ref_5") +ei_add_failtest("partialpivlu_int") +ei_add_failtest("fullpivlu_int") +ei_add_failtest("llt_int") +ei_add_failtest("ldlt_int") +ei_add_failtest("qr_int") +ei_add_failtest("colpivqr_int") +ei_add_failtest("fullpivqr_int") +ei_add_failtest("jacobisvd_int") +ei_add_failtest("eigensolver_int") +ei_add_failtest("eigensolver_cplx") + if (EIGEN_FAILTEST_FAILURE_COUNT) message(FATAL_ERROR "${EIGEN_FAILTEST_FAILURE_COUNT} out of ${EIGEN_FAILTEST_COUNT} failtests FAILED. " diff --git a/gtsam/3rdparty/Eigen/failtest/colpivqr_int.cpp b/gtsam/3rdparty/Eigen/failtest/colpivqr_int.cpp new file mode 100644 index 000000000..db11910d4 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/colpivqr_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/QR" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + ColPivHouseholderQR > qr(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/eigensolver_cplx.cpp b/gtsam/3rdparty/Eigen/failtest/eigensolver_cplx.cpp new file mode 100644 index 000000000..c2e21e189 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/eigensolver_cplx.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/Eigenvalues" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR std::complex +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + EigenSolver > eig(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/eigensolver_int.cpp b/gtsam/3rdparty/Eigen/failtest/eigensolver_int.cpp new file mode 100644 index 000000000..eda8dc20b --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/eigensolver_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/Eigenvalues" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + EigenSolver > eig(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/fullpivlu_int.cpp b/gtsam/3rdparty/Eigen/failtest/fullpivlu_int.cpp new file mode 100644 index 000000000..e9d2c6eb3 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/fullpivlu_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/LU" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + FullPivLU > lu(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/fullpivqr_int.cpp b/gtsam/3rdparty/Eigen/failtest/fullpivqr_int.cpp new file mode 100644 index 000000000..d182a7b6b --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/fullpivqr_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/QR" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + FullPivHouseholderQR > qr(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/jacobisvd_int.cpp b/gtsam/3rdparty/Eigen/failtest/jacobisvd_int.cpp new file mode 100644 index 000000000..12790aef1 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/jacobisvd_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/SVD" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + JacobiSVD > qr(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/ldlt_int.cpp b/gtsam/3rdparty/Eigen/failtest/ldlt_int.cpp new file mode 100644 index 000000000..243e45746 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/ldlt_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/Cholesky" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + LDLT > ldlt(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/llt_int.cpp b/gtsam/3rdparty/Eigen/failtest/llt_int.cpp new file mode 100644 index 000000000..cb020650d --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/llt_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/Cholesky" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + LLT > llt(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/partialpivlu_int.cpp b/gtsam/3rdparty/Eigen/failtest/partialpivlu_int.cpp new file mode 100644 index 000000000..98ef282ea --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/partialpivlu_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/LU" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + PartialPivLU > lu(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/qr_int.cpp b/gtsam/3rdparty/Eigen/failtest/qr_int.cpp new file mode 100644 index 000000000..ce200e818 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/qr_int.cpp @@ -0,0 +1,14 @@ +#include "../Eigen/QR" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define SCALAR int +#else +#define SCALAR float +#endif + +using namespace Eigen; + +int main() +{ + HouseholderQR > qr(Matrix::Random(10,10)); +} diff --git a/gtsam/3rdparty/Eigen/failtest/ref_1.cpp b/gtsam/3rdparty/Eigen/failtest/ref_1.cpp new file mode 100644 index 000000000..8b798d53d --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/ref_1.cpp @@ -0,0 +1,18 @@ +#include "../Eigen/Core" + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +#define CV_QUALIFIER const +#else +#define CV_QUALIFIER +#endif + +using namespace Eigen; + +void call_ref(Ref a) { } + +int main() +{ + VectorXf a(10); + CV_QUALIFIER VectorXf& ac(a); + call_ref(ac); +} diff --git a/gtsam/3rdparty/Eigen/failtest/ref_2.cpp b/gtsam/3rdparty/Eigen/failtest/ref_2.cpp new file mode 100644 index 000000000..0b779ccf5 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/ref_2.cpp @@ -0,0 +1,15 @@ +#include "../Eigen/Core" + +using namespace Eigen; + +void call_ref(Ref a) { } + +int main() +{ + MatrixXf A(10,10); +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD + call_ref(A.row(3)); +#else + call_ref(A.col(3)); +#endif +} diff --git a/gtsam/3rdparty/Eigen/failtest/ref_3.cpp b/gtsam/3rdparty/Eigen/failtest/ref_3.cpp new file mode 100644 index 000000000..f46027d48 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/ref_3.cpp @@ -0,0 +1,15 @@ +#include "../Eigen/Core" + +using namespace Eigen; + +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD +void call_ref(Ref a) { } +#else +void call_ref(const Ref &a) { } +#endif + +int main() +{ + VectorXf a(10); + call_ref(a+a); +} diff --git a/gtsam/3rdparty/Eigen/failtest/ref_4.cpp b/gtsam/3rdparty/Eigen/failtest/ref_4.cpp new file mode 100644 index 000000000..6c11fa4cb --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/ref_4.cpp @@ -0,0 +1,15 @@ +#include "../Eigen/Core" + +using namespace Eigen; + +void call_ref(Ref > a) {} + +int main() +{ + MatrixXf A(10,10); +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD + call_ref(A.transpose()); +#else + call_ref(A); +#endif +} diff --git a/gtsam/3rdparty/Eigen/failtest/ref_5.cpp b/gtsam/3rdparty/Eigen/failtest/ref_5.cpp new file mode 100644 index 000000000..846d52795 --- /dev/null +++ b/gtsam/3rdparty/Eigen/failtest/ref_5.cpp @@ -0,0 +1,16 @@ +#include "../Eigen/Core" + +using namespace Eigen; + +void call_ref(Ref a) { } + +int main() +{ + VectorXf a(10); + DenseBase &ac(a); +#ifdef EIGEN_SHOULD_FAIL_TO_BUILD + call_ref(ac); +#else + call_ref(ac.derived()); +#endif +} diff --git a/gtsam/3rdparty/Eigen/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/CMakeLists.txt index d32451df6..3739268d2 100644 --- a/gtsam/3rdparty/Eigen/test/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/test/CMakeLists.txt @@ -222,6 +222,8 @@ ei_add_test(sizeoverflow) ei_add_test(prec_inverse_4x4) ei_add_test(vectorwiseop) ei_add_test(special_numbers) +ei_add_test(rvalue_types) +ei_add_test(mpl2only) ei_add_test(simplicial_cholesky) ei_add_test(conjugate_gradient) @@ -283,3 +285,9 @@ mark_as_advanced(EIGEN_TEST_EIGEN2) if(EIGEN_TEST_EIGEN2) add_subdirectory(eigen2) endif() + + +option(EIGEN_TEST_BUILD_DOCUMENTATION "Test building the doxygen documentation" OFF) +IF(EIGEN_TEST_BUILD_DOCUMENTATION) + add_dependencies(buildtests doc) +ENDIF() diff --git a/gtsam/3rdparty/Eigen/test/array.cpp b/gtsam/3rdparty/Eigen/test/array.cpp index c607da631..68f6b3d7a 100644 --- a/gtsam/3rdparty/Eigen/test/array.cpp +++ b/gtsam/3rdparty/Eigen/test/array.cpp @@ -109,6 +109,8 @@ template void comparisons(const ArrayType& m) VERIFY(! (m1 < m3).all() ); VERIFY(! (m1 > m3).all() ); } + VERIFY(!(m1 > m2 && m1 < m2).any()); + VERIFY((m1 <= m2 || m1 >= m2).all()); // comparisons to scalar VERIFY( (m1 != (m1(r,c)+1) ).any() ); diff --git a/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp b/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp index 9a50f99ab..9667e1f14 100644 --- a/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp +++ b/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp @@ -102,6 +102,7 @@ template void comparisons(const MatrixType& m) VERIFY( (m1.array() > (m1(r,c)-1) ).any() ); VERIFY( (m1.array() < (m1(r,c)+1) ).any() ); VERIFY( (m1.array() == m1(r,c) ).any() ); + VERIFY( m1.cwiseEqual(m1(r,c)).any() ); // test Select VERIFY_IS_APPROX( (m1.array() void test_conjugate_gradient_T() { - ConjugateGradient, Lower> cg_colmajor_lower_diag; - ConjugateGradient, Upper> cg_colmajor_upper_diag; + ConjugateGradient, Lower > cg_colmajor_lower_diag; + ConjugateGradient, Upper > cg_colmajor_upper_diag; + ConjugateGradient, Lower|Upper> cg_colmajor_loup_diag; ConjugateGradient, Lower, IdentityPreconditioner> cg_colmajor_lower_I; ConjugateGradient, Upper, IdentityPreconditioner> cg_colmajor_upper_I; CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_lower_diag) ); CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_upper_diag) ); + CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_loup_diag) ); CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_lower_I) ); CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_upper_I) ); } diff --git a/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp b/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp index 8e36adbe3..84663ad1f 100644 --- a/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp @@ -172,6 +172,8 @@ void test_geo_alignedbox() CALL_SUBTEST_9( alignedbox(AlignedBox1i()) ); CALL_SUBTEST_10( alignedbox(AlignedBox2i()) ); CALL_SUBTEST_11( alignedbox(AlignedBox3i()) ); + + CALL_SUBTEST_14( alignedbox(AlignedBox(4)) ); } CALL_SUBTEST_12( specificTest1() ); CALL_SUBTEST_13( specificTest2() ); diff --git a/gtsam/3rdparty/Eigen/test/lu.cpp b/gtsam/3rdparty/Eigen/test/lu.cpp index 25f86755a..374652694 100644 --- a/gtsam/3rdparty/Eigen/test/lu.cpp +++ b/gtsam/3rdparty/Eigen/test/lu.cpp @@ -100,7 +100,9 @@ template void lu_invertible() LU.h */ typedef typename NumTraits::Real RealScalar; - int size = internal::random(1,EIGEN_TEST_MAX_SIZE); + DenseIndex size = MatrixType::RowsAtCompileTime; + if( size==Dynamic) + size = internal::random(1,EIGEN_TEST_MAX_SIZE); MatrixType m1(size, size), m2(size, size), m3(size, size); FullPivLU lu; @@ -122,6 +124,10 @@ template void lu_invertible() m2 = lu.solve(m3); VERIFY_IS_APPROX(m3, m1*m2); VERIFY_IS_APPROX(m2, lu.inverse()*m3); + + // Regression test for Bug 302 + MatrixType m4 = MatrixType::Random(size,size); + VERIFY_IS_APPROX(lu.solve(m3*m4), lu.solve(m3)*m4); } template void lu_partial_piv() @@ -171,6 +177,7 @@ void test_lu() { for(int i = 0; i < g_repeat; i++) { CALL_SUBTEST_1( lu_non_invertible() ); + CALL_SUBTEST_1( lu_invertible() ); CALL_SUBTEST_1( lu_verify_assert() ); CALL_SUBTEST_2( (lu_non_invertible >()) ); diff --git a/gtsam/3rdparty/Eigen/test/mapped_matrix.cpp b/gtsam/3rdparty/Eigen/test/mapped_matrix.cpp index de9dbbde3..58904fa37 100644 --- a/gtsam/3rdparty/Eigen/test/mapped_matrix.cpp +++ b/gtsam/3rdparty/Eigen/test/mapped_matrix.cpp @@ -114,6 +114,28 @@ template void check_const_correctness(const PlainObjec VERIFY( !(Map::Flags & LvalueBit) ); } +template +void map_not_aligned_on_scalar() +{ + typedef Matrix MatrixType; + typedef typename MatrixType::Index Index; + Index size = 11; + Scalar* array1 = internal::aligned_new((size+1)*(size+1)+1); + Scalar* array2 = reinterpret_cast(sizeof(Scalar)/2+std::size_t(array1)); + Map > map2(array2, size, size, OuterStride<>(size+1)); + MatrixType m2 = MatrixType::Random(size,size); + map2 = m2; + VERIFY_IS_EQUAL(m2, map2); + + typedef Matrix VectorType; + Map map3(array2, size); + MatrixType v3 = VectorType::Random(size); + map3 = v3; + VERIFY_IS_EQUAL(v3, map3); + + internal::aligned_delete(array1, (size+1)*(size+1)+1); +} + void test_mapped_matrix() { for(int i = 0; i < g_repeat; i++) { @@ -137,5 +159,7 @@ void test_mapped_matrix() CALL_SUBTEST_8( map_static_methods(RowVector3d()) ); CALL_SUBTEST_9( map_static_methods(VectorXcd(8)) ); CALL_SUBTEST_10( map_static_methods(VectorXf(12)) ); + + CALL_SUBTEST_11( map_not_aligned_on_scalar() ); } } diff --git a/gtsam/3rdparty/Eigen/test/product_extra.cpp b/gtsam/3rdparty/Eigen/test/product_extra.cpp index 744a1ef7f..ea2486937 100644 --- a/gtsam/3rdparty/Eigen/test/product_extra.cpp +++ b/gtsam/3rdparty/Eigen/test/product_extra.cpp @@ -109,8 +109,67 @@ void mat_mat_scalar_scalar_product() double det = 6.0, wt = 0.5; VERIFY_IS_APPROX(dNdxy.transpose()*dNdxy*det*wt, det*wt*dNdxy.transpose()*dNdxy); } + +template +void zero_sized_objects(const MatrixType& m) +{ + typedef typename MatrixType::Scalar Scalar; + const int PacketSize = internal::packet_traits::size; + const int PacketSize1 = PacketSize>1 ? PacketSize-1 : 1; + DenseIndex rows = m.rows(); + DenseIndex cols = m.cols(); -void zero_sized_objects() + { + MatrixType res, a(rows,0), b(0,cols); + VERIFY_IS_APPROX( (res=a*b), MatrixType::Zero(rows,cols) ); + VERIFY_IS_APPROX( (res=a*a.transpose()), MatrixType::Zero(rows,rows) ); + VERIFY_IS_APPROX( (res=b.transpose()*b), MatrixType::Zero(cols,cols) ); + VERIFY_IS_APPROX( (res=b.transpose()*a.transpose()), MatrixType::Zero(cols,rows) ); + } + + { + MatrixType res, a(rows,cols), b(cols,0); + res = a*b; + VERIFY(res.rows()==rows && res.cols()==0); + b.resize(0,rows); + res = b*a; + VERIFY(res.rows()==0 && res.cols()==cols); + } + + { + Matrix a; + Matrix b; + Matrix res; + VERIFY_IS_APPROX( (res=a*b), MatrixType::Zero(PacketSize,1) ); + VERIFY_IS_APPROX( (res=a.lazyProduct(b)), MatrixType::Zero(PacketSize,1) ); + } + + { + Matrix a; + Matrix b; + Matrix res; + VERIFY_IS_APPROX( (res=a*b), MatrixType::Zero(PacketSize1,1) ); + VERIFY_IS_APPROX( (res=a.lazyProduct(b)), MatrixType::Zero(PacketSize1,1) ); + } + + { + Matrix a(PacketSize,0); + Matrix b(0,1); + Matrix res; + VERIFY_IS_APPROX( (res=a*b), MatrixType::Zero(PacketSize,1) ); + VERIFY_IS_APPROX( (res=a.lazyProduct(b)), MatrixType::Zero(PacketSize,1) ); + } + + { + Matrix a(PacketSize1,0); + Matrix b(0,1); + Matrix res; + VERIFY_IS_APPROX( (res=a*b), MatrixType::Zero(PacketSize1,1) ); + VERIFY_IS_APPROX( (res=a.lazyProduct(b)), MatrixType::Zero(PacketSize1,1) ); + } +} + +void bug_127() { // Bug 127 // @@ -171,7 +230,8 @@ void test_product_extra() CALL_SUBTEST_2( mat_mat_scalar_scalar_product() ); CALL_SUBTEST_3( product_extra(MatrixXcf(internal::random(1,EIGEN_TEST_MAX_SIZE/2), internal::random(1,EIGEN_TEST_MAX_SIZE/2))) ); CALL_SUBTEST_4( product_extra(MatrixXcd(internal::random(1,EIGEN_TEST_MAX_SIZE/2), internal::random(1,EIGEN_TEST_MAX_SIZE/2))) ); + CALL_SUBTEST_1( zero_sized_objects(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); } - CALL_SUBTEST_5( zero_sized_objects() ); + CALL_SUBTEST_5( bug_127() ); CALL_SUBTEST_6( unaligned_objects() ); } diff --git a/gtsam/3rdparty/Eigen/test/product_mmtr.cpp b/gtsam/3rdparty/Eigen/test/product_mmtr.cpp index 7d6746800..aeba009f4 100644 --- a/gtsam/3rdparty/Eigen/test/product_mmtr.cpp +++ b/gtsam/3rdparty/Eigen/test/product_mmtr.cpp @@ -24,8 +24,8 @@ template void mmtr(int size) DenseIndex othersize = internal::random(1,200); - MatrixColMaj matc(size, size); - MatrixRowMaj matr(size, size); + MatrixColMaj matc = MatrixColMaj::Zero(size, size); + MatrixRowMaj matr = MatrixRowMaj::Zero(size, size); MatrixColMaj ref1(size, size), ref2(size, size); MatrixColMaj soc(size,othersize); soc.setRandom(); diff --git a/gtsam/3rdparty/Eigen/test/real_qz.cpp b/gtsam/3rdparty/Eigen/test/real_qz.cpp index 7d743a734..a1766c6d9 100644 --- a/gtsam/3rdparty/Eigen/test/real_qz.cpp +++ b/gtsam/3rdparty/Eigen/test/real_qz.cpp @@ -25,6 +25,22 @@ template void real_qz(const MatrixType& m) MatrixType A = MatrixType::Random(dim,dim), B = MatrixType::Random(dim,dim); + + // Regression test for bug 985: Randomly set rows or columns to zero + Index k=internal::random(0, dim-1); + switch(internal::random(0,10)) { + case 0: + A.row(k).setZero(); break; + case 1: + A.col(k).setZero(); break; + case 2: + B.row(k).setZero(); break; + case 3: + B.col(k).setZero(); break; + default: + break; + } + RealQZ qz(A,B); VERIFY_IS_EQUAL(qz.info(), Success); diff --git a/gtsam/3rdparty/Eigen/test/redux.cpp b/gtsam/3rdparty/Eigen/test/redux.cpp index 0d176e500..50b473838 100644 --- a/gtsam/3rdparty/Eigen/test/redux.cpp +++ b/gtsam/3rdparty/Eigen/test/redux.cpp @@ -53,6 +53,14 @@ template void matrixRedux(const MatrixType& m) VERIFY_IS_APPROX(m1_for_prod.block(r0,c0,r1,c1).prod(), m1_for_prod.block(r0,c0,r1,c1).eval().prod()); VERIFY_IS_APPROX(m1.block(r0,c0,r1,c1).real().minCoeff(), m1.block(r0,c0,r1,c1).real().eval().minCoeff()); VERIFY_IS_APPROX(m1.block(r0,c0,r1,c1).real().maxCoeff(), m1.block(r0,c0,r1,c1).real().eval().maxCoeff()); + + // regression for bug 1090 + const int R1 = MatrixType::RowsAtCompileTime>=2 ? MatrixType::RowsAtCompileTime/2 : 6; + const int C1 = MatrixType::ColsAtCompileTime>=2 ? MatrixType::ColsAtCompileTime/2 : 6; + if(R1<=rows-r0 && C1<=cols-c0) + { + VERIFY_IS_APPROX( (m1.template block(r0,c0).sum()), m1.block(r0,c0,R1,C1).sum() ); + } // test empty objects VERIFY_IS_APPROX(m1.block(r0,c0,0,0).sum(), Scalar(0)); diff --git a/gtsam/3rdparty/Eigen/test/ref.cpp b/gtsam/3rdparty/Eigen/test/ref.cpp index 32eb31048..44bbd3bf1 100644 --- a/gtsam/3rdparty/Eigen/test/ref.cpp +++ b/gtsam/3rdparty/Eigen/test/ref.cpp @@ -229,6 +229,28 @@ void call_ref() VERIFY_EVALUATION_COUNT( call_ref_7(c,c), 0); } +typedef Matrix RowMatrixXd; +int test_ref_overload_fun1(Ref ) { return 1; } +int test_ref_overload_fun1(Ref ) { return 2; } +int test_ref_overload_fun1(Ref ) { return 3; } + +int test_ref_overload_fun2(Ref ) { return 4; } +int test_ref_overload_fun2(Ref ) { return 5; } + +// See also bug 969 +void test_ref_overloads() +{ + MatrixXd Ad, Bd; + RowMatrixXd rAd, rBd; + VERIFY( test_ref_overload_fun1(Ad)==1 ); + VERIFY( test_ref_overload_fun1(rAd)==2 ); + + MatrixXf Af, Bf; + VERIFY( test_ref_overload_fun2(Ad)==4 ); + VERIFY( test_ref_overload_fun2(Ad+Bd)==4 ); + VERIFY( test_ref_overload_fun2(Af+Bf)==5 ); +} + void test_ref() { for(int i = 0; i < g_repeat; i++) { @@ -249,4 +271,6 @@ void test_ref() CALL_SUBTEST_5( ref_matrix(MatrixXi(internal::random(1,10),internal::random(1,10))) ); CALL_SUBTEST_6( call_ref() ); } + + CALL_SUBTEST_7( test_ref_overloads() ); } diff --git a/gtsam/3rdparty/Eigen/test/sparse_basic.cpp b/gtsam/3rdparty/Eigen/test/sparse_basic.cpp index 498ecfe29..abe6a9d14 100644 --- a/gtsam/3rdparty/Eigen/test/sparse_basic.cpp +++ b/gtsam/3rdparty/Eigen/test/sparse_basic.cpp @@ -24,6 +24,7 @@ template void sparse_basic(const SparseMatrixType& re double density = (std::max)(8./(rows*cols), 0.01); typedef Matrix DenseMatrix; typedef Matrix DenseVector; + typedef Matrix RowDenseVector; Scalar eps = 1e-6; Scalar s1 = internal::random(); @@ -52,7 +53,7 @@ template void sparse_basic(const SparseMatrixType& re refMat.coeffRef(nonzeroCoords[0].x(), nonzeroCoords[0].y()) = Scalar(5); VERIFY_IS_APPROX(m, refMat); - /* + // test InnerIterators and Block expressions for (int t=0; t<10; ++t) { @@ -61,23 +62,54 @@ template void sparse_basic(const SparseMatrixType& re int w = internal::random(1,cols-j-1); int h = internal::random(1,rows-i-1); - // VERIFY_IS_APPROX(m.block(i,j,h,w), refMat.block(i,j,h,w)); + VERIFY_IS_APPROX(m.block(i,j,h,w), refMat.block(i,j,h,w)); for(int c=0; c void sparse_basic(const SparseMatrixType& re VERIFY_IS_APPROX(m.row(r) + m.row(r), (m + m).row(r)); VERIFY_IS_APPROX(m.row(r) + m.row(r), refMat.row(r) + refMat.row(r)); } - */ + // test assertion VERIFY_RAISES_ASSERT( m.coeffRef(-1,1) = 0 ); @@ -274,6 +306,8 @@ template void sparse_basic(const SparseMatrixType& re refM4.setRandom(); // sparse cwise* dense VERIFY_IS_APPROX(m3.cwiseProduct(refM4), refM3.cwiseProduct(refM4)); + // dense cwise* sparse + VERIFY_IS_APPROX(refM4.cwiseProduct(m3), refM4.cwiseProduct(refM3)); // VERIFY_IS_APPROX(m3.cwise()/refM4, refM3.cwise()/refM4); // test aliasing @@ -326,6 +360,15 @@ template void sparse_basic(const SparseMatrixType& re refMat2.col(i) = refMat2.col(i) * s1; VERIFY_IS_APPROX(m2,refMat2); } + + VERIFY_IS_APPROX(DenseVector(m2.col(j0)), refMat2.col(j0)); + VERIFY_IS_APPROX(m2.col(j0), refMat2.col(j0)); + + VERIFY_IS_APPROX(RowDenseVector(m2.row(j0)), refMat2.row(j0)); + VERIFY_IS_APPROX(m2.row(j0), refMat2.row(j0)); + + VERIFY_IS_APPROX(m2.block(j0,j1,n0,n0), refMat2.block(j0,j1,n0,n0)); + VERIFY_IS_APPROX((2*m2).block(j0,j1,n0,n0), (2*refMat2).block(j0,j1,n0,n0)); } // test prune @@ -488,6 +531,20 @@ template void sparse_basic(const SparseMatrixType& re SparseMatrixType m1(rows, rows); m1.setIdentity(); VERIFY_IS_APPROX(m1, refMat1); + for(int k=0; k(0,rows-1); + Index j = internal::random(0,rows-1); + Scalar v = internal::random(); + m1.coeffRef(i,j) = v; + refMat1.coeffRef(i,j) = v; + VERIFY_IS_APPROX(m1, refMat1); + if(internal::random(0,10)<2) + m1.makeCompressed(); + } + m1.setIdentity(); + refMat1.setIdentity(); + VERIFY_IS_APPROX(m1, refMat1); } } diff --git a/gtsam/3rdparty/Eigen/test/sparse_solver.h b/gtsam/3rdparty/Eigen/test/sparse_solver.h index 244e81c1b..e1619d62a 100644 --- a/gtsam/3rdparty/Eigen/test/sparse_solver.h +++ b/gtsam/3rdparty/Eigen/test/sparse_solver.h @@ -67,6 +67,22 @@ void check_sparse_solving(Solver& solver, const typename Solver::MatrixType& A, VERIFY(oldb.isApprox(db) && "sparse solver testing: the rhs should not be modified!"); VERIFY(x.isApprox(refX,test_precision())); } + + // if not too large, do some extra check: + if(A.rows()<2000) + { + + // test expression as input + { + solver.compute(0.5*(A+A)); + Rhs x = solver.solve(b); + VERIFY(x.isApprox(refX,test_precision())); + + Solver solver2(0.5*(A+A)); + Rhs x2 = solver2.solve(b); + VERIFY(x2.isApprox(refX,test_precision())); + } + } } template @@ -161,7 +177,10 @@ int generate_sparse_spd_problem(Solver& , typename Solver::MatrixType& A, typena dA = dM * dM.adjoint(); halfA.resize(size,size); - halfA.template selfadjointView().rankUpdate(M); + if(Solver::UpLo==(Lower|Upper)) + halfA = A; + else + halfA.template selfadjointView().rankUpdate(M); return size; } @@ -274,7 +293,17 @@ int generate_sparse_square_problem(Solver&, typename Solver::MatrixType& A, Dens return size; } -template void check_sparse_square_solving(Solver& solver) + +struct prune_column { + int m_col; + prune_column(int col) : m_col(col) {} + template + bool operator()(int, int col, const Scalar&) const { + return col != m_col; + } +}; + +template void check_sparse_square_solving(Solver& solver, bool checkDeficient = false) { typedef typename Solver::MatrixType Mat; typedef typename Mat::Scalar Scalar; @@ -306,6 +335,13 @@ template void check_sparse_square_solving(Solver& solver) b = DenseVector::Zero(size); check_sparse_solving(solver, A, b, dA, b); } + // regression test for Bug 792 (structurally rank deficient matrices): + if(checkDeficient && size>1) { + int col = internal::random(0,size-1); + A.prune(prune_column(col)); + solver.compute(A); + VERIFY_IS_EQUAL(solver.info(), NumericalIssue); + } } // First, get the folder diff --git a/gtsam/3rdparty/Eigen/test/sparselu.cpp b/gtsam/3rdparty/Eigen/test/sparselu.cpp index 52371cb12..b3d67aea8 100644 --- a/gtsam/3rdparty/Eigen/test/sparselu.cpp +++ b/gtsam/3rdparty/Eigen/test/sparselu.cpp @@ -41,12 +41,15 @@ template void test_sparselu_T() SparseLU, AMDOrdering > sparselu_amd; SparseLU, NaturalOrdering > sparselu_natural; - check_sparse_square_solving(sparselu_colamd); - check_sparse_square_solving(sparselu_amd); - check_sparse_square_solving(sparselu_natural); + check_sparse_square_solving(sparselu_colamd, true); + check_sparse_square_solving(sparselu_amd, true); + check_sparse_square_solving(sparselu_natural,true); check_sparse_square_abs_determinant(sparselu_colamd); check_sparse_square_abs_determinant(sparselu_amd); + + check_sparse_square_determinant(sparselu_colamd); + check_sparse_square_determinant(sparselu_amd); } void test_sparselu() diff --git a/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp b/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp index 6cd1acdda..d32fd10cc 100644 --- a/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp @@ -111,6 +111,18 @@ template void vectorwiseop_array(const ArrayType& m) m2.rowwise() /= m2.colwise().sum(); VERIFY_IS_APPROX(m2, m1.rowwise() / m1.colwise().sum()); } + + // all/any + Array mb(rows,cols); + mb = (m1.real()<=0.7).colwise().all(); + VERIFY( (mb.col(c) == (m1.real().col(c)<=0.7).all()).all() ); + mb = (m1.real()<=0.7).rowwise().all(); + VERIFY( (mb.row(r) == (m1.real().row(r)<=0.7).all()).all() ); + + mb = (m1.real()>=0.7).colwise().any(); + VERIFY( (mb.col(c) == (m1.real().col(c)>=0.7).any()).all() ); + mb = (m1.real()>=0.7).rowwise().any(); + VERIFY( (mb.row(r) == (m1.real().row(r)>=0.7).any()).all() ); } template void vectorwiseop_matrix(const MatrixType& m) diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/Eigen/CMakeLists.txt index e06f1238b..e1fbf97e2 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CMakeLists.txt @@ -1,6 +1,6 @@ -set(Eigen_HEADERS AdolcForward BVH IterativeSolvers MatrixFunctions MoreVectorization AutoDiff AlignedVector3 Polynomials - FFT NonLinearOptimization SparseExtra IterativeSolvers - NumericalDiff Skyline MPRealSupport OpenGLSupport KroneckerProduct Splines LevenbergMarquardt +set(Eigen_HEADERS AdolcForward AlignedVector3 ArpackSupport AutoDiff BVH FFT IterativeSolvers KroneckerProduct LevenbergMarquardt + MatrixFunctions MoreVectorization MPRealSupport NonLinearOptimization NumericalDiff OpenGLSupport Polynomials + Skyline SparseExtra Splines ) install(FILES diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h index 8d42e69b9..fde3ff61a 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h @@ -631,7 +631,7 @@ template struct NumTraits > { typedef AutoDiffScalar::Real,DerType::RowsAtCompileTime,DerType::ColsAtCompileTime> > Real; typedef AutoDiffScalar NonInteger; - typedef AutoDiffScalar& Nested; + typedef AutoDiffScalar Nested; enum{ RequireInitialization = 1 }; diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/CMakeLists.txt index f3180b52b..d8b9f4068 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/CMakeLists.txt @@ -1,7 +1,10 @@ ADD_SUBDIRECTORY(AutoDiff) ADD_SUBDIRECTORY(BVH) +ADD_SUBDIRECTORY(Eigenvalues) ADD_SUBDIRECTORY(FFT) ADD_SUBDIRECTORY(IterativeSolvers) +ADD_SUBDIRECTORY(KroneckerProduct) +ADD_SUBDIRECTORY(LevenbergMarquardt) ADD_SUBDIRECTORY(MatrixFunctions) ADD_SUBDIRECTORY(MoreVectorization) ADD_SUBDIRECTORY(NonLinearOptimization) @@ -9,5 +12,4 @@ ADD_SUBDIRECTORY(NumericalDiff) ADD_SUBDIRECTORY(Polynomials) ADD_SUBDIRECTORY(Skyline) ADD_SUBDIRECTORY(SparseExtra) -ADD_SUBDIRECTORY(KroneckerProduct) ADD_SUBDIRECTORY(Splines) diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h index 9fcc8a8d9..68fc997f7 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h @@ -133,8 +133,8 @@ class DGMRES : public IterativeSolverBase > * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - DGMRES(const MatrixType& A) : Base(A),m_restart(30),m_neig(0),m_r(0),m_maxNeig(5),m_isDeflAllocated(false),m_isDeflInitialized(false) - {} + template + explicit DGMRES(const EigenBase& A) : Base(A.derived()), m_restart(30),m_neig(0),m_r(0),m_maxNeig(5),m_isDeflAllocated(false),m_isDeflInitialized(false) {} ~DGMRES() {} diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h index c8c84069e..ea5deb26d 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h @@ -246,20 +246,7 @@ struct traits > * \endcode * * By default the iterations start with x=0 as an initial guess of the solution. - * One can control the start using the solveWithGuess() method. Here is a step by - * step execution example starting with a random guess and printing the evolution - * of the estimated error: - * * \code - * x = VectorXd::Random(n); - * solver.setMaxIterations(1); - * int i = 0; - * do { - * x = solver.solveWithGuess(b,x); - * std::cout << i << " : " << solver.error() << std::endl; - * ++i; - * } while (solver.info()!=Success && i<100); - * \endcode - * Note that such a step by step excution is slightly slower. + * One can control the start using the solveWithGuess() method. * * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner */ @@ -298,7 +285,8 @@ public: * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - GMRES(const MatrixType& A) : Base(A), m_restart(30) {} + template + explicit GMRES(const EigenBase& A) : Base(A.derived()), m_restart(30) {} ~GMRES() {} diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/MINRES.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/MINRES.h index 0e56342a8..670f274bb 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/MINRES.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/MINRES.h @@ -37,22 +37,31 @@ namespace Eigen { typedef typename Dest::Scalar Scalar; typedef Matrix VectorType; + // Check for zero rhs + const RealScalar rhsNorm2(rhs.squaredNorm()); + if(rhsNorm2 == 0) + { + x.setZero(); + iters = 0; + tol_error = 0; + return; + } + // initialize const int maxIters(iters); // initialize maxIters to iters const int N(mat.cols()); // the size of the matrix - const RealScalar rhsNorm2(rhs.squaredNorm()); const RealScalar threshold2(tol_error*tol_error*rhsNorm2); // convergence threshold (compared to residualNorm2) // Initialize preconditioned Lanczos -// VectorType v_old(N); // will be initialized inside loop + VectorType v_old(N); // will be initialized inside loop VectorType v( VectorType::Zero(N) ); //initialize v VectorType v_new(rhs-mat*x); //initialize v_new RealScalar residualNorm2(v_new.squaredNorm()); -// VectorType w(N); // will be initialized inside loop + VectorType w(N); // will be initialized inside loop VectorType w_new(precond.solve(v_new)); // initialize w_new // RealScalar beta; // will be initialized inside loop RealScalar beta_new2(v_new.dot(w_new)); - eigen_assert(beta_new2 >= 0 && "PRECONDITIONER IS NOT POSITIVE DEFINITE"); + eigen_assert(beta_new2 >= 0.0 && "PRECONDITIONER IS NOT POSITIVE DEFINITE"); RealScalar beta_new(sqrt(beta_new2)); const RealScalar beta_one(beta_new); v_new /= beta_new; @@ -62,14 +71,14 @@ namespace Eigen { RealScalar c_old(1.0); RealScalar s(0.0); // the sine of the Givens rotation RealScalar s_old(0.0); // the sine of the Givens rotation -// VectorType p_oold(N); // will be initialized in loop + VectorType p_oold(N); // will be initialized in loop VectorType p_old(VectorType::Zero(N)); // initialize p_old=0 VectorType p(p_old); // initialize p=0 RealScalar eta(1.0); iters = 0; // reset iters - while ( iters < maxIters ){ - + while ( iters < maxIters ) + { // Preconditioned Lanczos /* Note that there are 4 variants on the Lanczos algorithm. These are * described in Paige, C. C. (1972). Computational variants of @@ -81,17 +90,17 @@ namespace Eigen { * A. Greenbaum, Iterative Methods for Solving Linear Systems, SIAM (1987). */ const RealScalar beta(beta_new); -// v_old = v; // update: at first time step, this makes v_old = 0 so value of beta doesn't matter - const VectorType v_old(v); // NOT SURE IF CREATING v_old EVERY ITERATION IS EFFICIENT + v_old = v; // update: at first time step, this makes v_old = 0 so value of beta doesn't matter +// const VectorType v_old(v); // NOT SURE IF CREATING v_old EVERY ITERATION IS EFFICIENT v = v_new; // update -// w = w_new; // update - const VectorType w(w_new); // NOT SURE IF CREATING w EVERY ITERATION IS EFFICIENT + w = w_new; // update +// const VectorType w(w_new); // NOT SURE IF CREATING w EVERY ITERATION IS EFFICIENT v_new.noalias() = mat*w - beta*v_old; // compute v_new const RealScalar alpha = v_new.dot(w); v_new -= alpha*v; // overwrite v_new w_new = precond.solve(v_new); // overwrite w_new beta_new2 = v_new.dot(w_new); // compute beta_new - eigen_assert(beta_new2 >= 0 && "PRECONDITIONER IS NOT POSITIVE DEFINITE"); + eigen_assert(beta_new2 >= 0.0 && "PRECONDITIONER IS NOT POSITIVE DEFINITE"); beta_new = sqrt(beta_new2); // compute beta_new v_new /= beta_new; // overwrite v_new for next iteration w_new /= beta_new; // overwrite w_new for next iteration @@ -107,21 +116,28 @@ namespace Eigen { s=beta_new/r1; // new sine // Update solution -// p_oold = p_old; - const VectorType p_oold(p_old); // NOT SURE IF CREATING p_oold EVERY ITERATION IS EFFICIENT + p_oold = p_old; +// const VectorType p_oold(p_old); // NOT SURE IF CREATING p_oold EVERY ITERATION IS EFFICIENT p_old = p; p.noalias()=(w-r2*p_old-r3*p_oold) /r1; // IS NOALIAS REQUIRED? x += beta_one*c*eta*p; + + /* Update the squared residual. Note that this is the estimated residual. + The real residual |Ax-b|^2 may be slightly larger */ residualNorm2 *= s*s; - if ( residualNorm2 < threshold2){ + if ( residualNorm2 < threshold2) + { break; } eta=-s*eta; // update eta iters++; // increment iteration number (for output purposes) } - tol_error = std::sqrt(residualNorm2 / rhsNorm2); // return error. Note that this is the estimated error. The real error |Ax-b|/|b| may be slightly larger + + /* Compute error. Note that this is the estimated error. The real + error |Ax-b|/|b| may be slightly larger */ + tol_error = std::sqrt(residualNorm2 / rhsNorm2); } } @@ -174,20 +190,7 @@ namespace Eigen { * \endcode * * By default the iterations start with x=0 as an initial guess of the solution. - * One can control the start using the solveWithGuess() method. Here is a step by - * step execution example starting with a random guess and printing the evolution - * of the estimated error: - * * \code - * x = VectorXd::Random(n); - * mr.setMaxIterations(1); - * int i = 0; - * do { - * x = mr.solveWithGuess(b,x); - * std::cout << i << " : " << mr.error() << std::endl; - * ++i; - * } while (mr.info()!=Success && i<100); - * \endcode - * Note that such a step by step excution is slightly slower. + * One can control the start using the solveWithGuess() method. * * \sa class ConjugateGradient, BiCGSTAB, SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner */ @@ -225,7 +228,8 @@ namespace Eigen { * this class becomes invalid. Call compute() to update it with the new * matrix A, or modify a copy of A. */ - MINRES(const MatrixType& A) : Base(A) {} + template + explicit MINRES(const EigenBase& A) : Base(A.derived()) {} /** Destructor. */ ~MINRES(){} @@ -250,6 +254,11 @@ namespace Eigen { template void _solveWithGuess(const Rhs& b, Dest& x) const { + typedef typename internal::conditional + >::type MatrixWrapperType; + m_iterations = Base::maxIterations(); m_error = Base::m_tolerance; @@ -259,7 +268,7 @@ namespace Eigen { m_error = Base::m_tolerance; typename Dest::ColXpr xj(x,j); - internal::minres(mp_matrix->template selfadjointView(), b.col(j), xj, + internal::minres(MatrixWrapperType(*mp_matrix), b.col(j), xj, Base::m_preconditioner, m_iterations, m_error); } diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/CMakeLists.txt index 8513803ce..d9690854d 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/CMakeLists.txt @@ -2,5 +2,5 @@ FILE(GLOB Eigen_LevenbergMarquardt_SRCS "*.h") INSTALL(FILES ${Eigen_LevenbergMarquardt_SRCS} - DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/LevenbergMarquardt COMPONENT Devel + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/LevenbergMarquardt COMPONENT Devel ) diff --git a/gtsam/3rdparty/Eigen/unsupported/test/minres.cpp b/gtsam/3rdparty/Eigen/unsupported/test/minres.cpp index fd12da548..509ebe09a 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/minres.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/minres.cpp @@ -14,15 +14,32 @@ template void test_minres_T() { - MINRES, Lower, DiagonalPreconditioner > minres_colmajor_diag; - MINRES, Lower, IdentityPreconditioner > minres_colmajor_I; + MINRES, Lower|Upper, DiagonalPreconditioner > minres_colmajor_diag; + MINRES, Lower, IdentityPreconditioner > minres_colmajor_lower_I; + MINRES, Upper, IdentityPreconditioner > minres_colmajor_upper_I; // MINRES, Lower, IncompleteLUT > minres_colmajor_ilut; //minres, SSORPreconditioner > minres_colmajor_ssor; - CALL_SUBTEST( check_sparse_square_solving(minres_colmajor_diag) ); - CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_I) ); + +// CALL_SUBTEST( check_sparse_square_solving(minres_colmajor_diag) ); // CALL_SUBTEST( check_sparse_square_solving(minres_colmajor_ilut) ); //CALL_SUBTEST( check_sparse_square_solving(minres_colmajor_ssor) ); + + // Diagonal preconditioner + MINRES, Lower, DiagonalPreconditioner > minres_colmajor_lower_diag; + MINRES, Upper, DiagonalPreconditioner > minres_colmajor_upper_diag; + MINRES, Upper|Lower, DiagonalPreconditioner > minres_colmajor_uplo_diag; + + // call tests for SPD matrix + CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_lower_I) ); + CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_upper_I) ); + + CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_lower_diag) ); + CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_upper_diag) ); +// CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_uplo_diag) ); + + // TO DO: symmetric semi-definite matrix + // TO DO: symmetric indefinite matrix } void test_minres() diff --git a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h index dddda7dd9..7d6f4e79f 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h +++ b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h @@ -57,7 +57,8 @@ #include // Options -#define MPREAL_HAVE_INT64_SUPPORT // Enable int64_t support if possible. Available only for MSVC 2010 & GCC. +// FIXME HAVE_INT64_SUPPORT leads to clashes with long int and int64_t on some systems. +//#define MPREAL_HAVE_INT64_SUPPORT // Enable int64_t support if possible. Available only for MSVC 2010 & GCC. #define MPREAL_HAVE_MSVC_DEBUGVIEW // Enable Debugger Visualizer for "Debug" builds in MSVC. #define MPREAL_HAVE_DYNAMIC_STD_NUMERIC_LIMITS // Enable extended std::numeric_limits specialization. // Meaning that "digits", "round_style" and similar members are defined as functions, not constants. diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 6746236be..b30edb3df 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -169,7 +169,8 @@ struct FixedDimension { }; /// Helper class to construct the product manifold of two other manifolds, M1 and M2 -/// Assumes nothing except manifold structure from M1 and M2 +/// Assumes nothing except manifold structure for M1 and M2, and the existence +/// of default constructor for those types template class ProductManifold: public std::pair { BOOST_CONCEPT_ASSERT((IsManifold)); diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 1de27c0a2..c6af89486 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -189,7 +189,7 @@ void print(const Matrix& A, const string &s, ostream& stream) { 0, // flags " ", // coeffSeparator ";\n", // rowSeparator - " ", // rowPrefix + " \t", // rowPrefix "", // rowSuffix "[\n", // matPrefix "\n ]" // matSuffix @@ -580,15 +580,6 @@ Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) { return M; } -/* ************************************************************************* */ -Matrix3 skewSymmetric(double wx, double wy, double wz) -{ - return (Matrix3() << - 0.0, -wz, +wy, - +wz, 0.0, -wx, - -wy, +wx, 0.0).finished(); -} - /* ************************************************************************* */ Matrix LLt(const Matrix& A) { diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 41ffa27b5..37a0d28b8 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -21,15 +21,17 @@ // \callgraph #pragma once +#include #include #include // Configuration from CMake -#include #include #include #include #include +#include #include +#include /** @@ -477,9 +479,15 @@ GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask * @param wz * @return a 3*3 skew symmetric matrix */ -GTSAM_EXPORT Matrix3 skewSymmetric(double wx, double wy, double wz); -template -inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { return skewSymmetric(w(0),w(1),w(2));} + +inline Matrix3 skewSymmetric(double wx, double wy, double wz) { + return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished(); +} + +template +inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { + return skewSymmetric(w(0), w(1), w(2)); +} /** Use Cholesky to calculate inverse square root of a matrix */ GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A); @@ -526,6 +534,75 @@ GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7); std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false); +/** + * Functor that implements multiplication of a vector b with the inverse of a + * matrix A. The derivatives are inspired by Mike Giles' "An extended collection + * of matrix derivative results for forward and reverse mode algorithmic + * differentiation", at https://people.maths.ox.ac.uk/gilesm/files/NA-08-01.pdf + */ +template +struct MultiplyWithInverse { + typedef Eigen::Matrix VectorN; + typedef Eigen::Matrix MatrixN; + + /// A.inverse() * b, with optional derivatives + VectorN operator()(const MatrixN& A, const VectorN& b, + OptionalJacobian H1 = boost::none, + OptionalJacobian H2 = boost::none) const { + const MatrixN invA = A.inverse(); + const VectorN c = invA * b; + // The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA] + if (H1) + for (size_t j = 0; j < N; j++) + H1->template middleCols(N * j) = -c[j] * invA; + // The derivative in b is easy, as invA*b is just a linear map: + if (H2) *H2 = invA; + return c; + } +}; + +/** + * Functor that implements multiplication with the inverse of a matrix, itself + * the result of a function f. It turn out we only need the derivatives of the + * operator phi(a): b -> f(a) * b + */ +template +struct MultiplyWithInverseFunction { + enum { M = traits::dimension }; + typedef Eigen::Matrix VectorN; + typedef Eigen::Matrix MatrixN; + + // The function phi should calculate f(a)*b, with derivatives in a and b. + // Naturally, the derivative in b is f(a). + typedef boost::function, OptionalJacobian)> + Operator; + + /// Construct with function as explained above + MultiplyWithInverseFunction(const Operator& phi) : phi_(phi) {} + + /// f(a).inverse() * b, with optional derivatives + VectorN operator()(const T& a, const VectorN& b, + OptionalJacobian H1 = boost::none, + OptionalJacobian H2 = boost::none) const { + MatrixN A; + phi_(a, b, boost::none, A); // get A = f(a) by calling f once + const MatrixN invA = A.inverse(); + const VectorN c = invA * b; + + if (H1) { + Eigen::Matrix H; + phi_(a, c, H, boost::none); // get derivative H of forward mapping + *H1 = -invA* H; + } + if (H2) *H2 = invA; + return c; + } + + private: + const Operator phi_; +}; + } // namespace gtsam #include diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index d0bca4a9d..b89e15637 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -117,6 +117,7 @@ # include #else # include +# include #endif #ifdef GTSAM_USE_TBB diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 0bf49182b..899e6227c 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -255,7 +255,6 @@ public: // g = F' * (b - E * P * E' * b) Eigen::Matrix matrixBlock; - typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix // a single point is observed in m cameras size_t m = Fs.size(); // cameras observing current point diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index a87aeb650..bffda9fef 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -58,6 +58,22 @@ Point3 Point3::operator/(double s) const { return Point3(x_ / s, y_ / s, z_ / s); } +/* ************************************************************************* */ +double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1, + OptionalJacobian<1, 3> H2) const { + double d = (p2 - *this).norm(); + if (H1) { + *H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z(); + *H1 = *H1 *(1. / d); + } + + if (H2) { + *H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z(); + *H2 = *H2 *(1. / d); + } + return d; +} + /* ************************************************************************* */ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { @@ -75,13 +91,27 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, } /* ************************************************************************* */ -Point3 Point3::cross(const Point3 &q) const { +Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const { + if (H_p) { + *H_p << skewSymmetric(-q.vector()); + } + if (H_q) { + *H_q << skewSymmetric(vector()); + } + return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_, x_ * q.y_ - y_ * q.x_); } /* ************************************************************************* */ -double Point3::dot(const Point3 &q) const { +double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian<1, 3> H_q) const { + if (H_p) { + *H_p << q.vector().transpose(); + } + if (H_q) { + *H_q << vector().transpose(); + } + return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_); } @@ -116,6 +146,12 @@ ostream &operator<<(ostream &os, const Point3& p) { return os; } +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const gtsam::Point3Pair &p) { + os << p.first << " <-> " << p.second; + return os; +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index e8cf0be7b..e19b74d1c 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -37,8 +37,8 @@ namespace gtsam { private: - double x_, y_, z_; - + double x_, y_, z_; + public: enum { dimension = 3 }; @@ -56,7 +56,7 @@ namespace gtsam { /// @{ /// Construct from 3-element vector - Point3(const Vector3& v) { + explicit Point3(const Vector3& v) { x_ = v(0); y_ = v(1); z_ = v(2); @@ -82,6 +82,11 @@ namespace gtsam { /// inverse Point3 operator - () const { return Point3(-x_,-y_,-z_);} + /// add vector on right + inline Point3 operator +(const Vector3& v) const { + return Point3(x_ + v[0], y_ + v[1], z_ + v[2]); + } + /// add Point3 operator + (const Point3& q) const; @@ -99,20 +104,8 @@ namespace gtsam { Point3 operator / (double s) const; /** distance between two points */ - inline double distance(const Point3& p2, - OptionalJacobian<1,3> H1 = boost::none, OptionalJacobian<1,3> H2 = boost::none) const { - double d = (p2 - *this).norm(); - if (H1) { - *H1 << x_-p2.x(), y_-p2.y(), z_-p2.z(); - *H1 = *H1 *(1./d); - } - - if (H2) { - *H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z(); - *H2 << *H2 *(1./d); - } - return d; - } + double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none) const; /** @deprecated The following function has been deprecated, use distance above */ inline double dist(const Point3& p2) const { @@ -126,17 +119,19 @@ namespace gtsam { Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; /** cross product @return this x q */ - Point3 cross(const Point3 &q) const; + Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // + OptionalJacobian<3, 3> H_q = boost::none) const; /** dot product @return this * q*/ - double dot(const Point3 &q) const; + double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, // + OptionalJacobian<1, 3> H_q = boost::none) const; /// @} /// @name Standard Interface /// @{ /// equality - bool operator ==(const Point3& q) const; + bool operator ==(const Point3& q) const; /** return vectorized form (column-wise)*/ Vector3 vector() const { return Vector3(x_,y_,z_); } @@ -192,6 +187,10 @@ namespace gtsam { /// @} }; +// Convenience typedef +typedef std::pair Point3Pair; +std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); + /// Syntactic sugar for multiplying coordinates by a scalar s*p inline Point3 operator*(double s, const Point3& p) { return p*s;} diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f373e5ad4..9954240fd 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -120,9 +120,9 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { Rot3 R = Rot3::Expmap(omega.vector()); double theta2 = omega.dot(omega); if (theta2 > std::numeric_limits::epsilon()) { - double omega_v = omega.dot(v); // translation parallel to axis - Point3 omega_cross_v = omega.cross(v); // points towards axis - Point3 t = (omega_cross_v - R * omega_cross_v + omega_v * omega) / theta2; + Point3 t_parallel = omega * omega.dot(v); // translation parallel to axis + Point3 omega_cross_v = omega.cross(v); // points towards axis + Point3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; return Pose3(R, t); } else { return Pose3(R, v); diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 6b28f5125..ef384c3ef 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -123,7 +123,7 @@ Vector3 Rot3::rpy() const { /* ************************************************************************* */ Vector Rot3::quaternion() const { - Quaternion q = toQuaternion(); + gtsam::Quaternion q = toQuaternion(); Vector v(4); v(0) = q.w(); v(1) = q.x(); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index e548f8eea..264be1537 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -59,7 +59,7 @@ namespace gtsam { #ifdef GTSAM_USE_QUATERNIONS /** Internal Eigen Quaternion */ - Quaternion quaternion_; + gtsam::Quaternion quaternion_; #else Matrix3 rot_; #endif @@ -146,13 +146,13 @@ namespace gtsam { } /// Positive yaw is to right (as in aircraft heading). See ypr - static Rot3 yaw (double t) { return Rz(t); } + static Rot3 Yaw (double t) { return Rz(t); } /// Positive pitch is up (increasing aircraft altitude).See ypr - static Rot3 pitch(double t) { return Ry(t); } + static Rot3 Pitch(double t) { return Ry(t); } //// Positive roll is to right (increasing yaw in aircraft). - static Rot3 roll (double t) { return Rx(t); } + static Rot3 Roll (double t) { return Rx(t); } /** * Returns rotation nRb from body to nav frame. @@ -163,11 +163,11 @@ namespace gtsam { * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf. * Assumes vehicle coordinate frame X forward, Y right, Z down. */ - static Rot3 ypr(double y, double p, double r) { return RzRyRx(r,p,y);} + static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);} /** Create from Quaternion coefficients */ - static Rot3 quaternion(double w, double x, double y, double z) { - Quaternion q(w, x, y, z); + static Rot3 Quaternion(double w, double x, double y, double z) { + gtsam::Quaternion q(w, x, y, z); return Rot3(q); } @@ -179,7 +179,7 @@ namespace gtsam { */ static Rot3 AxisAngle(const Vector3& axis, double angle) { #ifdef GTSAM_USE_QUATERNIONS - return Quaternion(Eigen::AngleAxis(angle, axis)); + return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); #else return SO3::AxisAngle(axis,angle); #endif @@ -310,10 +310,10 @@ namespace gtsam { * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula */ - static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) { + static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) { if(H) *H = Rot3::ExpmapDerivative(v); #ifdef GTSAM_USE_QUATERNIONS - return traits::Expmap(v); + return traits::Expmap(v); #else return traits::Expmap(v); #endif @@ -419,13 +419,13 @@ namespace gtsam { /** * Use RQ to calculate yaw-pitch-roll angle representation - * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) + * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r) */ Vector3 ypr() const; /** * Use RQ to calculate roll-pitch-yaw angle representation - * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) + * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r) */ Vector3 rpy() const; @@ -460,7 +460,7 @@ namespace gtsam { /** Compute the quaternion representation of this rotation. * @return The quaternion */ - Quaternion toQuaternion() const; + gtsam::Quaternion toQuaternion() const; /** * Converts to a generic matrix to allow for use with matlab @@ -479,6 +479,8 @@ namespace gtsam { GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); /// @} + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); } @@ -486,7 +488,15 @@ namespace gtsam { static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); } static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); } static Rot3 rodriguez(double wx, double wy, double wz) { return Rodrigues(wx, wy, wz); } - /// @} + static Rot3 yaw (double t) { return Yaw(t); } + static Rot3 pitch(double t) { return Pitch(t); } + static Rot3 roll (double t) { return Roll(t); } + static Rot3 ypr(double y, double p, double r) { return Ypr(r,p,y);} + static Rot3 quaternion(double w, double x, double y, double z) { + return Rot3::Quaternion(w, x, y, z); + } + /// @} +#endif private: diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index a7b654070..c3636000b 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -51,7 +51,7 @@ Rot3::Rot3(double R11, double R12, double R13, } /* ************************************************************************* */ -Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) { +Rot3::Rot3(const gtsam::Quaternion& q) : rot_(q.toRotationMatrix()) { } /* ************************************************************************* */ @@ -191,8 +191,8 @@ Point3 Rot3::r2() const { return Point3(rot_.col(1)); } Point3 Rot3::r3() const { return Point3(rot_.col(2)); } /* ************************************************************************* */ -Quaternion Rot3::toQuaternion() const { - return Quaternion(rot_); +gtsam::Quaternion Rot3::toQuaternion() const { + return gtsam::Quaternion(rot_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index b255b082d..f8a01141b 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -27,14 +27,12 @@ using namespace std; namespace gtsam { - static const Matrix I3 = eye(3); - /* ************************************************************************* */ Rot3::Rot3() : quaternion_(Quaternion::Identity()) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) : - quaternion_((Eigen::Matrix3d() << + quaternion_((Matrix3() << col1.x(), col2.x(), col3.x(), col1.y(), col2.y(), col3.y(), col1.z(), col2.z(), col3.z()).finished()) {} @@ -43,36 +41,36 @@ namespace gtsam { Rot3::Rot3(double R11, double R12, double R13, double R21, double R22, double R23, double R31, double R32, double R33) : - quaternion_((Eigen::Matrix3d() << + quaternion_((Matrix3() << R11, R12, R13, R21, R22, R23, R31, R32, R33).finished()) {} /* ************************************************************************* */ - Rot3::Rot3(const Quaternion& q) : + Rot3::Rot3(const gtsam::Quaternion& q) : quaternion_(q) { } /* ************************************************************************* */ Rot3 Rot3::Rx(double t) { - return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); + return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); } /* ************************************************************************* */ Rot3 Rot3::Ry(double t) { - return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); + return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); } /* ************************************************************************* */ Rot3 Rot3::Rz(double t) { - return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); + return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); } /* ************************************************************************* */ Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3( - Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * - Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * - Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); + gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * + gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * + gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); } /* ************************************************************************* */ @@ -91,16 +89,16 @@ namespace gtsam { /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { - Matrix R = matrix(); + const Matrix3 R = matrix(); if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = R; - Eigen::Vector3d r = R * p.vector(); + const Vector3 r = R * p.vector(); return Point3(r.x(), r.y(), r.z()); } /* ************************************************************************* */ Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) { - return traits::Logmap(R.quaternion_, H); + return traits::Logmap(R.quaternion_, H); } /* ************************************************************************* */ @@ -130,7 +128,7 @@ namespace gtsam { Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); } /* ************************************************************************* */ - Quaternion Rot3::toQuaternion() const { return quaternion_; } + gtsam::Quaternion Rot3::toQuaternion() const { return quaternion_; } /* ************************************************************************* */ diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index af5803cb7..459f15561 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -11,8 +11,10 @@ /** * @file SO3.cpp - * @brief 3*3 matrix representation o SO(3) + * @brief 3*3 matrix representation of SO(3) * @author Frank Dellaert + * @author Luca Carlone + * @author Duy Nguyen Ta * @date December 2014 */ @@ -23,66 +25,109 @@ namespace gtsam { +namespace so3 { + +void ExpmapFunctor::init(bool nearZeroApprox) { + nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::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)] +} + +ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) + : theta2(omega.dot(omega)) { + const double wx = omega.x(), wy = omega.y(), wz = omega.z(); + 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; + } +} + +ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) + : theta2(angle * angle) { + const double ax = axis.x(), ay = axis.y(), az = axis.z(); + K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; + W = K * angle; + init(nearZeroApprox); + if (!nearZero) { + theta = angle; + KK = K * K; + } +} + +SO3 ExpmapFunctor::expmap() const { + if (nearZero) + return I_3x3 + W; + else + return I_3x3 + sin_theta * K + one_minus_cos * KK; +} + +DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) + : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { + if (nearZero) + dexp_ = I_3x3 - 0.5 * W; + else { + a = one_minus_cos / theta; + b = 1.0 - sin_theta / theta; + dexp_ = I_3x3 - a * K + b * KK; + } +} + +Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + if (H1) { + if (nearZero) { + *H1 = 0.5 * skewSymmetric(v); + } else { + // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK + const Vector3 Kv = K * v; + const double Da = (sin_theta - 2.0 * a) / theta2; + const double Db = (one_minus_cos - 3.0 * b) / theta2; + *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() - + skewSymmetric(Kv * b / theta) + + (a * I_3x3 - b * K) * skewSymmetric(v / theta); + } + } + if (H2) *H2 = dexp_; + return dexp_ * v; +} + +Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + const Matrix3 invDexp = dexp_.inverse(); + const Vector3 c = invDexp * v; + if (H1) { + Matrix3 D_dexpv_omega; + applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping + *H1 = -invDexp* D_dexpv_omega; + } + if (H2) *H2 = invDexp; + return c; +} + +} // namespace so3 + /* ************************************************************************* */ -// Near zero, we just have I + skew(omega) -static SO3 FirstOrder(const Vector3& omega) { - Matrix3 R; - R(0, 0) = 1.; - R(1, 0) = omega.z(); - R(2, 0) = -omega.y(); - R(0, 1) = -omega.z(); - R(1, 1) = 1.; - R(2, 1) = omega.x(); - R(0, 2) = omega.y(); - R(1, 2) = -omega.x(); - R(2, 2) = 1.; - return R; -} - SO3 SO3::AxisAngle(const Vector3& axis, double theta) { - if (theta*theta > std::numeric_limits::epsilon()) { - using std::cos; - using std::sin; - - // get components of axis \omega, where is a unit vector - const double& wx = axis.x(), wy = axis.y(), wz = axis.z(); - - const double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta; - const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta, - wz_sintheta = wz * sintheta; - - const double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz; - const double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz; - const double C22 = c_1 * wz * wz; - - Matrix3 R; - R(0, 0) = costheta + C00; - R(1, 0) = wz_sintheta + C01; - R(2, 0) = -wy_sintheta + C02; - R(0, 1) = -wz_sintheta + C01; - R(1, 1) = costheta + C11; - R(2, 1) = wx_sintheta + C12; - R(0, 2) = wy_sintheta + C02; - R(1, 2) = -wx_sintheta + C12; - R(2, 2) = costheta + C22; - return R; - } else { - return FirstOrder(axis*theta); - } - + return so3::ExpmapFunctor(axis, theta).expmap(); } -/// simply convert omega to axis/angle representation SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { - if (H) *H = ExpmapDerivative(omega); + if (H) { + so3::DexpFunctor impl(omega); + *H = impl.dexp(); + return impl.expmap(); + } else + return so3::ExpmapFunctor(omega).expmap(); +} - double theta2 = omega.dot(omega); - if (theta2 > std::numeric_limits::epsilon()) { - double theta = std::sqrt(theta2); - return AxisAngle(omega / theta, theta); - } else { - return FirstOrder(omega); - } +Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { + return so3::DexpFunctor(omega).dexp(); } /* ************************************************************************* */ @@ -96,7 +141,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2); // Get trace(R) - double tr = R.trace(); + const double tr = R.trace(); Vector3 omega; @@ -112,7 +157,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); } else { double magnitude; - double tr_3 = tr - 3.0; // always negative + const double tr_3 = tr - 3.0; // always negative if (tr_3 < -1e-7) { double theta = acos((tr - 1.0) / 2.0); magnitude = theta / (2.0 * sin(theta)); @@ -129,53 +174,13 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { } /* ************************************************************************* */ -Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { +Matrix3 SO3::LogmapDerivative(const Vector3& omega) { using std::cos; using std::sin; double theta2 = omega.dot(omega); if (theta2 <= std::numeric_limits::epsilon()) return I_3x3; double theta = std::sqrt(theta2); // rotation angle -#ifdef DUY_VERSION - /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) - Matrix3 X = skewSymmetric(omega); - Matrix3 X2 = X*X; - double vi = theta/2.0; - double s1 = sin(vi)/vi; - double s2 = (theta - sin(theta))/(theta*theta*theta); - return I_3x3 - 0.5*s1*s1*X + s2*X2; -#else // Luca's version - /** - * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega) - * where Jr = ExpmapDerivative(thetahat); - * This maps a perturbation in the tangent space (omega) to - * a perturbation on the manifold (expmap(Jr * omega)) - */ - // element of Lie algebra so(3): X = omega^, normalized by normx - const Matrix3 Y = skewSymmetric(omega) / theta; - return I_3x3 - ((1 - cos(theta)) / (theta)) * Y - + (1 - sin(theta) / theta) * Y * Y; // right Jacobian -#endif -} - -/* ************************************************************************* */ -Matrix3 SO3::LogmapDerivative(const Vector3& omega) { - using std::cos; - using std::sin; - - double theta2 = omega.dot(omega); - if (theta2 <= std::numeric_limits::epsilon()) return I_3x3; - double theta = std::sqrt(theta2); // rotation angle -#ifdef DUY_VERSION - /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) - Matrix3 X = skewSymmetric(omega); - Matrix3 X2 = X*X; - double vi = theta/2.0; - double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); - return I_3x3 + 0.5*X - s2*X2; -#else // Luca's version /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega @@ -183,11 +188,10 @@ Matrix3 SO3::LogmapDerivative(const Vector3& omega) { * This maps a perturbation on the manifold (expmap(omega)) * to a perturbation in the tangent space (Jrinv * omega) */ - const Matrix3 X = skewSymmetric(omega); // element of Lie algebra so(3): X = omega^ - return I_3x3 + 0.5 * X - + (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X - * X; -#endif + const Matrix3 W = skewSymmetric(omega); // element of Lie algebra so(3): W = omega^ + return I_3x3 + 0.5 * W + + (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * + W * W; } /* ************************************************************************* */ diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 580287eac..3152fa2fb 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -13,6 +13,8 @@ * @file SO3.h * @brief 3*3 matrix representation of SO(3) * @author Frank Dellaert + * @author Luca Carlone + * @author Duy Nguyen Ta * @date December 2014 */ @@ -97,15 +99,15 @@ public: */ static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none); + /// Derivative of Expmap + static Matrix3 ExpmapDerivative(const Vector3& omega); + /** * Log map at identity - returns the canonical coordinates * \f$ [R_x,R_y,R_z] \f$ of this rotation */ static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none); - /// Derivative of Expmap - static Matrix3 ExpmapDerivative(const Vector3& omega); - /// Derivative of Logmap static Matrix3 LogmapDerivative(const Vector3& omega); @@ -128,6 +130,61 @@ public: /// @} }; +// This namespace exposes two functors that allow for saving computation when +// exponential map and its derivatives are needed at the same location in so<3> +// The second functor also implements dedicated methods to apply dexp and/or inv(dexp) +namespace so3 { + +/// Functor implementing Exponential map +class ExpmapFunctor { + protected: + const double theta2; + Matrix3 W, K, KK; + bool nearZero; + double theta, sin_theta, one_minus_cos; // only defined if !nearZero + + void init(bool nearZeroApprox = false); + + public: + /// Constructor with element of Lie algebra so(3) + ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); + + /// Constructor with axis-angle + ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false); + + /// Rodrigues formula + SO3 expmap() const; +}; + +/// Functor that implements Exponential map *and* its derivatives +class DexpFunctor : public ExpmapFunctor { + const Vector3 omega; + double a, b; + Matrix3 dexp_; + + public: + /// Constructor with element of Lie algebra so(3) + DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); + + // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation + // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, + // Information Theory, and Lie Groups", Volume 2, 2008. + // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) + // This maps a perturbation v in the tangent space to + // a perturbation on the manifold Expmap(dexp * v) */ + const Matrix3& dexp() const { return dexp_; } + + /// Multiplies with dexp(), with optional derivatives + Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; + + /// Multiplies with dexp().inverse(), with optional derivatives + Vector3 applyInvDexp(const Vector3& v, + OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; +}; +} // namespace so3 + template<> struct traits : public internal::LieGroup { }; diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 7729bd354..aaf0aa953 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -32,10 +32,6 @@ # pragma clang diagnostic pop #endif -#ifdef GTSAM_USE_TBB -#include -#endif - #include #include #include @@ -46,14 +42,13 @@ namespace gtsam { /* ************************************************************************* */ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { - Unit3 direction(point); - if (H) { - // 3*3 Derivative of representation with respect to point is 3*3: - Matrix3 D_p_point; - point.normalize(D_p_point); // TODO, this calculates norm a second time :-( - // Calculate the 2*3 Jacobian + // 3*3 Derivative of representation with respect to point is 3*3: + Matrix3 D_p_point; + Point3 normalized = point.normalize(H ? &D_p_point : 0); + Unit3 direction; + direction.p_ = normalized.vector(); + if (H) *H << direction.basis().transpose() * D_p_point; - } return direction; } @@ -69,43 +64,97 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { return Unit3(d[0], d[1], d[2]); } -#ifdef GTSAM_USE_TBB -tbb::mutex unit3BasisMutex; -#endif - /* ************************************************************************* */ -const Matrix32& Unit3::basis() const { +const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { #ifdef GTSAM_USE_TBB - tbb::mutex::scoped_lock lock(unit3BasisMutex); + // NOTE(hayk): At some point it seemed like this reproducably resulted in deadlock. However, I + // can't see the reason why and I can no longer reproduce it. It may have been a red herring, or + // there is still a latent bug to watch out for. + tbb::mutex::scoped_lock lock(B_mutex_); #endif - // Return cached version if exists - if (B_) return *B_; + // Return cached basis if available and the Jacobian isn't needed. + if (B_ && !H) { + return *B_; + } + + // Return cached basis and derivatives if available. + if (B_ && H && H_B_) { + *H = *H_B_; + return *B_; + } + + // Get the unit vector and derivative wrt this. + // NOTE(hayk): We can't call point3(), because it would recursively call basis(). + const Point3 n(p_); // Get the axis of rotation with the minimum projected length of the point - Vector3 axis; - double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z()); - if ((mx <= my) && (mx <= mz)) - axis = Vector3(1.0, 0.0, 0.0); - else if ((my <= mx) && (my <= mz)) - axis = Vector3(0.0, 1.0, 0.0); - else if ((mz <= mx) && (mz <= my)) - axis = Vector3(0.0, 0.0, 1.0); - else + Point3 axis; + double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); + if ((mx <= my) && (mx <= mz)) { + axis = Point3(1.0, 0.0, 0.0); + } else if ((my <= mx) && (my <= mz)) { + axis = Point3(0.0, 1.0, 0.0); + } else if ((mz <= mx) && (mz <= my)) { + axis = Point3(0.0, 0.0, 1.0); + } else { assert(false); + } - // Create the two basis vectors - Vector3 b1 = p_.cross(axis).normalized(); - Vector3 b2 = p_.cross(b1).normalized(); + // 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 = n.cross(axis, H ? &H_B1_n : 0); - // Create the basis matrix + // Normalize result to get a unit vector: b1 = B1 / |B1|. + Matrix33 H_b1_B1; + Point3 b1 = B1.normalize(H ? &H_b1_B1 : 0); + + // 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 = n.cross(b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0); + + // Create the basis by stacking b1 and b2. B_.reset(Matrix32()); - (*B_) << b1, b2; + (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + + 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; + + // Cache the derivative and fill the result. + H_B_.reset(Matrix62()); + (*H_B_) << H_b1_p, H_b2_p; + *H = *H_B_; + } + return *B_; } /* ************************************************************************* */ -/// The print fuction +Point3 Unit3::point3(OptionalJacobian<3, 2> H) const { + if (H) + *H = basis(); + return Point3(p_); +} + +/* ************************************************************************* */ +Vector3 Unit3::unitVector(OptionalJacobian<3, 2> H) const { + if (H) + *H = basis(); + return p_; +} + +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Unit3& pair) { + os << pair.p_ << endl; + return os; +} + +/* ************************************************************************* */ void Unit3::print(const std::string& s) const { cout << s << ":" << p_ << endl; } @@ -116,11 +165,72 @@ Matrix3 Unit3::skew() const { } /* ************************************************************************* */ -Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) 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); + + Matrix32 H_qn_q; + Point3 qn = q.point3(H_q ? &H_qn_q : 0); + + // Compute the dot product of the Point3s. + Matrix13 H_dot_pn, H_dot_qn; + double d = pn.dot(qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0); + + if (H_p) { + (*H_p) << H_dot_pn * H_pn_p; + } + + if (H_q) { + (*H_q) = H_dot_qn * H_qn_q; + } + + return d; +} + +/* ************************************************************************* */ +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 - Vector2 xi = basis().transpose() * q.p_; - if (H) - *H = basis().transpose() * q.basis(); + Matrix23 Bt = basis().transpose(); + Vector2 xi = Bt * q.p_; + if (H_q) { + *H_q = Bt * q.basis(); + } + return xi; +} + +/* ************************************************************************* */ +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); + + // 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(); + Vector2 xi = Bt * qn.vector(); + + if (H_p) { + // Derivatives of each basis vector. + const Matrix32& H_b1_p = H_B_p.block<3, 2>(0, 0); + const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0); + + // Derivatives of the two entries of xi wrt the basis vectors. + Matrix13 H_xi1_b1 = qn.vector().transpose(); + Matrix13 H_xi2_b2 = qn.vector().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; + *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; + *H_q = H_xi_qu * H_qn_q; + } + return xi; } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index b7e243419..428211c6b 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -20,6 +20,7 @@ #pragma once +#include #include #include #include @@ -31,6 +32,10 @@ #include +#ifdef GTSAM_USE_TBB +#include +#endif + namespace gtsam { /// Represents a 3D point on a unit sphere. @@ -40,6 +45,11 @@ private: Vector3 p_; ///< The location of the point on the unit sphere mutable boost::optional B_; ///< Cached basis + mutable boost::optional H_B_; ///< Cached basis derivative + +#ifdef GTSAM_USE_TBB + mutable tbb::mutex B_mutex_; ///< Mutex to protect the cached basis. +#endif public: @@ -71,6 +81,23 @@ public: p_.normalize(); } + /// Construct from 2D point in plane at focal length f + /// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point + explicit Unit3(const Point2& p, double f = 1.0) : p_(p.x(), p.y(), f) { + p_.normalize(); + } + + /// Copy constructor + Unit3(const Unit3& u) { + p_ = u.p_; + } + + /// Copy assignment + Unit3& operator=(const Unit3 & u) { + p_ = u.p_; + return *this; + } + /// Named constructor from Point3 with optional Jacobian static Unit3 FromPoint3(const Point3& point, // OptionalJacobian<2, 3> H = boost::none); @@ -83,6 +110,8 @@ public: /// @name Testable /// @{ + friend std::ostream& operator<<(std::ostream& os, const Unit3& pair); + /// The print fuction void print(const std::string& s = std::string()) const; @@ -99,37 +128,50 @@ public: * Returns the local coordinate frame to tangent plane * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions * tangent to the sphere at the current direction. + * Provides derivatives of the basis with the two basis vectors stacked up as a 6x1. */ - const Matrix32& basis() const; + const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const; /// Return skew-symmetric associated with 3D point on unit sphere Matrix3 skew() const; /// Return unit-norm Point3 - Point3 point3(OptionalJacobian<3, 2> H = boost::none) const { - if (H) - *H = basis(); - return Point3(p_); - } + Point3 point3(OptionalJacobian<3, 2> H = boost::none) const; /// Return unit-norm Vector - const Vector3& unitVector(boost::optional H = boost::none) const { - if (H) - *H = basis(); - return p_; - } + Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const; /// Return scaled direction as Point3 friend Point3 operator*(double s, const Unit3& d) { return Point3(s * d.p_); } + /// Return dot product with q + double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, // + OptionalJacobian<1,2> H2 = boost::none) const; + /// Signed, vector-valued error between two directions - Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const; + /// @deprecated, errorVector has the proper derivatives, this confusingly has only the second. + Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const; + + /// Signed, vector-valued error between two directions + /// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal. + Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, // + OptionalJacobian<2, 2> H_q = boost::none) const; /// Distance between two directions double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; + /// Cross-product between two Unit3s + Unit3 cross(const Unit3& q) const { + return Unit3(p_.cross(q.p_)); + } + + /// Cross-product w Point3 + Point3 cross(const Point3& q) const { + return point3().cross(q); + } + /// @} /// @name Manifold diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 3b0906b44..5bc645a58 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -135,7 +135,7 @@ TEST( CalibratedCamera, Dproject_point_pose) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject_point_pose2) { - static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const CalibratedCamera camera(pose1); Matrix Dpose, Dpoint; camera.project(point1, Dpose, Dpoint); @@ -165,7 +165,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject_point_pose2_infinity) { - static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const CalibratedCamera camera(pose1); Matrix Dpose, Dpoint; camera.project2(pointAtInfinity, Dpose, Dpoint); diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index fe27b2911..bfff0a182 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -20,7 +20,7 @@ GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix) //************************************************************************* // Create two cameras and corresponding essential matrix E -Rot3 c1Rc2 = Rot3::yaw(M_PI_2); +Rot3 c1Rc2 = Rot3::Yaw(M_PI_2); Point3 c1Tc2(0.1, 0, 0); EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); @@ -98,8 +98,8 @@ Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { } TEST (EssentialMatrix, transform_to) { // test with a more complicated EssentialMatrix - Rot3 aRb2 = Rot3::yaw(M_PI / 3.0) * Rot3::pitch(M_PI_4) - * Rot3::roll(M_PI / 6.0); + Rot3 aRb2 = Rot3::Yaw(M_PI / 3.0) * Rot3::Pitch(M_PI_4) + * Rot3::Roll(M_PI / 6.0); Point3 aTb2(19.2, 3.7, 5.9); EssentialMatrix E(aRb2, Unit3(aTb2)); //EssentialMatrix E(aRb, Unit3(aTb).retract(Vector2(0.1, 0))); @@ -159,7 +159,7 @@ TEST (EssentialMatrix, FromPose3_a) { //************************************************************************* TEST (EssentialMatrix, FromPose3_b) { Matrix actualH; - Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); + Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); Point3 c1Tc2(0.4, 0.5, 0.6); EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras @@ -181,7 +181,7 @@ TEST (EssentialMatrix, streaming) { //************************************************************************* TEST (EssentialMatrix, epipoles) { // Create an E - Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); + Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); Point3 c1Tc2(0.4, 0.5, 0.6); EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 11931449f..180abb0d6 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -59,7 +59,7 @@ OrientedPlane3 transform_(const OrientedPlane3& plane, const Pose3& xr) { } TEST (OrientedPlane3, transform) { - gtsam::Pose3 pose(gtsam::Rot3::ypr(-M_PI / 4.0, 0.0, 0.0), + gtsam::Pose3 pose(gtsam::Rot3::Ypr(-M_PI / 4.0, 0.0, 0.0), gtsam::Point3(2.0, 3.0, 4.0)); OrientedPlane3 plane(-1, 0, 0, 5); OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 33aaf3542..b2579c0d9 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -257,7 +257,7 @@ TEST( PinholeCamera, Dproject2) // Add a test with more arbitrary rotation TEST( PinholeCamera, Dproject3) { - static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const Camera camera(pose1); Matrix Dpose, Dpoint; camera.project2(point1, Dpose, Dpoint); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index dc294899f..8f3eadc51 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -172,7 +172,7 @@ TEST( PinholePose, Dproject2) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject3) { - static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const Camera camera(pose1); Matrix Dpose, Dpoint; camera.project2(point1, Dpose, Dpoint); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 6811ed122..246d23c9a 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -133,6 +133,17 @@ TEST (Point3, distance) { EXPECT(assert_equal(numH2, H2, 1e-8)); } +/* ************************************************************************* */ +TEST(Point3, cross) { + Matrix aH1, aH2; + boost::function f = + boost::bind(&Point3::cross, _1, _2, boost::none, boost::none); + const Point3 omega(0, 1, 0), theta(4, 6, 8); + omega.cross(theta, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9007ce1bd..56e1e022c 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -556,12 +556,12 @@ TEST( Pose3, between ) /* ************************************************************************* */ // some shared test values - pulled from equivalent test in Pose2 Point3 l1(1, 0, 0), l2(1, 1, 0), l3(2, 2, 0), l4(1, 4,-4); -Pose3 x1, x2(Rot3::ypr(0.0, 0.0, 0.0), l2), x3(Rot3::ypr(M_PI/4.0, 0.0, 0.0), l2); +Pose3 x1, x2(Rot3::Ypr(0.0, 0.0, 0.0), l2), x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2); Pose3 - xl1(Rot3::ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)), - xl2(Rot3::ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)), - xl3(Rot3::ypr(1.0, 0.0, 0.0), Point3(2, 2, 0)), - xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4)); + xl1(Rot3::Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)), + xl2(Rot3::Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)), + xl3(Rot3::Ypr(1.0, 0.0, 0.0), Point3(2, 2, 0)), + xl4(Rot3::Ypr(0.0, 0.0, 1.0), Point3(1, 4,-4)); /* ************************************************************************* */ double range_proxy(const Pose3& pose, const Point3& point) { @@ -654,9 +654,9 @@ TEST( Pose3, unicycle ) { // velocity in X should be X in inertial frame, rather than global frame Vector x_step = delta(6,3,1.0); - EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), expmap_default(x1, x_step), tol)); - EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), expmap_default(x2, x_step), tol)); - EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2.0) * x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), l1), expmap_default(x1, x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), Point3(2,1,0)), expmap_default(x2, x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::Ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2.0) * x_step), tol)); } /* ************************************************************************* */ @@ -715,7 +715,42 @@ TEST( Pose3, ExpmapDerivative1) { } /* ************************************************************************* */ -TEST( Pose3, LogmapDerivative1) { +TEST(Pose3, ExpmapDerivative2) { + // Iserles05an (Lie-group Methods) says: + // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) + // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) + // where A(t): T -> se(3) is a trajectory in the tangent space of SE(3) + // and dexp[A] is a linear map from 4*4 to 4*4 derivatives of se(3) + // Hence, the above matrix equation is typed: 4*4 = SE(3) * linear_map(4*4) + + // In GTSAM, we don't work with the Lie-algebra elements A directly, but with 6-vectors. + // xi is easy: d Expmap(xi(t)) / dt = ExmapDerivative[xi(t)] * xi'(t) + + // Let's verify the above formula. + + auto xi = [](double t) { + Vector6 v; + v << 2 * t, sin(t), 4 * t * t, 2 * t, sin(t), 4 * t * t; + return v; + }; + auto xi_dot = [](double t) { + Vector6 v; + v << 2, cos(t), 8 * t, 2, cos(t), 8 * t; + return v; + }; + + // We define a function T + auto T = [xi](double t) { return Pose3::Expmap(xi(t)); }; + + for (double t = -2.0; t < 2.0; t += 0.3) { + const Matrix expected = numericalDerivative11(T, t); + const Matrix actual = Pose3::ExpmapDerivative(xi(t)) * xi_dot(t); + CHECK(assert_equal(expected, actual, 1e-7)); + } +} + +/* ************************************************************************* */ +TEST( Pose3, LogmapDerivative) { Matrix6 actualH; Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; Pose3 p = Pose3::Expmap(w); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index a61467b82..4b3c84e01 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -243,72 +243,6 @@ TEST(Rot3, retract_localCoordinates2) Vector d21 = t2.localCoordinates(t1); EXPECT(assert_equal(t1, t2.retract(d21))); } -/* ************************************************************************* */ -Vector w = Vector3(0.1, 0.27, -0.2); - -// Left trivialization Derivative of exp(w) wrpt w: -// How does exp(w) change when w changes? -// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 -// => y = log (exp(-w) * exp(w+dw)) -Vector3 testDexpL(const Vector3& dw) { - return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); -} - -TEST( Rot3, ExpmapDerivative) { - Matrix actualDexpL = Rot3::ExpmapDerivative(w); - Matrix expectedDexpL = numericalDerivative11(testDexpL, - Vector3::Zero(), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7)); - - Matrix actualDexpInvL = Rot3::LogmapDerivative(w); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7)); -} - -/* ************************************************************************* */ -Vector3 thetahat(0.1, 0, 0.1); -TEST( Rot3, ExpmapDerivative2) -{ - Matrix Jexpected = numericalDerivative11( - boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); - - Matrix Jactual = Rot3::ExpmapDerivative(thetahat); - CHECK(assert_equal(Jexpected, Jactual)); - - Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat); - CHECK(assert_equal(Jexpected, Jactual2)); -} - -/* ************************************************************************* */ -TEST( Rot3, jacobianExpmap ) -{ - Matrix Jexpected = numericalDerivative11(boost::bind( - &Rot3::Expmap, _1, boost::none), thetahat); - Matrix3 Jactual; - const Rot3 R = Rot3::Expmap(thetahat, Jactual); - EXPECT(assert_equal(Jexpected, Jactual)); -} - -/* ************************************************************************* */ -TEST( Rot3, LogmapDerivative ) -{ - Rot3 R = Rot3::Expmap(thetahat); // some rotation - Matrix Jexpected = numericalDerivative11(boost::bind( - &Rot3::Logmap, _1, boost::none), R); - Matrix3 Jactual = Rot3::LogmapDerivative(thetahat); - EXPECT(assert_equal(Jexpected, Jactual)); -} - -/* ************************************************************************* */ -TEST( Rot3, jacobianLogmap ) -{ - Rot3 R = Rot3::Expmap(thetahat); // some rotation - Matrix Jexpected = numericalDerivative11(boost::bind( - &Rot3::Logmap, _1, boost::none), R); - Matrix3 Jactual; - Rot3::Logmap(R, Jactual); - EXPECT(assert_equal(Jexpected, Jactual)); -} - /* ************************************************************************* */ TEST(Rot3, manifold_expmap) { @@ -501,17 +435,17 @@ TEST( Rot3, yaw_pitch_roll ) double t = 0.1; // yaw is around z axis - CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t))); + CHECK(assert_equal(Rot3::Rz(t),Rot3::Yaw(t))); // pitch is around y axis - CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t))); + CHECK(assert_equal(Rot3::Ry(t),Rot3::Pitch(t))); // roll is around x axis - CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t))); + CHECK(assert_equal(Rot3::Rx(t),Rot3::Roll(t))); // Check compound rotation - Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3); - CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3))); + Rot3 expected = Rot3::Yaw(0.1) * Rot3::Pitch(0.2) * Rot3::Roll(0.3); + CHECK(assert_equal(expected,Rot3::Ypr(0.1,0.2,0.3))); CHECK(assert_equal((Vector)Vector3(0.1, 0.2, 0.3),expected.ypr())); } @@ -531,14 +465,14 @@ TEST( Rot3, RQ) CHECK(assert_equal(expected,R.xyz(),1e-6)); CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); - // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal((Vector)Vector3(0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); + // Try using ypr call, asserting that Rot3::Ypr(y,p,r).ypr()==[y;p;r] + CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::Ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal((Vector)Vector3(0.3,0.2,0.1),Rot3::Ypr(0.1,0.2,0.3).rpy())); // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal((Vector)Vector3(0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); - CHECK(assert_equal((Vector)Vector3(0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); - CHECK(assert_equal((Vector)Vector3(0.0,0.0,0.1),Rot3::roll (0.1).ypr())); + CHECK(assert_equal((Vector)Vector3(0.1,0.0,0.0),Rot3::Yaw (0.1).ypr())); + CHECK(assert_equal((Vector)Vector3(0.0,0.1,0.0),Rot3::Pitch(0.1).ypr())); + CHECK(assert_equal((Vector)Vector3(0.0,0.0,0.1),Rot3::Roll (0.1).ypr())); // Try RQ to recover calibration from 3*3 sub-block of projection matrix Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0).finished(); @@ -594,9 +528,9 @@ TEST(Rot3, quaternion) { // Check creating Rot3 from quaternion EXPECT(assert_equal(R1, Rot3(q1))); - EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z()))); + EXPECT(assert_equal(R1, Rot3::Quaternion(q1.w(), q1.x(), q1.y(), q1.z()))); EXPECT(assert_equal(R2, Rot3(q2))); - EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z()))); + EXPECT(assert_equal(R2, Rot3::Quaternion(q2.w(), q2.x(), q2.y(), q2.z()))); // Check converting Rot3 to quaterion EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs()))); diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index bc32e0df0..68e87d5ba 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -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,16 +24,14 @@ using namespace std; using namespace gtsam; //****************************************************************************** -TEST(SO3 , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); +TEST(SO3, Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); } //****************************************************************************** -TEST(SO3 , Constructor) { - SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); -} +TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } //****************************************************************************** SO3 id; @@ -42,46 +40,214 @@ SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); //****************************************************************************** -TEST(SO3 , Local) { +TEST(SO3, Local) { Vector3 expected(0, 0, 0.1); Vector3 actual = traits::Local(R1, R2); - EXPECT(assert_equal((Vector)expected,actual)); + EXPECT(assert_equal((Vector)expected, actual)); } //****************************************************************************** -TEST(SO3 , Retract) { +TEST(SO3, Retract) { Vector3 v(0, 0, 0.1); SO3 actual = traits::Retract(R1, v); EXPECT(actual.isApprox(R2)); } //****************************************************************************** -TEST(SO3 , Invariants) { - EXPECT(check_group_invariants(id,id)); - EXPECT(check_group_invariants(id,R1)); - EXPECT(check_group_invariants(R2,id)); - EXPECT(check_group_invariants(R2,R1)); +TEST(SO3, Invariants) { + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, R1)); + EXPECT(check_group_invariants(R2, id)); + EXPECT(check_group_invariants(R2, R1)); - EXPECT(check_manifold_invariants(id,id)); - EXPECT(check_manifold_invariants(id,R1)); - EXPECT(check_manifold_invariants(R2,id)); - EXPECT(check_manifold_invariants(R2,R1)); + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, R1)); + EXPECT(check_manifold_invariants(R2, id)); + EXPECT(check_manifold_invariants(R2, R1)); } //****************************************************************************** -TEST(SO3 , LieGroupDerivatives) { - CHECK_LIE_GROUP_DERIVATIVES(id,id); - CHECK_LIE_GROUP_DERIVATIVES(id,R2); - CHECK_LIE_GROUP_DERIVATIVES(R2,id); - CHECK_LIE_GROUP_DERIVATIVES(R2,R1); +TEST(SO3, LieGroupDerivatives) { + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, R2); + CHECK_LIE_GROUP_DERIVATIVES(R2, id); + CHECK_LIE_GROUP_DERIVATIVES(R2, R1); } //****************************************************************************** -TEST(SO3 , ChartDerivatives) { - CHECK_CHART_DERIVATIVES(id,id); - CHECK_CHART_DERIVATIVES(id,R2); - CHECK_CHART_DERIVATIVES(R2,id); - CHECK_CHART_DERIVATIVES(R2,R1); +TEST(SO3, ChartDerivatives) { + CHECK_CHART_DERIVATIVES(id, id); + CHECK_CHART_DERIVATIVES(id, R2); + CHECK_CHART_DERIVATIVES(R2, id); + CHECK_CHART_DERIVATIVES(R2, R1); +} + +/* ************************************************************************* */ +namespace exmap_derivative { +static const Vector3 w(0.1, 0.27, -0.2); +} +// Left trivialized Derivative of exp(w) wrpt w: +// How does exp(w) change when w changes? +// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 +// => y = log (exp(-w) * exp(w+dw)) +Vector3 testDexpL(const Vector3& dw) { + using exmap_derivative::w; + return SO3::Logmap(SO3::Expmap(-w) * SO3::Expmap(w + dw)); +} + +TEST(SO3, ExpmapDerivative) { + using exmap_derivative::w; + const Matrix actualDexpL = SO3::ExpmapDerivative(w); + const Matrix expectedDexpL = + numericalDerivative11(testDexpL, Vector3::Zero(), 1e-2); + EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-7)); + + const Matrix actualDexpInvL = SO3::LogmapDerivative(w); + EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7)); +} + +/* ************************************************************************* */ +TEST(SO3, ExpmapDerivative2) { + const Vector3 theta(0.1, 0, 0.1); + const Matrix Jexpected = numericalDerivative11( + boost::bind(&SO3::Expmap, _1, boost::none), theta); + + CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); + CHECK(assert_equal(Matrix3(Jexpected.transpose()), + SO3::ExpmapDerivative(-theta))); +} + +/* ************************************************************************* */ +TEST(SO3, ExpmapDerivative3) { + const Vector3 theta(10, 20, 30); + const Matrix Jexpected = numericalDerivative11( + boost::bind(&SO3::Expmap, _1, boost::none), theta); + + CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); + CHECK(assert_equal(Matrix3(Jexpected.transpose()), + SO3::ExpmapDerivative(-theta))); +} + +/* ************************************************************************* */ +TEST(SO3, ExpmapDerivative4) { + // Iserles05an (Lie-group Methods) says: + // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) + // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) + // where A(t): R -> so(3) is a trajectory in the tangent space of SO(3) + // and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3) + // Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3) + + // In GTSAM, we don't work with the skew-symmetric matrices A directly, but + // with 3-vectors. + // omega is easy: d Expmap(w(t)) / dt = ExmapDerivative[w(t)] * w'(t) + + // Let's verify the above formula. + + auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; + auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; + + // We define a function R + auto R = [w](double t) { return SO3::Expmap(w(t)); }; + + for (double t = -2.0; t < 2.0; t += 0.3) { + const Matrix expected = numericalDerivative11(R, t); + const Matrix actual = SO3::ExpmapDerivative(w(t)) * w_dot(t); + CHECK(assert_equal(expected, actual, 1e-7)); + } +} + +/* ************************************************************************* */ +TEST(SO3, ExpmapDerivative5) { + auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; + auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; + + // Now define R as mapping local coordinates to neighborhood around R0. + const SO3 R0 = SO3::Expmap(Vector3(0.1, 0.4, 0.2)); + auto R = [R0, w](double t) { return R0.expmap(w(t)); }; + + for (double t = -2.0; t < 2.0; t += 0.3) { + const Matrix expected = numericalDerivative11(R, t); + const Matrix actual = SO3::ExpmapDerivative(w(t)) * w_dot(t); + CHECK(assert_equal(expected, actual, 1e-7)); + } +} + +/* ************************************************************************* */ +TEST(SO3, ExpmapDerivative6) { + const Vector3 thetahat(0.1, 0, 0.1); + const Matrix Jexpected = numericalDerivative11( + boost::bind(&SO3::Expmap, _1, boost::none), thetahat); + Matrix3 Jactual; + SO3::Expmap(thetahat, Jactual); + EXPECT(assert_equal(Jexpected, Jactual)); +} + +/* ************************************************************************* */ +TEST(SO3, LogmapDerivative) { + const Vector3 thetahat(0.1, 0, 0.1); + const SO3 R = SO3::Expmap(thetahat); // some rotation + const Matrix Jexpected = numericalDerivative11( + boost::bind(&SO3::Logmap, _1, boost::none), R); + const Matrix3 Jactual = SO3::LogmapDerivative(thetahat); + EXPECT(assert_equal(Jexpected, Jactual)); +} + +/* ************************************************************************* */ +TEST(SO3, JacobianLogmap) { + const Vector3 thetahat(0.1, 0, 0.1); + const SO3 R = SO3::Expmap(thetahat); // some rotation + const Matrix Jexpected = numericalDerivative11( + boost::bind(&SO3::Logmap, _1, boost::none), R); + Matrix3 Jactual; + SO3::Logmap(R, Jactual); + EXPECT(assert_equal(Jexpected, Jactual)); +} + +/* ************************************************************************* */ +TEST(SO3, ApplyDexp) { + Matrix aH1, aH2; + for (bool nearZeroApprox : {true, false}) { + boost::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v); + }; + for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), + Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { + so3::DexpFunctor local(omega, nearZeroApprox); + for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), + Vector3(0.4, 0.3, 0.2)}) { + EXPECT(assert_equal(Vector3(local.dexp() * v), + local.applyDexp(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(local.dexp(), aH2)); + } + } + } +} + +/* ************************************************************************* */ +TEST(SO3, ApplyInvDexp) { + Matrix aH1, aH2; + for (bool nearZeroApprox : {true, false}) { + boost::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v); + }; + for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), + Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { + so3::DexpFunctor local(omega, nearZeroApprox); + Matrix invDexp = local.dexp().inverse(); + for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), + Vector3(0.4, 0.3, 0.2)}) { + EXPECT(assert_equal(Vector3(invDexp * v), + local.applyInvDexp(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(invDexp, aH2)); + } + } + } } //****************************************************************************** @@ -90,4 +256,3 @@ int main() { return TestRegistry::runAllTests(tr); } //****************************************************************************** - diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index bd18143cb..c3df95abc 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -40,7 +40,7 @@ static const boost::shared_ptr sharedCal = // boost::make_shared(1500, 1200, 0, 640, 480); // Looking along X-axis, 1 meter above ground plane (x-y) -static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2); +static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); PinholeCamera camera1(pose1, *sharedCal); @@ -150,7 +150,7 @@ TEST( triangulation, fourPoses) { EXPECT(assert_equal(landmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); SimpleCamera camera3(pose3, *sharedCal); Point2 z3 = camera3.project(landmark); @@ -167,7 +167,7 @@ TEST( triangulation, fourPoses) { EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way - Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera camera4(pose4, *sharedCal); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION @@ -214,7 +214,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { EXPECT(assert_equal(landmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); SimpleCamera camera3(pose3, K3); Point2 z3 = camera3.project(landmark); @@ -232,7 +232,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way - Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); Cal3_S2 K4(700, 500, 0, 640, 480); SimpleCamera camera4(pose4, K4); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index e55caaa3c..dbe315807 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -18,11 +18,17 @@ * @brief Tests the Unit3 class */ -#include -#include #include #include #include +#include +#include +#include +#include +#include +#include +#include + #include @@ -35,6 +41,7 @@ using namespace boost::assign; using namespace gtsam; using namespace std; +using gtsam::symbol_shorthand::U; GTSAM_CONCEPT_TESTABLE_INST(Unit3) GTSAM_CONCEPT_MANIFOLD_INST(Unit3) @@ -63,13 +70,13 @@ static Unit3 rotate_(const Rot3& R, const Unit3& p) { } TEST(Unit3, rotate) { - Rot3 R = Rot3::yaw(0.5); + Rot3 R = Rot3::Yaw(0.5); Unit3 p(1, 0, 0); Unit3 expected = Unit3(R.column(1)); Unit3 actual = R * p; EXPECT(assert_equal(expected, actual, 1e-8)); Matrix actualH, expectedH; - + // Use numerical derivatives to calculate the expected Jacobian { expectedH = numericalDerivative21(rotate_, R, p); R.rotate(p, actualH, boost::none); @@ -88,13 +95,14 @@ static Unit3 unrotate_(const Rot3& R, const Unit3& p) { } TEST(Unit3, unrotate) { - Rot3 R = Rot3::yaw(-M_PI / 4.0); + Rot3 R = Rot3::Yaw(-M_PI / 4.0); Unit3 p(1, 0, 0); Unit3 expected = Unit3(1, 1, 0); Unit3 actual = R.unrotate(p); EXPECT(assert_equal(expected, actual, 1e-8)); Matrix actualH, expectedH; + // Use numerical derivatives to calculate the expected Jacobian { expectedH = numericalDerivative21(unrotate_, R, p); R.unrotate(p, actualH, boost::none); @@ -107,6 +115,37 @@ TEST(Unit3, unrotate) { } } +TEST(Unit3, dot) { + Unit3 p(1, 0.2, 0.3); + Unit3 q = p.retract(Vector2(0.5, 0)); + Unit3 r = p.retract(Vector2(0.8, 0)); + Unit3 t = p.retract(Vector2(0, 0.3)); + EXPECT(assert_equal(1.0, p.dot(p), 1e-8)); + EXPECT(assert_equal(0.877583, p.dot(q), 1e-5)); + EXPECT(assert_equal(0.696707, p.dot(r), 1e-5)); + EXPECT(assert_equal(0.955336, p.dot(t), 1e-5)); + + // Use numerical derivatives to calculate the expected Jacobians + Matrix H1, H2; + boost::function f = boost::bind(&Unit3::dot, _1, _2, // + boost::none, boost::none); + { + p.dot(q, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, q), H2, 1e-9)); + } + { + p.dot(r, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, r), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, r), H2, 1e-9)); + } + { + p.dot(t, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, t), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, t), H2, 1e-9)); + } +} + //******************************************************************************* TEST(Unit3, error) { Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // @@ -116,6 +155,7 @@ TEST(Unit3, error) { EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5)); Matrix actual, expected; + // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), q); @@ -130,6 +170,45 @@ TEST(Unit3, error) { } } +//******************************************************************************* +TEST(Unit3, error2) { + Unit3 p(0.1, -0.2, 0.8); + Unit3 q = p.retract(Vector2(0.2, -0.1)); + Unit3 r = p.retract(Vector2(0.8, 0)); + + // Hard-coded as simple regression values + EXPECT(assert_equal((Vector)(Vector2(0.0, 0.0)), p.errorVector(p), 1e-8)); + EXPECT(assert_equal((Vector)(Vector2(0.198337495, -0.0991687475)), p.errorVector(q), 1e-5)); + EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.errorVector(r), 1e-5)); + + Matrix actual, expected; + // Use numerical derivatives to calculate the expected Jacobian + { + expected = numericalDerivative21( + boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); + p.errorVector(q, actual, boost::none); + EXPECT(assert_equal(expected, actual, 1e-9)); + } + { + expected = numericalDerivative21( + boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); + p.errorVector(r, actual, boost::none); + EXPECT(assert_equal(expected, actual, 1e-9)); + } + { + expected = numericalDerivative22( + boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); + p.errorVector(q, boost::none, actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + } + { + expected = numericalDerivative22( + boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); + p.errorVector(r, boost::none, actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + } +} + //******************************************************************************* TEST(Unit3, distance) { Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // @@ -155,6 +234,11 @@ TEST(Unit3, distance) { } //******************************************************************************* +TEST(Unit3, localCoordinates0) { + Unit3 p; + Vector actual = p.localCoordinates(p); + EXPECT(assert_equal(zero(2), actual, 1e-8)); +} TEST(Unit3, localCoordinates) { { @@ -219,12 +303,45 @@ TEST(Unit3, localCoordinates) { } //******************************************************************************* +// 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; + B_vec << B.col(0), B.col(1); + return B_vec; +} + TEST(Unit3, basis) { - Unit3 p; - Matrix32 expected; - expected << 0, 0, 0, -1, 1, 0; - Matrix actual = p.basis(); - EXPECT(assert_equal(expected, actual, 1e-8)); + Unit3 p(0.1, -0.2, 0.9); + + Matrix expected(3, 2); + expected << 0.0, -0.994169047, 0.97618706, + -0.0233922129, 0.216930458, 0.105264958; + + Matrix62 actualH; + Matrix actual = p.basis(actualH); + EXPECT(assert_equal(expected, actual, 1e-6)); + + Matrix62 expectedH = numericalDerivative11( + boost::bind(BasisTest, _1, boost::none), p); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + +//******************************************************************************* +/// Check the basis derivatives of a bunch of random Unit3s. +TEST(Unit3, basis_derivatives) { + int num_tests = 100; + boost::mt19937 rng(42); + for (int i = 0; i < num_tests; i++) { + Unit3 p = Unit3::Random(rng); + + Matrix62 actualH; + p.basis(actualH); + + Matrix62 expectedH = numericalDerivative11( + boost::bind(BasisTest, _1, boost::none), p); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); + } } //******************************************************************************* @@ -298,6 +415,46 @@ TEST (Unit3, FromPoint3) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +//******************************************************************************* +TEST(Unit3, ErrorBetweenFactor) { + std::vector data; + data.push_back(Unit3(1.0, 0.0, 0.0)); + data.push_back(Unit3(0.0, 0.0, 1.0)); + + NonlinearFactorGraph graph; + Values initial_values; + + // Add prior factors. + SharedNoiseModel R_prior = noiseModel::Unit::Create(2); + for (size_t i = 0; i < data.size(); i++) { + graph.add(PriorFactor(U(i), data[i], R_prior)); + } + + // Add process factors using the dot product error function. + SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01); + for (size_t i = 0; i < data.size() - 1; i++) { + Expression exp(Expression(U(i)), &Unit3::errorVector, Expression(U(i + 1))); + graph.addExpressionFactor(R_process, Vector2::Zero(), exp); + } + + // Add initial values. Since there is no identity, just pick something. + for (size_t i = 0; i < data.size(); i++) { + initial_values.insert(U(i), Unit3(0.0, 1.0, 0.0)); + } + + Values values = GaussNewtonOptimizer(graph, initial_values).optimize(); + + // Check that the y-value is very small for each. + for (size_t i = 0; i < data.size(); i++) { + EXPECT(assert_equal(0.0, values.at(U(i)).unitVector().y(), 1e-3)); + } + + // Check that the dot product between variables is close to 1. + for (size_t i = 0; i < data.size() - 1; i++) { + EXPECT(assert_equal(1.0, values.at(U(i)).dot(values.at(U(i + 1))), 1e-2)); + } +} + /* ************************************************************************* */ TEST(actualH, Serialization) { Unit3 p(0, 1, 0); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index c3ab56200..c6e613b14 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include #include #include @@ -376,6 +377,15 @@ class TriangulationResult: public boost::optional { status_(s) { } public: + + /** + * Default constructor, only for serialization + */ + TriangulationResult() {} + + /** + * Constructor + */ TriangulationResult(const Point3& p) : status_(VALID) { reset(p); diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index b3b8b9a41..6db13adb6 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -141,7 +141,20 @@ namespace gtsam { /* ************************************************************************* */ pair GaussianBayesNet::matrix() const { - return GaussianFactorGraph(*this).jacobian(); + 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); + } + // add remaining keys in case Bayes net is incomplete + BOOST_FOREACH (Key key, keys) + ordering.push_back(key); + // return matrix and RHS + return factorGraph.jacobian(ordering); } ///* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index f2678d56c..d4df57298 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -349,11 +349,11 @@ void JacobianFactor::print(const string& s, const KeyFormatter& formatter) const { static const Eigen::IOFormat matlab( Eigen::StreamPrecision, // precision - 0, // flags + 0, // flags " ", // coeffSeparator ";\n", // rowSeparator - " ", // rowPrefix - "", // rowSuffix + "\t", // rowPrefix + "", // rowSuffix "[\n", // matPrefix "\n ]" // matSuffix ); diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index a697a5e4c..a961a79bd 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -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) @@ -66,64 +66,69 @@ void PreintegratedCombinedMeasurements::resetIntegration() { //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::integrateMeasurement( - const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT) { - - const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion - + const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { // Update preintegrated measurements. - Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr - Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix93 G1,G2; - PreintegrationBase::update(measuredAcc, measuredOmega, deltaT, - &D_incrR_integratedOmega, &F_9x9, &G1, &G2); + Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix93 B, C; + PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); - // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that - // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we - // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ + // Update preintegrated measurements covariance: as in [2] we consider a first + // order propagation that can be seen as a prediction phase in an EKF + // framework. In this implementation, in contrast to [2], we consider the + // uncertainty of the bias selection and we keep correlation between biases + // and preintegrated measurements // Single Jacobians to propagate covariance - Matrix3 H_vel_biasacc = -dRij * deltaT; - Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * deltaT; + // TODO(frank): should we not also accout for bias on position? + Matrix3 theta_H_biasOmega = - C.topRows<3>(); + Matrix3 vel_H_biasAcc = -B.bottomRows<3>(); // overall Jacobian wrt preintegrated measurements (df/dx) - Eigen::Matrix F; + Eigen::Matrix F; F.setZero(); - F.block<9, 9>(0, 0) = F_9x9; - F.block<3, 3>(0, 12) = H_angles_biasomega; - F.block<3, 3>(6, 9) = H_vel_biasacc; + F.block<9, 9>(0, 0) = A; + F.block<3, 3>(0, 12) = theta_H_biasOmega; + F.block<3, 3>(6, 9) = vel_H_biasAcc; F.block<6, 6>(9, 9) = I_6x6; + // propagate uncertainty + // TODO(frank): use noiseModel routine so we can have arbitrary noise models. + const Matrix3& aCov = p().accelerometerCovariance; + const Matrix3& wCov = p().gyroscopeCovariance; + const Matrix3& iCov = p().integrationCovariance; + // first order uncertainty propagation - // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - Eigen::Matrix G_measCov_Gt; + // Optimized matrix multiplication (1/dt) * G * measurementCovariance * + // G.transpose() + Eigen::Matrix G_measCov_Gt; G_measCov_Gt.setZero(15, 15); // BLOCK DIAGONAL TERMS - D_t_t(&G_measCov_Gt) = deltaT * p().integrationCovariance; - D_v_v(&G_measCov_Gt) = (1 / deltaT) * (H_vel_biasacc) - * (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0)) - * (H_vel_biasacc.transpose()); - D_R_R(&G_measCov_Gt) = (1 / deltaT) * (H_angles_biasomega) - * (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3)) - * (H_angles_biasomega.transpose()); - D_a_a(&G_measCov_Gt) = (1 / deltaT) * p().biasAccCovariance; - D_g_g(&G_measCov_Gt) = (1 / deltaT) * p().biasOmegaCovariance; + D_t_t(&G_measCov_Gt) = dt * iCov; + D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc * + (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) * + (vel_H_biasAcc.transpose()); + D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega * + (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) * + (theta_H_biasOmega.transpose()); + D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; + D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance; // OFF BLOCK DIAGONAL TERMS - Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0) - * H_angles_biasomega.transpose(); + Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) * + theta_H_biasOmega.transpose(); D_v_R(&G_measCov_Gt) = temp; D_R_v(&G_measCov_Gt) = temp.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; } //------------------------------------------------------------------------------ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, - const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit, + const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt, const bool use2ndOrderIntegration) { if (!use2ndOrderIntegration) throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); @@ -134,11 +139,12 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( p->integrationCovariance = integrationErrorCovariance; p->biasAccCovariance = biasAccCovariance; p->biasOmegaCovariance = biasOmegaCovariance; - p->biasAccOmegaInit = biasAccOmegaInit; + p->biasAccOmegaInt = biasAccOmegaInt; p_ = p; resetIntegration(); preintMeasCov_.setZero(); } +#endif //------------------------------------------------------------------------------ // CombinedImuFactor methods //------------------------------------------------------------------------------ @@ -238,6 +244,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, } //------------------------------------------------------------------------------ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 CombinedImuFactor::CombinedImuFactor( Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity, @@ -254,7 +261,7 @@ CombinedImuFactor::CombinedImuFactor( p->use2ndOrderCoriolis = use2ndOrderCoriolis; _PIM_.p_ = p; } -//------------------------------------------------------------------------------ + void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, @@ -268,5 +275,7 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, pose_j = pvb.pose; vel_j = pvb.velocity; } +#endif } /// namespace gtsam + diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 6ed382966..3141f8245 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -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) @@ -59,17 +59,19 @@ namespace gtsam { */ class PreintegratedCombinedMeasurements : public PreintegrationBase { +public: + /// Parameters for pre-integration: /// Usage: Create just a single Params and pass a shared pointer to the constructor - struct Params : PreintegrationBase::Params { + struct Params : PreintegrationParams { Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk - Matrix6 biasAccOmegaInit; ///< covariance of bias used for pre-integration + Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration /// See two named constructors below for good values of n_gravity in body frame Params(const Vector3& n_gravity) : - PreintegrationBase::Params(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( - I_3x3), biasAccOmegaInit(I_6x6) { + PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( + I_3x3), biasAccOmegaInt(I_6x6) { } // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis @@ -93,7 +95,7 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params); ar& BOOST_SERIALIZATION_NVP(biasAccCovariance); ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance); - ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInit); + ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt); } }; @@ -111,28 +113,46 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase { friend class CombinedImuFactor; public: + /// @name Constructors + /// @{ + /** * Default constructor, initializes the class with no measurements * @param bias Current estimate of acceleration and rotation rate biases */ - PreintegratedCombinedMeasurements(const boost::shared_ptr& p, - const imuBias::ConstantBias& biasHat) + PreintegratedCombinedMeasurements( + const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : PreintegrationBase(p, biasHat) { preintMeasCov_.setZero(); } - Params& p() const { return *boost::static_pointer_cast(p_);} + /// @} - /// print - void print(const std::string& s = "Preintegrated Measurements:") const; - - /// equals - bool equals(const PreintegratedCombinedMeasurements& expected, - double tol = 1e-9) const; + /// @name Basic utilities + /// @{ /// Re-initialize PreintegratedCombinedMeasurements void resetIntegration(); + /// const reference to params, shadows definition in base class + Params& p() const { return *boost::static_pointer_cast(p_);} + /// @} + + /// @name Access instance variables + /// @{ + Matrix preintMeasCov() const { return preintMeasCov_; } + /// @} + + /// @name Testable + /// @{ + void print(const std::string& s = "Preintegrated Measurements:") const; + bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const; + /// @} + + /// @name Main functionality + /// @{ + /** * Add a single IMU measurement to the preintegration. * @param measuredAcc Measured acceleration (in body frame, as given by the @@ -145,9 +165,9 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase { void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT); - /// methods to access class variables - Matrix preintMeasCov() const { return preintMeasCov_; } + /// @} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// deprecated constructor /// NOTE(frank): assumes Z-Down convention, only second order integration supported PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat, @@ -155,7 +175,8 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase { const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, - const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true); + const Matrix6& biasAccOmegaInt, const bool use2ndOrderIntegration = true); +#endif private: /// Serialization function @@ -255,6 +276,7 @@ public: boost::optional H4 = boost::none, boost::optional H5 = boost::none, boost::optional H6 = boost::none) const; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @deprecated typename typedef gtsam::PreintegratedCombinedMeasurements CombinedPreintegratedMeasurements; @@ -271,6 +293,7 @@ public: CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); +#endif private: diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index bfd3ebb52..87913cda6 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -58,10 +58,10 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, // Estimate Rotation double yaw = atan2(nV.y(), nV.x()); - Rot3 nRy = Rot3::yaw(yaw); // yaw frame + Rot3 nRy = Rot3::Yaw(yaw); // yaw frame Point3 yV = nRy.inverse() * nV; // velocity in yaw frame double pitch = -atan2(yV.z(), yV.x()), roll = 0; - Rot3 nRb = Rot3::ypr(yaw, pitch, roll); + Rot3 nRb = Rot3::Ypr(yaw, pitch, roll); // Construct initial pose Pose3 nTb(nRb, nT); // nTb diff --git a/gtsam/navigation/ImuBias.cpp b/gtsam/navigation/ImuBias.cpp new file mode 100644 index 000000000..0dbc5736f --- /dev/null +++ b/gtsam/navigation/ImuBias.cpp @@ -0,0 +1,82 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ImuBias.cpp + * @date Feb 2, 2012 + * @author Vadim Indelman, Stephen Williams + */ + +#include "ImuBias.h" + +#include +#include + +namespace gtsam { + +/// All bias models live in the imuBias namespace +namespace imuBias { + +/* + * NOTES: + * - Earth-rate correction: + * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defined by the user). + * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. + * + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant. + * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. + * + * - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G. + */ +// // H1: Jacobian w.r.t. IMUBias +// // H2: Jacobian w.r.t. pose +// Vector CorrectGyroWithEarthRotRate(Vector measurement, const Pose3& pose, const Vector& w_earth_rate_G, +// boost::optional H1=boost::none, boost::optional H2=boost::none) const { +// +// Matrix R_G_to_I( pose.rotation().matrix().transpose() ); +// Vector w_earth_rate_I = R_G_to_I * w_earth_rate_G; +// +// if (H1){ +// Matrix zeros3_3(zeros(3,3)); +// Matrix m_eye3(-eye(3)); +// +// *H1 = collect(2, &zeros3_3, &m_eye3); +// } +// +// if (H2){ +// Matrix zeros3_3(zeros(3,3)); +// Matrix H = -skewSymmetric(w_earth_rate_I); +// +// *H2 = collect(2, &H, &zeros3_3); +// } +// +// //TODO: Make sure H2 is correct. +// +// return measurement - biasGyro_ - w_earth_rate_I; +// +//// Vector bias_gyro_temp((Vector(3) << -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2))); +//// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G; +// } +/// ostream operator +std::ostream& operator<<(std::ostream& os, const ConstantBias& bias) { + os << "acc = " << Point3(bias.accelerometer()); + os << " gyro = " << Point3(bias.gyroscope()); + return os; +} + +/// print with optional string +void ConstantBias::print(const std::string& s) const { + std::cout << s << *this << std::endl; +} + +} // namespace imuBias + +} // namespace gtsam + diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 047f24e8f..11799f310 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -17,22 +17,11 @@ #pragma once -#include -#include +#include #include +#include #include -/* - * NOTES: - * - Earth-rate correction: - * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defined by the user). - * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. - * + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant. - * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. - * - * - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G. - */ - namespace gtsam { /// All bias models live in the imuBias namespace @@ -78,62 +67,32 @@ public: /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ Vector3 correctAccelerometer(const Vector3& measurement, - OptionalJacobian<3, 6> H = boost::none) const { - if (H) { - (*H) << -I_3x3, Z_3x3; - } + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + if (H1) (*H1) << -I_3x3, Z_3x3; + if (H2) (*H2) << I_3x3; return measurement - biasAcc_; } /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ Vector3 correctGyroscope(const Vector3& measurement, - OptionalJacobian<3, 6> H = boost::none) const { - if (H) { - (*H) << Z_3x3, -I_3x3; - } + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + if (H1) (*H1) << Z_3x3, -I_3x3; + if (H2) (*H2) << I_3x3; return measurement - biasGyro_; } -// // H1: Jacobian w.r.t. IMUBias -// // H2: Jacobian w.r.t. pose -// Vector CorrectGyroWithEarthRotRate(Vector measurement, const Pose3& pose, const Vector& w_earth_rate_G, -// boost::optional H1=boost::none, boost::optional H2=boost::none) const { -// -// Matrix R_G_to_I( pose.rotation().matrix().transpose() ); -// Vector w_earth_rate_I = R_G_to_I * w_earth_rate_G; -// -// if (H1){ -// Matrix zeros3_3(zeros(3,3)); -// Matrix m_eye3(-eye(3)); -// -// *H1 = collect(2, &zeros3_3, &m_eye3); -// } -// -// if (H2){ -// Matrix zeros3_3(zeros(3,3)); -// Matrix H = -skewSymmetric(w_earth_rate_I); -// -// *H2 = collect(2, &H, &zeros3_3); -// } -// -// //TODO: Make sure H2 is correct. -// -// return measurement - biasGyro_ - w_earth_rate_I; -// -//// Vector bias_gyro_temp((Vector(3) << -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2))); -//// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G; -// } + /// @} + /// @name Testable + /// @{ -/// @} -/// @name Testable -/// @{ + /// ostream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const ConstantBias& bias); -/// print with optional string - void print(const std::string& s = "") const { - // explicit printing for now. - std::cout << s + ".Acc [" << biasAcc_.transpose() << "]" << std::endl; - std::cout << s + ".Gyro [" << biasGyro_.transpose() << "]" << std::endl; - } + /// print with optional string + void print(const std::string& s = "") const; /** equality up to tolerance */ inline bool equals(const ConstantBias& expected, double tol = 1e-5) const { @@ -201,7 +160,6 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("imuBias::ConstantBias", *this); ar & BOOST_SERIALIZATION_NVP(biasAcc_); ar & BOOST_SERIALIZATION_NVP(biasGyro_); } diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 70527d91d..145359d91 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -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,7 +29,7 @@ namespace gtsam { using namespace std; //------------------------------------------------------------------------------ -// Inner class PreintegratedMeasurements +// Inner class PreintegratedImuMeasurements //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::print(const string& s) const { PreintegrationBase::print(s); @@ -52,45 +52,39 @@ void PreintegratedImuMeasurements::resetIntegration() { //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { - - static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished(); - // Update preintegrated measurements (also get Jacobian) - Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix93 G1, G2; - Matrix3 D_incrR_integratedOmega; - PreintegrationBase::update(measuredAcc, measuredOmega, dt, - &D_incrR_integratedOmega, &F, &G1, &G2); + Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix93 B, C; + PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); // first order covariance propagation: - // as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF - /* --------------------------------------------------------------------------------------------*/ - // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G' - // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise - // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) -#ifdef OLD_JACOBIAN_CALCULATION - Matrix9 G; - G << G1, Gi, G2; - Matrix9 Cov; - Cov << p().accelerometerCovariance / dt, Z_3x3, Z_3x3, - Z_3x3, p().integrationCovariance * dt, Z_3x3, - Z_3x3, Z_3x3, p().gyroscopeCovariance / dt; - preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G * Cov * G.transpose(); -#else - preintMeasCov_ = F * preintMeasCov_ * F.transpose() - + Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt) - + G1 * (p().accelerometerCovariance / dt) * G1.transpose() - + G2 * (p().gyroscopeCovariance / dt) * G2.transpose(); -#endif + // as in [2] we consider a first order propagation that can be seen as a + // prediction phase in EKF + + // propagate uncertainty + // TODO(frank): use noiseModel routine so we can have arbitrary noise models. + const Matrix3& aCov = p().accelerometerCovariance; + const Matrix3& wCov = p().gyroscopeCovariance; + const Matrix3& iCov = p().integrationCovariance; + + // (1/dt) allows to pass from continuous time noise to discrete time noise + preintMeasCov_ = A * preintMeasCov_ * A.transpose(); + 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(); } //------------------------------------------------------------------------------ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 PreintegratedImuMeasurements::PreintegratedImuMeasurements( const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) { if (!use2ndOrderIntegration) - throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); + throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); biasHat_ = biasHat; boost::shared_ptr p = Params::MakeSharedD(); p->gyroscopeCovariance = measuredOmegaCovariance; @@ -100,7 +94,6 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements( resetIntegration(); } -//------------------------------------------------------------------------------ void PreintegratedImuMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor) { @@ -108,6 +101,7 @@ void PreintegratedImuMeasurements::integrateMeasurement( p_->body_P_sensor = body_P_sensor; integrateMeasurement(measuredAcc, measuredOmega, deltaT); } +#endif //------------------------------------------------------------------------------ // ImuFactor methods @@ -119,9 +113,18 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, } //------------------------------------------------------------------------------ -gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +NonlinearFactor::shared_ptr ImuFactor::clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new This(*this))); +} + +//------------------------------------------------------------------------------ +std::ostream& operator<<(std::ostream& os, const ImuFactor& f) { + os << " preintegrated measurements:\n" << f._PIM_; + ; + // Print standard deviations on covariance only + os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose(); + return os; } //------------------------------------------------------------------------------ @@ -130,18 +133,15 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ")\n"; - Base::print(""); - _PIM_.print(" preintegrated measurements:"); - // Print standard deviations on covariance only - cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() - << endl; + cout << *this << endl; } //------------------------------------------------------------------------------ bool ImuFactor::equals(const NonlinearFactor& other, double tol) const { const This *e = dynamic_cast(&other); - return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) - && Base::equals(*e, tol); + const bool base = Base::equals(*e, tol); + const bool pim = _PIM_.equals(e->_PIM_, tol); + return e != nullptr && base && pim; } //------------------------------------------------------------------------------ @@ -155,14 +155,62 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, } //------------------------------------------------------------------------------ +PreintegratedImuMeasurements ImuFactor::Merge( + const PreintegratedImuMeasurements& pim01, + const PreintegratedImuMeasurements& pim12) { + if (!pim01.matchesParamsWith(pim12)) + throw std::domain_error( + "Cannot merge PreintegratedImuMeasurements with different params"); + + if (pim01.params()->body_P_sensor) + throw std::domain_error( + "Cannot merge PreintegratedImuMeasurements with sensor pose yet"); + + // the bias for the merged factor will be the bias from 01 + PreintegratedImuMeasurements pim02 = pim01; + + Matrix9 H1, H2; + pim02.mergeWith(pim12, &H1, &H2); + + pim02.preintMeasCov_ = H1 * pim01.preintMeasCov_ * H1.transpose() + + H2 * pim12.preintMeasCov_ * H2.transpose(); + + return pim02; +} + +//------------------------------------------------------------------------------ +ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01, + const shared_ptr& f12) { + // IMU bias keys must be the same. + if (f01->key5() != f12->key5()) + throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same"); + + // expect intermediate pose, velocity keys to matchup. + if (f01->key3() != f12->key1() || f01->key4() != f12->key2()) + throw std::domain_error( + "ImuFactor::Merge: intermediate pose, velocity keys need to match up"); + + // return new factor + auto pim02 = + Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); + return boost::make_shared(f01->key1(), // P0 + f01->key2(), // V0 + f12->key3(), // P2 + f12->key4(), // V2 + f01->key5(), // B + pim02); +} + +//------------------------------------------------------------------------------ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedMeasurements& pim, const Vector3& n_gravity, + const PreintegratedImuMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, const bool use2ndOrderCoriolis) : - Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, - pose_j, vel_j, bias), _PIM_(pim) { - boost::shared_ptr p = boost::make_shared< - PreintegratedMeasurements::Params>(pim.p()); +Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias), _PIM_(pim) { + boost::shared_ptr p = boost::make_shared< + PreintegratedImuMeasurements::Params>(pim.p()); p->n_gravity = n_gravity; p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; @@ -170,10 +218,9 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, _PIM_.p_ = p; } -//------------------------------------------------------------------------------ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, - PreintegratedMeasurements& pim, const Vector3& n_gravity, + PreintegratedImuMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) { // use deprecated predict PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity, @@ -181,5 +228,8 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, pose_j = pvb.pose; vel_j = pvb.velocity; } +#endif +//------------------------------------------------------------------------------ -} // namespace gtsam +} + // namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 855c14365..73a2f05d2 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -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,9 @@ protected: ///< (first-order propagation from *measurementCovariance*). /// Default constructor for serialization - PreintegratedImuMeasurements() {} + PreintegratedImuMeasurements() { + preintMeasCov_.setZero(); + } public: @@ -80,12 +82,22 @@ public: * @param bias Current estimate of acceleration and rotation rate biases * @param p Parameters, typically fixed in a single application */ - PreintegratedImuMeasurements(const boost::shared_ptr& p, - const imuBias::ConstantBias& biasHat) : + PreintegratedImuMeasurements(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : PreintegrationBase(p, biasHat) { preintMeasCov_.setZero(); } +/** + * Construct preintegrated directly from members: base class and preintMeasCov + * @param base PreintegrationBase instance + * @param preintMeasCov Covariance matrix used in noise model. + */ + PreintegratedImuMeasurements(const PreintegrationBase& base, const Matrix9& preintMeasCov) + : PreintegrationBase(base), + preintMeasCov_(preintMeasCov) { + } + /// print void print(const std::string& s = "Preintegrated Measurements:") const; @@ -108,6 +120,7 @@ public: /// Return pre-integrated measurement covariance Matrix preintMeasCov() const { return preintMeasCov_; } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @deprecated constructor /// NOTE(frank): assumes Z-Down convention, only second order integration supported PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat, @@ -121,6 +134,7 @@ public: void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt, boost::optional body_P_sensor); +#endif private: @@ -128,8 +142,9 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); - ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); + ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size())); } }; @@ -167,7 +182,7 @@ public: #endif /** Default constructor - only use for serialization */ - ImuFactor(); + ImuFactor() {} /** * Constructor @@ -186,14 +201,13 @@ public: /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const; - /** implement functions needed for Testable */ - - /// print + /// @name Testable + /// @{ + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&); virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /// equals virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + /// @} /** Access the preintegrated measurements. */ @@ -211,10 +225,19 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; + /// Merge two pre-integrated measurement classes + static PreintegratedImuMeasurements Merge( + const PreintegratedImuMeasurements& pim01, + const PreintegratedImuMeasurements& pim12); + + /// Merge two factors + static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12); + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @deprecated typename typedef PreintegratedImuMeasurements PreintegratedMeasurements; - /// @deprecated constructor, in the new one gravity, coriolis settings are in Params + /// @deprecated constructor, in the new one gravity, coriolis settings are in PreintegrationParams ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& n_gravity, const Vector3& omegaCoriolis, @@ -222,11 +245,12 @@ public: const bool use2ndOrderCoriolis = false); /// @deprecated use PreintegrationBase::predict, - /// in the new one gravity, coriolis settings are in Params + /// in the new one gravity, coriolis settings are in PreintegrationParams static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, PreintegratedMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); +#endif private: @@ -241,4 +265,10 @@ private: }; // class ImuFactor +template <> +struct traits : public Testable {}; + +template <> +struct traits : public Testable {}; + } /// namespace gtsam diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index f70bec8c6..fc1e69190 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -60,7 +60,7 @@ public: static Point3 unrotate(const Rot2& R, const Point3& p, boost::optional HR = boost::none) { - Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR, boost::none); + Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none); if (HR) { // assign to temporary first to avoid error in Win-Debug mode Matrix H = HR->col(2); diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 6266c328f..860eaa85c 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -74,11 +74,17 @@ Matrix7 NavState::matrix() const { return T; } +//------------------------------------------------------------------------------ +ostream& operator<<(ostream& os, const NavState& state) { + os << "R:" << state.attitude(); + os << "p:" << state.position() << endl; + os << "v:" << Point3(state.velocity()) << endl; + return os; +} + //------------------------------------------------------------------------------ void NavState::print(const string& s) const { - attitude().print(s + ".R"); - position().print(s + ".p"); - gtsam::print((Vector) v_, s + ".v"); + cout << s << *this << endl; } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 9561aa77b..4c8b50776 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -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) @@ -75,18 +75,9 @@ public: /// @name Component Access /// @{ - inline const Rot3& attitude() const { - return R_; - } - inline const Point3& position() const { - return t_; - } - inline const Velocity3& velocity() const { - return v_; - } - const Rot3& attitude(OptionalJacobian<3, 9> H) const; - const Point3& position(OptionalJacobian<3, 9> H) const; - const Velocity3& velocity(OptionalJacobian<3, 9> H) const; + const Rot3& attitude(OptionalJacobian<3, 9> H = boost::none) const; + const Point3& position(OptionalJacobian<3, 9> H = boost::none) const; + const Velocity3& velocity(OptionalJacobian<3, 9> H = boost::none) const; const Pose3 pose() const { return Pose3(attitude(), position()); @@ -124,6 +115,9 @@ public: /// @name Testable /// @{ + /// Output stream operator + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const NavState& state); + /// print void print(const std::string& s = "") const; @@ -229,6 +223,8 @@ public: false, OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = boost::none) const; + /// @} + private: /// @{ /// serialization diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index 708d1a3f6..df38df0b8 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -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,12 +29,26 @@ void PreintegratedRotation::Params::print(const string& s) const { cout << s << endl; cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl; if (omegaCoriolis) - cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" - << endl; + cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl; if (body_P_sensor) body_P_sensor->print("body_P_sensor"); } +bool PreintegratedRotation::Params::equals( + const PreintegratedRotation::Params& other, double tol) const { + if (body_P_sensor) { + if (!other.body_P_sensor + || !assert_equal(*body_P_sensor, *other.body_P_sensor, tol)) + return false; + } + if (omegaCoriolis) { + if (!other.omegaCoriolis + || !equal_with_abs_tol(*omegaCoriolis, *other.omegaCoriolis, tol)) + return false; + } + return equal_with_abs_tol(gyroscopeCovariance, other.gyroscopeCovariance, tol); +} + void PreintegratedRotation::resetIntegration() { deltaTij_ = 0.0; deltaRij_ = Rot3(); @@ -42,14 +56,14 @@ void PreintegratedRotation::resetIntegration() { } void PreintegratedRotation::print(const string& s) const { - cout << s << endl; + cout << s; cout << " deltaTij [" << deltaTij_ << "]" << endl; cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl; } bool PreintegratedRotation::equals(const PreintegratedRotation& other, double tol) const { - return deltaRij_.equals(other.deltaRij_, tol) + return p_->equals(*other.p_,tol) && deltaRij_.equals(other.deltaRij_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); } @@ -76,12 +90,18 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega, } void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega, - const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega, - Matrix3* F) { - + const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, + OptionalJacobian<3, 3> F) { + Matrix3 D_incrR_integratedOmega; const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, D_incrR_integratedOmega); + // If asked, pass first derivative as well + if (optional_D_incrR_integratedOmega) { + *optional_D_incrR_integratedOmega << D_incrR_integratedOmega; + } + // Update deltaTij and rotation deltaTij_ += deltaT; deltaRij_ = deltaRij_.compose(incrR, F); @@ -89,7 +109,7 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega, // Update Jacobian const Matrix3 incrRt = incrR.transpose(); delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - - *D_incrR_integratedOmega * deltaT; + - D_incrR_integratedOmega * deltaT; } Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr, diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 002afea76..b170ea863 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -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,56 +32,76 @@ namespace gtsam { * It includes the definitions of the preintegrated rotation. */ class PreintegratedRotation { -public: - + public: /// Parameters for pre-integration: /// Usage: Create just a single Params and pass a shared pointer to the constructor struct Params { - Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements - boost::optional omegaCoriolis; ///< Coriolis constant - boost::optional body_P_sensor; ///< The pose of the sensor in the body frame + Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements + boost::optional omegaCoriolis; ///< Coriolis constant + boost::optional body_P_sensor; ///< The pose of the sensor in the body frame - Params() : - gyroscopeCovariance(I_3x3) { - } + Params() : gyroscopeCovariance(I_3x3) {} virtual void print(const std::string& s) const; + virtual bool equals(const Params& other, double tol=1e-9) const; - private: + private: /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance); + namespace bs = ::boost::serialization; + ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size())); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); ar & BOOST_SERIALIZATION_NVP(body_P_sensor); } }; -protected: - double deltaTij_; ///< Time interval from i to j - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - + protected: /// Parameters boost::shared_ptr p_; - /// Default constructor for serialization - PreintegratedRotation() { - } + double deltaTij_; ///< Time interval from i to j + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + + /// Default constructor for serialization + PreintegratedRotation() {} + + public: + /// @name Constructors + /// @{ -public: /// Default constructor, resets integration to zero - explicit PreintegratedRotation(const boost::shared_ptr& p) : - p_(p) { + explicit PreintegratedRotation(const boost::shared_ptr& p) : p_(p) { resetIntegration(); } + /// Explicit initialization of all class members + PreintegratedRotation(const boost::shared_ptr& p, + double deltaTij, const Rot3& deltaRij, + const Matrix3& delRdelBiasOmega) + : p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_(delRdelBiasOmega) {} + + /// @} + + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements void resetIntegration(); + /// check parameters equality: checks whether shared pointer points to same Params object. + bool matchesParamsWith(const PreintegratedRotation& other) const { + return p_ == other.p_; + } + /// @} + /// @name Access instance variables /// @{ + const boost::shared_ptr& params() const { + return p_; + } const double& deltaTij() const { return deltaTij_; } @@ -95,41 +115,47 @@ public: /// @name Testable /// @{ - void print(const std::string& s) const; bool equals(const PreintegratedRotation& other, double tol) const; - /// @} + /// @name Main functionality + /// @{ + /// Take the gyro measurement, correct it using the (constant) bias estimate /// and possibly the sensor pose, and then integrate it forward in time to yield /// an incremental rotation. - Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, - double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const; + Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> D_incrR_integratedOmega) const; /// Calculate an incremental rotation given the gyro measurement and a time interval, /// and update both deltaTij_ and deltaRij_. - void integrateMeasurement(const Vector3& measuredOmega, - const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega, - Matrix3* F); + void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none, + OptionalJacobian<3, 3> F = boost::none); /// Return a bias corrected version of the integrated rotation, with optional Jacobian Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr, - OptionalJacobian<3, 3> H = boost::none) const; + OptionalJacobian<3, 3> H = boost::none) const; /// Integrate coriolis correction in body frame rot_i Vector3 integrateCoriolis(const Rot3& rot_i) const; -private: + /// @} + + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // NOLINT - ar & BOOST_SERIALIZATION_NVP(p_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NOLINT + ar& BOOST_SERIALIZATION_NVP(p_); + ar& BOOST_SERIALIZATION_NVP(deltaTij_); + ar& BOOST_SERIALIZATION_NVP(deltaRij_); + ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } }; -} // namespace gtsam +template <> +struct traits : public Testable {}; + +} /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index bb01971e6..c3f203849 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -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) @@ -20,6 +20,7 @@ **/ #include "PreintegrationBase.h" +#include #include using namespace std; @@ -27,229 +28,254 @@ using namespace std; namespace gtsam { //------------------------------------------------------------------------------ -void PreintegrationBase::Params::print(const string& s) const { - PreintegratedRotation::Params::print(s); - cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]" - << endl; - cout << "integrationCovariance:\n[\n" << accelerometerCovariance << "\n]" - << endl; - if (omegaCoriolis && use2ndOrderCoriolis) - cout << "Using 2nd-order Coriolis" << endl; - if (body_P_sensor) - body_P_sensor->print(" "); - cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl; +PreintegrationBase::PreintegrationBase(const boost::shared_ptr& p, + const Bias& biasHat) + : p_(p), biasHat_(biasHat), deltaTij_(0.0) { + resetIntegration(); } //------------------------------------------------------------------------------ void PreintegrationBase::resetIntegration() { deltaTij_ = 0.0; - deltaXij_ = NavState(); - delRdelBiasOmega_ = Z_3x3; - delPdelBiasAcc_ = Z_3x3; - delPdelBiasOmega_ = Z_3x3; - delVdelBiasAcc_ = Z_3x3; - delVdelBiasOmega_ = Z_3x3; + preintegrated_.setZero(); + preintegrated_H_biasAcc_.setZero(); + preintegrated_H_biasOmega_.setZero(); +} + +//------------------------------------------------------------------------------ +ostream& operator<<(ostream& os, const PreintegrationBase& pim) { + os << " deltaTij " << pim.deltaTij_ << endl; + os << " deltaRij " << Point3(pim.theta()) << endl; + os << " deltaPij " << Point3(pim.deltaPij()) << endl; + os << " deltaVij " << Point3(pim.deltaVij()) << endl; + os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl; + os << " acc_bias " << Point3(pim.biasHat_.accelerometer()) << endl; + return os; } //------------------------------------------------------------------------------ void PreintegrationBase::print(const string& s) const { - cout << s << endl; - cout << " deltaTij [" << deltaTij_ << "]" << endl; - cout << " deltaRij.ypr = (" << deltaRij().ypr().transpose() << ")" << endl; - cout << " deltaPij [ " << deltaPij().transpose() << " ]" << endl; - cout << " deltaVij [ " << deltaVij().transpose() << " ]" << endl; - biasHat_.print(" biasHat"); + cout << s << *this << endl; } //------------------------------------------------------------------------------ bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const { - return fabs(deltaTij_ - other.deltaTij_) < tol - && deltaXij_.equals(other.deltaXij_, tol) + const bool params_match = p_->equals(*other.p_, tol); + return params_match && fabs(deltaTij_ - other.deltaTij_) < tol && biasHat_.equals(other.biasHat_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) - && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol); + && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) + && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol) + && equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol); } //------------------------------------------------------------------------------ -pair PreintegrationBase::correctMeasurementsByBiasAndSensorPose( - const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, - OptionalJacobian<3, 3> D_correctedAcc_measuredAcc, - OptionalJacobian<3, 3> D_correctedAcc_measuredOmega, - OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const { - - // Correct for bias in the sensor frame - Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc); - Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega); +pair 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 { + assert(p().body_P_sensor); // Compensate for sensor-body displacement if needed: we express the quantities // (originally in the IMU frame) into the body frame // Equations below assume the "body" frame is the CG - if (p().body_P_sensor) { - // Correct omega to rotation rate vector in the body frame - const Matrix3 bRs = p().body_P_sensor->rotation().matrix(); - j_correctedOmega = bRs * j_correctedOmega; - // Correct acceleration - j_correctedAcc = bRs * j_correctedAcc; + // Get sensor to body rotation matrix + const Matrix3 bRs = p().body_P_sensor->rotation().matrix(); - // Jacobians - if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs; - if (D_correctedAcc_measuredOmega) *D_correctedAcc_measuredOmega = Matrix3::Zero(); - if (D_correctedOmega_measuredOmega) *D_correctedOmega_measuredOmega = bRs; + // Convert angular velocity and acceleration from sensor to body frame + Vector3 correctedAcc = bRs * unbiasedAcc; + const Vector3 correctedOmega = bRs * unbiasedOmega; - // Centrifugal acceleration - const Vector3 b_arm = p().body_P_sensor->translation().vector(); - if (!b_arm.isZero()) { - // Subtract out the the centripetal acceleration from the measured one - // to get linear acceleration vector in the body frame: - const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega); - const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm - j_correctedAcc -= body_Omega_body * b_velocity_bs; - // Update derivative: centrifugal causes the correlation between acc and omega!!! - if (D_correctedAcc_measuredOmega) { - double wdp = j_correctedOmega.dot(b_arm); - *D_correctedAcc_measuredOmega = -(diag(Vector3::Constant(wdp)) - + j_correctedOmega * b_arm.transpose()) * bRs.matrix() - + 2 * b_arm * j_measuredOmega.transpose(); - } + // 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; + + // Centrifugal acceleration + const Vector3 b_arm = p().body_P_sensor->translation().vector(); + if (!b_arm.isZero()) { + // Subtract out the the centripetal acceleration from the unbiased one + // to get linear acceleration vector in the body frame: + const Matrix3 body_Omega_body = skewSymmetric(correctedOmega); + const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm + correctedAcc -= body_Omega_body * b_velocity_bs; + + // Update derivative: centrifugal causes the correlation between acc and omega!!! + if (D_correctedAcc_unbiasedOmega) { + double wdp = correctedOmega.dot(b_arm); + *D_correctedAcc_unbiasedOmega = -(diag(Vector3::Constant(wdp)) + + correctedOmega * b_arm.transpose()) * bRs.matrix() + + 2 * b_arm * unbiasedOmega.transpose(); } } - // Do update in one fell swoop - return make_pair(j_correctedAcc, j_correctedOmega); + return make_pair(correctedAcc, correctedOmega); } //------------------------------------------------------------------------------ -NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc, - const Vector3& j_measuredOmega, const double dt, - OptionalJacobian<9, 9> D_updated_current, - OptionalJacobian<9, 3> D_updated_measuredAcc, - OptionalJacobian<9, 3> D_updated_measuredOmega) const { +// See extensive discussion in ImuFactor.lyx +Vector9 PreintegrationBase::UpdatePreintegrated( + const Vector3& a_body, const Vector3& w_body, double dt, + const Vector9& preintegrated, OptionalJacobian<9, 9> A, + OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) { + const auto theta = preintegrated.segment<3>(0); + const auto position = preintegrated.segment<3>(3); + const auto velocity = preintegrated.segment<3>(6); - Vector3 j_correctedAcc, j_correctedOmega; - Matrix3 D_correctedAcc_measuredAcc, // - D_correctedAcc_measuredOmega, // - D_correctedOmega_measuredOmega; - bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor; - boost::tie(j_correctedAcc, j_correctedOmega) = - correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega, - (needDerivs ? &D_correctedAcc_measuredAcc : 0), - (needDerivs ? &D_correctedAcc_measuredOmega : 0), - (needDerivs ? &D_correctedOmega_measuredOmega : 0)); - // Do update in one fell swoop - Matrix93 D_updated_correctedAcc, D_updated_correctedOmega; - NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, D_updated_current, - (needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc), - (needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega)); - if (needDerivs) { - *D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc; - *D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega; - if (!p().body_P_sensor->translation().vector().isZero()) { - *D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega; - } + // This functor allows for saving computation when exponential map and its + // derivatives are needed at the same location in so<3> + so3::DexpFunctor local(theta); + + // Calculate exact mean propagation + Matrix3 w_tangent_H_theta, invH; + const Vector3 w_tangent = // angular velocity mapped back to tangent space + local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); + const SO3 R = local.expmap(); + const Vector3 a_nav = R * a_body; + const double dt22 = 0.5 * dt * dt; + + Vector9 preintegratedPlus; + preintegratedPlus << // new preintegrated vector: + theta + w_tangent* dt, // theta + position + velocity* dt + a_nav* dt22, // position + velocity + a_nav* dt; // velocity + + if (A) { + // Exact derivative of R*a with respect to theta: + const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp(); + + A->setIdentity(); + A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta + A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... + A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity + A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta } - return updated; + if (B) { + B->block<3, 3>(0, 0) = Z_3x3; + B->block<3, 3>(3, 0) = R * dt22; + B->block<3, 3>(6, 0) = R * dt; + } + if (C) { + C->block<3, 3>(0, 0) = invH * dt; + C->block<3, 3>(3, 0) = Z_3x3; + C->block<3, 3>(6, 0) = Z_3x3; + } + + return preintegratedPlus; } //------------------------------------------------------------------------------ -void PreintegrationBase::update(const Vector3& j_measuredAcc, - const Vector3& j_measuredOmega, const double dt, - Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, - Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) { +void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, + double dt, Matrix9* A, + Matrix93* B, Matrix93* C) { + // Correct for bias in the sensor frame + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); - // Save current rotation for updating Jacobians - const Rot3 oldRij = deltaXij_.attitude(); + // Possibly correct for sensor pose + Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; + if (p().body_P_sensor) + boost::tie(acc, omega) = correctMeasurementsBySensorPose( acc, omega, + D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); - // Do update + // Do update deltaTij_ += dt; - deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt, - D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional + preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C); - // Update Jacobians - // TODO(frank): we are repeating some computation here: accessible in F ? - Vector3 j_correctedAcc, j_correctedOmega; - boost::tie(j_correctedAcc, j_correctedOmega) = - correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega); + if (p().body_P_sensor) { + // More complicated derivatives in case of non-trivial sensor pose + *C *= D_correctedOmega_omega; + if (!p().body_P_sensor->translation().vector().isZero()) + *C += *B* D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + } - Matrix3 D_acc_R; - oldRij.rotate(j_correctedAcc, D_acc_R); - const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; + // D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias + preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B); - const Vector3 integratedOmega = j_correctedOmega * dt; - const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! - const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - - *D_incrR_integratedOmega * dt; - - double dt22 = 0.5 * dt * dt; - const Matrix3 dRij = oldRij.matrix(); // expensive - delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; - delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; - delVdelBiasAcc_ += -dRij * dt; - delVdelBiasOmega_ += D_acc_biasOmega * dt; + // D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias + preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C); } +void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, + double dt) { + // NOTE(frank): integrateMeasuremtn always needs to compute the derivatives, + // even when not of interest to the caller. Provide scratch space here. + Matrix9 A; + Matrix93 B, C; + integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); +} //------------------------------------------------------------------------------ Vector9 PreintegrationBase::biasCorrectedDelta( const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { - // Correct deltaRij, derivative is delRdelBiasOmega_ + // We correct for a change between bias_i and the biasHat_ used to integrate + // This is a simple linear correction with obvious derivatives const imuBias::ConstantBias biasIncr = bias_i - biasHat_; - Matrix3 D_correctedRij_bias; - const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); - const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, - H ? &D_correctedRij_bias : 0); - if (H) - D_correctedRij_bias *= delRdelBiasOmega_; - - Vector9 xi; - Matrix3 D_dR_correctedRij; - // TODO(frank): could line below be simplified? It is equivalent to - // LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) - NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); - NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer() - + delPdelBiasOmega_ * biasIncr.gyroscope(); - NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer() - + delVdelBiasOmega_ * biasIncr.gyroscope(); + const Vector9 biasCorrected = + preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() + + preintegrated_H_biasOmega_ * biasIncr.gyroscope(); if (H) { - Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; - D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias; - D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_; - D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_; - (*H) << D_dR_bias, D_dP_bias, D_dV_bias; + (*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_; } - return xi; + return biasCorrected; } //------------------------------------------------------------------------------ NavState PreintegrationBase::predict(const NavState& state_i, - const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 6> H2) const { - // correct for bias + const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 6> H2) const { + // TODO(frank): make sure this stuff is still correct Matrix96 D_biasCorrected_bias; - Vector9 biasCorrected = biasCorrectedDelta(bias_i, - H2 ? &D_biasCorrected_bias : 0); + Vector9 biasCorrected = + biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0); - // integrate on tangent space + // Correct for initial velocity and gravity Matrix9 D_delta_state, D_delta_biasCorrected; Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, - p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0, - H2 ? &D_delta_biasCorrected : 0); + p().omegaCoriolis, p().use2ndOrderCoriolis, + H1 ? &D_delta_state : 0, + H2 ? &D_delta_biasCorrected : 0); // Use retract to get back to NavState manifold Matrix9 D_predict_state, D_predict_delta; NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta); - if (H1) - *H1 = D_predict_state + D_predict_delta * D_delta_state; - if (H2) - *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias; + if (H1) *H1 = D_predict_state + D_predict_delta* D_delta_state; + if (H2) *H2 = D_predict_delta* D_delta_biasCorrected* D_biasCorrected_bias; return state_j; } +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::computeError(const NavState& state_i, + const NavState& state_j, + const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2, + OptionalJacobian<9, 6> H3) const { + // Predict state at time j + Matrix9 D_predict_state_i; + Matrix96 D_predict_bias_i; + NavState predictedState_j = predict( + state_i, bias_i, H1 ? &D_predict_state_i : 0, H3 ? &D_predict_bias_i : 0); + + // Calculate error + Matrix9 D_error_state_j, D_error_predict; + Vector9 error = + state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0, + H1 || H3 ? &D_error_predict : 0); + + if (H1) *H1 << D_error_predict* D_predict_state_i; + if (H2) *H2 << D_error_state_j; + if (H3) *H3 << D_error_predict* D_predict_bias_i; + + return error; +} + //------------------------------------------------------------------------------ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, @@ -262,43 +288,120 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, NavState state_i(pose_i, vel_i); NavState state_j(pose_j, vel_j); - /// Predict state at time j - Matrix99 D_predict_state_i; - Matrix96 D_predict_bias_i; - NavState predictedState_j = predict(state_i, bias_i, - H1 || H2 ? &D_predict_state_i : 0, H5 ? &D_predict_bias_i : 0); - - Matrix9 D_error_state_j, D_error_predict; - Vector9 error = state_j.localCoordinates(predictedState_j, - H3 || H4 ? &D_error_state_j : 0, H1 || H2 || H5 ? &D_error_predict : 0); + // Predict state at time j + Matrix9 D_error_state_i, D_error_state_j; + Vector9 error = computeError(state_i, state_j, bias_i, + H1 || H2 ? &D_error_state_i : 0, H3 || H4 ? &D_error_state_j : 0, H5); // Separate out derivatives in terms of 5 arguments // Note that doing so requires special treatment of velocities, as when treated as // separate variables the retract applied will not be the semi-direct product in NavState // Instead, the velocities in nav are updated using a straight addition // This is difference is accounted for by the R().transpose calls below - if (H1) - *H1 << D_error_predict * D_predict_state_i.leftCols<6>(); - if (H2) - *H2 - << D_error_predict * D_predict_state_i.rightCols<3>() - * state_i.R().transpose(); - if (H3) - *H3 << D_error_state_j.leftCols<6>(); - if (H4) - *H4 << D_error_state_j.rightCols<3>() * state_j.R().transpose(); - if (H5) - *H5 << D_error_predict * D_predict_bias_i; + if (H1) *H1 << D_error_state_i.leftCols<6>(); + if (H2) *H2 << D_error_state_i.rightCols<3>() * state_i.R().transpose(); + if (H3) *H3 << D_error_state_j.leftCols<6>(); + if (H4) *H4 << D_error_state_j.rightCols<3>() * state_j.R().transpose(); return error; } //------------------------------------------------------------------------------ +// sugar for derivative blocks +#define D_R_R(H) (H)->block<3,3>(0,0) +#define D_R_t(H) (H)->block<3,3>(0,3) +#define D_R_v(H) (H)->block<3,3>(0,6) +#define D_t_R(H) (H)->block<3,3>(3,0) +#define D_t_t(H) (H)->block<3,3>(3,3) +#define D_t_v(H) (H)->block<3,3>(3,6) +#define D_v_R(H) (H)->block<3,3>(6,0) +#define D_v_t(H) (H)->block<3,3>(6,3) +#define D_v_v(H) (H)->block<3,3>(6,6) + +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::Compose(const Vector9& zeta01, + const Vector9& zeta12, double deltaT12, + OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2) { + const auto t01 = zeta01.segment<3>(0); + const auto p01 = zeta01.segment<3>(3); + const auto v01 = zeta01.segment<3>(6); + + const auto t12 = zeta12.segment<3>(0); + const auto p12 = zeta12.segment<3>(3); + const auto v12 = zeta12.segment<3>(6); + + Matrix3 R01_H_t01, R12_H_t12; + const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01); + const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12); + + Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity + const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12); + + Matrix3 t02_H_R02; + Vector9 zeta02; + const Matrix3 R = R01.matrix(); + zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta + p01 + v01 * deltaT12 + R * p12, // position + v01 + R * v12; // velocity + + if (H1) { + H1->setIdentity(); + D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01; + D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01; + D_t_v(H1) = I_3x3 * deltaT12; + D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01; + } + + if (H2) { + H2->setZero(); + D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12; + D_t_t(H2) = R; + D_v_v(H2) = R; + } + + return zeta02; +} + +//------------------------------------------------------------------------------ +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 (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(); + deltaTij_ = t01 + t12; + + Vector9 zeta01 = preintegrated(); + Vector9 zeta12 = pim12.preintegrated(); + + // 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_H_biasAcc_ = + (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_; + + preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + + (*H2) * pim12.preintegrated_H_biasOmega_; +} + +//------------------------------------------------------------------------------ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, const Vector3& omegaCoriolis, - const bool use2ndOrderCoriolis) { - // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility + const bool use2ndOrderCoriolis) const { +// NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility boost::shared_ptr q = boost::make_shared(p()); q->n_gravity = n_gravity; q->omegaCoriolis = omegaCoriolis; @@ -306,7 +409,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, p_ = q; return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i); } - +#endif //------------------------------------------------------------------------------ -}/// namespace gtsam +} // namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 07225dd9a..f8639cf79 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -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,13 +21,16 @@ #pragma once -#include +#include #include #include -#include +#include + +#include namespace gtsam { +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @deprecated struct PoseVelocityBias { Pose3 pose; @@ -44,6 +47,7 @@ struct PoseVelocityBias { return NavState(pose, velocity); } }; +#endif /** * PreintegrationBase is the base class for PreintegratedMeasurements @@ -52,171 +56,140 @@ struct PoseVelocityBias { * to access, print, and compare them. */ class PreintegrationBase { + public: + typedef imuBias::ConstantBias Bias; + typedef PreintegrationParams Params; -public: + protected: - /// Parameters for pre-integration: - /// Usage: Create just a single Params and pass a shared pointer to the constructor - struct Params: PreintegratedRotation::Params { - Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer - Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty - bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration - Vector3 n_gravity; ///< Gravity vector in nav frame + /// Parameters. Declared mutable only for deprecated predict method. + /// TODO(frank): make const once deprecated method is removed +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + mutable +#endif + boost::shared_ptr p_; - /// The Params constructor insists on getting the navigation frame gravity vector - /// For convenience, two commonly used conventions are provided by named constructors below - Params(const Vector3& n_gravity) : - accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis( - false), n_gravity(n_gravity) { - } + /// Acceleration and gyro bias used for preintegration + Bias biasHat_; - // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis - static boost::shared_ptr MakeSharedD(double g = 9.81) { - return boost::make_shared(Vector3(0, 0, g)); - } - - // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis - static boost::shared_ptr MakeSharedU(double g = 9.81) { - return boost::make_shared(Vector3(0, 0, -g)); - } - - void print(const std::string& s) const; - - protected: - /// Default constructor for serialization only: uninitialized! - Params(); - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params); - ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance); - ar & BOOST_SERIALIZATION_NVP(integrationCovariance); - ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); - ar & BOOST_SERIALIZATION_NVP(n_gravity); - } - }; - -protected: - - double deltaTij_; ///< Time interval from i to j + /// Time interval from i to j + double deltaTij_; /** * Preintegrated navigation state, from frame i to frame j * 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] */ - NavState deltaXij_; + Vector9 preintegrated_; - /// Parameters - boost::shared_ptr p_; - - /// Acceleration and gyro bias used for preintegration - imuBias::ConstantBias biasHat_; - - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias + Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias /// Default constructor for serialization PreintegrationBase() { - } - -public: - - /** - * Constructor, initializes the variables in the base class - * @param bias Current estimate of acceleration and rotation rate biases - * @param p Parameters, typically fixed in a single application - */ - PreintegrationBase(const boost::shared_ptr& p, - const imuBias::ConstantBias& biasHat) : - p_(p), biasHat_(biasHat) { resetIntegration(); } +public: + /// @name Constructors + /// @{ + + /** + * Constructor, initializes the variables in the base class + * @param p Parameters, typically fixed in a single application + * @param bias Current estimate of acceleration and rotation rate biases + */ + PreintegrationBase(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); + + /// @} + + /// @name Basic utilities + /// @{ /// Re-initialize PreintegratedMeasurements void resetIntegration(); + /// check parameters equality: checks whether shared pointer points to same Params object. + bool matchesParamsWith(const PreintegrationBase& other) const { + return p_ == other.p_; + } + + /// shared pointer to params + const boost::shared_ptr& params() const { + return p_; + } + + /// const reference to params const Params& p() const { return *boost::static_pointer_cast(p_); } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 void set_body_P_sensor(const Pose3& body_P_sensor) { p_->body_P_sensor = body_P_sensor; } +#endif +/// @} - /// getters - const NavState& deltaXij() const { - return deltaXij_; - } - const double& deltaTij() const { - return deltaTij_; - } - const Rot3& deltaRij() const { - return deltaXij_.attitude(); - } - Vector3 deltaPij() const { - return deltaXij_.position().vector(); - } - Vector3 deltaVij() const { - return deltaXij_.velocity(); - } - const imuBias::ConstantBias& biasHat() const { - return biasHat_; - } - const Matrix3& delRdelBiasOmega() const { - return delRdelBiasOmega_; - } - const Matrix3& delPdelBiasAcc() const { - return delPdelBiasAcc_; - } - const Matrix3& delPdelBiasOmega() const { - return delPdelBiasOmega_; - } - const Matrix3& delVdelBiasAcc() const { - return delVdelBiasAcc_; - } - const Matrix3& delVdelBiasOmega() const { - return delVdelBiasOmega_; - } + /// @name Instance variables access + /// @{ + const imuBias::ConstantBias& biasHat() const { return biasHat_; } + const double& deltaTij() const { return deltaTij_; } + + const Vector9& preintegrated() const { return preintegrated_; } + + Vector3 theta() const { return preintegrated_.head<3>(); } + Vector3 deltaPij() const { return preintegrated_.segment<3>(3); } + Vector3 deltaVij() const { return preintegrated_.tail<3>(); } + + Rot3 deltaRij() const { return Rot3::Expmap(theta()); } + NavState deltaXij() const { return NavState::Retract(preintegrated_); } + + const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; } + const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; } // Exposed for MATLAB - Vector6 biasHatVector() const { - return biasHat_.vector(); - } + Vector6 biasHatVector() const { return biasHat_.vector(); } + /// @} - /// print + /// @name Testable + /// @{ + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim); void print(const std::string& s) const; - - /// check equality bool equals(const PreintegrationBase& other, double tol) const; + /// @} + + /// @name Main functionality + /// @{ /// Subtract estimate and correct for sensor pose /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc) /// Ignore D_correctedOmega_measuredAcc as it is trivially zero - std::pair correctMeasurementsByBiasAndSensorPose( - const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, - OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none, - OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none, - OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const; + std::pair 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; - /// Calculate the updated preintegrated measurement, does not modify - /// It takes measured quantities in the j frame - NavState updatedDeltaXij(const Vector3& j_measuredAcc, - const Vector3& j_measuredOmega, const double dt, - OptionalJacobian<9, 9> D_updated_current = boost::none, - OptionalJacobian<9, 3> D_updated_measuredAcc = boost::none, - OptionalJacobian<9, 3> D_updated_measuredOmega = boost::none) const; + // Update integrated vector on tangent manifold preintegrated with acceleration + // Static, functional version. + static Vector9 UpdatePreintegrated(const Vector3& a_body, + const Vector3& w_body, double dt, + const Vector9& preintegrated, + OptionalJacobian<9, 9> A = boost::none, + OptionalJacobian<9, 3> B = boost::none, + OptionalJacobian<9, 3> C = boost::none); /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame - void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, - const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, - Matrix93* D_udpated_measuredAcc, Matrix93* D_updated_measuredOmega); + /// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double deltaT, + Matrix9* A, Matrix93* B, Matrix93* C); + + // Version without derivatives + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double deltaT); /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far @@ -225,8 +198,14 @@ public: /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = - boost::none) const; + OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 6> H2 = boost::none) const; + + /// Calculate error given navStates + Vector9 computeError(const NavState& state_i, const NavState& state_j, + const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, + OptionalJacobian<9, 6> H3) const; /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, @@ -236,25 +215,47 @@ 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 + static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, + double deltaT12, + OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 9> H2 = boost::none); + + /// Merge in a different set of measurements and update bias derivatives accordingly + /// The derivatives apply to the preintegrated Vector9 + void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2); + /// @} + + /** Dummy clone for MATLAB */ + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(); + } + + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + /// @deprecated predict PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const; + + /// @} +#endif private: /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; ar & BOOST_SERIALIZATION_NVP(p_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(deltaXij_); ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size())); + ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size())); + ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size())); } }; diff --git a/gtsam/navigation/PreintegrationParams.cpp b/gtsam/navigation/PreintegrationParams.cpp new file mode 100644 index 000000000..61cd1617c --- /dev/null +++ b/gtsam/navigation/PreintegrationParams.cpp @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PreintegrationParams.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include "PreintegrationParams.h" + +using namespace std; + +namespace gtsam { + +//------------------------------------------------------------------------------ +void PreintegrationParams::print(const string& s) const { + PreintegratedRotation::Params::print(s); + cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]" + << endl; + cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]" + << endl; + if (omegaCoriolis && use2ndOrderCoriolis) + cout << "Using 2nd-order Coriolis" << endl; + if (body_P_sensor) body_P_sensor->print(" "); + cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl; +} + +//------------------------------------------------------------------------------ +bool PreintegrationParams::equals(const PreintegratedRotation::Params& other, + double tol) const { + auto e = dynamic_cast(&other); + return e != nullptr && PreintegratedRotation::Params::equals(other, tol) && + use2ndOrderCoriolis == e->use2ndOrderCoriolis && + equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance, + tol) && + equal_with_abs_tol(integrationCovariance, e->integrationCovariance, + tol) && + equal_with_abs_tol(n_gravity, e->n_gravity, tol); +} + +} // namespace gtsam diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h new file mode 100644 index 000000000..f68f711f1 --- /dev/null +++ b/gtsam/navigation/PreintegrationParams.h @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PreintegrationParams.h + * @author Frank Dellaert + **/ + +#pragma once + +#include +#include + +namespace gtsam { + +/// Parameters for pre-integration: +/// Usage: Create just a single Params and pass a shared pointer to the constructor +struct PreintegrationParams: PreintegratedRotation::Params { + Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer + Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty + bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration + Vector3 n_gravity; ///< Gravity vector in nav frame + + /// The Params constructor insists on getting the navigation frame gravity vector + /// For convenience, two commonly used conventions are provided by named constructors below + PreintegrationParams(const Vector3& n_gravity) + : accelerometerCovariance(I_3x3), + integrationCovariance(I_3x3), + use2ndOrderCoriolis(false), + n_gravity(n_gravity) {} + + // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis + static boost::shared_ptr MakeSharedD(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, g)); + } + + // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis + static boost::shared_ptr MakeSharedU(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, -g)); + } + + void print(const std::string& s) const; + bool equals(const PreintegratedRotation::Params& other, double tol) const; + +protected: + /// Default constructor for serialization only: uninitialized! + PreintegrationParams() {} + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & boost::serialization::make_nvp("PreintegratedRotation_Params", + boost::serialization::base_object(*this)); + ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size())); + ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size())); + ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); + ar & BOOST_SERIALIZATION_NVP(n_gravity); + } +}; + +} // namespace gtsam diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h new file mode 100644 index 000000000..ad684f5f8 --- /dev/null +++ b/gtsam/navigation/Scenario.h @@ -0,0 +1,97 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Scenario.h + * @brief Simple class to test navigation scenarios + * @author Frank Dellaert + */ + +#pragma once +#include +#include + +namespace gtsam { + +/// Simple trajectory simulator. +class Scenario { + public: + // Quantities a Scenario needs to specify: + + virtual Pose3 pose(double t) const = 0; + virtual Vector3 omega_b(double t) const = 0; + virtual Vector3 velocity_n(double t) const = 0; + virtual Vector3 acceleration_n(double t) const = 0; + + // Derived quantities: + + Rot3 rotation(double t) const { return pose(t).rotation(); } + NavState navState(double t) const { return NavState(pose(t), velocity_n(t)); } + + Vector3 velocity_b(double t) const { + const Rot3 nRb = rotation(t); + return nRb.transpose() * velocity_n(t); + } + + Vector3 acceleration_b(double t) const { + const Rot3 nRb = rotation(t); + return nRb.transpose() * acceleration_n(t); + } +}; + +/** + * Scenario with constant twist 3D trajectory. + * Note that a plane flying level does not feel any acceleration: gravity is + * being perfectly compensated by the lift generated by the wings, and drag is + * compensated by thrust. The accelerometer will add the gravity component back + * in, as modeled by the specificForce method in ScenarioRunner. + */ +class ConstantTwistScenario : public Scenario { + public: + /// Construct scenario with constant twist [w,v] + ConstantTwistScenario(const Vector3& w, const Vector3& v) + : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)) {} + + Pose3 pose(double t) const override { return Pose3::Expmap(twist_ * t); } + Vector3 omega_b(double t) const override { return twist_.head<3>(); } + Vector3 velocity_n(double t) const override { + return rotation(t).matrix() * twist_.tail<3>(); + } + Vector3 acceleration_n(double t) const override { return rotation(t) * a_b_; } + + private: + const Vector6 twist_; + const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b +}; + +/// Accelerating from an arbitrary initial state, with optional rotation +class AcceleratingScenario : public Scenario { + public: + /// Construct scenario with constant acceleration in navigation frame and + /// optional angular velocity in body: rotating while translating... + AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0, + const Vector3& a_n, + const Vector3& omega_b = Vector3::Zero()) + : nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(a_n), omega_b_(omega_b) {} + + Pose3 pose(double t) const override { + return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0); + } + Vector3 omega_b(double t) const override { return omega_b_; } + Vector3 velocity_n(double t) const override { return v0_ + a_n_ * t; } + Vector3 acceleration_n(double t) const override { return a_n_; } + + private: + const Rot3 nRb_; + const Vector3 p0_, v0_, a_n_, omega_b_; +}; + +} // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp new file mode 100644 index 000000000..79f3f42cc --- /dev/null +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -0,0 +1,107 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ScenarioRunner.h + * @brief Simple class to test navigation scenarios + * @author Frank Dellaert + */ + +#include +#include + +#include +#include + +using namespace std; +using namespace boost::assign; + +namespace gtsam { + +static double intNoiseVar = 0.0000001; +static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; + +PreintegratedImuMeasurements ScenarioRunner::integrate( + double T, const Bias& estimatedBias, bool corrupted) const { + PreintegratedImuMeasurements pim(p_, estimatedBias); + + const double dt = imuSampleTime(); + const size_t nrSteps = T / dt; + double t = 0; + for (size_t k = 0; k < nrSteps; k++, t += dt) { + Vector3 measuredOmega = + corrupted ? measuredAngularVelocity(t) : actualAngularVelocity(t); + Vector3 measuredAcc = + corrupted ? measuredSpecificForce(t) : actualSpecificForce(t); + pim.integrateMeasurement(measuredAcc, measuredOmega, dt); + } + + return pim; +} + +NavState ScenarioRunner::predict(const PreintegratedImuMeasurements& pim, + const Bias& estimatedBias) const { + const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0)); + return pim.predict(state_i, estimatedBias); +} + +Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N, + const Bias& estimatedBias) const { + gttic_(estimateCovariance); + + // Get predict prediction from ground truth measurements + NavState prediction = predict(integrate(T)); + + // Sample ! + Matrix samples(9, N); + Vector9 sum = Vector9::Zero(); + for (size_t i = 0; i < N; i++) { + auto pim = integrate(T, estimatedBias, true); + NavState sampled = predict(pim); + Vector9 xi = sampled.localCoordinates(prediction); + samples.col(i) = xi; + sum += xi; + } + + // Compute MC covariance + Vector9 sampleMean = sum / N; + Matrix9 Q; + Q.setZero(); + for (size_t i = 0; i < N; i++) { + Vector9 xi = samples.col(i) - sampleMean; + Q += xi * xi.transpose(); + } + + return Q / (N - 1); +} + +Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const { + Matrix samples(6, N); + Vector6 sum = Vector6::Zero(); + for (size_t i = 0; i < N; i++) { + samples.col(i) << accSampler_.sample() / sqrt_dt_, + gyroSampler_.sample() / sqrt_dt_; + sum += samples.col(i); + } + + // Compute MC covariance + Vector6 sampleMean = sum / N; + Matrix6 Q; + Q.setZero(); + for (size_t i = 0; i < N; i++) { + Vector6 xi = samples.col(i) - sampleMean; + Q += xi * xi.transpose(); + } + + return Q / (N - 1); +} + +} // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h new file mode 100644 index 000000000..b038a831b --- /dev/null +++ b/gtsam/navigation/ScenarioRunner.h @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ScenarioRunner.h + * @brief Simple class to test navigation scenarios + * @author Frank Dellaert + */ + +#pragma once +#include +#include +#include + +namespace gtsam { + +// Convert covariance to diagonal noise model, if possible, otherwise throw +static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) { + bool smart = true; + auto model = noiseModel::Gaussian::Covariance(covariance, smart); + auto diagonal = boost::dynamic_pointer_cast(model); + if (!diagonal) + throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal"); + return diagonal; +} + +/* + * Simple class to test navigation scenarios. + * Takes a trajectory scenario as input, and can generate IMU measurements + */ +class ScenarioRunner { + public: + typedef imuBias::ConstantBias Bias; + typedef boost::shared_ptr SharedParams; + + private: + const Scenario* scenario_; + const SharedParams p_; + const double imuSampleTime_, sqrt_dt_; + const Bias estimatedBias_; + + // Create two samplers for acceleration and omega noise + mutable Sampler gyroSampler_, accSampler_; + + public: + ScenarioRunner(const Scenario* scenario, const SharedParams& p, + double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias()) + : scenario_(scenario), + p_(p), + imuSampleTime_(imuSampleTime), + sqrt_dt_(std::sqrt(imuSampleTime)), + estimatedBias_(bias), + // NOTE(duy): random seeds that work well: + gyroSampler_(Diagonal(p->gyroscopeCovariance), 10), + accSampler_(Diagonal(p->accelerometerCovariance), 29284) {} + + // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) + // also, uses g=10 for easy debugging + const Vector3& gravity_n() const { return p_->n_gravity; } + + // A gyro simply measures angular velocity in body frame + Vector3 actualAngularVelocity(double t) const { + return scenario_->omega_b(t); + } + + // An accelerometer measures acceleration in body, but not gravity + Vector3 actualSpecificForce(double t) const { + Rot3 bRn = scenario_->rotation(t).transpose(); + return scenario_->acceleration_b(t) - bRn * gravity_n(); + } + + // versions corrupted by bias and noise + Vector3 measuredAngularVelocity(double t) const { + return actualAngularVelocity(t) + estimatedBias_.gyroscope() + + gyroSampler_.sample() / sqrt_dt_; + } + Vector3 measuredSpecificForce(double t) const { + return actualSpecificForce(t) + estimatedBias_.accelerometer() + + accSampler_.sample() / sqrt_dt_; + } + + const double& imuSampleTime() const { return imuSampleTime_; } + + /// Integrate measurements for T seconds into a PIM + PreintegratedImuMeasurements integrate(double T, + const Bias& estimatedBias = Bias(), + bool corrupted = false) const; + + /// Predict predict given a PIM + NavState predict(const PreintegratedImuMeasurements& pim, + const Bias& estimatedBias = Bias()) const; + + /// Compute a Monte Carlo estimate of the predict covariance using N samples + Matrix9 estimateCovariance(double T, size_t N = 1000, + const Bias& estimatedBias = Bias()) const; + + /// Estimate covariance of sampled noise for sanity-check + Matrix6 estimateNoiseCovariance(size_t N = 1000) const; +}; + +} // namespace gtsam diff --git a/gtsam/navigation/tests/imuFactorTesting.h b/gtsam/navigation/tests/imuFactorTesting.h new file mode 100644 index 000000000..3247b56ee --- /dev/null +++ b/gtsam/navigation/tests/imuFactorTesting.h @@ -0,0 +1,76 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file imuFactorTesting.h + * @brief Common testing infrastructure + * @author Frank Dellaert + */ + +#pragma once + +#include +#include + +using namespace std; +using namespace gtsam; + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::V; +using symbol_shorthand::B; + +typedef imuBias::ConstantBias Bias; +static const Vector3 Z_3x1 = Vector3::Zero(); +static const Bias kZeroBiasHat, kZeroBias; + +static const Vector3 kZeroOmegaCoriolis(0, 0, 0); +static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1); + +static const double kGravity = 10; +static const Vector3 kGravityAlongNavZDown(0, 0, kGravity); + +// Realistic MEMS white noise characteristics. Angular and velocity random walk +// expressed in degrees respectively m/s per sqrt(hr). +auto radians = [](double t) { return t * M_PI / 180; }; +static const double kGyroSigma = radians(0.5) / 60; // 0.5 degree ARW +static const double kAccelSigma = 0.1 / 60; // 10 cm VRW + +namespace testing { + +struct ImuMeasurement { + ImuMeasurement(const Vector3& acc, const Vector3& gyro, double dt) + : acc(acc), gyro(gyro), dt(dt) {} + const Vector3 acc, gyro; + const double dt; +}; + +template +void integrateMeasurements(const vector& measurements, + PIM* pim) { + for (const auto& m : measurements) + pim->integrateMeasurement(m.acc, m.gyro, m.dt); +} + +struct SomeMeasurements : vector { + SomeMeasurements() { + reserve(102); + const double dt = 0.01, pi100 = M_PI / 100; + emplace_back(Vector3(0.1, 0, 0), Vector3(pi100, 0, 0), dt); + emplace_back(Vector3(0.1, 0, 0), Vector3(pi100, 0, 0), dt); + for (int i = 1; i < 100; i++) { + emplace_back(Vector3(0.05, 0.09, 0.01), + Vector3(pi100, pi100 * 3, 2 * pi100), dt); + } + } +}; + +} // namespace testing diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 2121eda35..02911acb1 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -423,7 +423,7 @@ TEST (AHRSFactor, predictTest) { // Predict Rot3 x; - Rot3 expectedRot = Rot3().ypr(20*M_PI, 0, 0); + Rot3 expectedRot = Rot3::Ypr(20*M_PI, 0, 0); Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis); EXPECT(assert_equal(expectedRot, actualRot, 1e-6)); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index d473207da..743fc9baf 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -13,8 +13,9 @@ * @file testCombinedImuFactor.cpp * @brief Unit test for Lupton-style combined IMU factor * @author Luca Carlone - * @author Stephen Williams + * @author Frank Dellaert * @author Richard Roberts + * @author Stephen Williams */ #include @@ -31,62 +32,23 @@ #include #include -using namespace std; -using namespace gtsam; +#include "imuFactorTesting.h" -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::V; -using symbol_shorthand::B; - -namespace { - -// Auxiliary functions to test preintegrated Jacobians -// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ -/* ************************************************************************* */ -CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, I_3x3, - I_3x3, I_3x3, I_3x3, I_3x3, I_6x6); - - list::const_iterator itAcc = measuredAccs.begin(); - list::const_iterator itOmega = measuredOmegas.begin(); - list::const_iterator itDeltaT = deltaTs.begin(); - for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { - result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); - } - return result; +namespace testing { +// Create default parameters with Z-down and above noise parameters +static boost::shared_ptr Params() { + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0001 * I_3x3; + return p; } - -Vector3 evaluatePreintegratedMeasurementsPosition( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaPij(); -} - -Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaVij(); -} - -Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return Rot3( - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaRij()); -} - } /* ************************************************************************* */ TEST( CombinedImuFactor, PreintegratedMeasurements ) { // Linearization point - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases + Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases // Measurements Vector3 measuredAcc(0.1, 0.0, 0.0); @@ -94,12 +56,13 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) { double deltaT = 0.5; double tol = 1e-6; + auto p = testing::Params(); + // Actual preintegrated values - ImuFactor::PreintegratedMeasurements expected1(bias, Z_3x3, Z_3x3, Z_3x3); + PreintegratedImuMeasurements expected1(p, bias); expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6); + PreintegratedCombinedMeasurements actual1(p, bias); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -111,46 +74,41 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) { /* ************************************************************************* */ TEST( CombinedImuFactor, ErrorWithBiases ) { - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) + Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) + Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); Vector3 v1(0.5, 0.0, 0.0); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0)); Vector3 v2(0.5, 0.0, 0.0); + auto p = testing::Params(); + p->omegaCoriolis = Vector3(0,0.1,0.1); + PreintegratedImuMeasurements pim( + p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); + // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0.1, 0.1; Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0 + 0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() - + Vector3(0.2, 0.0, 0.0); + Vector3 measuredAcc = + x1.rotation().unrotate(-p->n_gravity) + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; double tol = 1e-6; - ImuFactor::PreintegratedMeasurements pim( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - I_3x3, I_3x3, I_3x3); - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor::CombinedPreintegratedMeasurements combined_pim( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - I_3x3, I_3x3, I_3x3, I_3x3, 2 * I_3x3, I_6x6); + PreintegratedCombinedMeasurements combined_pim(p, + Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim, gravity, - omegaCoriolis); + ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim); noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov()); CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), - combined_pim, gravity, omegaCoriolis); + combined_pim); Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias); Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias, @@ -174,130 +132,75 @@ TEST( CombinedImuFactor, ErrorWithBiases ) { } /* ************************************************************************* */ -TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { - // Linearization point - imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases +TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { + auto p = testing::Params(); + testing::SomeMeasurements measurements; - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); + boost::function preintegrated = + [=](const Vector3& a, const Vector3& w) { + PreintegratedImuMeasurements pim(p, Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.preintegrated(); + }; - // Measurements - list measuredAccs, measuredOmegas; - list deltaTs; - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - for (int i = 1; i < 100; i++) { - measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back( - Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); - deltaTs.push_back(0.01); - } + // Actual pre-integrated values + PreintegratedCombinedMeasurements pim(p); + testing::integrateMeasurements(measurements, &pim); - // Actual preintegrated values - CombinedImuFactor::CombinedPreintegratedMeasurements pim = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs); - - // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, - measuredOmegas, deltaTs), bias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); - Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, - measuredOmegas, deltaTs), bias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); - Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - - Matrix expectedDelRdelBias = - numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, - measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); - Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); - - // Compare Jacobians - EXPECT(assert_equal(expectedDelPdelBiasAcc, pim.delPdelBiasAcc())); - EXPECT(assert_equal(expectedDelPdelBiasOmega, pim.delPdelBiasOmega())); - EXPECT(assert_equal(expectedDelVdelBiasAcc, pim.delVdelBiasAcc())); - EXPECT(assert_equal(expectedDelVdelBiasOmega, pim.delVdelBiasOmega())); - EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, pim.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1), + pim.preintegrated_H_biasAcc())); + EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), + pim.preintegrated_H_biasOmega(), 1e-3)); } /* ************************************************************************* */ TEST(CombinedImuFactor, PredictPositionAndVelocity) { - imuBias::ConstantBias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) + const Bias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) + + auto p = testing::Params(); // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; - measuredOmega << 0, 0.1, 0; //M_PI/10.0+0.3; - Vector3 measuredAcc; - measuredAcc << 0, 1.1, -9.81; - double deltaT = 0.001; + const Vector3 measuredOmega(0, 0.1, 0); // M_PI/10.0+0.3; + const Vector3 measuredAcc(0, 1.1, -kGravity); + const double deltaT = 0.001; - CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3, - I_3x3, I_3x3, 2 * I_3x3, I_6x6); + PreintegratedCombinedMeasurements pim(p, bias); for (int i = 0; i < 1000; ++i) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - noiseModel::Gaussian::shared_ptr combinedmodel = + const noiseModel::Gaussian::shared_ptr combinedmodel = noiseModel::Gaussian::Covariance(pim.preintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim, - gravity, omegaCoriolis); + const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = pim.predict(x1, v1, bias, gravity, - omegaCoriolis); - Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - Vector3 expectedVelocity; - expectedVelocity << 0, 1, 0; - EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); - EXPECT( - assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); + const NavState actual = pim.predict(NavState(), bias); + const Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); + const Vector3 expectedVelocity(0, 1, 0); + EXPECT(assert_equal(expectedPose, actual.pose())); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity()))); } /* ************************************************************************* */ TEST(CombinedImuFactor, PredictRotation) { - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) - CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3, - I_3x3, I_3x3, 2 * I_3x3, I_6x6); - Vector3 measuredAcc; - measuredAcc << 0, 0, -9.81; - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10.0; - double deltaT = 0.001; - double tol = 1e-4; + const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) + auto p = testing::Params(); + PreintegratedCombinedMeasurements pim(p, bias); + const Vector3 measuredAcc = - kGravityAlongNavZDown; + const Vector3 measuredOmega(0, 0, M_PI / 10.0); + const double deltaT = 0.001; + const double tol = 1e-4; for (int i = 0; i < 1000; ++i) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim, - gravity, omegaCoriolis); + const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); // Predict - Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2; - Vector3 v(0, 0, 0), v2; - CombinedImuFactor::Predict(x, v, x2, v2, bias, pim, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); - EXPECT(assert_equal(expectedPose, x2, tol)); + const Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2; + const Vector3 v(0, 0, 0), v2; + const NavState actual = pim.predict(NavState(x, v), bias); + const Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expectedPose, actual.pose(), tol)); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index 8c93020c9..6149c1651 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -89,7 +89,7 @@ TEST(GPSData, init) { // Check values values EXPECT(assert_equal((Vector )Vector3(29.9575, -29.0564, -1.95993), nV, 1e-4)); - EXPECT( assert_equal(Rot3::ypr(-0.770131, 0.046928, 0), T.rotation(), 1e-5)); + EXPECT( assert_equal(Rot3::Ypr(-0.770131, 0.046928, 0), T.rotation(), 1e-5)); Point3 expectedT(2.38461, -2.31289, -0.156011); EXPECT(assert_equal(expectedT, T.translation(), 1e-5)); } diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index 4d5df3f05..596b76f6a 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -16,113 +16,135 @@ */ #include +#include + #include +#include using namespace std; using namespace gtsam; +typedef imuBias::ConstantBias Bias; + Vector biasAcc1(Vector3(0.2, -0.1, 0)); Vector biasGyro1(Vector3(0.1, -0.3, -0.2)); -imuBias::ConstantBias bias1(biasAcc1, biasGyro1); +Bias bias1(biasAcc1, biasGyro1); Vector biasAcc2(Vector3(0.1, 0.2, 0.04)); Vector biasGyro2(Vector3(-0.002, 0.005, 0.03)); -imuBias::ConstantBias bias2(biasAcc2, biasGyro2); +Bias bias2(biasAcc2, biasGyro2); /* ************************************************************************* */ -TEST( ImuBias, Constructor) { +TEST(ImuBias, Constructor) { // Default Constructor - imuBias::ConstantBias bias1; + Bias bias1; // Acc + Gyro Constructor - imuBias::ConstantBias bias2(biasAcc2, biasGyro2); + Bias bias2(biasAcc2, biasGyro2); // Copy Constructor - imuBias::ConstantBias bias3(bias2); + Bias bias3(bias2); } /* ************************************************************************* */ -TEST( ImuBias, inverse) { - imuBias::ConstantBias biasActual = bias1.inverse(); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1, - -biasGyro1); +TEST(ImuBias, inverse) { + Bias biasActual = bias1.inverse(); + Bias biasExpected = Bias(-biasAcc1, -biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, compose) { - imuBias::ConstantBias biasActual = bias2.compose(bias1); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias( - biasAcc1 + biasAcc2, biasGyro1 + biasGyro2); +TEST(ImuBias, compose) { + Bias biasActual = bias2.compose(bias1); + Bias biasExpected = Bias(biasAcc1 + biasAcc2, biasGyro1 + biasGyro2); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, between) { +TEST(ImuBias, between) { // p.between(q) == q - p - imuBias::ConstantBias biasActual = bias2.between(bias1); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias( - biasAcc1 - biasAcc2, biasGyro1 - biasGyro2); + Bias biasActual = bias2.between(bias1); + Bias biasExpected = Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, localCoordinates) { +TEST(ImuBias, localCoordinates) { Vector deltaActual = Vector(bias2.localCoordinates(bias1)); - Vector deltaExpected = (imuBias::ConstantBias(biasAcc1 - biasAcc2, - biasGyro1 - biasGyro2)).vector(); + Vector deltaExpected = + (Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2)).vector(); EXPECT(assert_equal(deltaExpected, deltaActual)); } /* ************************************************************************* */ -TEST( ImuBias, retract) { +TEST(ImuBias, retract) { Vector6 delta; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; - imuBias::ConstantBias biasActual = bias2.retract(delta); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias( - biasAcc2 + delta.block<3, 1>(0, 0), biasGyro2 + delta.block<3, 1>(3, 0)); + Bias biasActual = bias2.retract(delta); + Bias biasExpected = Bias(biasAcc2 + delta.block<3, 1>(0, 0), + biasGyro2 + delta.block<3, 1>(3, 0)); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, Logmap) { +TEST(ImuBias, Logmap) { Vector deltaActual = bias2.Logmap(bias1); Vector deltaExpected = bias1.vector(); EXPECT(assert_equal(deltaExpected, deltaActual)); } /* ************************************************************************* */ -TEST( ImuBias, Expmap) { +TEST(ImuBias, Expmap) { Vector6 delta; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; - imuBias::ConstantBias biasActual = bias2.Expmap(delta); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta); + Bias biasActual = bias2.Expmap(delta); + Bias biasExpected = Bias(delta); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, operatorSub) { - imuBias::ConstantBias biasActual = -bias1; - imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1); +TEST(ImuBias, operatorSub) { + Bias biasActual = -bias1; + Bias biasExpected(-biasAcc1, -biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, operatorAdd) { - imuBias::ConstantBias biasActual = bias2 + bias1; - imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1, - biasGyro2 + biasGyro1); +TEST(ImuBias, operatorAdd) { + Bias biasActual = bias2 + bias1; + Bias biasExpected(biasAcc2 + biasAcc1, biasGyro2 + biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, operatorSubB) { - imuBias::ConstantBias biasActual = bias2 - bias1; - imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1, - biasGyro2 - biasGyro1); +TEST(ImuBias, operatorSubB) { + Bias biasActual = bias2 - bias1; + Bias biasExpected(biasAcc2 - biasAcc1, biasGyro2 - biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } +/* ************************************************************************* */ +TEST(ImuBias, Correct1) { + Matrix aH1, aH2; + const Vector3 measurement(1, 2, 3); + boost::function f = boost::bind( + &Bias::correctAccelerometer, _1, _2, boost::none, boost::none); + bias1.correctAccelerometer(measurement, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); +} + +/* ************************************************************************* */ +TEST(ImuBias, Correct2) { + Matrix aH1, aH2; + const Vector3 measurement(1, 2, 3); + boost::function f = + boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none); + bias1.correctGyroscope(measurement, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 084213e20..f1d761cb0 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -12,36 +12,37 @@ /** * @file testImuFactor.cpp * @brief Unit test for ImuFactor - * @author Luca Carlone, Stephen Williams, Richard Roberts, Frank Dellaert + * @author Luca Carlone + * @author Frank Dellaert + * @author Richard Roberts + * @author Stephen Williams */ #include -#include +#include #include #include #include #include -#include #include #include #include #include #include -#include -using namespace std; -using namespace gtsam; +#include "imuFactorTesting.h" -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::V; -using symbol_shorthand::B; - -static const Vector3 kGravityAlongNavZDown(0, 0, 9.81); -static const Vector3 kZeroOmegaCoriolis(0, 0, 0); -static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1); -static const imuBias::ConstantBias kZeroBiasHat, kZeroBias; +namespace testing { +// Create default parameters with Z-down and above noise parameters +static boost::shared_ptr Params() { + auto p = PreintegrationParams::MakeSharedD(kGravity); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0001 * I_3x3; + return p; +} +} /* ************************************************************************* */ namespace { @@ -49,185 +50,34 @@ namespace { /* ************************************************************************* */ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias) { + const Bias& bias) { return Rot3::Expmap( factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3)); } -// Define covariance matrices -/* ************************************************************************* */ -double accNoiseVar = 0.01; -double omegaNoiseVar = 0.03; -double intNoiseVar = 0.0001; -const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; -const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3; -const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; -const Vector3 accNoiseVar2(0.01, 0.02, 0.03); -const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02); -int32_t accSamplerSeed = 29284, omegaSamplerSeed = 10; - -// Auxiliary functions to test preintegrated Jacobians -// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ -/* ************************************************************************* */ -PreintegratedImuMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - PreintegratedImuMeasurements result(bias, kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance); - - list::const_iterator itAcc = measuredAccs.begin(); - list::const_iterator itOmega = measuredOmegas.begin(); - list::const_iterator itDeltaT = deltaTs.begin(); - for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { - result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); - } - return result; -} - -Vector3 evaluatePreintegratedMeasurementsPosition( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaPij(); -} - -Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaVij(); -} - -Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return Rot3( - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaRij()); -} - -Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, - const double deltaT) { - return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); -} - -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { - return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); -} - } // namespace /* ************************************************************************* */ -bool MonteCarlo(const PreintegratedImuMeasurements& pim, - const NavState& state, const imuBias::ConstantBias& bias, double dt, - boost::optional body_P_sensor, const Vector3& measuredAcc, - const Vector3& measuredOmega, const Vector3& accNoiseVar, - const Vector3& omegaNoiseVar, size_t N = 10000, - size_t M = 1) { - // Get mean prediction from "ground truth" measurements - PreintegratedImuMeasurements pim1 = pim; - for (size_t k = 0; k < M; k++) - pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor); - NavState prediction = pim1.predict(state, bias); - Matrix9 actual = pim1.preintMeasCov(); - - // Do a Monte Carlo analysis to determine empirical density on the predicted state - Sampler sampleAccelerationNoise((accNoiseVar/dt).cwiseSqrt(), accSamplerSeed); - Sampler sampleOmegaNoise((omegaNoiseVar/dt).cwiseSqrt(), omegaSamplerSeed); - Matrix samples(9, N); - Vector9 sum = Vector9::Zero(); - for (size_t i = 0; i < N; i++) { - PreintegratedImuMeasurements pim2 = pim; - for (size_t k = 0; k < M; k++) { - Vector3 perturbedAcc = measuredAcc + sampleAccelerationNoise.sample(); - Vector3 perturbedOmega = measuredOmega + sampleOmegaNoise.sample(); - pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, - body_P_sensor); - } - NavState sampled = pim2.predict(state, bias); - Vector9 xi = sampled.localCoordinates(prediction); - samples.col(i) = xi; - sum += xi; - } - - Vector9 sampleMean = sum / N; - Matrix9 Q; - Q.setZero(); - for (size_t i = 0; i < N; i++) { - Vector9 xi = samples.col(i); - xi -= sampleMean; - Q += xi * (xi.transpose() / (N - 1)); - } - - // Compare MonteCarlo value with actual (computed) value - return assert_equal(Matrix(Q), actual, 1e-4); -} - -#if 1 - -/* ************************************************************************* */ -TEST(ImuFactor, StraightLine) { - // Set up IMU measurements - static const double g = 10; // make gravity 10 to make this easy to check - static const double v = 50.0, a = 0.2, dt = 3.0; - const double dt22 = dt * dt / 2; +TEST(ImuFactor, Accelerating) { + const double a = 0.2, v = 50; // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X // The body itself has Z axis pointing down - static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); - static const Point3 initial_position(10, 20, 0); - static const Vector3 initial_velocity(v, 0, 0); - static const NavState state1(nRb, initial_position, initial_velocity); + const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + const Point3 initial_position(10, 20, 0); + const Vector3 initial_velocity(v, 0, 0); - // set up acceleration in X direction, no angular velocity. - // Since body Z-axis is pointing down, accelerometer measures table exerting force in negative Z - Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0); + const AcceleratingScenario scenario(nRb, initial_position, initial_velocity, + Vector3(a, 0, 0)); - // Create parameters assuming nav frame has Z up - boost::shared_ptr p = - PreintegratedImuMeasurements::Params::MakeSharedU(g); - p->accelerometerCovariance = kMeasuredAccCovariance; - p->gyroscopeCovariance = kMeasuredOmegaCovariance; + const double T = 3.0; // seconds + ScenarioRunner runner(&scenario, testing::Params(), T / 10); - // Check G1 and G2 derivatives of pim.update + PreintegratedImuMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - // Now, preintegrate for 3 seconds, in 10 steps - PreintegratedImuMeasurements pim(p, kZeroBiasHat); - for (size_t i = 0; i < 10; i++) - pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10); - - Matrix93 aG1,aG2; - boost::function f = - boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, dt/10, - boost::none, boost::none, boost::none); - pim.updatedDeltaXij(measuredAcc, measuredOmega, dt / 10, boost::none, aG1, aG2); - EXPECT(assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7)); - EXPECT(assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7)); - - // Do Monte-Carlo analysis - PreintegratedImuMeasurements pimMC(kZeroBiasHat, p->accelerometerCovariance, - p->gyroscopeCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise - EXPECT( - MonteCarlo(pimMC, state1, kZeroBias, dt / 10, boost::none, measuredAcc, - measuredOmega, Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000)); - - // Check integrated quantities in body frame: gravity measured by IMU is integrated! - Vector3 b_deltaP(a * dt22, 0, -g * dt22); - Vector3 b_deltaV(a * dt, 0, -g * dt); - EXPECT(assert_equal(Rot3(), pim.deltaRij())); - EXPECT(assert_equal(b_deltaP, pim.deltaPij())); - EXPECT(assert_equal(b_deltaV, pim.deltaVij())); - - // Check bias-corrected delta: also in body frame - Vector9 expectedBC; - expectedBC << Vector3(0, 0, 0), b_deltaP, b_deltaV; - EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(kZeroBias))); - - // Check prediction in nav, note we move along x in body, along y in nav - Point3 expected_position(10 + v * dt, 20 + a * dt22, 0); - Velocity3 expected_velocity(v, a * dt, 0); - NavState expected(nRb, expected_position, expected_velocity); - EXPECT(assert_equal(expected, pim.predict(state1, kZeroBias))); + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); } /* ************************************************************************* */ @@ -237,41 +87,53 @@ TEST(ImuFactor, PreintegratedMeasurements) { Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); double deltaT = 0.5; - // Expected preintegrated values - Vector3 expectedDeltaP1; - expectedDeltaP1 << 0.5 * 0.1 * 0.5 * 0.5, 0, 0; + // Expected pre-integrated values + Vector3 expectedDeltaR1(0.5 * M_PI / 100.0, 0.0, 0.0); + Vector3 expectedDeltaP1(0.5 * 0.1 * 0.5 * 0.5, 0, 0); Vector3 expectedDeltaV1(0.05, 0.0, 0.0); - Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0); - double expectedDeltaT1(0.5); - // Actual preintegrated values - PreintegratedImuMeasurements actual1(kZeroBiasHat, kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance); - actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + // 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())); + DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9); - EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()))); - EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()))); - EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()))); - DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-9); + actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + EXPECT(assert_equal(expectedDeltaR1, actual.theta())); + EXPECT(assert_equal(expectedDeltaP1, actual.deltaPij())); + EXPECT(assert_equal(expectedDeltaV1, actual.deltaVij())); + DOUBLES_EQUAL(0.5, actual.deltaTij(), 1e-9); + + // Check derivatives of computeError + Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + NavState x1, x2 = actual.predict(x1, bias); + + { + Matrix9 aH1, aH2; + Matrix96 aH3; + actual.computeError(x1, x2, bias, aH1, aH2, aH3); + boost::function f = + boost::bind(&PreintegrationBase::computeError, actual, _1, _2, _3, + boost::none, boost::none, boost::none); + EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); + } // Integrate again - Vector3 expectedDeltaP2; - expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0; - Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) - + expectedDeltaR1.matrix() * measuredAcc * 0.5; - Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0); - double expectedDeltaT2(1); + Vector3 expectedDeltaR2(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0); + Vector3 expectedDeltaP2(0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0); + Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + + Rot3::Expmap(expectedDeltaR1) * measuredAcc * 0.5; - // Actual preintegrated values - PreintegratedImuMeasurements actual2 = actual1; - actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - - EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()))); - EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()))); - EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()))); - DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-9); + // Actual pre-integrated values + actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + EXPECT(assert_equal(expectedDeltaR2, actual.theta())); + EXPECT(assert_equal(expectedDeltaP2, actual.deltaPij())); + EXPECT(assert_equal(expectedDeltaV2, actual.deltaVij())); + DOUBLES_EQUAL(1.0, actual.deltaTij(), 1e-9); } - /* ************************************************************************* */ // Common linearization point and measurements for tests namespace common { @@ -296,12 +158,8 @@ static const NavState state2(x2, v2); /* ************************************************************************* */ TEST(ImuFactor, PreintegrationBaseMethods) { using namespace common; - boost::shared_ptr p = - PreintegratedImuMeasurements::Params::MakeSharedD(); - p->gyroscopeCovariance = kMeasuredOmegaCovariance; + auto p = testing::Params(); p->omegaCoriolis = Vector3(0.02, 0.03, 0.04); - p->accelerometerCovariance = kMeasuredAccCovariance; - p->integrationCovariance = kIntegrationErrorCovariance; p->use2ndOrderCoriolis = true; PreintegratedImuMeasurements pim(p, kZeroBiasHat); @@ -311,7 +169,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) { // biasCorrectedDelta Matrix96 actualH; pim.biasCorrectedDelta(kZeroBias, actualH); - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, boost::none), kZeroBias); EXPECT(assert_equal(expectedH, actualH)); @@ -323,7 +181,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) { boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none, boost::none), state1); EXPECT(assert_equal(eH1, aH1)); - Matrix eH2 = numericalDerivative11( + Matrix eH2 = numericalDerivative11( boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, boost::none), kZeroBias); EXPECT(assert_equal(eH2, aH2)); @@ -334,21 +192,20 @@ TEST(ImuFactor, PreintegrationBaseMethods) { /* ************************************************************************* */ TEST(ImuFactor, ErrorAndJacobians) { using namespace common; - PreintegratedImuMeasurements pim(kZeroBiasHat, kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance); + PreintegratedImuMeasurements pim(testing::Params()); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT(assert_equal(state2, pim.predict(state1, kZeroBias))); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, - kZeroOmegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); // Expected error Vector expectedError(9); expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0; EXPECT( - assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, kZeroBias))); + assert_equal(expectedError, + factor.evaluateError(x1, v1, x2, v2, kZeroBias))); Values values; values.insert(X(1), x1); @@ -364,16 +221,19 @@ TEST(ImuFactor, ErrorAndJacobians) { // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a, H5a); + (void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a, + H5a); // Make sure rotation part is correct when error is interpreted as axis-angle // Jacobians are around zero, so the rotation part is the same as: Matrix H1Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias), x1); + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias), + x1); EXPECT(assert_equal(H1Rot3, H1a.topRows(3))); Matrix H3Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), x2); + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), + x2); EXPECT(assert_equal(H3Rot3, H3a.topRows(3))); // Evaluate error with wrong values @@ -389,7 +249,7 @@ TEST(ImuFactor, ErrorAndJacobians) { Matrix cov = pim.preintMeasCov(); Matrix R = RtR(cov.inverse()); Vector whitened = R * expectedError; - EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-5)); + EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-4)); // Make sure linearization is correct EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); @@ -400,7 +260,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { using common::x1; using common::v1; using common::v2; - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0)); @@ -411,22 +271,23 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)); - PreintegratedImuMeasurements pim(biasHat, kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance); + auto p = testing::Params(); + p->omegaCoriolis = kNonZeroOmegaCoriolis; + + Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)); + PreintegratedImuMeasurements pim(p, biasHat); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Make sure of biasCorrectedDelta Matrix96 actualH; pim.biasCorrectedDelta(bias, actualH); - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, boost::none), bias); EXPECT(assert_equal(expectedH, actualH)); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, - kNonZeroOmegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); Values values; values.insert(X(1), x1); @@ -445,7 +306,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { using common::x1; using common::v1; using common::v2; - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0)); @@ -456,16 +317,16 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - PreintegratedImuMeasurements pim( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), - kMeasuredAccCovariance, kMeasuredOmegaCovariance, - kIntegrationErrorCovariance); + auto p = testing::Params(); + p->omegaCoriolis = kNonZeroOmegaCoriolis; + p->use2ndOrderCoriolis = true; + + PreintegratedImuMeasurements pim(p, + Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1))); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - bool use2ndOrderCoriolis = true; - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, - kNonZeroOmegaCoriolis, boost::none, use2ndOrderCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); Values values; values.insert(X(1), x1); @@ -488,13 +349,16 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) { Vector3 measuredOmega(0.1, 0, 0); double deltaT = 0.5; - // Compute numerical derivatives - Matrix expectedDelRdelBiasOmega = numericalDerivative11( - boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), - Vector3(biasOmega)); + auto evaluateRotation = [=](const Vector3 biasOmega) { + return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); + }; - const Matrix3 Jr = Rot3::ExpmapDerivative( - (measuredOmega - biasOmega) * deltaT); + // Compute numerical derivatives + Matrix expectedDelRdelBiasOmega = + numericalDerivative11(evaluateRotation, biasOmega); + + const Matrix3 Jr = + Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign @@ -510,9 +374,14 @@ TEST(ImuFactor, PartialDerivativeLogmap) { // Measurements Vector3 deltatheta(0, 0, 0); + auto evaluateLogRotation = [=](const Vector3 deltatheta) { + return Rot3::Logmap( + Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); + }; + // Compute numerical derivatives - Matrix expectedDelFdeltheta = numericalDerivative11( - boost::bind(&evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); + Matrix expectedDelFdeltheta = + numericalDerivative11(evaluateLogRotation, deltatheta); Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat); @@ -554,190 +423,111 @@ TEST(ImuFactor, fistOrderExponential) { /* ************************************************************************* */ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); + testing::SomeMeasurements measurements; - // Measurements - list measuredAccs, measuredOmegas; - list deltaTs; - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - for (int i = 1; i < 100; i++) { - measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back( - Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); - deltaTs.push_back(0.01); - } + boost::function preintegrated = + [=](const Vector3& a, const Vector3& w) { + PreintegratedImuMeasurements pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.preintegrated(); + }; - // Actual preintegrated values - PreintegratedImuMeasurements preintegrated = - evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas, - deltaTs); + // Actual pre-integrated values + PreintegratedImuMeasurements pim(testing::Params()); + testing::integrateMeasurements(measurements, &pim); - // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, - measuredOmegas, deltaTs), kZeroBias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); - Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, - measuredOmegas, deltaTs), kZeroBias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); - Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - - Matrix expectedDelRdelBias = - numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, - measuredAccs, measuredOmegas, deltaTs), kZeroBias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); - Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); - - // Compare Jacobians - EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); - EXPECT( - assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); - EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); - EXPECT( - assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); - EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); - EXPECT( - assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega())); + EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1), + pim.preintegrated_H_biasAcc())); + EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), + pim.preintegrated_H_biasOmega(), 1e-3)); } -#endif /* ************************************************************************* */ -Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& measuredAcc, const Vector3& measuredOmega) { - return pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega).first; +Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, + const Vector3& measuredAcc, const Vector3& measuredOmega) { + Vector3 correctedAcc = pim.biasHat().correctAccelerometer(measuredAcc); + Vector3 correctedOmega = pim.biasHat().correctGyroscope(measuredOmega); + return pim.correctMeasurementsBySensorPose(correctedAcc, correctedOmega).first; } TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); - Vector3 v1(Vector3(0.5, 0.0, 0.0)); + const Rot3 nRb = Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)); + const Point3 p1(5.0, 1.0, -50.0); + const Vector3 v1(0.5, 0.0, 0.0); + + const Vector3 a = nRb * Vector3(0.2, 0.0, 0.0); + const AcceleratingScenario scenario(nRb, p1, v1, a, + Vector3(0, 0, M_PI / 10.0 + 0.3)); + + auto p = testing::Params(); + p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(0, M_PI / 2, 0)), + Point3(0.1, 0.05, 0.01)); + p->omegaCoriolis = kNonZeroOmegaCoriolis; + + Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); + + const double T = 3.0; // seconds + ScenarioRunner runner(&scenario, p, T / 10); + + // PreintegratedImuMeasurements pim = runner.integrate(T); + // EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); + // + // Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); + // + /////////////////////////////////////////////////////////////////////////////////////////// + Pose3 x1(nRb, p1); + + // Measurements + Vector3 measuredOmega = runner.actualAngularVelocity(0); + Vector3 measuredAcc = runner.actualSpecificForce(0); + + // Get mean prediction from "ground truth" measurements + const Vector3 accNoiseVar2(0.01, 0.02, 0.03); + const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02); + PreintegratedImuMeasurements pim(p, biasHat); + + // Check updatedDeltaXij derivatives + Matrix3 D_correctedAcc_measuredOmega = Z_3x3; + pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega, + boost::none, D_correctedAcc_measuredOmega, boost::none); + Matrix3 expectedD = numericalDerivative11( + boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6); + EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5)); + + double dt = 0.1; + +// TODO(frank): revive derivative tests +// Matrix93 G1, G2; +// Vector9 preint = +// pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2); +// +// Matrix93 expectedG1 = numericalDerivative21( +// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, +// dt, boost::none, boost::none, boost::none), measuredAcc, +// measuredOmega, 1e-6); +// EXPECT(assert_equal(expectedG1, G1, 1e-5)); +// +// Matrix93 expectedG2 = numericalDerivative22( +// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, +// dt, boost::none, boost::none, boost::none), measuredAcc, +// measuredOmega, 1e-6); +// EXPECT(assert_equal(expectedG2, G2, 1e-5)); + + Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) + + // integrate at least twice to get position information + // otherwise factor cov noise from preint_cov is not positive definite + pim.integrateMeasurement(measuredAcc, measuredOmega, dt); + pim.integrateMeasurement(measuredAcc, measuredOmega, dt); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); - // Measurements - Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10.0 + 0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) - + Vector3(0.2, 0.0, 0.0); - double dt = 0.1; - - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, M_PI/2, 0)), Point3(0.1, 0.05, 0.01)); - imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); - - // Get mean prediction from "ground truth" measurements - PreintegratedImuMeasurements pim(biasHat, accNoiseVar2.asDiagonal(), - omegaNoiseVar2.asDiagonal(), Z_3x3, true); // MonteCarlo does not sample integration noise - pim.set_body_P_sensor(body_P_sensor); - - // Check updatedDeltaXij derivatives - Matrix3 D_correctedAcc_measuredOmega = Matrix3::Zero(); - pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, boost::none, D_correctedAcc_measuredOmega, boost::none); - Matrix3 expectedD = numericalDerivative11(boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6); - EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5)); - - Matrix93 G1, G2; - NavState preint = pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2); -// Matrix9 preintCov = G1*((accNoiseVar2/dt).asDiagonal())*G1.transpose() + G2*((omegaNoiseVar2/dt).asDiagonal())*G2.transpose(); - - Matrix93 expectedG1 = numericalDerivative21( - boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, - dt, boost::none, boost::none, boost::none), measuredAcc, measuredOmega, - 1e-6); - EXPECT(assert_equal(expectedG1, G1, 1e-5)); - - Matrix93 expectedG2 = numericalDerivative22( - boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, - dt, boost::none, boost::none, boost::none), measuredAcc, measuredOmega, - 1e-6); - EXPECT(assert_equal(expectedG2, G2, 1e-5)); - -#if 0 - /* - * This code is to verify the quality of the generated samples - * by checking if the covariance of the generated noises matches - * with the input covariance, and visualizing the nonlinearity of - * the sample set using the following matlab script: - * - noises = dlmread('noises.txt'); - cov(noises) - - samples = dlmread('noises.txt'); - figure(1); - for i=1:9 - subplot(3,3,i) - hist(samples(:,i), 500) - end - */ - size_t N = 10000; - Matrix samples(9,N); - Sampler sampleAccelerationNoise((accNoiseVar2/dt).cwiseSqrt(), 29284); - Sampler sampleOmegaNoise((omegaNoiseVar2/dt).cwiseSqrt(), 10); - ofstream samplesOut("preintSamples.txt"); - ofstream noiseOut("noises.txt"); - for (size_t i = 0; iprint("noise: "); // Make sure the noise is valid - // Make sure linearization is correct double diffDelta = 1e-8; EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); @@ -754,167 +542,143 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { /* ************************************************************************* */ TEST(ImuFactor, PredictPositionAndVelocity) { - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) + Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements Vector3 measuredOmega; measuredOmega << 0, 0, 0; // M_PI/10.0+0.3; Vector3 measuredAcc; - measuredAcc << 0, 1, -9.81; + measuredAcc << 0, 1, -kGravity; double deltaT = 0.001; - PreintegratedImuMeasurements pim( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - kMeasuredAccCovariance, kMeasuredOmegaCovariance, - kIntegrationErrorCovariance, true); + PreintegratedImuMeasurements pim(testing::Params(), + Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); for (int i = 0; i < 1000; ++i) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, - kZeroOmegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, - kGravityAlongNavZDown, kZeroOmegaCoriolis); - Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - Vector3 expectedVelocity; - expectedVelocity << 0, 1, 0; - EXPECT(assert_equal(expectedPose, poseVelocity.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); + NavState actual = pim.predict(NavState(x1, v1), bias); + NavState expected(Rot3(), Point3(0, 0.5, 0), Vector3(0, 1, 0)); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST(ImuFactor, PredictRotation) { - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) + Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10; // M_PI/10.0+0.3; Vector3 measuredAcc; - measuredAcc << 0, 0, -9.81; + measuredAcc << 0, 0, -kGravity; double deltaT = 0.001; - PreintegratedImuMeasurements pim( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - kMeasuredAccCovariance, kMeasuredOmegaCovariance, - kIntegrationErrorCovariance, true); + PreintegratedImuMeasurements pim(testing::Params(), + Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); for (int i = 0; i < 1000; ++i) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, - kZeroOmegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); // Predict - Pose3 x1, x2; - Vector3 v1 = Vector3(0, 0.0, 0.0); - Vector3 v2; - ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, - kZeroOmegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); - Vector3 expectedVelocity; - expectedVelocity << 0, 0, 0; - EXPECT(assert_equal(expectedPose, x2)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2))); + NavState actual = pim.predict(NavState(), bias); + NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(), Z_3x1); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST(ImuFactor, PredictArbitrary) { - imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); + Pose3 x1; + const Vector3 v1(0, 0, 0); + + const AcceleratingScenario scenario(x1.rotation(), x1.translation(), v1, + Vector3(0.1, 0.2, 0), Vector3(M_PI / 10, M_PI / 10, M_PI / 10)); + + const double T = 3.0; // seconds + ScenarioRunner runner(&scenario, testing::Params(), T / 10); + // + // PreintegratedImuMeasurements pim = runner.integrate(T); + // EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); + // + // Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); + ////////////////////////////////////////////////////////////////////////////////// + + Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); // Measurements - Vector3 measuredOmega(M_PI / 10, M_PI / 10, M_PI / 10); - Vector3 measuredAcc(0.1, 0.2, -9.81); + Vector3 measuredOmega = runner.actualAngularVelocity(0); + Vector3 measuredAcc = runner.actualSpecificForce(0); + + auto p = testing::Params(); + p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise + PreintegratedImuMeasurements pim(p, biasHat); + Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); +// EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega, +// Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000)); + double dt = 0.001; - - ImuFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance, - kMeasuredOmegaCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise - Pose3 x1; - Vector3 v1 = Vector3(0, 0, 0); - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); - EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, - measuredAcc, measuredOmega, Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar))); - for (int i = 0; i < 1000; ++i) pim.integrateMeasurement(measuredAcc, measuredOmega, dt); -// Matrix expected(9,9); -// expected << // -// 0.0299999995, 2.46739898e-10, 2.46739896e-10, -0.0144839494, 0.044978128, 0.0100471195, -0.0409843415, 0.134423822, 0.0383280513, // -// 2.46739898e-10, 0.0299999995, 2.46739902e-10, -0.0454268484, -0.0149428917, 0.00609093775, -0.13554868, -0.0471183681, 0.0247643646, // -// 2.46739896e-10, 2.46739902e-10, 0.0299999995, 0.00489577218, 0.00839301168, 0.000448720395, 0.00879031682, 0.0162199769, 0.00112485862, // -// -0.0144839494, -0.0454268484, 0.00489577218, 0.142448905, 0.00345595825, -0.0225794125, 0.34774305, 0.0119449979, -0.075667905, // -// 0.044978128, -0.0149428917, 0.00839301168, 0.00345595825, 0.143318431, 0.0200549262, 0.0112877167, 0.351503176, 0.0629164907, // -// 0.0100471195, 0.00609093775, 0.000448720395, -0.0225794125, 0.0200549262, 0.0104041905, -0.0580647212, 0.051116506, 0.0285371399, // -// -0.0409843415, -0.13554868, 0.00879031682, 0.34774305, 0.0112877167, -0.0580647212, 0.911721561, 0.0412249672, -0.205920425, // -// 0.134423822, -0.0471183681, 0.0162199769, 0.0119449979, 0.351503176, 0.051116506, 0.0412249672, 0.928013807, 0.169935105, // -// 0.0383280513, 0.0247643646, 0.00112485862, -0.075667905, 0.0629164907, 0.0285371399, -0.205920425, 0.169935105, 0.09407764; -// EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-7)); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, - kZeroOmegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); // Predict - Pose3 x2; - Vector3 v2; - ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, - kZeroOmegaCoriolis); + NavState actual = pim.predict(NavState(x1, v1), bias); // Regression test for Imu Refactor Rot3 expectedR( // +0.903715275, -0.250741668, 0.347026393, // +0.347026393, 0.903715275, -0.250741668, // -0.250741668, 0.347026393, 0.903715275); - Point3 expectedT(-0.505517319, 0.569413747, 0.0861035711); - Vector3 expectedV(-1.59121524, 1.55353139, 0.3376838540); - Pose3 expectedPose(expectedR, expectedT); - EXPECT(assert_equal(expectedPose, x2, 1e-7)); - EXPECT(assert_equal(Vector(expectedV), v2, 1e-7)); + Point3 expectedT(-0.516077031, 0.57842919, 0.0876478403); + Vector3 expectedV(-1.62337767, 1.57954409, 0.343833571); + NavState expected(expectedR, expectedT, expectedV); + EXPECT(assert_equal(expected, actual, 1e-7)); } /* ************************************************************************* */ TEST(ImuFactor, bodyPSensorNoBias) { - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) + Bias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) + + // Rotate sensor (z-down) to body (same as navigation) i.e. z-up + auto p = testing::Params(); + p->n_gravity = Vector3(0, 0, -kGravity); // z-up nav frame + p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0, 0, 0)); // Measurements - Vector3 n_gravity(0, 0, -9.81); // z-up nav frame - Vector3 omegaCoriolis(0, 0, 0); - // Sensor frame is z-down // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame Vector3 s_omegaMeas_ns(0, 0.1, M_PI / 10); // Acc measurement is acceleration of sensor in the sensor frame, when stationary, // table exerts an equal and opposite force w.r.t gravity - Vector3 s_accMeas(0, 0, -9.81); + Vector3 s_accMeas(0, 0, -kGravity); double dt = 0.001; - // Rotate sensor (z-down) to body (same as navigation) i.e. z-up - Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0)); - - ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true); + PreintegratedImuMeasurements pim(p, bias); for (int i = 0; i < 1000; ++i) - pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, body_P_sensor); + pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, n_gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); // Predict - Pose3 x1; - Vector3 v1(0, 0, 0); - PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity, - omegaCoriolis); + NavState actual = pim.predict(NavState(), bias); - Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0)); - EXPECT(assert_equal(expectedPose, poseVelocity.pose)); + Pose3 expectedPose(Rot3::Ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expectedPose, actual.pose())); Vector3 expectedVelocity(0, 0, 0); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity()))); } /* ************************************************************************* */ @@ -926,7 +690,7 @@ TEST(ImuFactor, bodyPSensorNoBias) { TEST(ImuFactor, bodyPSensorWithBias) { using noiseModel::Diagonal; - typedef imuBias::ConstantBias Bias; + typedef Bias Bias; int numFactors = 80; Vector6 noiseBetweenBiasSigma; @@ -935,21 +699,19 @@ TEST(ImuFactor, bodyPSensorWithBias) { SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma); // Measurements - Vector3 n_gravity(0, 0, -9.81); - Vector3 omegaCoriolis(0, 0, 0); - // Sensor frame is z-down // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame Vector3 measuredOmega(0, 0.01, 0); // Acc measurement is acceleration of sensor in the sensor frame, when stationary, // table exerts an equal and opposite force w.r.t gravity - Vector3 measuredAcc(0, 0, -9.81); + Vector3 measuredAcc(0, 0, -kGravity); - Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); - - Matrix3 accCov = 1e-7 * I_3x3; - Matrix3 gyroCov = 1e-8 * I_3x3; - Matrix3 integrationCov = 1e-9 * I_3x3; + auto p = testing::Params(); + p->n_gravity = Vector3(0, 0, -kGravity); + p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3()); + p->accelerometerCovariance = 1e-7 * I_3x3; + p->gyroscopeCovariance = 1e-8 * I_3x3; + p->integrationCovariance = 1e-9 * I_3x3; double deltaT = 0.005; // Specify noise values on priors @@ -986,17 +748,13 @@ TEST(ImuFactor, bodyPSensorWithBias) { // Now add IMU factors and bias noise models Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0)); for (int i = 1; i < numFactors; i++) { - ImuFactor::PreintegratedMeasurements pim = - ImuFactor::PreintegratedMeasurements(priorBias, accCov, gyroCov, - integrationCov, true); + PreintegratedImuMeasurements pim = PreintegratedImuMeasurements(p, + priorBias); for (int j = 0; j < 200; ++j) - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT, - body_P_sensor); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factors - graph.add( - ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim, n_gravity, - omegaCoriolis)); + graph.add(ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim)); graph.add(BetweenFactor(B(i - 1), B(i), zeroBias, biasNoiseModel)); values.insert(X(i), Pose3()); @@ -1013,6 +771,102 @@ TEST(ImuFactor, bodyPSensorWithBias) { EXPECT(assert_equal(biasExpected, biasActual, 1e-3)); } +/* ************************************************************************* */ +static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6; + +struct ImuFactorMergeTest { + boost::shared_ptr p_; + const ConstantTwistScenario forward_, loop_; + + ImuFactorMergeTest() + : p_(PreintegratedImuMeasurements::Params::MakeSharedU(kGravity)), + forward_(Z_3x1, Vector3(kVelocity, 0, 0)), + loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) { + // arbitrary noise values + p_->gyroscopeCovariance = I_3x3 * 0.01; + p_->accelerometerCovariance = I_3x3 * 0.02; + p_->accelerometerCovariance = I_3x3 * 0.03; + } + + int TestScenario(TestResult& result_, const std::string& name_, + const Scenario& scenario, + const Bias& bias01, + const Bias& bias12, double tol) { + // Test merge by creating a 01, 12, and 02 PreintegratedRotation, + // then checking the merge of 01-12 matches 02. + PreintegratedImuMeasurements pim01(p_, bias01); + PreintegratedImuMeasurements pim12(p_, bias12); + PreintegratedImuMeasurements pim02_expected(p_, bias01); + + double deltaT = 0.01; + ScenarioRunner runner(&scenario, p_, deltaT); + // TODO(frank) can this loop just go into runner ? + for (int i = 0; i < 100; i++) { + double t = i * deltaT; + // integrate the measurements appropriately + Vector3 accel_meas = runner.actualSpecificForce(t); + Vector3 omega_meas = runner.actualAngularVelocity(t); + pim02_expected.integrateMeasurement(accel_meas, omega_meas, deltaT); + if (i < 50) { + pim01.integrateMeasurement(accel_meas, omega_meas, deltaT); + } else { + pim12.integrateMeasurement(accel_meas, omega_meas, deltaT); + } + } + auto actual_pim02 = ImuFactor::Merge(pim01, pim12); + EXPECT(assert_equal(pim02_expected.preintegrated(), + actual_pim02.preintegrated(), tol)); + EXPECT(assert_equal(pim02_expected, actual_pim02, tol)); + + ImuFactor::shared_ptr factor01 = + boost::make_shared(X(0), V(0), X(1), V(1), B(0), pim01); + ImuFactor::shared_ptr factor12 = + boost::make_shared(X(1), V(1), X(2), V(2), B(0), pim12); + ImuFactor::shared_ptr factor02_expected = boost::make_shared( + X(0), V(0), X(2), V(2), B(0), pim02_expected); + + ImuFactor::shared_ptr factor02_merged = ImuFactor::Merge(factor01, factor12); + EXPECT(assert_equal(*factor02_expected, *factor02_merged, tol)); + return result_.getFailureCount(); + } + + void TestScenarios(TestResult& result_, const std::string& name_, + const Bias& bias01, + const Bias& bias12, double tol) { + for (auto scenario : {forward_, loop_}) + TestScenario(result_, name_, scenario, bias01, bias12, tol); + } +}; + +/* ************************************************************************* */ +// Test case with zero biases +TEST(ImuFactor, MergeZeroBias) { + ImuFactorMergeTest mergeTest; + mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4); +} + +// Test case with identical biases: we expect an exact answer. +TEST(ImuFactor, MergeConstantBias) { + ImuFactorMergeTest mergeTest; + Bias bias(Vector3(0.03, -0.02, 0.01), Vector3(-0.01, 0.02, -0.03)); + mergeTest.TestScenarios(result_, name_, bias, bias, 1e-4); +} + +// Test case with different biases where we expect there to be some variation. +TEST(ImuFactor, MergeChangingBias) { + ImuFactorMergeTest mergeTest; + mergeTest.TestScenarios(result_, name_, + Bias(Vector3(0.03, -0.02, 0.01), Vector3(-0.01, 0.02, -0.03)), + Bias(Vector3(0.01, 0.02, 0.03), Vector3(0.03, -0.02, 0.01)), 1e-1); +} + +// Test case with non-zero coriolis +TEST(ImuFactor, MergeWithCoriolis) { + ImuFactorMergeTest mergeTest; + mergeTest.p_->omegaCoriolis = Vector3(0.1, 0.2, -0.1); + mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp new file mode 100644 index 000000000..a7796d1b2 --- /dev/null +++ b/gtsam/navigation/tests/testImuFactorSerialization.cpp @@ -0,0 +1,77 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testImuFactor.cpp + * @brief Unit test for ImuFactor + * @author Luca Carlone + * @author Frank Dellaert + * @author Richard Roberts + * @author Stephen Williams + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, + "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, + "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, + "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, + "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST(ImuFactor, serialization) { + using namespace gtsam::serializationTestHelpers; + + // Create default parameters with Z-down and above noise paramaters + auto p = PreintegrationParams::MakeSharedD(9.81); + p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3()); + p->accelerometerCovariance = 1e-7 * I_3x3; + p->gyroscopeCovariance = 1e-8 * I_3x3; + p->integrationCovariance = 1e-9 * I_3x3; + + const double deltaT = 0.005; + const imuBias::ConstantBias priorBias( + Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) + + PreintegratedImuMeasurements pim(p, priorBias); + + // measurements are needed for non-inf noise model, otherwise will throw err + // when deserialize + const Vector3 measuredOmega(0, 0.01, 0); + const Vector3 measuredAcc(0, 0, -9.81); + + for (int j = 0; j < 200; ++j) + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + ImuFactor factor(1, 2, 3, 4, 5, pim); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 950c6ce63..38aecfcbc 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -40,7 +40,7 @@ Point3 nM(22653.29982, -1956.83010, 44202.47862); // Let's assume scale factor, double scale = 255.0 / 50000.0; // ...ground truth orientation, -Rot3 nRb = Rot3::yaw(-0.1); +Rot3 nRb = Rot3::Yaw(-0.1); Rot2 theta = nRb.yaw(); // ...and bias Point3 bias(10, -10, 50); diff --git a/gtsam/navigation/tests/testPoseVelocityBias.cpp b/gtsam/navigation/tests/testPoseVelocityBias.cpp index ce419fdd0..cc2a47498 100644 --- a/gtsam/navigation/tests/testPoseVelocityBias.cpp +++ b/gtsam/navigation/tests/testPoseVelocityBias.cpp @@ -25,6 +25,8 @@ using namespace std; using namespace gtsam; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + // Should be seen as between(pvb1,pvb2), i.e., written as pvb2 \omin pvb1 Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) { Matrix3 R1 = pvb1.pose.rotation().matrix(); @@ -55,6 +57,7 @@ TEST(PoseVelocityBias, error) { expected << 0.0, -0.5, 6.0, 4.0, 0.0, 3.0, 0.1, 0.2, 0.3; EXPECT(assert_equal(expected, actual, 1e-9)); } +#endif /* ************************************************************************************************/ int main() { diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testPreintegrationBase.cpp new file mode 100644 index 000000000..b733181ac --- /dev/null +++ b/gtsam/navigation/tests/testPreintegrationBase.cpp @@ -0,0 +1,150 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testPreintegrationBase.cpp + * @brief Unit test for the InertialNavFactor + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include "imuFactorTesting.h" + +static const double kDt = 0.1; + +Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { + return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta); +} + +namespace testing { +// Create default parameters with Z-down and above noise parameters +static boost::shared_ptr Params() { + auto p = PreintegrationParams::MakeSharedD(kGravity); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0001 * I_3x3; + return p; +} +} + +/* ************************************************************************* */ +TEST(PreintegrationBase, UpdateEstimate1) { + PreintegrationBase pim(testing::Params()); + const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); + Vector9 zeta; + zeta.setZero(); + Matrix9 aH1; + Matrix93 aH2, aH3; + pim.UpdatePreintegrated(acc, omega, kDt, zeta, aH1, aH2, aH3); + EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); +} + +/* ************************************************************************* */ +TEST(PreintegrationBase, UpdateEstimate2) { + PreintegrationBase pim(testing::Params()); + const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); + Vector9 zeta; + zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; + Matrix9 aH1; + Matrix93 aH2, aH3; + pim.UpdatePreintegrated(acc, omega, kDt, zeta, aH1, aH2, aH3); + // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 + EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3)); + EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8)); + EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); +} + +/* ************************************************************************* */ +TEST(PreintegrationBase, computeError) { + PreintegrationBase pim(testing::Params()); + NavState x1, x2; + imuBias::ConstantBias bias; + Matrix9 aH1, aH2; + Matrix96 aH3; + pim.computeError(x1, x2, bias, aH1, aH2, aH3); + boost::function f = + boost::bind(&PreintegrationBase::computeError, pim, _1, _2, _3, + boost::none, boost::none, boost::none); + // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 + EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); +} + +/* ************************************************************************* */ +TEST(PreintegrationBase, Compose) { + testing::SomeMeasurements measurements; + PreintegrationBase pim(testing::Params()); + testing::integrateMeasurements(measurements, &pim); + + boost::function f = + [pim](const Vector9& zeta01, const Vector9& zeta12) { + return PreintegrationBase::Compose(zeta01, zeta12, pim.deltaTij()); + }; + + // Expected merge result + PreintegrationBase expected_pim02(testing::Params()); + testing::integrateMeasurements(measurements, &expected_pim02); + testing::integrateMeasurements(measurements, &expected_pim02); + + // Actual result + Matrix9 H1, H2; + PreintegrationBase actual_pim02 = pim; + actual_pim02.mergeWith(pim, &H1, &H2); + + const Vector9 zeta = pim.preintegrated(); + const Vector9 actual_zeta = + PreintegrationBase::Compose(zeta, zeta, pim.deltaTij()); + EXPECT(assert_equal(expected_pim02.preintegrated(), actual_zeta, 1e-7)); + EXPECT(assert_equal(numericalDerivative21(f, zeta, zeta), H1, 1e-7)); + EXPECT(assert_equal(numericalDerivative22(f, zeta, zeta), H2, 1e-7)); +} + +/* ************************************************************************* */ + TEST(PreintegrationBase, MergedBiasDerivatives) { + testing::SomeMeasurements measurements; + + boost::function 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), + expected_pim02.preintegrated_H_biasAcc())); + EXPECT(assert_equal(numericalDerivative22(f, Z_3x1, Z_3x1), + expected_pim02.preintegrated_H_biasOmega(), 1e-7)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp new file mode 100644 index 000000000..bc965b058 --- /dev/null +++ b/gtsam/navigation/tests/testScenario.cpp @@ -0,0 +1,137 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testScenario.cpp + * @brief Unit test Scenario class + * @author Frank Dellaert + */ + +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static const double kDegree = M_PI / 180.0; + +/* ************************************************************************* */ +TEST(Scenario, Spin) { + // angular velocity 6 kDegree/sec + const double w = 6 * kDegree; + const Vector3 W(0, 0, w), V(0, 0, 0); + const ConstantTwistScenario scenario(W, V); + + const double T = 10; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9)); + EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9)); + + const Pose3 T10 = scenario.pose(T); + EXPECT(assert_equal(Vector3(0, 0, 60 * kDegree), T10.rotation().xyz(), 1e-9)); + EXPECT(assert_equal(Point3(0, 0, 0), T10.translation(), 1e-9)); +} + +/* ************************************************************************* */ +TEST(Scenario, Forward) { + const double v = 2; // m/s + const Vector3 W(0, 0, 0), V(v, 0, 0); + const ConstantTwistScenario scenario(W, V); + + const double T = 15; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9)); + EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9)); + + const Pose3 T15 = scenario.pose(T); + EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9)); + EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9)); +} + +/* ************************************************************************* */ +TEST(Scenario, Circle) { + // Forward velocity 2m/s, angular velocity 6 kDegree/sec around Z + const double v = 2, w = 6 * kDegree; + const Vector3 W(0, 0, w), V(v, 0, 0); + const ConstantTwistScenario scenario(W, V); + + const double T = 15; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9)); + EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9)); + + // R = v/w, so test if circle is of right size + const double R = v / w; + const Pose3 T15 = scenario.pose(T); + EXPECT(assert_equal(Vector3(0, 0, 90 * kDegree), T15.rotation().xyz(), 1e-9)); + EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9)); +} + +/* ************************************************************************* */ +TEST(Scenario, Loop) { + // Forward velocity 2m/s + // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) + const double v = 2, w = 6 * kDegree; + const Vector3 W(0, -w, 0), V(v, 0, 0); + const ConstantTwistScenario scenario(W, V); + + const double T = 30; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9)); + EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9)); + + // R = v/w, so test if loop crests at 2*R + const double R = v / w; + const Pose3 T30 = scenario.pose(30); + EXPECT(assert_equal(Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9)); + EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9)); +} + +/* ************************************************************************* */ +TEST(Scenario, Accelerating) { + // Set up body pointing towards y axis, and start at 10,20,0 with velocity + // going in X. The body itself has Z axis pointing down + const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + const Point3 P0(10, 20, 0); + const Vector3 V0(50, 0, 0); + + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0.1, 0.2, 0.3); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); + + const double T = 3; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(Vector3(V0 + T * A), scenario.velocity_n(T), 1e-9)); + EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9)); + + { + // Check acceleration in nav + Matrix expected = numericalDerivative11( + boost::bind(&Scenario::velocity_n, scenario, _1), T); + EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9)); + } + + const Pose3 T3 = scenario.pose(3); + EXPECT(assert_equal(nRb.expmap(T * W), T3.rotation(), 1e-9)); + EXPECT(assert_equal(Point3(10 + T * 50, 20 + a * T * T / 2, 0), + T3.translation(), 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarios.cpp new file mode 100644 index 000000000..c2fdb963d --- /dev/null +++ b/gtsam/navigation/tests/testScenarios.cpp @@ -0,0 +1,385 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testScenarioRunner.cpp + * @brief test ImuFacor with ScenarioRunner class + * @author Frank Dellaert + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static const double kDegree = M_PI / 180.0; +static const double kDt = 1e-2; + +// realistic white noise strengths are 0.5 deg/sqrt(hr) and 0.1 (m/s)/sqrt(h) +static const double kGyroSigma = 0.5 * kDegree / 60; +static const double kAccelSigma = 0.1 / 60.0; + +static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3); +static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias); + +// Create default parameters with Z-up and above noise parameters +static boost::shared_ptr defaultParams() { + auto p = PreintegrationParams::MakeSharedU(10); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0000001 * I_3x3; + return p; +} + +#define EXPECT_NEAR(a, b, c) EXPECT(assert_equal(Vector(a), Vector(b), c)); + +/* ************************************************************************* */ +TEST(ScenarioRunner, Spin) { + // angular velocity 6 degree/sec + const double w = 6 * kDegree; + const Vector3 W(0, 0, w), V(0, 0, 0); + const ConstantTwistScenario scenario(W, V); + + auto p = defaultParams(); + ScenarioRunner runner(&scenario, p, kDt); + const double T = 2 * kDt; // seconds + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + +#if 0 + // Check sampled noise is kosher + Matrix6 expected; + expected << p->accelerometerCovariance / kDt, Z_3x3, // + Z_3x3, p->gyroscopeCovariance / kDt; + Matrix6 actual = runner.estimateNoiseCovariance(10000); + EXPECT(assert_equal(expected, actual, 1e-2)); +#endif + + // Check calculated covariance against Monte Carlo estimate + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); +} + +/* ************************************************************************* */ +namespace forward { +const double v = 2; // m/s +ConstantTwistScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); +} +/* ************************************************************************* */ +TEST(ScenarioRunner, Forward) { + using namespace forward; + ScenarioRunner runner(&scenario, defaultParams(), kDt); + const double T = 0.1; // seconds + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, ForwardWithBias) { + using namespace forward; + ScenarioRunner runner(&scenario, defaultParams(), kDt); + const double T = 0.1; // seconds + + auto pim = runner.integrate(T, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 1000, kNonZeroBias); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Circle) { + // Forward velocity 2m/s, angular velocity 6 degree/sec + const double v = 2, w = 6 * kDegree; + ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); + + ScenarioRunner runner(&scenario, defaultParams(), kDt); + const double T = 0.1; // seconds + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Loop) { + // Forward velocity 2m/s + // Pitch up with angular velocity 6 degree/sec (negative in FLU) + const double v = 2, w = 6 * kDegree; + ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); + + ScenarioRunner runner(&scenario, defaultParams(), kDt); + const double T = 0.1; // seconds + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); +} + +/* ************************************************************************* */ +namespace initial { +const Rot3 nRb; +const Point3 P0; +const Vector3 V0(0, 0, 0); +} + +/* ************************************************************************* */ +namespace accelerating { +using namespace initial; +const double a = 0.2; // m/s^2 +const Vector3 A(0, a, 0); +const AcceleratingScenario scenario(nRb, P0, V0, A); + +const double T = 3; // seconds +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating) { + using namespace accelerating; + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingWithBias) { + using namespace accelerating; + ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + + auto pim = runner.integrate(T, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingAndRotating) { + using namespace initial; + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0, 0.1, 0); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); + + const double T = 10; // seconds + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +namespace initial2 { +// No rotation, but non-zero position and velocities +const Rot3 nRb; +const Point3 P0(10, 20, 0); +const Vector3 V0(50, 0, 0); +} + +/* ************************************************************************* */ +namespace accelerating2 { +using namespace initial2; +const double a = 0.2; // m/s^2 +const Vector3 A(0, a, 0); +const AcceleratingScenario scenario(nRb, P0, V0, A); + +const double T = 3; // seconds +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating2) { + using namespace accelerating2; + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingWithBias2) { + using namespace accelerating2; + ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + + auto pim = runner.integrate(T, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingAndRotating2) { + using namespace initial2; + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0, 0.1, 0); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); + + const double T = 10; // seconds + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +namespace initial3 { +// Rotation only +// Set up body pointing towards y axis. The body itself has Z axis pointing down +const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); +const Point3 P0; +const Vector3 V0(0, 0, 0); +} + +/* ************************************************************************* */ +namespace accelerating3 { +using namespace initial3; +const double a = 0.2; // m/s^2 +const Vector3 A(0, a, 0); +const AcceleratingScenario scenario(nRb, P0, V0, A); + +const double T = 3; // seconds +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating3) { + using namespace accelerating3; + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingWithBias3) { + using namespace accelerating3; + ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + + auto pim = runner.integrate(T, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingAndRotating3) { + using namespace initial3; + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0, 0.1, 0); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); + + const double T = 10; // seconds + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +namespace initial4 { +// Both rotation and translation +// Set up body pointing towards y axis, and start at 10,20,0 with velocity +// going in X. The body itself has Z axis pointing down +const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); +const Point3 P0(10, 20, 0); +const Vector3 V0(50, 0, 0); +} + +/* ************************************************************************* */ +namespace accelerating4 { +using namespace initial4; +const double a = 0.2; // m/s^2 +const Vector3 A(0, a, 0); +const AcceleratingScenario scenario(nRb, P0, V0, A); + +const double T = 3; // seconds +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating4) { + using namespace accelerating4; + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingWithBias4) { + using namespace accelerating4; + ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + + auto pim = runner.integrate(T, kNonZeroBias); + Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingAndRotating4) { + using namespace initial4; + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0, 0.1, 0); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); + + const double T = 10; // seconds + ScenarioRunner runner(&scenario, defaultParams(), T / 10); + + auto pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); + + Matrix9 estimatedCov = runner.estimateCovariance(T); + EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1); + EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + auto result = TestRegistry::runAllTests(tr); + tictoc_print_(); + return result; +} +/* ************************************************************************* */ diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 5d6d28061..21e17a648 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -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,6 +23,7 @@ #include #include #include + namespace gtsam { /** @@ -74,7 +75,7 @@ public: } /** - * Error function *without* the NoiseModel, \f$ h(x)-z \f$. + * Error function *without* the NoiseModel, \f$ z-h(x) -> Local(h(x),z) \f$. * We override this method to provide * both the function evaluation and its derivative(s) in H. */ @@ -82,10 +83,12 @@ public: boost::optional&> H = boost::none) const { if (H) { const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); - return traits::Local(measured_, value); + // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here + // because it would use the tangent space of the measurement instead of the value. + return -traits::Local(value, measured_); } else { const T value = expression_.value(x); - return traits::Local(measured_, value); + return -traits::Local(value, measured_); } } @@ -96,7 +99,7 @@ public: // In case noise model is constrained, we need to provide a noise model SharedDiagonal noiseModel; - if (noiseModel_->isConstrained()) { + if (noiseModel_ && noiseModel_->isConstrained()) { noiseModel = boost::static_pointer_cast( noiseModel_)->unit(); } @@ -116,11 +119,13 @@ public: T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here ! // Evaluate error and set RHS vector b - Ab(size()).col(0) = -traits::Local(measured_, value); + Ab(size()).col(0) = traits::Local(value, measured_); // Whiten the corresponding system, Ab already contains RHS - Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models - noiseModel_->WhitenSystem(Ab.matrix(), b); + if (noiseModel_) { + Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models + noiseModel_->WhitenSystem(Ab.matrix(), b); + } return factor; } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index ace35e530..9e42afa33 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -22,11 +22,11 @@ #include #include #include +#include #include #include #include -#include #include #include @@ -189,12 +189,24 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( } } else { // Straightforward damping: + + // initialize noise model cache to a reasonable default size + NoiseCacheVector noises(6); BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) { size_t dim = key_value.value.dim(); - Matrix A = Matrix::Identity(dim, dim); - Vector b = Vector::Zero(dim); - SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); - damped += boost::make_shared(key_value.key, A, b, model); + + if (dim > noises.size()) + noises.resize(dim); + + NoiseCacheItem& item = noises[dim-1]; + + // Initialize noise model, A and b if we haven't done so already + if(!item.model) { + item.A = Matrix::Identity(dim, dim); + item.b = Vector::Zero(dim); + item.model = noiseModel::Isotropic::Sigma(dim, sigma); + } + damped += boost::make_shared(key_value.key, item.A, item.b, item.model); } } gttoc(damp); @@ -239,7 +251,15 @@ void LevenbergMarquardtOptimizer::iterate() { // Keep increasing lambda until we make make progress while (true) { - boost::timer::cpu_timer timer; + +#ifdef GTSAM_USING_NEW_BOOST_TIMERS + boost::timer::cpu_timer lamda_iteration_timer; + lamda_iteration_timer.start(); +#else + boost::timer lamda_iteration_timer; + lamda_iteration_timer.restart(); +#endif + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "trying lambda = " << state_.lambda << endl; @@ -324,9 +344,14 @@ void LevenbergMarquardtOptimizer::iterate() { if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) { // do timing - double iterationTime = 1e-9 * timer.elapsed().wall; +#ifdef GTSAM_USING_NEW_BOOST_TIMERS + double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall; +#else + double iterationTime = lamda_iteration_timer.elapsed(); +#endif if (state_.iterations == 0) cout << "iter cost cost_change lambda success iter_time" << endl; + cout << boost::format("% 4d % 8e % 3.2e % 3.2e % 4d % 3.2e") % state_.iterations % newError % costChange % state_.lambda % systemSolvedSuccessfully % iterationTime << endl; diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index a965c6cf0..2be4a218e 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -272,6 +272,17 @@ public: GaussianFactorGraph::shared_ptr buildDampedSystem(const GaussianFactorGraph& linear); friend class ::NonlinearOptimizerMoreOptimizationTest; + /** Small struct to cache objects needed for damping. + * This is used in buildDampedSystem */ + struct NoiseCacheItem { + Matrix A; + Vector b; + SharedDiagonal model; + }; + + /// Noise model Cache + typedef std::vector NoiseCacheVector; + void writeLogFile(double currentError); /// @} diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 05ead7fc3..a5bfab3f5 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -49,8 +49,12 @@ namespace gtsam { const Key key; ///< The key const ValueType& value; ///< The value - _ValuesConstKeyValuePair(Key _key, const ValueType& _value) : key(_key), value(_value) {} - _ValuesConstKeyValuePair(const _ValuesKeyValuePair& rhs) : key(rhs.key), value(rhs.value) {} + _ValuesConstKeyValuePair(Key _key, const ValueType& _value) : + key(_key), value(_value) { + } + _ValuesConstKeyValuePair(const _ValuesKeyValuePair& rhs) : + key(rhs.key), value(rhs.value) { + } }; /* ************************************************************************* */ @@ -59,17 +63,20 @@ namespace gtsam { // need to use a struct here for later partial specialization template struct ValuesCastHelper { - static CastedKeyValuePairType cast(KeyValuePairType key_value) { - // Static cast because we already checked the type during filtering - return CastedKeyValuePairType(key_value.key, const_cast&>(static_cast&>(key_value.value)).value()); - } + static CastedKeyValuePairType cast(KeyValuePairType key_value) { + // Static cast because we already checked the type during filtering + return CastedKeyValuePairType(key_value.key, + const_cast&>(static_cast&>(key_value.value)).value()); + } }; // partial specialized version for ValueType == Value template struct ValuesCastHelper { static CastedKeyValuePairType cast(KeyValuePairType key_value) { // Static cast because we already checked the type during filtering - // in this case the casted and keyvalue pair are essentially the same type (key, Value&) so perhaps this could be done with just a cast of the key_value? + // in this case the casted and keyvalue pair are essentially the same type + // (key, Value&) so perhaps this could be done with just a cast of the key_value? return CastedKeyValuePairType(key_value.key, key_value.value); } }; @@ -78,7 +85,8 @@ namespace gtsam { struct ValuesCastHelper { static CastedKeyValuePairType cast(KeyValuePairType key_value) { // Static cast because we already checked the type during filtering - // in this case the casted and keyvalue pair are essentially the same type (key, Value&) so perhaps this could be done with just a cast of the key_value? + // in this case the casted and keyvalue pair are essentially the same type + // (key, Value&) so perhaps this could be done with just a cast of the key_value? return CastedKeyValuePairType(key_value.key, key_value.value); } }; @@ -126,23 +134,29 @@ namespace gtsam { } private: - Filtered(const boost::function& filter, Values& values) : - begin_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, values.begin(), values.end()), - &ValuesCastHelper::cast)), - end_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, values.end(), values.end()), - &ValuesCastHelper::cast)), - constBegin_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, ((const Values&)values).begin(), ((const Values&)values).end()), - &ValuesCastHelper::cast)), - constEnd_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, ((const Values&)values).end(), ((const Values&)values).end()), - &ValuesCastHelper::cast)) {} + Filtered( + const boost::function& filter, + Values& values) : + begin_( + boost::make_transform_iterator( + boost::make_filter_iterator(filter, values.begin(), values.end()), + &ValuesCastHelper::cast)), end_( + boost::make_transform_iterator( + boost::make_filter_iterator(filter, values.end(), values.end()), + &ValuesCastHelper::cast)), constBegin_( + boost::make_transform_iterator( + boost::make_filter_iterator(filter, + ((const Values&) values).begin(), + ((const Values&) values).end()), + &ValuesCastHelper::cast)), constEnd_( + boost::make_transform_iterator( + boost::make_filter_iterator(filter, + ((const Values&) values).end(), + ((const Values&) values).end()), + &ValuesCastHelper::cast)) { + } friend class Values; iterator begin_; @@ -191,7 +205,9 @@ namespace gtsam { friend class Values; const_iterator begin_; const_iterator end_; - ConstFiltered(const boost::function& filter, const Values& values) { + ConstFiltered( + const boost::function& filter, + const Values& values) { // We remove the const from values to create a non-const Filtered // view, then pull the const_iterators out of it. const Filtered filtered(filter, const_cast(values)); @@ -247,7 +263,8 @@ namespace gtsam { /* ************************************************************************* */ template<> - inline bool Values::filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { + inline bool Values::filterHelper(const boost::function filter, + const ConstKeyValuePair& key_value) { // Filter and check the type return filter(key_value.key); } @@ -361,8 +378,14 @@ namespace gtsam { if(item == values_.end()) throw ValuesKeyDoesNotExist("at", j); - return internal::handle()(j,item->second); - } + // Check the type and throw exception if incorrect + const Value& value = *item->second; + try { + return dynamic_cast&>(value).value(); + } catch (std::bad_cast &) { + throw ValuesIncorrectType(j, typeid(value), typeid(ValueType)); + } + } /* ************************************************************************* */ template @@ -372,10 +395,11 @@ namespace gtsam { if(item != values_.end()) { // dynamic cast the type and throw exception if incorrect + const Value& value = *item->second; try { - return dynamic_cast&>(*item->second).value(); + return dynamic_cast&>(value).value(); } catch (std::bad_cast &) { - throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); + throw ValuesIncorrectType(j, typeid(value), typeid(ValueType)); } } else { return boost::none; diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index c3bc75fb9..ba7a040cd 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -59,18 +59,19 @@ namespace gtsam { /* ************************************************************************* */ bool Values::equals(const Values& other, double tol) const { - if(this->size() != other.size()) + if (this->size() != other.size()) return false; - for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) { - if(typeid(it1->value) != typeid(it2->value)) - return false; - if(it1->key != it2->key) - return false; - if(!it1->value.equals_(it2->value, tol)) + for (const_iterator it1 = this->begin(), it2 = other.begin(); + it1 != this->end(); ++it1, ++it2) { + const Value& value1 = it1->value; + const Value& value2 = it2->value; + if (typeid(value1) != typeid(value2) || it1->key != it2->key + || !value1.equals_(value2, tol)) { return false; + } } return true; // We return false earlier if we find anything that does not match - } +} /* ************************************************************************* */ bool Values::exists(Key j) const { @@ -86,7 +87,6 @@ namespace gtsam { VectorValues::const_iterator vector_item = delta.find(key_value->key); Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument if(vector_item != delta.end()) { -// const Vector& singleDelta = delta[key_value->key]; // Delta for this value const Vector& singleDelta = vector_item->second; Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract result.values_.insert(key, retractedValue); // Add retracted result directly to result values @@ -149,12 +149,13 @@ namespace gtsam { void Values::update(Key j, const Value& val) { // Find the value to update KeyValueMap::iterator item = values_.find(j); - if(item == values_.end()) + if (item == values_.end()) throw ValuesKeyDoesNotExist("update", j); // Cast to the derived type - if(typeid(*item->second) != typeid(val)) - throw ValuesIncorrectType(j, typeid(*item->second), typeid(val)); + const Value& old_value = *item->second; + if (typeid(old_value) != typeid(val)) + throw ValuesIncorrectType(j, typeid(old_value), typeid(val)); values_.replace(item, val.clone_()); } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 774e2c42b..a61d01f23 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -32,6 +32,7 @@ #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" +#pragma GCC diagnostic ignored "-Wunused-local-typedefs" #endif #include #ifdef __GNUC__ diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index 6edcfac8f..19fd257e8 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -12,20 +12,28 @@ namespace gtsam { -// Generics -template +// Generic between, assumes existence of traits::Between +template Expression between(const Expression& t1, const Expression& t2) { return Expression(traits::Between, t1, t2); } -// Generics -template +// Generic compose, assumes existence of traits::Compose +template Expression compose(const Expression& t1, const Expression& t2) { return Expression(traits::Compose, t1, t2); } +// Some typedefs typedef Expression Double_; +typedef Expression Vector1_; +typedef Expression Vector2_; typedef Expression Vector3_; +typedef Expression Vector4_; +typedef Expression Vector5_; +typedef Expression Vector6_; +typedef Expression Vector7_; +typedef Expression Vector8_; +typedef Expression Vector9_; -} // \namespace gtsam - +} // \namespace gtsam diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 377d6cd34..16117845e 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -62,7 +62,7 @@ using namespace gtsam; // Check that ceres rotation convention is the same TEST(AdaptAutoDiff, Rotation) { Vector3 axisAngle(0.1, 0.2, 0.3); - Matrix3 expected = Rot3::rodriguez(axisAngle).matrix(); + Matrix3 expected = Rot3::Rodrigues(axisAngle).matrix(); Matrix3 actual; ceres::AngleAxisToRotationMatrix(axisAngle.data(), actual.data()); EXPECT(assert_equal(expected, actual)); diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h index 1ad27865d..a5bcac822 100644 --- a/gtsam/sam/RangeFactor.h +++ b/gtsam/sam/RangeFactor.h @@ -31,8 +31,7 @@ struct Range; * Works for any two types A1,A2 for which the functor Range() is defined * @addtogroup SAM */ -template ::result_type> +template class RangeFactor : public ExpressionFactor2 { private: typedef RangeFactor This; diff --git a/gtsam/sam/tests/testBearingFactor.cpp b/gtsam/sam/tests/testBearingFactor.cpp index b7fcfc9aa..12635a7e5 100644 --- a/gtsam/sam/tests/testBearingFactor.cpp +++ b/gtsam/sam/tests/testBearingFactor.cpp @@ -81,23 +81,33 @@ TEST(BearingFactor, Serialization3D) { } /* ************************************************************************* */ -TEST(BearingFactor, 3D) { - // Serialize the factor - std::string serialized = serializeXML(factor3D); - - // And de-serialize it - BearingFactor3D factor; - deserializeXML(serialized, factor); - - // Set the linearization point - Values values; - values.insert(poseKey, Pose3()); - values.insert(pointKey, Point3(1, 0, 0)); - - EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), - values, 1e-7, 1e-5); - EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); -} +// TODO(frank): this test is disabled (for now) because the macros below are +// incompatible with the Unit3 localCoordinates. The issue is the following: +// For factors, we want to use Local(value, measured), because we need the error +// to be expressed in the tangent space of value. This surfaced in the Unit3 case +// where the tangent space can be radically didfferent from one value to the next. +// For derivatives, we want Local(constant, varying), because we need a derivative +// in a constant tangent space. But since the macros below call whitenedError +// which calls Local(value,measured), we actually call the reverse. This does not +// matter for types with a commutative Local, but matters a lot for Unit3. +// More thinking needed about what the right thing is, here... +//TEST(BearingFactor, 3D) { +// // Serialize the factor +// std::string serialized = serializeXML(factor3D); +// +// // And de-serialize it +// BearingFactor3D factor; +// deserializeXML(serialized, factor); +// +// // Set the linearization point +// Values values; +// values.insert(poseKey, Pose3()); +// values.insert(pointKey, Point3(1, 0, 0)); +// +// EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), +// values, 1e-7, 1e-5); +// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +//} /* ************************************************************************* */ int main() { diff --git a/gtsam/sam/tests/testBearingRangeFactor.cpp b/gtsam/sam/tests/testBearingRangeFactor.cpp index e11e62628..4c7a9ab91 100644 --- a/gtsam/sam/tests/testBearingRangeFactor.cpp +++ b/gtsam/sam/tests/testBearingRangeFactor.cpp @@ -80,23 +80,25 @@ TEST(BearingRangeFactor, Serialization3D) { } /* ************************************************************************* */ -TEST(BearingRangeFactor, 3D) { - // Serialize the factor - std::string serialized = serializeXML(factor3D); - - // And de-serialize it - BearingRangeFactor3D factor; - deserializeXML(serialized, factor); - - // Set the linearization point - Values values; - values.insert(poseKey, Pose3()); - values.insert(pointKey, Point3(1, 0, 0)); - - EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), - values, 1e-7, 1e-5); - EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); -} +// TODO(frank): this test is disabled (for now) because the macros below are +// incompatible with the Unit3 localCoordinates. See testBearingFactor... +//TEST(BearingRangeFactor, 3D) { +// // Serialize the factor +// std::string serialized = serializeXML(factor3D); +// +// // And de-serialize it +// BearingRangeFactor3D factor; +// deserializeXML(serialized, factor); +// +// // Set the linearization point +// Values values; +// values.insert(poseKey, Pose3()); +// values.insert(pointKey, Point3(1, 0, 0)); +// +// EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), +// values, 1e-7, 1e-5); +// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +//} /* ************************************************************************* */ int main() { diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index a738d4cf0..8305fce12 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -85,11 +85,11 @@ namespace gtsam { /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const T& p, boost::optional H = boost::none) const { - if (H) (*H) = eye(traits::GetDimension(p)); - // manifold equivalent of h(x)-z -> log(z,h(x)) + Vector evaluateError(const T& x, boost::optional H = boost::none) const { + if (H) (*H) = eye(traits::GetDimension(x)); + // manifold equivalent of z-x -> Local(x,z) // TODO(ASL) Add Jacobians. - return traits::Local(prior_,p); + return -traits::Local(x, prior_); } const VALUE & prior() const { return prior_; } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 3f043a469..01a8fcf8d 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -50,6 +50,7 @@ private: typedef SmartFactorBase This; typedef typename CAMERA::Measurement Z; +protected: /** * As of Feb 22, 2015, the noise model is the same for all measurements and * is isotropic. This allows for moving most calculations of Schur complement @@ -58,7 +59,6 @@ private: */ SharedIsotropic noiseModel_; -protected: /** * 2D measurement and noise model for each of the m views * We keep a copy of measurements for I/O and computing the error. @@ -93,6 +93,9 @@ public: /// We use the new CameraSte data structure to refer to a set of cameras typedef CameraSet Cameras; + /// Default Constructor, for serialization + SmartFactorBase() {} + /// Constructor SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, boost::optional body_P_sensor = boost::none) : diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 5146c5a32..09fe84caa 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -137,7 +137,7 @@ protected: /// @name Parameters /// @{ - const SmartProjectionParams params_; + SmartProjectionParams params_; /// @} /// @name Caching triangulation @@ -154,6 +154,11 @@ public: /// shorthand for a set of cameras typedef CameraSet Cameras; + /** + * Default constructor, only for serialization + */ + SmartProjectionFactor() {} + /** * Constructor * @param body_P_sensor pose of the camera in the body frame @@ -246,7 +251,7 @@ public: /// triangulate bool triangulateForLinearize(const Cameras& cameras) const { triangulateSafe(cameras); // imperative, might reset result_ - return (result_); + return bool(result_); } /// linearize returns a Hessianfactor that is an approximation of error(p) diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index c091ff79d..455e0de87 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -59,6 +59,11 @@ public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; + /** + * Default constructor, only for serialization + */ + SmartProjectionPoseFactor() {} + /** * Constructor * @param Isotropic measurement noise diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 68c27e76b..50516afe4 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -510,7 +510,7 @@ GraphAndValues load3D(const string& filename) { Key id; double x, y, z, roll, pitch, yaw; ls >> id >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::ypr(yaw,pitch,roll); + Rot3 R = Rot3::Ypr(yaw,pitch,roll); Point3 t = Point3(x, y, z); initial->insert(id, Pose3(R,t)); } @@ -518,7 +518,7 @@ GraphAndValues load3D(const string& filename) { Key id; double x, y, z, qx, qy, qz, qw; ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::quaternion(qw, qx, qy, qz); + Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); Point3 t = Point3(x, y, z); initial->insert(id, Pose3(R,t)); } @@ -537,7 +537,7 @@ GraphAndValues load3D(const string& filename) { Key id1, id2; double x, y, z, roll, pitch, yaw; ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::ypr(yaw,pitch,roll); + Rot3 R = Rot3::Ypr(yaw,pitch,roll); Point3 t = Point3(x, y, z); Matrix m = eye(6); for (int i = 0; i < 6; i++) @@ -553,7 +553,7 @@ GraphAndValues load3D(const string& filename) { Key id1, id2; double x, y, z, qx, qy, qz, qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::quaternion(qw, qx, qy, qz); + Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); Point3 t = Point3(x, y, z); for (int i = 0; i < 6; i++){ for (int j = i; j < 6; j++){ diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 8e83ec503..ccad83f01 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -34,7 +34,7 @@ Point3 landmark4(10, 0.5, 1.2); Point3 landmark5(10, -0.5, 1.2); // First camera pose, looking along X-axis, 1 meter above ground plane (x-y) -Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); +Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); // Second camera 1 meter to the right of first camera Pose3 pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); // Third camera 1 meter above the first camera @@ -123,7 +123,7 @@ Camera cam3(pose_above, sharedBundlerK); template CAMERA perturbCameraPose(const CAMERA& camera) { - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 cameraPose = camera.pose(); Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 0406c3d27..4e8e3dbf3 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -137,23 +137,23 @@ TEST( dataSet, readG2o3D) boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); Values expectedValues; - Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); + Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); Point3 p0 = Point3(0.000000, 0.000000, 0.000000); expectedValues.insert(0, Pose3(R0, p0)); - Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); Point3 p1 = Point3(1.001367, 0.015390, 0.004948); expectedValues.insert(1, Pose3(R1, p1)); - Rot3 R2 = Rot3::quaternion(0.421446, -0.351729, -0.597838, 0.584174 ); + Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 ); Point3 p2 = Point3(1.993500, 0.023275, 0.003793); expectedValues.insert(2, Pose3(R2, p2)); - Rot3 R3 = Rot3::quaternion(0.067024, 0.331798, -0.200659, 0.919323); + Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323); Point3 p3 = Point3(2.004291, 1.024305, 0.018047); expectedValues.insert(3, Pose3(R3, p3)); - Rot3 R4 = Rot3::quaternion(0.765488, -0.035697, -0.462490, 0.445933); + Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933); Point3 p4 = Point3(0.999908, 1.055073, 0.020212); expectedValues.insert(4, Pose3(R4, p4)); @@ -163,27 +163,27 @@ TEST( dataSet, readG2o3D) NonlinearFactorGraph expectedGraph; Point3 p01 = Point3(1.001367, 0.015390, 0.004948); - Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); Point3 p12 = Point3(0.523923, 0.776654, 0.326659); - Rot3 R12 = Rot3::quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); + Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); expectedGraph.add(BetweenFactor(1, 2, Pose3(R12,p12), model)); Point3 p23 = Point3(0.910927, 0.055169, -0.411761); - Rot3 R23 = Rot3::quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); + Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); expectedGraph.add(BetweenFactor(2, 3, Pose3(R23,p23), model)); Point3 p34 = Point3(0.775288, 0.228798, -0.596923); - Rot3 R34 = Rot3::quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); + Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); expectedGraph.add(BetweenFactor(3, 4, Pose3(R34,p34), model)); Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); - Rot3 R14 = Rot3::quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); + Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); expectedGraph.add(BetweenFactor(1, 4, Pose3(R14,p14), model)); Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); - Rot3 R30 = Rot3::quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); + Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); expectedGraph.add(BetweenFactor(3, 0, Pose3(R30,p30), model)); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); @@ -199,11 +199,11 @@ TEST( dataSet, readG2o3DNonDiagonalNoise) boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); Values expectedValues; - Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); + Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); Point3 p0 = Point3(0.000000, 0.000000, 0.000000); expectedValues.insert(0, Pose3(R0, p0)); - Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); Point3 p1 = Point3(1.001367, 0.015390, 0.004948); expectedValues.insert(1, Pose3(R1, p1)); @@ -223,7 +223,7 @@ TEST( dataSet, readG2o3DNonDiagonalNoise) noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(Info.inverse()); NonlinearFactorGraph expectedGraph; Point3 p01 = Point3(1.001367, 0.015390, 0.004948); - Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2)); @@ -452,7 +452,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ SfM_data readData; readBAL(filenameToRead, readData); - Pose3 poseChange = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3)); + Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3)); Values value; for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 4a7b4c3fe..f1e830d03 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -47,7 +47,7 @@ TEST (OrientedPlane3Factor, lm_translation_error) { // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose Symbol init_sym('x', 0); - Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); Vector sigmas(6); sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; PriorFactor pose_prior(init_sym, init_pose, @@ -94,7 +94,7 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose Symbol init_sym('x', 0); - Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); PriorFactor pose_prior(init_sym, init_pose, noiseModel::Diagonal::Sigmas( (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index db04a74eb..3c7d5f2b2 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -30,7 +30,7 @@ const gtsam::Key poseKey = 1; // Pose3 examples const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0); -const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3)); +const Rot3 rot3A, rot3B = Rot3::Pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3)); // Pose2 examples const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 2f39701f7..2fd471c9c 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -27,7 +27,7 @@ const gtsam::Key poseKey = 1; // Pose3 examples const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0); -const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::RzRyRx(0.1, 0.2, 0.3); +const Rot3 rot3A, rot3B = Rot3::Pitch(-M_PI_2), rot3C = Rot3::RzRyRx(0.1, 0.2, 0.3); // Pose2 examples const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index bf5969d4d..96052bd0f 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -18,6 +18,7 @@ #include #include +#include #include using namespace std; @@ -29,9 +30,13 @@ static SharedNoiseModel unit3(noiseModel::Unit::Create(3)); /* ************************************************************************* */ #include #include + +namespace gtsam { + class PinholeFactor: public SmartFactorBase > { public: typedef SmartFactorBase > Base; + PinholeFactor() {} PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { } virtual double error(const Values& values) const { @@ -43,6 +48,11 @@ public: } }; +/// traits +template<> +struct traits : public Testable {}; +} + TEST(SmartFactorBase, Pinhole) { PinholeFactor f= PinholeFactor(unit2); f.add(Point2(), 1); @@ -52,9 +62,13 @@ TEST(SmartFactorBase, Pinhole) { /* ************************************************************************* */ #include + +namespace gtsam { + class StereoFactor: public SmartFactorBase { public: typedef SmartFactorBase Base; + StereoFactor() {} StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { } virtual double error(const Values& values) const { @@ -66,6 +80,11 @@ public: } }; +/// traits +template<> +struct traits : public Testable {}; +} + TEST(SmartFactorBase, Stereo) { StereoFactor f(unit3); f.add(StereoPoint2(), 1); @@ -73,6 +92,24 @@ TEST(SmartFactorBase, Stereo) { EXPECT_LONGS_EQUAL(2 * 3, f.dim()); } +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST(SmartFactorBase, serialize) { + using namespace gtsam::serializationTestHelpers; + PinholeFactor factor(unit2); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index d4f60b085..467aefe91 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -22,6 +22,7 @@ #include "smartFactorScenarios.h" #include #include +#include #include #include #include @@ -46,7 +47,7 @@ template PinholeCamera perturbCameraPoseAndCalibration( const PinholeCamera& camera) { GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 cameraPose = camera.pose(); Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); @@ -60,7 +61,7 @@ PinholeCamera perturbCameraPoseAndCalibration( /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, perturbCameraPose) { using namespace vanilla; - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 perturbed_level_pose = level_pose.compose(noise_pose); Camera actualCamera(perturbed_level_pose, K2); @@ -843,6 +844,26 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { EXPECT(assert_equal(yActual, yExpected, 1e-7)); } + +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST( SmartProjectionCameraFactor, serialize) { + using namespace vanilla; + using namespace gtsam::serializationTestHelpers; + SmartFactor factor(unit2); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 8796dfe65..1c1bc3c03 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -162,7 +163,7 @@ TEST( SmartProjectionPoseFactor, noisy ) { Values values; values.insert(x1, cam1.pose()); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); @@ -195,7 +196,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses + Pose3 cameraPose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); @@ -204,7 +205,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ SimpleCamera cam3(cameraPose3, *K); // create arbitrary body_Pose_sensor (transforms from sensor to body) - Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // + Pose3 sensor_to_body = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // // These are the poses we want to estimate, from camera measurements Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); @@ -262,7 +263,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-7) - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); Values values; values.insert(x1, bodyPose1); values.insert(x2, bodyPose2); @@ -316,8 +317,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { groundTruth.insert(x3, cam3.pose()); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -538,8 +539,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -605,8 +606,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -666,8 +667,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -791,7 +792,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -843,7 +844,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { graph.push_back(PriorFactor(x1, level_pose, noisePrior)); graph.push_back(PriorFactor(x2, pose_right, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, level_pose); @@ -907,8 +908,8 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -994,7 +995,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac graph.push_back( PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -1062,8 +1063,8 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) graph.push_back( PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -1107,7 +1108,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) { SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); smartFactor1->add(measurements_cam1, views); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, cam1.pose()); @@ -1147,7 +1148,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { boost::shared_ptr factor = smartFactorInstance->linearize( values); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(level_pose)); @@ -1160,7 +1161,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { // Hessian is invariant to rotations in the nondegenerate case EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); Values tranValues; @@ -1202,7 +1203,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr factor = smartFactor->linearize(values); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(level_pose)); @@ -1215,7 +1216,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { // Hessian is invariant to rotations in the nondegenerate case EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); Values tranValues; @@ -1277,8 +1278,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -1356,8 +1357,8 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { graph.push_back( PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -1387,6 +1388,27 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { values.at(x3))); } +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST(SmartProjectionPoseFactor, serialize) { + using namespace vanillaPose; + using namespace gtsam::serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor(model, sharedK, boost::none, params); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 5ac92b4a9..1d2baefee 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -37,7 +37,7 @@ static const boost::shared_ptr sharedCal = // boost::make_shared(1500, 1200, 0, 640, 480); // Looking along X-axis, 1 meter above ground plane (x-y) -static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2); +static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); SimpleCamera camera1(pose1, *sharedCal); diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 51737285a..1a7fdf3de 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -100,7 +100,7 @@ PoseRTV PoseRTV::flyingDynamics( double pitch2 = r2.pitch(); double forward_accel = -thrust * sin(pitch2); // r2, pitch (in global frame?) controls forward force double loss_lift = lift*fabs(sin(pitch2)); - Rot3 yaw_correction_bn = Rot3::yaw(yaw2); + Rot3 yaw_correction_bn = Rot3::Yaw(yaw2); Point3 forward(forward_accel, 0.0, 0.0); Vector Acc_n = yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 3fff06de1..3fc6a0197 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)), - pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)), - pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)); + pose2(Point3(1.0, 1.0, 0.0), Rot3::Ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)), + pose3(Point3(2.0, 2.0, 0.0), Rot3::Ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)), + pose4(Point3(3.0, 3.0, 0.0), Rot3::Ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Unit::Create(6); @@ -102,9 +102,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), - pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), - pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)); + pose2(Point3(1.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), + pose3(Point3(2.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), + pose4(Point3(3.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 0386d8bcd..db2f8f7f8 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -193,7 +193,7 @@ TEST( testPoseRTV, transformed_from_1 ) { Point3 T(1.0, 2.0, 3.0); Velocity3 V(2.0, 3.0, 4.0); PoseRTV start(R, T, V); - Pose3 transform(Rot3::yaw(M_PI_2), Point3(1.0, 2.0, 3.0)); + Pose3 transform(Rot3::Yaw(M_PI_2), Point3(1.0, 2.0, 3.0)); Matrix actDTrans, actDGlobal; PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans); @@ -212,7 +212,7 @@ TEST( testPoseRTV, transformed_from_2 ) { Point3 T(1.0, 2.0, 3.0); Velocity3 V(2.0, 3.0, 4.0); PoseRTV start(R, T, V); - Pose3 transform(Rot3::yaw(M_PI_2), Point3(1.0, 2.0, 3.0)); + Pose3 transform(Rot3::Yaw(M_PI_2), Point3(1.0, 2.0, 3.0)); Matrix actDTrans, actDGlobal; PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans); @@ -229,14 +229,14 @@ TEST( testPoseRTV, transformed_from_2 ) { TEST(testPoseRTV, RRTMbn) { EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3))); EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()))); - EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)))); + EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::Ypr(0.1, 0.2, 0.3)))); } /* ************************************************************************* */ TEST(testPoseRTV, RRTMnb) { EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3))); EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()))); - EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)))); + EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::Ypr(0.1, 0.2, 0.3)))); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index a0d969c4d..1fb2cf39e 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -16,7 +16,7 @@ const double tol=1e-5; const double h = 0.01; //const double deg2rad = M_PI/180.0; -//Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); +//Pose3 g1(Rot3::Ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); //Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0).finished()); Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0).finished()); diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index 1423ef113..de1d3f77b 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -54,7 +54,7 @@ int main(int argc, char** argv){ string calibration_loc = findExampleDataFile("VO_calibration.txt"); string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); - + //read camera calibration info from file // focal lengths fx, fy, skew s, principal point u0, v0, baseline b cout << "Reading calibration info" << endl; @@ -67,17 +67,17 @@ int main(int argc, char** argv){ cout << "Reading camera poses" << endl; ifstream pose_file(pose_loc.c_str()); - int i; + int pose_index; MatrixRowMajor m(4,4); //read camera pose parameters and use to make initial estimates of camera poses - while (pose_file >> i) { + while (pose_file >> pose_index) { for (int i = 0; i < 16; i++) pose_file >> m.data()[i]; - initial_estimate.insert(i, Pose3(m)); + initial_estimate.insert(pose_index, Pose3(m)); } - + // landmark keys - size_t l; + size_t landmark_key; // pixel coordinates uL, uR, v (same for left/right images due to rectification) // landmark coordinates X, Y, Z in camera frame, resulting from triangulation @@ -90,14 +90,14 @@ int main(int argc, char** argv){ SmartFactor::shared_ptr factor(new SmartFactor(model, K)); size_t current_l = 3; // hardcoded landmark ID from first measurement - while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) { + while (factor_file >> pose_index >> landmark_key >> uL >> uR >> v >> X >> Y >> Z) { - if(current_l != l) { + if(current_l != landmark_key) { graph.push_back(factor); factor = SmartFactor::shared_ptr(new SmartFactor(model, K)); - current_l = l; + current_l = landmark_key; } - factor->add(Point2(uL,v), i); + factor->add(Point2(uL,v), pose_index); } Pose3 firstPose = initial_estimate.at(1); diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index 2ab3a6a9e..f83cb19fb 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -64,7 +64,7 @@ Rot2 Pose3Upright::rotation2() const { /* ************************************************************************* */ Rot3 Pose3Upright::rotation() const { - return Rot3::yaw(theta()); + return Rot3::Yaw(theta()); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 8d75d767c..e93a7953f 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -17,79 +17,38 @@ #include -#include -#include #include - #include namespace gtsam { -Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : - R_(R), t_(t), s_(s) { -} - Similarity3::Similarity3() : - R_(), t_(), s_(1){ + R_(), t_(), s_(1) { } Similarity3::Similarity3(double s) : - s_ (s) { + s_(s) { } -Similarity3::Similarity3(const Rotation& R, const Translation& t, double s) : - R_ (R), t_ (t), s_ (s) { +Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) : + R_(R), t_(t), s_(s) { } -Similarity3::operator Pose3() const { - return Pose3(R_, s_*t_); +Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : + R_(R), t_(t), s_(s) { } -Similarity3 Similarity3::identity() { - return Similarity3(); } +Similarity3::Similarity3(const Matrix4& T) : + R_(T.topLeftCorner<3, 3>()), t_(T.topRightCorner<3, 1>()), s_(1.0 / T(3, 3)) { +} -//Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { -// return Vector7(); -//} -// -//Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { -// return Similarity3(); -//} +bool Similarity3::equals(const Similarity3& other, double tol) const { + return R_.equals(other.R_, tol) && t_.equals(other.t_, tol) + && s_ < (other.s_ + tol) && s_ > (other.s_ - tol); +} bool Similarity3::operator==(const Similarity3& other) const { - return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); -} - -bool Similarity3::equals(const Similarity3& sim, double tol) const { - return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol) - && s_ < (sim.s_+tol) && s_ > (sim.s_-tol); -} - -Similarity3::Translation Similarity3::transform_from(const Translation& p) const { - return R_ * (s_ * p) + t_; -} - -Matrix7 Similarity3::AdjointMap() const{ - const Matrix3 R = R_.matrix(); - const Vector3 t = t_.vector(); - Matrix3 A = s_ * skewSymmetric(t) * R; - Matrix7 adj; - adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), 1; - return adj; -} - -inline Similarity3::Translation Similarity3::operator*(const Translation& p) const { - return transform_from(p); -} - -Similarity3 Similarity3::inverse() const { - Rotation Rt = R_.inverse(); - Translation sRt = R_.inverse() * (-s_ * t_); - return Similarity3(Rt, sRt, 1.0/s_); -} - -Similarity3 Similarity3::operator*(const Similarity3& T) const { - return Similarity3(R_ * T.R_, ((1.0/T.s_)*t_) + R_ * T.t_, s_*T.s_); + return R_.matrix() == other.R_.matrix() && t_ == other.t_ && s_ == other.s_; } void Similarity3::print(const std::string& s) const { @@ -100,31 +59,171 @@ void Similarity3::print(const std::string& s) const { std::cout << "s: " << scale() << std::endl; } +Similarity3 Similarity3::identity() { + return Similarity3(); +} +Similarity3 Similarity3::operator*(const Similarity3& T) const { + return Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_); +} + +Similarity3 Similarity3::inverse() const { + Rot3 Rt = R_.inverse(); + Point3 sRt = R_.inverse() * (-s_ * t_); + return Similarity3(Rt, sRt, 1.0 / s_); +} + +Point3 Similarity3::transform_from(const Point3& p, // + OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const { + Point3 q = R_ * p + t_; + if (H1) { + const Matrix3 R = R_.matrix(); + Matrix3 DR = s_ * R * skewSymmetric(-p.x(), -p.y(), -p.z()); + // TODO(frank): explain the derivative in lambda + *H1 << DR, s_ * R, s_ * p.vector(); + } + if (H2) + *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix() + return s_ * q; +} + +Point3 Similarity3::operator*(const Point3& p) const { + return transform_from(p); +} + +Matrix4 Similarity3::wedge(const Vector7& xi) { + // http://www.ethaneade.org/latex2html/lie/node29.html + const auto w = xi.head<3>(); + const auto u = xi.segment<3>(3); + double lambda = xi[6]; + Matrix4 W; + W << skewSymmetric(w), u, 0, 0, 0, -lambda; + return W; +} + +Matrix7 Similarity3::AdjointMap() const { + // http://www.ethaneade.org/latex2html/lie/node30.html + const Matrix3 R = R_.matrix(); + const Vector3 t = t_.vector(); + const Matrix3 A = s_ * skewSymmetric(t) * R; + Matrix7 adj; + adj << + R, Z_3x3, Matrix31::Zero(), // 3*7 + A, s_ * R, -s_ * t, // 3*7 + Matrix16::Zero(), 1; // 1*7 + return adj; +} + +Matrix3 Similarity3::GetV(Vector3 w, double lambda) { + // http://www.ethaneade.org/latex2html/lie/node29.html + double lambda2 = lambda * lambda; + double theta2 = w.transpose() * w; + double theta = sqrt(theta2); + double A, B, C; + // TODO(frank): eliminate copy/paste + if (theta2 > 1e-9 && lambda2 > 1e-9) { + const double X = sin(theta) / theta; + const double Y = (1 - cos(theta)) / theta2; + const double Z = (1 - X) / theta2; + const double W = (0.5 - Y) / theta2; + const double alpha = lambda2 / (lambda2 + theta2); + const double beta = (exp(-lambda) - 1 + lambda) / lambda2; + const double gamma = Y - (lambda * Z); + const double mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda)) + / (lambda2 * lambda); + const double upsilon = Z - (lambda * W); + A = (1 - exp(-lambda)) / lambda; + B = alpha * (beta - gamma) + gamma; + C = alpha * (mu - upsilon) + upsilon; + } else if (theta2 <= 1e-9 && lambda2 > 1e-9) { + //Taylor series expansions + const double Y = 0.5 - theta2 / 24.0; + const double Z = 1.0 / 6.0 - theta2 / 120.0; + const double W = 1.0 / 24.0 - theta2 / 720.0; + const double alpha = lambda2 / (lambda2 + theta2); + const double beta = (exp(-lambda) - 1 + lambda) / lambda2; + const double gamma = Y - (lambda * Z); + const double mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda)) + / (lambda2 * lambda); + const double upsilon = Z - (lambda * W); + A = (1 - exp(-lambda)) / lambda; + B = alpha * (beta - gamma) + gamma; + C = alpha * (mu - upsilon) + upsilon; + } else if (theta2 > 1e-9 && lambda2 <= 1e-9) { + const double X = sin(theta) / theta; + const double Y = (1 - cos(theta)) / theta2; + const double Z = (1 - X) / theta2; + const double W = (0.5 - Y) / theta2; + const double alpha = lambda2 / (lambda2 + theta2); + const double beta = 0.5 - lambda / 6.0 + lambda2 / 24.0 + - (lambda * lambda2) / 120; + const double gamma = Y - (lambda * Z); + const double mu = 1.0 / 6.0 - lambda / 24 + lambda2 / 120 + - (lambda * lambda2) / 720; + const double upsilon = Z - (lambda * W); + if (lambda < 1e-9) { + A = 1 - lambda / 2.0 + lambda2 / 6.0; + } else { + A = (1 - exp(-lambda)) / lambda; + } + B = alpha * (beta - gamma) + gamma; + C = alpha * (mu - upsilon) + upsilon; + } else { + const double Y = 0.5 - theta2 / 24.0; + const double Z = 1.0 / 6.0 - theta2 / 120.0; + const double W = 1.0 / 24.0 - theta2 / 720.0; + const double gamma = Y - (lambda * Z); + const double upsilon = Z - (lambda * W); + if (lambda < 1e-9) { + A = 1 - lambda / 2.0 + lambda2 / 6.0; + } else { + A = (1 - exp(-lambda)) / lambda; + } + B = gamma; + C = upsilon; + } + const Matrix3 Wx = skewSymmetric(w[0], w[1], w[2]); + return A * I_3x3 + B * Wx + C * Wx * Wx; +} + +Vector7 Similarity3::Logmap(const Similarity3& T, OptionalJacobian<7, 7> Hm) { + // To get the logmap, calculate w and lambda, then solve for u as shown by Ethan at + // www.ethaneade.org/latex2html/lie/node29.html + const Vector3 w = Rot3::Logmap(T.R_); + const double lambda = log(T.s_); + Vector7 result; + result << w, GetV(w, lambda).inverse() * T.t_.vector(), lambda; + if (Hm) { + throw std::runtime_error("Similarity3::Logmap: derivative not implemented"); + } + return result; +} + +Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { + const auto w = v.head<3>(); + const auto u = v.segment<3>(3); + const double lambda = v[6]; + if (Hm) { + throw std::runtime_error("Similarity3::Expmap: derivative not implemented"); + } + const Matrix3 V = GetV(w, lambda); + return Similarity3(Rot3::Expmap(w), Point3(V * u), exp(lambda)); +} + std::ostream &operator<<(std::ostream &os, const Similarity3& p) { - os << "[" << p.rotation().xyz().transpose() << " " << p.translation().vector().transpose() << " " << - p.scale() << "]\';"; + os << "[" << p.rotation().xyz().transpose() << " " + << p.translation().vector().transpose() << " " << p.scale() << "]\';"; return os; } -Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) { - // Will retracting or localCoordinating R work if R is not a unit rotation? - // Also, how do we actually get s out? Seems like we need to store it somewhere. - Rotation r; //Create a zero rotation to do our retraction. - return Similarity3( // - r.retract(v.head<3>()), // retract rotation using v[0,1,2] - Translation(v.segment<3>(3)), // Retract the translation - 1.0 + v[6]); //finally, update scale using v[6] +const Matrix4 Similarity3::matrix() const { + Matrix4 T; + T.topRows<3>() << R_.matrix(), t_.vector(); + T.bottomRows<1>() << 0, 0, 0, 1.0/s_; + return T; } -Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobian H) { - Rotation r; //Create a zero rotation to do the retraction - Vector7 v; - v.head<3>() = r.localCoordinates(other.R_); - v.segment<3>(3) = other.t_.vector(); - //v.segment<3>(3) = translation().localCoordinates(other.translation()); - v[6] = other.s_ - 1.0; - return v; -} +Similarity3::operator Pose3() const { + return Pose3(R_, s_ * t_); } - +} diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index eec2124ee..853261fc2 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -32,13 +32,15 @@ class Pose3; */ class Similarity3: public LieGroup { - /** Pose Concept requirements */ + /// @name Pose Concept + /// @{ typedef Rot3 Rotation; typedef Point3 Translation; + /// @} private: - Rotation R_; - Translation t_; + Rot3 R_; + Point3 t_; double s_; public: @@ -46,17 +48,21 @@ public: /// @name Constructors /// @{ + /// Default constructor Similarity3(); /// Construct pure scaling Similarity3(double s); /// Construct from GTSAM types - Similarity3(const Rotation& R, const Translation& t, double s); + Similarity3(const Rot3& R, const Point3& t, double s); /// Construct from Eigen types Similarity3(const Matrix3& R, const Vector3& t, double s); + /// Construct from matrix [R t; 0 s^-1] + Similarity3(const Matrix4& T); + /// @} /// @name Testable /// @{ @@ -64,7 +70,7 @@ public: /// Compare with tolerance bool equals(const Similarity3& sim, double tol) const; - /// Compare with standard tolerance + /// Exact equality bool operator==(const Similarity3& other) const; /// Print with optional string @@ -79,74 +85,118 @@ public: /// Return an identity transform static Similarity3 identity(); + /// Composition + Similarity3 operator*(const Similarity3& T) const; + /// Return the inverse Similarity3 inverse() const; - Translation transform_from(const Translation& p) const; + /// @} + /// @name Group action on Point3 + /// @{ + + /// Action on a point p is s*(R*p+t) + Point3 transform_from(const Point3& p, // + OptionalJacobian<3, 7> H1 = boost::none, // + OptionalJacobian<3, 3> H2 = boost::none) const; /** syntactic sugar for transform_from */ - inline Translation operator*(const Translation& p) const; - - Similarity3 operator*(const Similarity3& T) const; + Point3 operator*(const Point3& p) const; /// @} - /// @name Standard interface + /// @name Lie Group /// @{ - /// Return a GTSAM rotation - const Rotation& rotation() const { - return R_; - }; + /** Log map at the identity + * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$ + */ + static Vector7 Logmap(const Similarity3& s, // + OptionalJacobian<7, 7> Hm = boost::none); - /// Return a GTSAM translation - const Translation& translation() const { - return t_; - }; + /** Exponential map at the identity + */ + static Similarity3 Expmap(const Vector7& v, // + OptionalJacobian<7, 7> Hm = boost::none); - /// Return the scale - double scale() const { - return s_; - }; - - /// Convert to a rigid body pose - operator Pose3() const; - - /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes - inline static size_t Dim() { - return 7; - }; - - /// Dimensionality of tangent space = 7 DOF - inline size_t dim() const { - return 7; - }; - - /// @} - /// @name Chart - /// @{ - - /// Update Similarity transform via 7-dim vector in tangent space + /// Chart at the origin struct ChartAtOrigin { - static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none); - - /// 7-dimensional vector v in tangent space that makes other = this->retract(v) - static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none); + static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) { + return Similarity3::Expmap(v, H); + } + static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) { + return Similarity3::Logmap(other, H); + } }; + using LieGroup::inverse; + + /** + * wedge for Similarity3: + * @param xi 7-dim twist (w,u,lambda) where + * @return 4*4 element of Lie algebra that can be exponentiated + * TODO(frank): rename to Hat, make part of traits + */ + static Matrix4 wedge(const Vector7& xi); + /// Project from one tangent space to another Matrix7 AdjointMap() const; /// @} - /// @name Stubs + /// @name Standard interface /// @{ - /// Not currently implemented, required because this is a lie group - static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none); - static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none); + /// Calculate 4*4 matrix group equivalent + const Matrix4 matrix() const; - using LieGroup::inverse; // version with derivative + /// Return a GTSAM rotation + const Rot3& rotation() const { + return R_; + } + + /// Return a GTSAM translation + const Point3& translation() const { + return t_; + } + + /// Return the scale + double scale() const { + return s_; + } + + /// Convert to a rigid body pose (R, s*t) + /// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast. + operator Pose3() const; + + /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes + inline static size_t Dim() { + return 7; + } + + /// Dimensionality of tangent space = 7 DOF + inline size_t dim() const { + return 7; + } + + /// @} + /// @name Helper functions + /// @{ + + /// Calculate expmap and logmap coefficients. +private: + static Matrix3 GetV(Vector3 w, double lambda); + + /// @} }; template<> -struct traits : public internal::LieGroup {}; +inline Matrix wedge(const Vector& xi) { + return Similarity3::wedge(xi); } + +template<> +struct traits : public internal::LieGroup {}; + +template<> +struct traits : public internal::LieGroup {}; + +} // namespace gtsam diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index 2f3c763dd..6ac3389ed 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -17,7 +17,7 @@ using namespace std; using namespace gtsam; static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); -Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); SimpleCamera level_camera(level_pose, *K); /* ************************************************************************* */ @@ -142,7 +142,7 @@ TEST(InvDepthFactor, backproject2) // backwards facing camera Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1).finished()); double inv_depth(1./10); - InvDepthCamera3 inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); + InvDepthCamera3 inv_camera(Pose3(Rot3::Ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); Point2 z = inv_camera.project(expected, inv_depth); Vector5 actual_vec; diff --git a/gtsam_unstable/geometry/tests/testPose3Upright.cpp b/gtsam_unstable/geometry/tests/testPose3Upright.cpp index 0929035b6..cd54a8813 100644 --- a/gtsam_unstable/geometry/tests/testPose3Upright.cpp +++ b/gtsam_unstable/geometry/tests/testPose3Upright.cpp @@ -58,9 +58,9 @@ TEST( testPose3Upright, conversions ) { EXPECT(assert_equal(Point3(1.0, 2.0, 3.0), pose.translation(), tol)); EXPECT(assert_equal(Point2(1.0, 2.0), pose.translation2(), tol)); EXPECT(assert_equal(Rot2::fromAngle(0.1), pose.rotation2(), tol)); - EXPECT(assert_equal(Rot3::yaw(0.1), pose.rotation(), tol)); + EXPECT(assert_equal(Rot3::Yaw(0.1), pose.rotation(), tol)); EXPECT(assert_equal(Pose2(1.0, 2.0, 0.1), pose.pose2(), tol)); - EXPECT(assert_equal(Pose3(Rot3::yaw(0.1), Point3(1.0, 2.0, 3.0)), pose.pose(), tol)); + EXPECT(assert_equal(Pose3(Rot3::Yaw(0.1), Point3(1.0, 2.0, 3.0)), pose.pose(), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 14dfb8f1a..111d53d59 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -13,89 +13,108 @@ * @file testSimilarity3.cpp * @brief Unit tests for Similarity3 class * @author Paul Drews + * @author Zhaoyang Lv */ #include -#include -#include -#include -#include #include #include #include +#include #include #include - +#include +#include +#include +#include #include + #include +#include +#include + using namespace gtsam; using namespace std; using symbol_shorthand::X; GTSAM_CONCEPT_TESTABLE_INST(Similarity3) -static Point3 P(0.2,0.7,-2); -static Rot3 R = Rot3::Rodrigues(0.3,0,0); -static Similarity3 T(R,Point3(3.5,-8.2,4.2),1); -static Similarity3 T2(Rot3::Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1); -static Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1); +static const Point3 P(0.2, 0.7, -2); +static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0); +static const double s = 4; +static const Similarity3 id; +static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1); +static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), 1); +static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1); +static const Similarity3 T4(R, P, s); +static const Similarity3 T5(R, P, 10); +static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform + +//****************************************************************************** +TEST(Similarity3, Concepts) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} //****************************************************************************** TEST(Similarity3, Constructors) { - Similarity3 test; + Similarity3 sim3_Construct1; + Similarity3 sim3_Construct2(s); + Similarity3 sim3_Construct3(R, P, s); + Similarity3 sim4_Construct4(R.matrix(), P.vector(), s); } //****************************************************************************** TEST(Similarity3, Getters) { - Similarity3 test; - EXPECT(assert_equal(Rot3(), test.rotation())); - EXPECT(assert_equal(Point3(), test.translation())); - EXPECT_DOUBLES_EQUAL(1.0, test.scale(), 1e-9); + Similarity3 sim3_default; + EXPECT(assert_equal(Rot3(), sim3_default.rotation())); + EXPECT(assert_equal(Point3(), sim3_default.translation())); + EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9); + + Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7); + EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), sim3.rotation())); + EXPECT(assert_equal(Point3(4, 5, 6), sim3.translation())); + EXPECT_DOUBLES_EQUAL(7.0, sim3.scale(), 1e-9); } //****************************************************************************** -TEST(Similarity3, Getters2) { - Similarity3 test(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7); - EXPECT(assert_equal(Rot3::ypr(1, 2, 3), test.rotation())); - EXPECT(assert_equal(Point3(4, 5, 6), test.translation())); - EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9); -} - TEST(Similarity3, AdjointMap) { - Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); - Matrix7 result; - result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000, - 6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000, - -2.5734, -5.8362, 2.8839, 33.1363, 0.3024, 30.1811, -42.0000, - 0, 0, 0, -0.2248, -0.3502, -0.9093, 0, - 0, 0, 0, 0.9024, -0.4269, -0.0587, 0, - 0, 0, 0, -0.3676, -0.8337, 0.4120, 0, - 0, 0, 0, 0, 0, 0, 1.0000; - EXPECT(assert_equal(result, test.AdjointMap(), 1e-3)); + const Matrix4 T = T2.matrix(); + // Check Ad with actual definition + Vector7 delta; + delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; + Matrix4 W = Similarity3::wedge(delta); + Matrix4 TW = Similarity3::wedge(T2.AdjointMap() * delta); + EXPECT(assert_equal(TW, Matrix4(T * W * T.inverse()), 1e-9)); } +//****************************************************************************** TEST(Similarity3, inverse) { - Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); - Matrix3 Re; - Re << -0.2248, 0.9024, -0.3676, - -0.3502, -0.4269, -0.8337, - -0.9093, -0.0587, 0.4120; + Similarity3 sim3(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); + Matrix3 Re; // some values from matlab + Re << -0.2248, 0.9024, -0.3676, -0.3502, -0.4269, -0.8337, -0.9093, -0.0587, 0.4120; Vector3 te(-9.8472, 59.7640, 10.2125); - Similarity3 expected(Re, te, 1.0/7.0); - EXPECT(assert_equal(expected, test.inverse(), 1e-3)); + Similarity3 expected(Re, te, 1.0 / 7.0); + EXPECT(assert_equal(expected, sim3.inverse(), 1e-4)); + EXPECT(assert_equal(sim3, sim3.inverse().inverse(), 1e-8)); + + // test lie group inverse + Matrix H1, H2; + EXPECT(assert_equal(expected, sim3.inverse(H1), 1e-4)); + EXPECT(assert_equal(sim3, sim3.inverse().inverse(H2), 1e-8)); } -TEST(Similarity3, multiplication) { - Similarity3 test1(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); - Similarity3 test2(Rot3::ypr(1,2,3).inverse(), Point3(8,9,10), 11); +//****************************************************************************** +TEST(Similarity3, Multiplication) { + Similarity3 test1(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); + Similarity3 test2(Rot3::Ypr(1, 2, 3).inverse(), Point3(8, 9, 10), 11); Matrix3 re; - re << 0.0688, 0.9863, -0.1496, - -0.5665, -0.0848, -0.8197, - -0.8211, 0.1412, 0.5530; + re << 0.0688, 0.9863, -0.1496, -0.5665, -0.0848, -0.8197, -0.8211, 0.1412, 0.5530; Vector3 te(-13.6797, 3.2441, -5.7794); Similarity3 expected(re, te, 77); - EXPECT(assert_equal(expected, test1*test2, 1e-2)); + EXPECT(assert_equal(expected, test1 * test2, 1e-2)); } //****************************************************************************** @@ -117,15 +136,14 @@ TEST(Similarity3, Manifold) { v3 << 0, 0, 0, 1, 2, 3, 0; EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); -// Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1); - Similarity3 other = Similarity3(Rot3::ypr(0.1, 0.2, 0.3),Point3(4,5,6),1); + Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(4, 5, 6), 1); Vector vlocal = sim.localCoordinates(other); EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2)); - Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0),Point3(4,5,6),1); - Rot3 R = Rot3::Rodrigues(0.3,0,0); + Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0), Point3(4, 5, 6), 1); + Rot3 R = Rot3::Rodrigues(0.3, 0, 0); Vector vlocal2 = sim.localCoordinates(other2); @@ -134,30 +152,29 @@ TEST(Similarity3, Manifold) { // TODO add unit tests for retract and localCoordinates } -/* ************************************************************************* */ -TEST( Similarity3, retract_first_order) -{ +//****************************************************************************** +TEST( Similarity3, retract_first_order) { Similarity3 id; Vector v = zero(7); v(0) = 0.3; - EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v),1e-2)); - v(3)=0.2;v(4)=0.7;v(5)=-2; - EXPECT(assert_equal(Similarity3(R, P, 1),id.retract(v),1e-2)); + EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v), 1e-2)); +// v(3) = 0.2; +// v(4) = 0.7; +// v(5) = -2; +// EXPECT(assert_equal(Similarity3(R, P, 1), id.retract(v), 1e-2)); } -/* ************************************************************************* */ -TEST(Similarity3, localCoordinates_first_order) -{ - Vector d12 = repeat(7,0.1); +//****************************************************************************** +TEST(Similarity3, localCoordinates_first_order) { + Vector d12 = repeat(7, 0.1); d12(6) = 1.0; - Similarity3 t1 = T, t2 = t1.retract(d12); + Similarity3 t1 = T1, t2 = t1.retract(d12); EXPECT(assert_equal(d12, t1.localCoordinates(t2))); } -/* ************************************************************************* */ -TEST(Similarity3, manifold_first_order) -{ - Similarity3 t1 = T; +//****************************************************************************** +TEST(Similarity3, manifold_first_order) { + Similarity3 t1 = T1; Similarity3 t2 = T3; Similarity3 origin; Vector d12 = t1.localCoordinates(t2); @@ -166,48 +183,137 @@ TEST(Similarity3, manifold_first_order) EXPECT(assert_equal(t1, t2.retract(d21))); } +//****************************************************************************** +// Return as a 4*4 Matrix +TEST(Similarity3, Matrix) { + Matrix4 expected; + expected << + 1, 0, 0, 1, + 0, 1, 0, 1, + 0, 0, 1, 0, + 0, 0, 0, 0.5; + Matrix4 actual = T6.matrix(); + EXPECT(assert_equal(expected, actual)); +} + +//***************************************************************************** +// Exponential and log maps +TEST(Similarity3, ExpLogMap) { + Vector7 delta; + delta << 0.1,0.2,0.3,0.4,0.5,0.6,0.7; + Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(delta)); + EXPECT(assert_equal(delta, actual)); + + Vector7 zeros; + zeros << 0,0,0,0,0,0,0; + Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity()); + EXPECT(assert_equal(zeros, logIdentity)); + + Similarity3 expZero = Similarity3::Expmap(zeros); + Similarity3 ident = Similarity3::identity(); + EXPECT(assert_equal(expZero, ident)); + + // Compare to matrix exponential, using expm in Lie.h + EXPECT(assert_equal(expm(delta), Similarity3::Expmap(delta), 1e-3)); +} + +//****************************************************************************** +// Group action on Point3 (with simpler transform) +TEST(Similarity3, GroupAction) { + EXPECT(assert_equal(Point3(2, 2, 0), T6 * Point3(0, 0, 0))); + EXPECT(assert_equal(Point3(4, 2, 0), T6 * Point3(1, 0, 0))); + + // Test group action on R^4 via matrix representation + Vector4 qh; + qh << 1, 0, 0, 1; + Vector4 ph; + ph << 2, 1, 0, 0.5; // equivalent to Point3(4, 2, 0) + EXPECT(assert_equal((Vector )ph, T6.matrix() * qh)); + + // Test some more... + Point3 pa = Point3(1, 0, 0); + Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.0); + Similarity3 Tb(Rot3(), Point3(1, 2, 3), 2.0); + EXPECT(assert_equal(Point3(2, 2, 3), Ta.transform_from(pa))); + EXPECT(assert_equal(Point3(4, 4, 6), Tb.transform_from(pa))); + + Similarity3 Tc(Rot3::Rz(M_PI/2.0), Point3(1, 2, 3), 1.0); + Similarity3 Td(Rot3::Rz(M_PI/2.0), Point3(1, 2, 3), 2.0); + EXPECT(assert_equal(Point3(1, 3, 3), Tc.transform_from(pa))); + EXPECT(assert_equal(Point3(2, 6, 6), Td.transform_from(pa))); + + // Test derivative + boost::function f = boost::bind( + &Similarity3::transform_from, _1, _2, boost::none, boost::none); + + Point3 q(1, 2, 3); + for (const auto T : { T1, T2, T3, T4, T5, T6 }) { + Point3 q(1, 0, 0); + Matrix H1 = numericalDerivative21(f, T1, q); + Matrix H2 = numericalDerivative22(f, T1, q); + Matrix actualH1, actualH2; + T1.transform_from(q, actualH1, actualH2); + EXPECT(assert_equal(H1, actualH1)); + EXPECT(assert_equal(H2, actualH2)); + } +} + +//****************************************************************************** +// Test very simple prior optimization example TEST(Similarity3, Optimization) { - Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); + // Create a PriorFactor with a Sim3 prior + Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); - Symbol key('x',1); + Symbol key('x', 1); PriorFactor factor(key, prior, model); + // Create graph NonlinearFactorGraph graph; graph.push_back(factor); + // Create initial estimate with identity transform Values initial; initial.insert(key, Similarity3()); + // Optimize Values result; LevenbergMarquardtParams params; params.setVerbosityLM("TRYCONFIG"); result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + + // After optimization, result should be prior EXPECT(assert_equal(prior, result.at(key), 1e-4)); } +//****************************************************************************** +// Test optimization with both Prior and BetweenFactors TEST(Similarity3, Optimization2) { Similarity3 prior = Similarity3(); - Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0); - Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0); - Similarity3 m3 = Similarity3(Rot3::ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0); - Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0); + Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI / 4.0, 0, 0), Point3(2.0, 0, 0), + 1.0); + Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), + Point3(sqrt(8) * 0.9, 0, 0), 1.0); + Similarity3 m3 = Similarity3(Rot3::Ypr(3 * M_PI / 4.0, 0, 0), + Point3(sqrt(32) * 0.8, 0, 0), 1.0); + Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), + Point3(6 * 0.7, 0, 0), 1.0); Similarity3 loop = Similarity3(1.42); //prior.print("Goal Transform"); - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 0.01); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, + 0.01); SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( - (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished()); + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished()); SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas( - (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished()); - PriorFactor factor(X(1), prior, model); + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished()); + PriorFactor factor(X(1), prior, model); // Prior ! BetweenFactor b1(X(1), X(2), m1, betweenNoise); BetweenFactor b2(X(2), X(3), m2, betweenNoise); BetweenFactor b3(X(3), X(4), m3, betweenNoise); BetweenFactor b4(X(4), X(5), m4, betweenNoise); BetweenFactor lc(X(5), X(1), loop, betweenNoise2); - - + // Create graph NonlinearFactorGraph graph; graph.push_back(factor); graph.push_back(b1); @@ -217,13 +323,16 @@ TEST(Similarity3, Optimization2) { graph.push_back(lc); //graph.print("Full Graph\n"); - Values initial; initial.insert(X(1), Similarity3()); - initial.insert(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1)); - initial.insert(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); - initial.insert(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3)); - initial.insert(X(5), Similarity3(Rot3::ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0)); + initial.insert(X(2), + Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), Point3(1, 0, 0), 1.1)); + initial.insert(X(3), + Similarity3(Rot3::Ypr(2.0 * M_PI / 2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); + initial.insert(X(4), + Similarity3(Rot3::Ypr(3.0 * M_PI / 2.0, 0, 0), Point3(0, 1, 0), 1.3)); + initial.insert(X(5), + Similarity3(Rot3::Ypr(4.0 * M_PI / 2.0, 0, 0), Point3(0, 0, 0), 1.0)); //initial.print("Initial Estimate\n"); @@ -247,6 +356,58 @@ TEST(Similarity3, Optimization2) { EXPECT(assert_equal(expected, result.at(X(5)), 0.4)); } +//****************************************************************************** +// Align points (p,q) assuming that p = T*q + noise +TEST(Similarity3, AlignScaledPointClouds) { +// Create ground truth + Point3 q1(0, 0, 0), q2(1, 0, 0), q3(0, 1, 0); + + // Create transformed cloud (noiseless) +// Point3 p1 = T4 * q1, p2 = T4 * q2, p3 = T4 * q3; + + // Create an unknown expression + Expression unknownT(0); // use key 0 + + // Create constant expressions for the ground truth points + Expression q1_(q1), q2_(q2), q3_(q3); + + // Create prediction expressions + Expression predict1(unknownT, &Similarity3::transform_from, q1_); + Expression predict2(unknownT, &Similarity3::transform_from, q2_); + Expression predict3(unknownT, &Similarity3::transform_from, q3_); + +//// Create Expression factor graph +// ExpressionFactorGraph graph; +// graph.addExpressionFactor(predict1, p1, R); // |T*q1 - p1| +// graph.addExpressionFactor(predict2, p2, R); // |T*q2 - p2| +// graph.addExpressionFactor(predict3, p3, R); // |T*q3 - p3| +} + +//****************************************************************************** +TEST(Similarity3 , Invariants) { + Similarity3 id; + + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T3)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T3)); + + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T3)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T3)); +} + +//****************************************************************************** +TEST(Similarity3 , LieGroupDerivatives) { + Similarity3 id; + + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T3); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index f6f1c4a5c..20ffbdd4f 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -60,7 +60,7 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, double pitch = atan2(-g1, sqrt(g2 * g2 + g3 * g3)); double yaw = 0; // This returns body-to-nav nRb - Rot3 bRn = Rot3::ypr(yaw, pitch, roll).inverse(); + Rot3 bRn = Rot3::Ypr(yaw, pitch, roll).inverse(); return Mechanization_bRn2(bRn, x_g, x_a); } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index e7118a36c..6651c005f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -314,7 +314,7 @@ public: /// triangulate bool triangulateForLinearize(const Cameras& cameras) const { triangulateSafe(cameras); // imperative, might reset result_ - return (result_); + return bool(result_); } /// linearize returns a Hessianfactor that is an approximation of error(p) @@ -583,7 +583,7 @@ public: /// Is result valid? bool isValid() const { - return result_; + return bool(result_); } /** return the degenerate state */ diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index 407c9a345..99f494ad9 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -22,7 +22,7 @@ static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); static SharedNoiseModel sigma(noiseModel::Unit::Create(2)); // camera pose at (0,0,1) looking straight along the x-axis. -Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); SimpleCamera level_camera(level_pose, *K); typedef InvDepthFactor3 InverseDepthFactor; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 99a4f90a4..3aa758701 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -24,8 +24,8 @@ using namespace gtsam; TEST( InvDepthFactorVariant1, optimize) { // Create two poses looking in the x-direction - Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0)); - Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5)); + Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0)); + Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5)); // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) Point3 landmark(5, 0, 1); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index 2c85b3dad..d20fc000c 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -24,8 +24,8 @@ using namespace gtsam; TEST( InvDepthFactorVariant2, optimize) { // Create two poses looking in the x-direction - Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); - Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); + Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); + Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) Point3 landmark(5, 0, 1); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index ec9aa90c3..a6eed54d5 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -24,8 +24,8 @@ using namespace gtsam; TEST( InvDepthFactorVariant3, optimize) { // Create two poses looking in the x-direction - Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); - Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); + Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); + Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) Point3 landmark(5, 0, 1); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index c6ad9a38b..a06c39182 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -18,7 +18,7 @@ SharedNoiseModel model1 = noiseModel::Unit::Create(1); const double tol = 1e-5; const Pose3 pose1(Rot3(), Point3(2.0, 3.0, 4.0)); -const Pose3 pose2(Rot3::pitch(-M_PI_2), Point3(2.0, 3.0, 4.0)); +const Pose3 pose2(Rot3::Pitch(-M_PI_2), Point3(2.0, 3.0, 4.0)); const Pose3 pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(2.0, 3.0, 4.0)); const Point3 point1(3.0, 4.0, 2.0); const gtsam::Key poseKey = 1, pointKey = 2; diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 2981f675d..5bad0e171 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -115,7 +115,7 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) { /* *************************************************************************/ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); @@ -154,7 +154,7 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, noisy ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); @@ -172,7 +172,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { Values values; values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); values.insert(x2, level_pose_right.compose(noise_pose)); @@ -206,7 +206,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); // create second camera 1 meter to the right of first camera @@ -257,8 +257,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -354,7 +354,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); @@ -397,8 +397,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -422,7 +422,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); @@ -466,8 +466,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -490,7 +490,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); @@ -548,7 +548,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -587,7 +587,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K); // // create second camera 1 meter to the right of first camera // Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); @@ -626,8 +626,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // graph.push_back(PriorFactor(x1, pose1, noisePrior)); // graph.push_back(PriorFactor(x2, pose2, noisePrior)); // -// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise +// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -648,7 +648,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -684,7 +684,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // graph.push_back(PriorFactor(x1, pose1, noisePrior)); // graph.push_back(PriorFactor(x2, pose2, noisePrior)); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -708,7 +708,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera @@ -754,7 +754,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise values.insert(x3, pose3 * noise_pose); @@ -796,7 +796,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -835,7 +835,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); // graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2*noise_pose); @@ -862,7 +862,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K); // // // create second camera 1 meter to the right of first camera @@ -908,8 +908,8 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); // graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); // -// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise +// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -935,7 +935,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // views.push_back(x2); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -955,7 +955,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor()); // smartFactor1->add(measurements_cam1,views, model, K2); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -977,7 +977,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera @@ -1005,7 +1005,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { smartFactorInstance->linearize(values); // hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); @@ -1021,7 +1021,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); Values tranValues; @@ -1047,7 +1047,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); // Second and third cameras in same place, which is a degenerate configuration @@ -1072,7 +1072,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactor = smartFactor->linearize( values); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); @@ -1087,7 +1087,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-6)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); Values tranValues; diff --git a/gtsampy.h b/gtsampy.h new file mode 100644 index 000000000..ca014ad5d --- /dev/null +++ b/gtsampy.h @@ -0,0 +1,2669 @@ +/** + + * GTSAM Wrap Module Definition + * + * These are the current classes available through the matlab toolbox interface, + * add more functions/classes as they are available. + * + * Requirements: + * Classes must start with an uppercase letter + * - Can wrap a typedef + * Only one Method/Constructor per line, though methods/constructors can extend across multiple lines + * Methods can return + * - Eigen types: Matrix, Vector + * - C/C++ basic types: string, bool, size_t, int, double, char, unsigned char + * - void + * - Any class with which be copied with boost::make_shared() + * - boost::shared_ptr of any object type + * Constructors + * - Overloads are supported + * - A class with no constructors can be returned from other functions but not allocated directly in MATLAB + * Methods + * - Constness has no effect + * - Specify by-value (not reference) return types, even if C++ method returns reference + * - Must start with a letter (upper or lowercase) + * - Overloads are supported + * Static methods + * - Must start with a letter (upper or lowercase) and use the "static" keyword + * - The first letter will be made uppercase in the generated MATLAB interface + * - Overloads are supported + * Arguments to functions any of + * - Eigen types: Matrix, Vector + * - Eigen types and classes as an optionally const reference + * - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char + * - Any class with which be copied with boost::make_shared() (except Eigen) + * - boost::shared_ptr of any object type (except Eigen) + * Comments can use either C++ or C style, with multiple lines + * Namespace definitions + * - Names of namespaces must start with a lowercase letter + * - start a namespace with "namespace {" + * - end a namespace with exactly "}" + * - Namespaces can be nested + * Namespace usage + * - Namespaces can be specified for classes in arguments and return values + * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" + * Includes in C++ wrappers + * - All includes will be collected and added in a single file + * - All namespaces must have angle brackets: + * - No default includes will be added + * Global/Namespace functions + * - Functions specified outside of a class are global + * - Can be overloaded with different arguments + * - Can have multiple functions of the same name in different namespaces + * Using classes defined in other modules + * - If you are using a class 'OtherClass' not wrapped in this definition file, add "class OtherClass;" to avoid a dependency error + * Virtual inheritance + * - Specify fully-qualified base classes, i.e. "virtual class Derived : ns::Base {" where "ns" is the namespace + * - Mark with 'virtual' keyword, e.g. "virtual class Base {", and also "virtual class Derived : ns::Base {" + * - Forward declarations must also be marked virtual, e.g. "virtual class ns::Base;" and + * also "virtual class ns::Derived;" + * - Pure virtual (abstract) classes should list no constructors in this interface file + * - Virtual classes must have a clone() function in C++ (though it does not have to be included + * in the MATLAB interface). clone() will be called whenever an object copy is needed, instead + * of using the copy constructor (which is used for non-virtual objects). + * - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree + * virtual boost::shared_ptr clone() const; + * Class Templates + * - Basic templates are supported either with an explicit list of types to instantiate, + * e.g. template class Class1 { ... }; + * or with typedefs, e.g. + * template class Class2 { ... }; + * typedef Class2 MyInstantiatedClass; + * - In the class definition, appearances of the template argument(s) will be replaced with their + * instantiated types, e.g. 'void setValue(const T& value);'. + * - To refer to the instantiation of the template class itself, use 'This', i.e. 'static This Create();' + * - To create new instantiations in other modules, you must copy-and-paste the whole class definition + * into the new module, but use only your new instantiation types. + * - When forward-declaring template instantiations, use the generated/typedefed name, e.g. + * class gtsam::Class1Pose2; + * class gtsam::MyInstantiatedClass; + * Boost.serialization within Matlab: + * - you need to mark classes as being serializable in the markup file (see this file for an example). + * - There are two options currently, depending on the class. To "mark" a class as serializable, + * add a function with a particular signature so that wrap will catch it. + * - Add "void serialize()" to a class to create serialization functions for a class. + * Adding this flag subsumes the serializable() flag below. Requirements: + * - A default constructor must be publicly accessible + * - Must not be an abstract base class + * - The class must have an actual boost.serialization serialize() function. + * - Add "void serializable()" to a class if you only want the class to be serialized as a + * part of a container (such as noisemodel). This version does not require a publicly + * accessible default constructor. + */ + +/** + * Status: + * - TODO: default values for arguments + * - WORKAROUND: make multiple versions of the same function for different configurations of default arguments + * - TODO: Handle gtsam::Rot3M conversions to quaternions + * - TODO: Parse return of const ref arguments + * - TODO: Parse std::string variants and convert directly to special string + * - TODO: Add enum support + * - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load + */ + +namespace std { + #include + template + class vector + { + //Do we need these? + //Capacity + /*size_t size() const; + size_t max_size() const; + //void resize(size_t sz); + size_t capacity() const; + bool empty() const; + void reserve(size_t n); + + //Element access + T* at(size_t n); + T* front(); + T* back(); + + //Modifiers + void assign(size_t n, const T& u); + void push_back(const T& x); + void pop_back();*/ + }; + //typedef std::vector + + #include + template + class list + { + + + }; + +} + +namespace gtsam { + +//************************************************************************* +// base +//************************************************************************* + +/** gtsam namespace functions */ +bool linear_independent(Matrix A, Matrix B, double tol); + +virtual class Value { + // No constructors because this is an abstract class + + // Testable + void print(string s) const; + + // Manifold + size_t dim() const; +}; + +#include +class LieScalar { + // Standard constructors + LieScalar(); + LieScalar(double d); + + // Standard interface + double value() const; + + // Testable + void print(string s) const; + bool equals(const gtsam::LieScalar& expected, double tol) const; + + // Group + static gtsam::LieScalar identity(); + gtsam::LieScalar inverse() const; + gtsam::LieScalar compose(const gtsam::LieScalar& p) const; + gtsam::LieScalar between(const gtsam::LieScalar& l2) const; + + // Manifold + size_t dim() const; + gtsam::LieScalar retract(Vector v) const; + Vector localCoordinates(const gtsam::LieScalar& t2) const; + + // Lie group + static gtsam::LieScalar Expmap(Vector v); + static Vector Logmap(const gtsam::LieScalar& p); +}; + +#include +class LieVector { + // Standard constructors + LieVector(); + LieVector(Vector v); + + // Standard interface + Vector vector() const; + + // Testable + void print(string s) const; + bool equals(const gtsam::LieVector& expected, double tol) const; + + // Group + static gtsam::LieVector identity(); + gtsam::LieVector inverse() const; + gtsam::LieVector compose(const gtsam::LieVector& p) const; + gtsam::LieVector between(const gtsam::LieVector& l2) const; + + // Manifold + size_t dim() const; + gtsam::LieVector retract(Vector v) const; + Vector localCoordinates(const gtsam::LieVector& t2) const; + + // Lie group + static gtsam::LieVector Expmap(Vector v); + static Vector Logmap(const gtsam::LieVector& p); + + // enabling serialization functionality + void serialize() const; +}; + +#include +class LieMatrix { + // Standard constructors + LieMatrix(); + LieMatrix(Matrix v); + + // Standard interface + Matrix matrix() const; + + // Testable + void print(string s) const; + bool equals(const gtsam::LieMatrix& expected, double tol) const; + + // Group + static gtsam::LieMatrix identity(); + gtsam::LieMatrix inverse() const; + gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const; + gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const; + + // Manifold + size_t dim() const; + gtsam::LieMatrix retract(Vector v) const; + Vector localCoordinates(const gtsam::LieMatrix & t2) const; + + // Lie group + static gtsam::LieMatrix Expmap(Vector v); + static Vector Logmap(const gtsam::LieMatrix& p); + + // enabling serialization functionality + void serialize() const; +}; + +//************************************************************************* +// geometry +//************************************************************************* + +class Point2 { + // Standard Constructors + Point2(); + Point2(double x, double y); + Point2(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Point2& pose, double tol) const; + + // Group + static gtsam::Point2 identity(); + gtsam::Point2 inverse() const; + gtsam::Point2 compose(const gtsam::Point2& p2) const; + gtsam::Point2 between(const gtsam::Point2& p2) const; + + // Manifold + gtsam::Point2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Point2& p) const; + + // Lie Group + static gtsam::Point2 Expmap(Vector v); + static Vector Logmap(const gtsam::Point2& p); + + // Standard Interface + double x() const; + double y() const; + Vector vector() const; + double dist(const gtsam::Point2& p2) const; + double norm() const; + + // enabling serialization functionality + void serialize() const; +}; + +// std::vector +class Point2Vector +{ + // Constructors + Point2Vector(); + Point2Vector(const gtsam::Point2Vector& v); + + //Capacity + size_t size() const; + size_t max_size() const; + void resize(size_t sz); + size_t capacity() const; + bool empty() const; + void reserve(size_t n); + + //Element access + gtsam::Point2 at(size_t n) const; + gtsam::Point2 front() const; + gtsam::Point2 back() const; + + //Modifiers + void assign(size_t n, const gtsam::Point2& u); + void push_back(const gtsam::Point2& x); + void pop_back(); +}; + +class StereoPoint2 { + // Standard Constructors + StereoPoint2(); + StereoPoint2(double uL, double uR, double v); + + // Testable + void print(string s) const; + bool equals(const gtsam::StereoPoint2& point, double tol) const; + + // Group + static gtsam::StereoPoint2 identity(); + gtsam::StereoPoint2 inverse() const; + gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; + gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; + + // Manifold + gtsam::StereoPoint2 retract(Vector v) const; + Vector localCoordinates(const gtsam::StereoPoint2& p) const; + + // Lie Group + static gtsam::StereoPoint2 Expmap(Vector v); + static Vector Logmap(const gtsam::StereoPoint2& p); + + // Standard Interface + Vector vector() const; + double uL() const; + double uR() const; + double v() const; + + // enabling serialization functionality + void serialize() const; +}; + +class Point3 { + // Standard Constructors + Point3(); + Point3(double x, double y, double z); + Point3(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Point3& p, double tol) const; + + // Group + static gtsam::Point3 identity(); + gtsam::Point3 inverse() const; + gtsam::Point3 compose(const gtsam::Point3& p2) const; + gtsam::Point3 between(const gtsam::Point3& p2) const; + + // Manifold + gtsam::Point3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Point3& p) const; + + // Lie Group + static gtsam::Point3 Expmap(Vector v); + static Vector Logmap(const gtsam::Point3& p); + + // Standard Interface + Vector vector() const; + double x() const; + double y() const; + double z() const; + + // enabling serialization functionality + void serialize() const; +}; + +class Rot2 { + // Standard Constructors and Named Constructors + Rot2(); + Rot2(double theta); + static gtsam::Rot2 fromAngle(double theta); + static gtsam::Rot2 fromDegrees(double theta); + static gtsam::Rot2 fromCosSin(double c, double s); + + // Testable + void print(string s) const; + bool equals(const gtsam::Rot2& rot, double tol) const; + + // Group + static gtsam::Rot2 identity(); + gtsam::Rot2 inverse(); + gtsam::Rot2 compose(const gtsam::Rot2& p2) const; + gtsam::Rot2 between(const gtsam::Rot2& p2) const; + + // Manifold + gtsam::Rot2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot2& p) const; + + // Lie Group + static gtsam::Rot2 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot2& p); + + // Group Action on Point2 + gtsam::Point2 rotate(const gtsam::Point2& point) const; + gtsam::Point2 unrotate(const gtsam::Point2& point) const; + + // Standard Interface + static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative + static gtsam::Rot2 atan2(double y, double x); + double theta() const; + double degrees() const; + double c() const; + double s() const; + Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; +}; + +class Rot3 { + // Standard Constructors and Named Constructors + Rot3(); + Rot3(Matrix R); + static gtsam::Rot3 Rx(double t); + static gtsam::Rot3 Ry(double t); + static gtsam::Rot3 Rz(double t); + static gtsam::Rot3 RzRyRx(double x, double y, double z); + static gtsam::Rot3 RzRyRx(Vector xyz); + static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading) + static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) + static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft) + static gtsam::Rot3 ypr(double y, double p, double r); + static gtsam::Rot3 quaternion(double w, double x, double y, double z); + static gtsam::Rot3 Rodrigues(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Rot3& rot, double tol) const; + + // Group + static gtsam::Rot3 identity(); + gtsam::Rot3 inverse() const; + gtsam::Rot3 compose(const gtsam::Rot3& p2) const; + gtsam::Rot3 between(const gtsam::Rot3& p2) const; + + // Manifold + //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options + gtsam::Rot3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot3& p) const; + + // Group Action on Point3 + gtsam::Point3 rotate(const gtsam::Point3& p) const; + gtsam::Point3 unrotate(const gtsam::Point3& p) const; + + // Standard Interface + static gtsam::Rot3 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot3& p); + Matrix matrix() const; + Matrix transpose() const; + gtsam::Point3 column(size_t index) const; + Vector xyz() const; + Vector ypr() const; + Vector rpy() const; + double roll() const; + double pitch() const; + double yaw() const; +// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly + Vector quaternion() const; + + // enabling serialization functionality + void serialize() const; +}; + +class Pose2 { + // Standard Constructor + Pose2(); + Pose2(const gtsam::Pose2& pose); + Pose2(double x, double y, double theta); + Pose2(double theta, const gtsam::Point2& t); + Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); + Pose2(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Pose2& pose, double tol) const; + + // Group + static gtsam::Pose2 identity(); + gtsam::Pose2 inverse() const; + gtsam::Pose2 compose(const gtsam::Pose2& p2) const; + gtsam::Pose2 between(const gtsam::Pose2& p2) const; + + // Manifold + gtsam::Pose2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose2& p) const; + + // Lie Group + static gtsam::Pose2 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose2& p); + Matrix AdjointMap() const; + Vector Adjoint(const Vector& xi) const; + static Matrix wedge(double vx, double vy, double w); + + // Group Actions on Point2 + gtsam::Point2 transform_from(const gtsam::Point2& p) const; + gtsam::Point2 transform_to(const gtsam::Point2& p) const; + + // Standard Interface + double x() const; + double y() const; + double theta() const; + gtsam::Rot2 bearing(const gtsam::Point2& point) const; + double range(const gtsam::Point2& point) const; + gtsam::Point2 translation() const; + gtsam::Rot2 rotation() const; + Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; +}; + +class Pose3 { + // Standard Constructors + Pose3(); + Pose3(const gtsam::Pose3& pose); + Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); + Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) + Pose3(Matrix t); + + // Testable + void print(string s) const; + bool equals(const gtsam::Pose3& pose, double tol) const; + + // Group + static gtsam::Pose3 identity(); + gtsam::Pose3 inverse() const; + gtsam::Pose3 compose(const gtsam::Pose3& p2) const; + gtsam::Pose3 between(const gtsam::Pose3& p2) const; + + // Manifold + gtsam::Pose3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose3& T2) const; + + // Lie Group + static gtsam::Pose3 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose3& p); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); + + // Group Action on Point3 + gtsam::Point3 transform_from(const gtsam::Point3& p) const; + gtsam::Point3 transform_to(const gtsam::Point3& p) const; + + // Standard Interface + gtsam::Rot3 rotation() const; + gtsam::Point3 translation() const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& pose); + + // enabling serialization functionality + void serialize() const; +}; + +// std::vector +class Pose3Vector +{ + Pose3Vector(); + size_t size() const; + bool empty() const; + gtsam::Pose3 at(size_t n) const; + void push_back(const gtsam::Pose3& x); +}; + +#include +class Unit3 { + // Standard Constructors + Unit3(); + Unit3(const gtsam::Point3& pose); + + // Testable + void print(string s) const; + bool equals(const gtsam::Unit3& pose, double tol) const; + + // Other functionality + Matrix basis() const; + Matrix skew() const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Unit3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Unit3& s) const; +}; + +#include +class EssentialMatrix { + // Standard Constructors + EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); + + // Testable + void print(string s) const; + bool equals(const gtsam::EssentialMatrix& pose, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::EssentialMatrix retract(Vector v) const; + Vector localCoordinates(const gtsam::EssentialMatrix& s) const; + + // Other methods: + gtsam::Rot3 rotation() const; + gtsam::Unit3 direction() const; + Matrix matrix() const; + double error(Vector vA, Vector vB); +}; + +class Cal3_S2 { + // Standard Constructors + Cal3_S2(); + Cal3_S2(double fx, double fy, double s, double u0, double v0); + Cal3_S2(Vector v); + Cal3_S2(double fov, int w, int h); + + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3_S2& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3_S2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3_S2& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Matrix matrix() const; + Matrix matrix_inverse() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class Cal3DS2_Base { + // Standard Constructors + Cal3DS2_Base(); + + // Testable + void print(string s) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + double k1() const; + double k2() const; + + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class Cal3DS2 : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3DS2(); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); + Cal3DS2(Vector v); + + // Testable + bool equals(const gtsam::Cal3DS2& rhs, double tol) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3DS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2& c) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class Cal3Unified : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3Unified(); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); + Cal3Unified(Vector v); + + // Testable + bool equals(const gtsam::Cal3Unified& rhs, double tol) const; + + // Standard Interface + double xi() const; + gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; + gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3Unified retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Unified& c) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class Cal3_S2Stereo { + // Standard Constructors + Cal3_S2Stereo(); + Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); + Cal3_S2Stereo(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + double baseline() const; +}; + +#include +class Cal3Bundler { + // Standard Constructors + Cal3Bundler(); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Bundler retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Bundler& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double k1() const; + double k2() const; + double u0() const; + double v0() const; + Vector vector() const; + Vector k() const; + //Matrix K() const; //FIXME: Uppercase + + // enabling serialization functionality + void serialize() const; +}; + +class CalibratedCamera { + // Standard Constructors and Named Constructors + CalibratedCamera(); + CalibratedCamera(const gtsam::Pose3& pose); + CalibratedCamera(const Vector& v); + static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); + + // Testable + void print(string s) const; + bool equals(const gtsam::CalibratedCamera& camera, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::CalibratedCamera retract(const Vector& d) const; + Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; + + // Action on Point3 + gtsam::Point2 project(const gtsam::Point3& point) const; + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + + // Standard Interface + gtsam::Pose3 pose() const; + double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods + + // enabling serialization functionality + void serialize() const; +}; + +template +class PinholeCamera { + // Standard Constructors and Named Constructors + PinholeCamera(); + PinholeCamera(const gtsam::Pose3& pose); + PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); + static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); + static This Level(const gtsam::Pose2& pose, double height); + static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const CALIBRATION& K); + + // Testable + void print(string s) const; + bool equals(const This& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + CALIBRATION calibration() const; + + // Manifold + This retract(const Vector& d) const; + Vector localCoordinates(const This& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& point); + + // enabling serialization functionality + void serialize() const; +}; + +virtual class SimpleCamera { + // Standard Constructors and Named Constructors + SimpleCamera(); + SimpleCamera(const gtsam::Pose3& pose); + SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); + static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height); + static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); + static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const gtsam::Cal3_S2& K); + + // Testable + void print(string s) const; + bool equals(const gtsam::SimpleCamera& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + gtsam::Cal3_S2 calibration() const; + + // Manifold + gtsam::SimpleCamera retract(const Vector& d) const; + Vector localCoordinates(const gtsam::SimpleCamera& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& point); + + // enabling serialization functionality + void serialize() const; + +}; + +// Some typedefs for common camera types +// PinholeCameraCal3_S2 is the same as SimpleCamera above +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; + +class StereoCamera { + // Standard Constructors and Named Constructors + StereoCamera(); + StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); + + // Testable + void print(string s) const; + bool equals(const gtsam::StereoCamera& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + double baseline() const; + gtsam::Cal3_S2Stereo calibration() const; + + // Manifold + gtsam::StereoCamera retract(const Vector& d) const; + Vector localCoordinates(const gtsam::StereoCamera& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + gtsam::StereoPoint2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include + +// Templates appear not yet supported for free functions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); + +//************************************************************************* +// Symbolic +//************************************************************************* + +#include +virtual class SymbolicFactor { + // Standard Constructors and Named Constructors + SymbolicFactor(const gtsam::SymbolicFactor& f); + SymbolicFactor(); + SymbolicFactor(size_t j); + SymbolicFactor(size_t j1, size_t j2); + SymbolicFactor(size_t j1, size_t j2, size_t j3); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); + static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); + + // From Factor + size_t size() const; + void print(string s) const; + bool equals(const gtsam::SymbolicFactor& other, double tol) const; + gtsam::KeyVector keys(); +}; + +#include +virtual class SymbolicFactorGraph { + SymbolicFactorGraph(); + SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); + SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); + + // From FactorGraph + void push_back(gtsam::SymbolicFactor* factor); + void print(string s) const; + bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; + size_t size() const; + bool exists(size_t idx) const; + + // Standard interface + gtsam::KeySet keys() const; + void push_back(gtsam::SymbolicFactor* factor); + void push_back(const gtsam::SymbolicFactorGraph& graph); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); + void push_back(const gtsam::SymbolicBayesTree& bayesTree); + + //Advanced Interface + void push_factor(size_t key); + void push_factor(size_t key1, size_t key2); + void push_factor(size_t key1, size_t key2, size_t key3); + void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); + + gtsam::SymbolicBayesNet* eliminateSequential(); + gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::SymbolicBayesTree* eliminateMultifrontal(); + gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector& keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering& ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector& keys); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& variables); +}; + +#include +virtual class SymbolicConditional : gtsam::SymbolicFactor { + // Standard Constructors and Named Constructors + SymbolicConditional(); + SymbolicConditional(const gtsam::SymbolicConditional& other); + SymbolicConditional(size_t key); + SymbolicConditional(size_t key, size_t parent); + SymbolicConditional(size_t key, size_t parent1, size_t parent2); + SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); + static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); + + // Testable + void print(string s) const; + bool equals(const gtsam::SymbolicConditional& other, double tol) const; + + // Standard interface + size_t nrFrontals() const; + size_t nrParents() const; +}; + +#include +class SymbolicBayesNet { + SymbolicBayesNet(); + SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); + // Testable + void print(string s) const; + bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; + + // Standard interface + size_t size() const; + void saveGraph(string s) const; + gtsam::SymbolicConditional* at(size_t idx) const; + gtsam::SymbolicConditional* front() const; + gtsam::SymbolicConditional* back() const; + void push_back(gtsam::SymbolicConditional* conditional); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); +}; + +#include +class SymbolicBayesTree { + + //Constructors + SymbolicBayesTree(); + SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); + + // Testable + void print(string s); + bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; + + //Standard Interface + //size_t findParentClique(const gtsam::IndexVector& parents) const; + size_t size(); + void saveGraph(string s) const; + void clear(); + void deleteCachedShortcuts(); + size_t numCachedSeparatorMarginals() const; + + gtsam::SymbolicConditional* marginalFactor(size_t key) const; + gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; + +// class SymbolicBayesTreeClique { +// BayesTreeClique(); +// BayesTreeClique(CONDITIONAL* conditional); +// // BayesTreeClique(const std::pair& result) : Base(result) {} +// +// bool equals(const This& other, double tol) const; +// void print(string s) const; +// void printTree() const; // Default indent of "" +// void printTree(string indent) const; +// size_t numCachedSeparatorMarginals() const; +// +// CONDITIONAL* conditional() const; +// bool isRoot() const; +// size_t treeSize() const; +// // const std::list& children() const { return children_; } +// // derived_ptr parent() const { return parent_.lock(); } +// +// // FIXME: need wrapped versions graphs, BayesNet +// // BayesNet shortcut(derived_ptr root, Eliminate function) const; +// // FactorGraph marginal(derived_ptr root, Eliminate function) const; +// // FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; +// +// void deleteCachedShortcuts(); +// }; + +#include +class VariableIndex { + // Standard Constructors and Named Constructors + VariableIndex(); + // TODO: Templetize constructor when wrap supports it + //template + //VariableIndex(const T& factorGraph, size_t nVariables); + //VariableIndex(const T& factorGraph); + VariableIndex(const gtsam::SymbolicFactorGraph& factorGraph); + VariableIndex(const gtsam::GaussianFactorGraph& factorGraph); + VariableIndex(const gtsam::NonlinearFactorGraph& factorGraph); + VariableIndex(const gtsam::VariableIndex& other); + + // Testable + bool equals(const gtsam::VariableIndex& other, double tol) const; + void print(string s) const; + + // Standard interface + size_t size() const; + size_t nFactors() const; + size_t nEntries() const; +}; + +//************************************************************************* +// linear +//************************************************************************* + +namespace noiseModel { +#include +virtual class Base { +}; + +virtual class Gaussian : gtsam::noiseModel::Base { + static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); + static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + Matrix R() const; + bool equals(gtsam::noiseModel::Base& expected, double tol); + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Diagonal : gtsam::noiseModel::Gaussian { + static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); + static gtsam::noiseModel::Diagonal* Variances(Vector variances); + static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); + Matrix R() const; + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Constrained : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas); + static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas); + static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances); + static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances); + static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions); + static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions); + + static gtsam::noiseModel::Constrained* All(size_t dim); + static gtsam::noiseModel::Constrained* All(size_t dim, double mu); + + gtsam::noiseModel::Constrained* unit() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Isotropic : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); + static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); + static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Unit : gtsam::noiseModel::Isotropic { + static gtsam::noiseModel::Unit* Create(size_t dim); + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +namespace mEstimator { +virtual class Base { +}; + +virtual class Null: gtsam::noiseModel::mEstimator::Base { + Null(); + void print(string s) const; + static gtsam::noiseModel::mEstimator::Null* Create(); + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Fair: gtsam::noiseModel::mEstimator::Base { + Fair(double c); + void print(string s) const; + static gtsam::noiseModel::mEstimator::Fair* Create(double c); + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Huber: gtsam::noiseModel::mEstimator::Base { + Huber(double k); + void print(string s) const; + static gtsam::noiseModel::mEstimator::Huber* Create(double k); + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Tukey: gtsam::noiseModel::mEstimator::Base { + Tukey(double k); + void print(string s) const; + static gtsam::noiseModel::mEstimator::Tukey* Create(double k); + + // enabling serialization functionality + void serializable() const; +}; + +}///\namespace mEstimator + +virtual class Robust : gtsam::noiseModel::Base { + Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +}///\namespace noiseModel + +#include +class Sampler { + //Constructors + Sampler(gtsam::noiseModel::Diagonal* model, int seed); + Sampler(Vector sigmas, int seed); + Sampler(int seed); + + //Standard Interface + size_t dim() const; + Vector sigmas() const; + gtsam::noiseModel::Diagonal* model() const; + Vector sample(); + Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); +}; + +#include +class VectorValues { + //Constructors + VectorValues(); + VectorValues(const gtsam::VectorValues& other); + + //Named Constructors + static gtsam::VectorValues Zero(const gtsam::VectorValues& model); + + //Standard Interface + size_t size() const; + size_t dim(size_t j) const; + bool exists(size_t j) const; + void print(string s) const; + bool equals(const gtsam::VectorValues& expected, double tol) const; + void insert(size_t j, Vector value); + Vector vector() const; + Vector at(size_t j) const; + void update(const gtsam::VectorValues& values); + + //Advanced Interface + void setZero(); + + gtsam::VectorValues add(const gtsam::VectorValues& c) const; + void addInPlace(const gtsam::VectorValues& c); + gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; + gtsam::VectorValues scale(double a) const; + void scaleInPlace(double a); + + bool hasSameStructure(const gtsam::VectorValues& other) const; + double dot(const gtsam::VectorValues& V) const; + double norm() const; + double squaredNorm() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianFactor { + gtsam::KeyVector keys() const; + void print(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + gtsam::GaussianFactor* clone() const; + gtsam::GaussianFactor* negate() const; + Matrix augmentedInformation() const; + Matrix information() const; + Matrix augmentedJacobian() const; + pair jacobian() const; + size_t size() const; + bool empty() const; +}; + +#include +virtual class JacobianFactor : gtsam::GaussianFactor { + //Constructors + JacobianFactor(); + JacobianFactor(const gtsam::GaussianFactor& factor); + JacobianFactor(Vector b_in); + JacobianFactor(size_t i1, Matrix A1, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + JacobianFactor(const gtsam::GaussianFactorGraph& graph); + + //Testable + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + size_t size() const; + Vector unweighted_error(const gtsam::VectorValues& c) const; + Vector error_vector(const gtsam::VectorValues& c) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + Matrix getA() const; + Vector getb() const; + size_t rows() const; + size_t cols() const; + bool isConstrained() const; + pair jacobianUnweighted() const; + Matrix augmentedJacobianUnweighted() const; + + void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; + gtsam::JacobianFactor whiten() const; + + pair eliminate(const gtsam::Ordering& keys) const; + + void setModel(bool anyConstrained, const Vector& sigmas); + + gtsam::noiseModel::Diagonal* get_model() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class HessianFactor : gtsam::GaussianFactor { + //Constructors + HessianFactor(); + HessianFactor(const gtsam::GaussianFactor& factor); + HessianFactor(size_t j, Matrix G, Vector g, double f); + HessianFactor(size_t j, Vector mu, Matrix Sigma); + HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, + Vector g2, double f); + HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, + Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, + double f); + HessianFactor(const gtsam::GaussianFactorGraph& factors); + + //Testable + size_t size() const; + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + size_t rows() const; + Matrix info() const; + double constantTerm() const; + Vector linearTerm() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class GaussianFactorGraph { + GaussianFactorGraph(); + GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); + GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); + + // From FactorGraph + void print(string s) const; + bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; + size_t size() const; + gtsam::GaussianFactor* at(size_t idx) const; + gtsam::KeySet keys() const; + bool exists(size_t idx) const; + + // Building the graph + void push_back(const gtsam::GaussianFactor* factor); + void push_back(const gtsam::GaussianConditional* factor); + void push_back(const gtsam::GaussianFactorGraph& graph); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + void push_back(const gtsam::GaussianBayesTree& bayesTree); + void add(const gtsam::GaussianFactor& factor); + void add(Vector b); + void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + + // error and probability + double error(const gtsam::VectorValues& c) const; + double probPrime(const gtsam::VectorValues& c) const; + + gtsam::GaussianFactorGraph clone() const; + gtsam::GaussianFactorGraph negate() const; + + // Optimizing and linear algebra + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + + // Elimination and marginals + gtsam::GaussianBayesNet* eliminateSequential(); + gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::GaussianBayesTree* eliminateMultifrontal(); + gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector& keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering& ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector& keys); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& variables); + + // Conversion to matrices + Matrix sparseJacobian_() const; + Matrix augmentedJacobian() const; + Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; + pair jacobian() const; + pair jacobian(const gtsam::Ordering& ordering) const; + Matrix augmentedHessian() const; + Matrix augmentedHessian(const gtsam::Ordering& ordering) const; + pair hessian() const; + pair hessian(const gtsam::Ordering& ordering) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianConditional : gtsam::GaussianFactor { + //Constructors + GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); + + //Constructors with no noise model + GaussianConditional(size_t key, Vector d, Matrix R); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T); + + //Standard Interface + void print(string s) const; + bool equals(const gtsam::GaussianConditional &cg, double tol) const; + + //Advanced Interface + gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; + gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; + void solveTransposeInPlace(gtsam::VectorValues& gy) const; + void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianDensity : gtsam::GaussianConditional { + //Constructors + GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + + //Standard Interface + void print(string s) const; + bool equals(const gtsam::GaussianDensity &cg, double tol) const; + Vector mean() const; + Matrix covariance() const; +}; + +#include +virtual class GaussianBayesNet { + //Constructors + GaussianBayesNet(); + GaussianBayesNet(const gtsam::GaussianConditional* conditional); + + // Testable + void print(string s) const; + bool equals(const gtsam::GaussianBayesNet& other, double tol) const; + size_t size() const; + + // FactorGraph derived interface + size_t size() const; + gtsam::GaussianConditional* at(size_t idx) const; + gtsam::KeySet keys() const; + bool exists(size_t idx) const; + + gtsam::GaussianConditional* front() const; + gtsam::GaussianConditional* back() const; + void push_back(gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; + gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; +}; + +#include +virtual class GaussianBayesTree { + // Standard Constructors and Named Constructors + GaussianBayesTree(); + GaussianBayesTree(const gtsam::GaussianBayesTree& other); + bool equals(const gtsam::GaussianBayesTree& other, double tol) const; + void print(string s); + size_t size() const; + bool empty() const; + size_t numCachedSeparatorMarginals() const; + void saveGraph(string s) const; + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + Matrix marginalCovariance(size_t key) const; + gtsam::GaussianConditional* marginalFactor(size_t key) const; + gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; + +class Errors { + //Constructors + Errors(); + Errors(const gtsam::VectorValues& V); + + //Testable + void print(string s); + bool equals(const gtsam::Errors& expected, double tol) const; +}; + +class GaussianISAM { + //Constructor + GaussianISAM(); + + //Standard Interface + void update(const gtsam::GaussianFactorGraph& newFactors); + void saveGraph(string s) const; + void clear(); +}; + +#include +virtual class IterativeOptimizationParameters { + string getVerbosity() const; + void setVerbosity(string s) ; + void print() const; +}; + +//virtual class IterativeSolver { +// IterativeSolver(); +// gtsam::VectorValues optimize (); +//}; + +#include +virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { + ConjugateGradientParameters(); + int getMinIterations() const ; + int getMaxIterations() const ; + int getReset() const; + double getEpsilon_rel() const; + double getEpsilon_abs() const; + + void setMinIterations(int value); + void setMaxIterations(int value); + void setReset(int value); + void setEpsilon_rel(double value); + void setEpsilon_abs(double value); + void print(); +}; + +#include +virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { + SubgraphSolverParameters(); + void print() const; +}; + +virtual class SubgraphSolver { + SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + gtsam::VectorValues optimize() const; +}; + +#include +class KalmanFilter { + KalmanFilter(size_t n); + // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); + gtsam::GaussianDensity* init(Vector x0, Matrix P0); + void print(string s) const; + static size_t step(gtsam::GaussianDensity* p); + gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); + gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, Matrix Q); + gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, + Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, + Vector z, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, + Vector z, Matrix Q); +}; + +//************************************************************************* +// nonlinear +//************************************************************************* + +#include +size_t symbol(char chr, size_t index); +char symbolChr(size_t key); +size_t symbolIndex(size_t key); + +// Default keyformatter +void PrintKeyList (const gtsam::KeyList& keys); +void PrintKeyList (const gtsam::KeyList& keys, string s); +void PrintKeyVector(const gtsam::KeyVector& keys); +void PrintKeyVector(const gtsam::KeyVector& keys, string s); +void PrintKeySet (const gtsam::KeySet& keys); +void PrintKeySet (const gtsam::KeySet& keys, string s); + +#include +class LabeledSymbol { + LabeledSymbol(size_t full_key); + LabeledSymbol(const gtsam::LabeledSymbol& key); + LabeledSymbol(unsigned char valType, unsigned char label, size_t j); + + size_t key() const; + unsigned char label() const; + unsigned char chr() const; + size_t index() const; + + gtsam::LabeledSymbol upper() const; + gtsam::LabeledSymbol lower() const; + gtsam::LabeledSymbol newChr(unsigned char c) const; + gtsam::LabeledSymbol newLabel(unsigned char label) const; + + void print(string s) const; +}; + +size_t mrsymbol(unsigned char c, unsigned char label, size_t j); +unsigned char mrsymbolChr(size_t key); +unsigned char mrsymbolLabel(size_t key); +size_t mrsymbolIndex(size_t key); + +#include +class Ordering { + // Standard Constructors and Named Constructors + Ordering(); + Ordering(const gtsam::Ordering& other); + + // Testable + void print(string s) const; + bool equals(const gtsam::Ordering& ord, double tol) const; + + // Standard interface + size_t size() const; + size_t at(size_t key) const; + void push_back(size_t key); + + // enabling serialization functionality + void serialize() const; +}; + +class NonlinearFactorGraph { + NonlinearFactorGraph(); + NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); + + // FactorGraph + void print(string s) const; + bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; + size_t size() const; + bool empty() const; + void remove(size_t i); + size_t nrFactors() const; + gtsam::NonlinearFactor* at(size_t idx) const; + void push_back(const gtsam::NonlinearFactorGraph& factors); + void push_back(gtsam::NonlinearFactor* factor); + void add(gtsam::NonlinearFactor* factor); + bool exists(size_t idx) const; + gtsam::KeySet keys() const; + + // NonlinearFactorGraph + double error(const gtsam::Values& values) const; + double probPrime(const gtsam::Values& values) const; + gtsam::Ordering orderingCOLAMD() const; + // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; + gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; + gtsam::NonlinearFactorGraph clone() const; + + // enabling serialization functionality + void serialize() const; +}; + +virtual class NonlinearFactor { + // Factor base class + size_t size() const; + gtsam::KeyVector keys() const; + void print(string s) const; + void printKeys(string s) const; + // NonlinearFactor + void equals(const gtsam::NonlinearFactor& other, double tol) const; + double error(const gtsam::Values& c) const; + size_t dim() const; + bool active(const gtsam::Values& c) const; + gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; + gtsam::NonlinearFactor* clone() const; + // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen +}; + +virtual class NoiseModelFactor: gtsam::NonlinearFactor { + void equals(const gtsam::NoiseModelFactor& other, double tol) const; + gtsam::noiseModel::Base* get_noiseModel() const; + Vector unwhitenedError(const gtsam::Values& x) const; + Vector whitenedError(const gtsam::Values& x) const; +}; + +#include +class Values { + Values(); + Values(const gtsam::Values& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t dim() const; + + void print(string s) const; + bool equals(const gtsam::Values& other, double tol) const; + + void insert(const gtsam::Values& values); + void update(const gtsam::Values& values); + void erase(size_t j); + void swap(gtsam::Values& values); + + bool exists(size_t j) const; + gtsam::KeyVector keys() const; + + gtsam::VectorValues zeroVectors() const; + + gtsam::Values retract(const gtsam::VectorValues& delta) const; + gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; + + // enabling serialization functionality + void serialize() const; + + // New in 4.0, we have to specialize every insert/update/at to generate wrappers + // Instead of the old: + // void insert(size_t j, const gtsam::Value& value); + // void update(size_t j, const gtsam::Value& val); + // gtsam::Value at(size_t j) const; + + void insert(size_t j, const gtsam::Point2& t); + void insert(size_t j, const gtsam::Point3& t); + void insert(size_t j, const gtsam::Rot2& t); + void insert(size_t j, const gtsam::Pose2& t); + void insert(size_t j, const gtsam::Rot3& t); + void insert(size_t j, const gtsam::Pose3& t); + void insert(size_t j, const gtsam::Cal3_S2& t); + void insert(size_t j, const gtsam::Cal3DS2& t); + void insert(size_t j, const gtsam::Cal3Bundler& t); + void insert(size_t j, const gtsam::EssentialMatrix& t); + void insert(size_t j, const gtsam::SimpleCamera& t); + void insert(size_t j, const gtsam::imuBias::ConstantBias& t); + void insert(size_t j, Vector t); + void insert(size_t j, Matrix t); + + // Fixed size version + void insertFixed(size_t j, Vector t, size_t n); + + void update(size_t j, const gtsam::Point2& t); + void update(size_t j, const gtsam::Point3& t); + void update(size_t j, const gtsam::Rot2& t); + void update(size_t j, const gtsam::Pose2& t); + void update(size_t j, const gtsam::Rot3& t); + void update(size_t j, const gtsam::Pose3& t); + void update(size_t j, const gtsam::Cal3_S2& t); + void update(size_t j, const gtsam::Cal3DS2& t); + void update(size_t j, const gtsam::Cal3Bundler& t); + void update(size_t j, const gtsam::EssentialMatrix& t); + void update(size_t j, const gtsam::imuBias::ConstantBias& t); + void update(size_t j, Vector t); + void update(size_t j, Matrix t); + + template + T at(size_t j); + + /// Fixed size versions, for n in 1..9 + void insertFixed(size_t j, Vector t, size_t n); + + /// Fixed size versions, for n in 1..9 + Vector atFixed(size_t j, size_t n); + + /// version for double + void insertDouble(size_t j, double c); + double atDouble(size_t j) const; +}; + +// Actually a FastList +#include +class KeyList { + KeyList(); + KeyList(const gtsam::KeyList& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t front() const; + size_t back() const; + void push_back(size_t key); + void push_front(size_t key); + void pop_back(); + void pop_front(); + void sort(); + void remove(size_t key); + + void serialize() const; +}; + +// Actually a FastSet +class KeySet { + KeySet(); + KeySet(const gtsam::KeySet& other); + KeySet(const gtsam::KeyVector& other); + KeySet(const gtsam::KeyList& other); + + // Testable + void print(string s) const; + bool equals(const gtsam::KeySet& other) const; + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(size_t key); + void merge(gtsam::KeySet& other); + bool erase(size_t key); // returns true if value was removed + bool count(size_t key) const; // returns true if value exists + + void serialize() const; +}; + +// Actually a vector +class KeyVector { + KeyVector(); + KeyVector(const gtsam::KeyVector& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t at(size_t i) const; + size_t front() const; + size_t back() const; + void push_back(size_t key) const; + + void serialize() const; +}; + +// Actually a FastMap +class KeyGroupMap { + KeyGroupMap(); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t at(size_t key) const; + int erase(size_t key); + bool insert2(size_t key, int val); +}; + +#include +class Marginals { + Marginals(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& solution); + + void print(string s) const; + Matrix marginalCovariance(size_t variable) const; + Matrix marginalInformation(size_t variable) const; + gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; + gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; +}; + +class JointMarginal { + Matrix at(size_t iVariable, size_t jVariable) const; + Matrix fullMatrix() const; + void print(string s) const; + void print() const; +}; + +#include +virtual class LinearContainerFactor : gtsam::NonlinearFactor { + + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); + LinearContainerFactor(gtsam::GaussianFactor* factor); + + gtsam::GaussianFactor* factor() const; +// const boost::optional& linearizationPoint() const; + + bool isJacobian() const; + gtsam::JacobianFactor* toJacobian() const; + gtsam::HessianFactor* toHessian() const; + + static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::Values& linearizationPoint); + + static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); + + // enabling serialization functionality + void serializable() const; +}; // \class LinearContainerFactor + +// Summarization functionality +//#include +// +//// Uses partial QR approach by default +//gtsam::GaussianFactorGraph summarize( +// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, +// const gtsam::KeySet& saved_keys); +// +//gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer( +// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, +// const gtsam::KeySet& saved_keys); + +//************************************************************************* +// Nonlinear optimizers +//************************************************************************* + +#include +#include +virtual class NonlinearOptimizerParams { + NonlinearOptimizerParams(); + void print(string s) const; + + int getMaxIterations() const; + double getRelativeErrorTol() const; + double getAbsoluteErrorTol() const; + double getErrorTol() const; + string getVerbosity() const; + + void setMaxIterations(int value); + void setRelativeErrorTol(double value); + void setAbsoluteErrorTol(double value); + void setErrorTol(double value); + void setVerbosity(string s); + + string getLinearSolverType() const; + + void setLinearSolverType(string solver); + void setOrdering(const gtsam::Ordering& ordering); + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); + + bool isMultifrontal() const; + bool isSequential() const; + bool isCholmod() const; + bool isIterative() const; +}; + +bool checkConvergence(double relativeErrorTreshold, + double absoluteErrorTreshold, double errorThreshold, + double currentError, double newError); + +#include +virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { + GaussNewtonParams(); +}; + +#include +virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { + LevenbergMarquardtParams(); + + double getlambdaInitial() const; + double getlambdaFactor() const; + double getlambdaUpperBound() const; + string getVerbosityLM() const; + + void setlambdaInitial(double value); + void setlambdaFactor(double value); + void setlambdaUpperBound(double value); + void setVerbosityLM(string s); +}; + +#include +virtual class DoglegParams : gtsam::NonlinearOptimizerParams { + DoglegParams(); + + double getDeltaInitial() const; + string getVerbosityDL() const; + + void setDeltaInitial(double deltaInitial) const; + void setVerbosityDL(string verbosityDL) const; +}; + +virtual class NonlinearOptimizer { + gtsam::Values optimize(); + gtsam::Values optimizeSafely(); + double error() const; + int iterations() const; + gtsam::Values values() const; + void iterate() const; +}; + +virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); +}; + +virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); + double getDelta() const; +}; + +virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); + double lambda() const; + void print(string str) const; +}; + +#include +class ISAM2GaussNewtonParams { + ISAM2GaussNewtonParams(); + + void print(string str) const; + + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); +}; + +class ISAM2DoglegParams { + ISAM2DoglegParams(); + + void print(string str) const; + + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); + double getInitialDelta() const; + void setInitialDelta(double initialDelta); + string getAdaptationMode() const; + void setAdaptationMode(string adaptationMode); + bool isVerbose() const; + void setVerbose(bool verbose); +}; + +class ISAM2ThresholdMapValue { + ISAM2ThresholdMapValue(char c, Vector thresholds); + ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); +}; + +class ISAM2ThresholdMap { + ISAM2ThresholdMap(); + ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(const gtsam::ISAM2ThresholdMapValue& value) const; +}; + +class ISAM2Params { + ISAM2Params(); + + void print(string str) const; + + /** Getters and Setters for all properties */ + void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& params); + void setOptimizationParams(const gtsam::ISAM2DoglegParams& params); + void setRelinearizeThreshold(double relinearizeThreshold); + void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& relinearizeThreshold); + int getRelinearizeSkip() const; + void setRelinearizeSkip(int relinearizeSkip); + bool isEnableRelinearization() const; + void setEnableRelinearization(bool enableRelinearization); + bool isEvaluateNonlinearError() const; + void setEvaluateNonlinearError(bool evaluateNonlinearError); + string getFactorization() const; + void setFactorization(string factorization); + bool isCacheLinearizedFactors() const; + void setCacheLinearizedFactors(bool cacheLinearizedFactors); + bool isEnableDetailedResults() const; + void setEnableDetailedResults(bool enableDetailedResults); + bool isEnablePartialRelinearizationCheck() const; + void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); +}; + +class ISAM2Clique { + + //Constructors + ISAM2Clique(); + + //Standard Interface + Vector gradientContribution() const; + void print(string s); +}; + +class ISAM2Result { + ISAM2Result(); + + void print(string str) const; + + /** Getters and Setters for all properties */ + size_t getVariablesRelinearized() const; + size_t getVariablesReeliminated() const; + size_t getCliques() const; +}; + +class ISAM2 { + ISAM2(); + ISAM2(const gtsam::ISAM2Params& params); + ISAM2(const gtsam::ISAM2& other); + + bool equals(const gtsam::ISAM2& other, double tol) const; + void print(string s) const; + void printStats() const; + void saveGraph(string s) const; + + gtsam::ISAM2Result update(); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); + // TODO: wrap the full version of update + //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys); + //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys, bool force_relinearize); + + gtsam::Values getLinearizationPoint() const; + gtsam::Values calculateEstimate() const; + gtsam::Value calculateEstimate(size_t key) const; + gtsam::Values calculateBestEstimate() const; + Matrix marginalCovariance(size_t key) const; + gtsam::VectorValues getDelta() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; + gtsam::VariableIndex getVariableIndex() const; + gtsam::ISAM2Params params() const; +}; + +#include +class NonlinearISAM { + NonlinearISAM(); + NonlinearISAM(int reorderInterval); + void print(string s) const; + void printStats() const; + void saveGraph(string s) const; + gtsam::Values estimate() const; + Matrix marginalCovariance(size_t key) const; + int reorderInterval() const; + int reorderCounter() const; + void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues); + void reorder_relinearize(); + + // These might be expensive as instead of a reference the wrapper will make a copy + gtsam::GaussianISAM bayesTree() const; + gtsam::Values getLinearizationPoint() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; +}; + +//************************************************************************* +// Nonlinear factor types +//************************************************************************* +#include +#include +#include + +#include +template +virtual class PriorFactor : gtsam::NoiseModelFactor { + PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + T prior() const; + + // enabling serialization functionality + void serialize() const; +}; + + +#include +template +virtual class BetweenFactor : gtsam::NoiseModelFactor { + BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); + T measured() const; + + // enabling serialization functionality + void serialize() const; +}; + + + +#include +template +virtual class NonlinearEquality : gtsam::NoiseModelFactor { + // Constructor - forces exact evaluation + NonlinearEquality(size_t j, const T& feasible); + // Constructor - allows inexact evaluation + NonlinearEquality(size_t j, const T& feasible, double error_gain); + + // enabling serialization functionality + void serialize() const; +}; + + +#include +template +virtual class RangeFactor : gtsam::NoiseModelFactor { + RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); +}; + +typedef gtsam::RangeFactor RangeFactorPosePoint2; +typedef gtsam::RangeFactor RangeFactorPosePoint3; +typedef gtsam::RangeFactor RangeFactorPose2; +typedef gtsam::RangeFactor RangeFactorPose3; +typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; +typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; +typedef gtsam::RangeFactor RangeFactorCalibratedCamera; +typedef gtsam::RangeFactor RangeFactorSimpleCamera; + + +#include +template +virtual class BearingFactor : gtsam::NoiseModelFactor { + BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::BearingFactor BearingFactor2D; + +#include +template +virtual class BearingRangeFactor : gtsam::NoiseModelFactor { + BearingRangeFactor(size_t poseKey, size_t pointKey, + const BEARING& measuredBearing, const RANGE& measuredRange, + const gtsam::noiseModel::Base* noiseModel); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::BearingRangeFactor BearingRangeFactor2D; + + +#include +template +virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k); + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor); + + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality, + const POSE& body_P_sensor); + + gtsam::Point2 measured() const; + CALIBRATION* calibration() const; + bool verboseCheirality() const; + bool throwCheirality() const; + + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; + + +#include +template +virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { + GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); + gtsam::Point2 measured() const; +}; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; + +template +virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { + GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); + gtsam::Point2 measured() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class SmartProjectionParams { + SmartProjectionParams(); + // TODO(frank): make these work: + // void setLinearizationMode(LinearizationMode linMode); + // void setDegeneracyMode(DegeneracyMode degMode); + void setRankTolerance(double rankTol); + void setEnableEPI(bool enableEPI); + void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); + void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); +}; + +#include +template +virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { + + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor, + const gtsam::SmartProjectionParams& params); + + void add(const gtsam::Point2& measured_i, size_t poseKey_i); + + // enabling serialization functionality + //void serialize() const; +}; + +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; + + +#include +template +virtual class GenericStereoFactor : gtsam::NoiseModelFactor { + GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); + gtsam::StereoPoint2 measured() const; + gtsam::Cal3_S2Stereo* calibration() const; + + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::GenericStereoFactor GenericStereoFactor3D; + +#include +template +virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { + PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); +}; + +typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; +typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; + +#include +template +virtual class PoseRotationPrior : gtsam::NoiseModelFactor { + PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); +}; + +typedef gtsam::PoseRotationPrior PoseRotationPrior2D; +typedef gtsam::PoseRotationPrior PoseRotationPrior3D; + +#include +virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { + EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* noiseModel); +}; + +#include +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model); +pair load2D(string filename); +pair load2D_robust(string filename, + gtsam::noiseModel::Base* model); +void save2D(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, + string filename); + +pair readG2o(string filename); +void writeG2o(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& estimate, string filename); + +//************************************************************************* +// Navigation +//************************************************************************* +namespace imuBias { +#include + +class ConstantBias { + // Standard Constructor + ConstantBias(); + ConstantBias(Vector biasAcc, Vector biasGyro); + + // Testable + void print(string s) const; + bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; + + // Group + static gtsam::imuBias::ConstantBias identity(); + gtsam::imuBias::ConstantBias inverse() const; + gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; + gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; + + // Manifold + gtsam::imuBias::ConstantBias retract(Vector v) const; + Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; + + // Lie Group + static gtsam::imuBias::ConstantBias Expmap(Vector v); + static Vector Logmap(const gtsam::imuBias::ConstantBias& b); + + // Standard Interface + Vector vector() const; + Vector accelerometer() const; + Vector gyroscope() const; + Vector correctAccelerometer(Vector measurement) const; + Vector correctGyroscope(Vector measurement) const; +}; + +}///\namespace imuBias + +#include +class PoseVelocityBias{ + PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); + }; +class PreintegratedImuMeasurements { + // Standard Constructor + PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); + PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); + // PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); + + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + Matrix delPdelBiasAcc() const; + Matrix delPdelBiasOmega() const; + Matrix delVdelBiasAcc() const; + Matrix delVdelBiasOmega() const; + Matrix delRdelBiasOmega() const; + Matrix preintMeasCov() const; + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, + Vector gravity, Vector omegaCoriolis) const; +}; + +virtual class ImuFactor : gtsam::NonlinearFactor { + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, + const gtsam::Pose3& body_P_sensor); + // Standard Interface + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; +}; + +#include +class PreintegratedCombinedMeasurements { + // Standard Constructor + PreintegratedCombinedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit); + PreintegratedCombinedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit, + bool use2ndOrderIntegration); + // PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol); + + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + Matrix delPdelBiasAcc() const; + Matrix delPdelBiasOmega() const; + Matrix delVdelBiasAcc() const; + Matrix delVdelBiasOmega() const; + Matrix delRdelBiasOmega() const; + Matrix preintMeasCov() const; + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, + Vector gravity, Vector omegaCoriolis) const; +}; + +virtual class CombinedImuFactor : gtsam::NonlinearFactor { + CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, + const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); + // Standard Interface + gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; +}; + +#include +class PreintegratedAhrsMeasurements { + // Standard Constructor + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); + + // get Data + gtsam::Rot3 deltaRij() const; + double deltaTij() const; + Vector biasHat() const; + + // Standard Interface + void integrateMeasurement(Vector measuredOmega, double deltaT); + void resetIntegration() ; +}; + +virtual class AHRSFactor : gtsam::NonlinearFactor { + AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); + AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, + const gtsam::Pose3& body_P_sensor); + + // Standard Interface + gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, + Vector bias) const; + gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, + Vector omegaCoriolis) const; +}; + +#include +//virtual class AttitudeFactor : gtsam::NonlinearFactor { +// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); +// AttitudeFactor(); +//}; +virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); + Rot3AttitudeFactor(); + void print(string s) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; +}; + +virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{ + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); + Pose3AttitudeFactor(); + void print(string s) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; +}; + +//************************************************************************* +// Utilities +//************************************************************************* + +namespace utilities { + + #include + gtsam::KeyList createKeyList(Vector I); + gtsam::KeyList createKeyList(string s, Vector I); + gtsam::KeyVector createKeyVector(Vector I); + gtsam::KeyVector createKeyVector(string s, Vector I); + gtsam::KeySet createKeySet(Vector I); + gtsam::KeySet createKeySet(string s, Vector I); + Matrix extractPoint2(const gtsam::Values& values); + Matrix extractPoint3(const gtsam::Values& values); + Matrix extractPose2(const gtsam::Values& values); + gtsam::Values allPose3s(gtsam::Values& values); + Matrix extractPose3(const gtsam::Values& values); + void perturbPoint2(gtsam::Values& values, double sigma, int seed); + void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); + void perturbPoint3(gtsam::Values& values, double sigma, int seed); + void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); + void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); + void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); + Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); + +} //\namespace utilities + +} diff --git a/python/.gitignore b/python/.gitignore new file mode 100644 index 000000000..d16386367 --- /dev/null +++ b/python/.gitignore @@ -0,0 +1 @@ +build/ \ No newline at end of file diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 000000000..f7ceb62b3 --- /dev/null +++ b/python/CMakeLists.txt @@ -0,0 +1,96 @@ +# Guard to avoid breaking this code in ccmake if by accident GTSAM_PYTHON_VERSION is set to an empty string +if(GTSAM_PYTHON_VERSION STREQUAL "") + set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version" FORCE) +endif() + +# The code below allows to clear the PythonLibs cache if we change GTSAM_PYTHON_VERSION +# Inspired from the solution found here: http://blog.bethcodes.com/cmake-tips-tricks-drop-down-list +if(NOT DEFINED GTSAM_LAST_PYTHON_VERSION) + set(GTSAM_LAST_PYTHON_VERSION ${GTSAM_PYTHON_VERSION} CACHE STRING "Python version used in the last build") + mark_as_advanced(FORCE GTSAM_LAST_PYTHON_VERSION) +endif() +if(NOT (${GTSAM_PYTHON_VERSION} MATCHES ${GTSAM_LAST_PYTHON_VERSION})) + unset(PYTHON_INCLUDE_DIR CACHE) + unset(PYTHON_INCLUDE_DIR2 CACHE) + unset(PYTHON_LIBRARY CACHE) + unset(PYTHON_LIBRARY_DEBUG CACHE) + set(GTSAM_LAST_PYTHON_VERSION ${GTSAM_PYTHON_VERSION} CACHE STRING "Updating python version used in the last build" FORCE) +endif() + +if(GTSAM_PYTHON_VERSION STREQUAL "Default") + # Search the default version. + find_package(PythonInterp) + find_package(PythonLibs) +else() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION}) + find_package(PythonLibs ${GTSAM_PYTHON_VERSION}) +endif() + +# Find NumPy C-API -- this is part of the numpy package +find_package(NumPy) + +# Compose strings used to specify the boost python version. They will be empty if we want to use the defaut +if(NOT GTSAM_PYTHON_VERSION STREQUAL "Default") + string(REPLACE "." "" BOOST_PYTHON_VERSION_SUFFIX ${GTSAM_PYTHON_VERSION}) # Remove '.' from version + string(SUBSTRING ${BOOST_PYTHON_VERSION_SUFFIX} 0 2 BOOST_PYTHON_VERSION_SUFFIX) # Trim version number to 2 digits + set(BOOST_PYTHON_VERSION_SUFFIX "-py${BOOST_PYTHON_VERSION_SUFFIX}") # Append '-py' prefix + string(TOUPPER ${BOOST_PYTHON_VERSION_SUFFIX} BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE) # Get uppercase string +else() + set(BOOST_PYTHON_VERSION_SUFFIX "") + set(BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE "") +endif() + +# Find Boost Python +find_package(Boost COMPONENTS python${BOOST_PYTHON_VERSION_SUFFIX}) + +if(Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_FOUND AND PYTHONLIBS_FOUND AND NUMPY_FOUND) + # Build library + include_directories(${NUMPY_INCLUDE_DIRS}) + include_directories(${PYTHON_INCLUDE_DIRS}) + include_directories(${Boost_INCLUDE_DIRS}) + include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/) + + # Build the python module library + add_subdirectory(handwritten) + + # Create and invoke setup.py, see https://bloerg.net/2012/11/10/cmake-and-distutils.html + set(SETUP_PY_IN "${CMAKE_CURRENT_SOURCE_DIR}/setup.py.in") + set(SETUP_PY "${CMAKE_CURRENT_BINARY_DIR}/setup.py") + + # Hacky way to figure out install folder - valid for Linux & Mac + # default pattern: prefix/lib/pythonX.Y/site-packages from https://docs.python.org/2/install/ + SET(PY_INSTALL_FOLDER "${CMAKE_INSTALL_PREFIX}/lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/site-packages") + + configure_file(${SETUP_PY_IN} ${SETUP_PY}) + + # TODO(frank): possibly support a different prefix a la matlab wrapper + install(CODE "execute_process(WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} COMMAND ${PYTHON_EXECUTABLE} setup.py -v install --prefix ${CMAKE_INSTALL_PREFIX})") +else() + # Disable python module if we didn't find required libraries + # message will print at end of main CMakeLists.txt + SET(GTSAM_PYTHON_WARNINGS "Python dependencies not found - Python module will not be built. Set GTSAM_BUILD_PYTHON to 'Off' to disable this warning. Details:") + + if(NOT PYTHONLIBS_FOUND) + if(GTSAM_PYTHON_VERSION STREQUAL "Default") + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Default PythonLibs not found") + else() + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- PythonLibs version ${GTSAM_PYTHON_VERSION} not found") + endif() + endif() + + if(NOT NUMPY_FOUND) + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Numpy not found") + endif() + + if(NOT Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_FOUND) + if(GTSAM_PYTHON_VERSION STREQUAL "Default") + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Default Boost python not found") + else() + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Boost Python for python ${GTSAM_PYTHON_VERSION} not found") + endif() + endif() + + # make available at top-level + SET(GTSAM_PYTHON_WARNINGS ${GTSAM_PYTHON_WARNINGS} PARENT_SCOPE) + +endif() diff --git a/python/README.md b/python/README.md new file mode 100644 index 000000000..f1c218cfb --- /dev/null +++ b/python/README.md @@ -0,0 +1,18 @@ +Python Wrapper and Packaging +============================ + +This directory contains the basic setup script and directory structure for the gtsam python module. +During the build of gtsam, when GTSAM_BUILD_PYTHON is enabled, the following instructions will run. + +* The handwritten module source files are compiled and linked with Boost Python, generating a shared + library which can then be imported by python +* A setup.py script is configured from setup.py.in +* The gtsam packages 'gtsam', 'gtsam_utils', 'gtsam_examples', and 'gtsam_tests' are installed into + the site-packages folder within the (possibly non-default) installation prefix folder. If + installing to a non-standard prefix, make sure that _prefix_/lib/pythonX.Y/site-packages is + present in your $PYTHONPATH + +The target version of Python to create the module can be set by defining GTSAM_PYTHON_VERSION to 'X.Y' (Example: 2.7 or 3.4), or 'Default' if you want to use the default python installed in your system. Note that if you specify a target version of python, you should also have the correspondening Boost +Python version installed (Example: libboost_python-py27.so or libboost_python-py34.so on Linux). +If you're using the default version, your default Boost Python library (Example: libboost_python.so on Linux) should correspond to the default python version in your system. + diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py new file mode 100644 index 000000000..f9e40517d --- /dev/null +++ b/python/gtsam/__init__.py @@ -0,0 +1 @@ +from ._libgtsam_python import * diff --git a/python/gtsam_examples/ImuFactorExample.py b/python/gtsam_examples/ImuFactorExample.py new file mode 100644 index 000000000..ca5e524ee --- /dev/null +++ b/python/gtsam_examples/ImuFactorExample.py @@ -0,0 +1,105 @@ +""" +A script validating the ImuFactor inference. +""" + +from __future__ import print_function +import math +import matplotlib.pyplot as plt +import numpy as np + +from mpl_toolkits.mplot3d import Axes3D + +import gtsam +from gtsam_utils import plotPose3 +from PreintegrationExample import PreintegrationExample, POSES_FIG + +# shorthand symbols: +BIAS_KEY = int(gtsam.Symbol('b', 0)) +V = lambda j: int(gtsam.Symbol('v', j)) +X = lambda i: int(gtsam.Symbol('x', i)) + +class ImuFactorExample(PreintegrationExample): + + def __init__(self): + self.velocity = np.array([2, 0, 0]) + self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) + self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + + forward_twist = (np.zeros(3), self.velocity) + loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity) + + accBias = np.array([-0.3, 0.1, 0.2]) + gyroBias = np.array([0.1, 0.3, -0.1]) + bias = gtsam.ConstantBias(accBias, gyroBias) + + super(ImuFactorExample, self).__init__(loop_twist, bias) + + def addPrior(self, i, graph): + state = self.scenario.navState(i) + graph.push_back(gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise)) + graph.push_back(gtsam.PriorFactorVector3(V(i), state.velocity(), self.velNoise)) + + def run(self): + graph = gtsam.NonlinearFactorGraph() + + i = 0 # state index + + # initialize data structure for pre-integrated IMU measurements + pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) + + # simulate the loop + T = 12 + actual_state_i = self.scenario.navState(0) + for k, t in enumerate(np.arange(0, T, self.dt)): + # get measurements and add them to PIM + measuredOmega = self.runner.measuredAngularVelocity(t) + measuredAcc = self.runner.measuredSpecificForce(t) + pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) + + # Plot IMU many times + if k % 10 == 0: + self.plotImu(t, measuredOmega, measuredAcc) + + # Plot every second + if k % 100 == 0: + self.plotGroundTruthPose(t) + + # create IMU factor every second + if (k + 1) % 100 == 0: + factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim) + graph.push_back(factor) + pim.resetIntegration() + actual_state_i = self.scenario.navState(t + self.dt) + i += 1 + + # add priors on beginning and end + num_poses = i + 1 + self.addPrior(0, graph) + self.addPrior(num_poses - 1, graph) + + initial = gtsam.Values() + initial.insert(BIAS_KEY, self.actualBias) + for i in range(num_poses): + state_i = self.scenario.navState(float(i)) + initial.insert(X(i), state_i.pose()) + initial.insert(V(i), state_i.velocity()) + + # optimize using Levenberg-Marquardt optimization + params = gtsam.LevenbergMarquardtParams() + params.setVerbosityLM("SUMMARY") + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = optimizer.optimize() + + # Plot resulting poses + i = 0 + while result.exists(X(i)): + pose_i = result.atPose3(X(i)) + plotPose3(POSES_FIG, pose_i, 0.1) + i += 1 + print(result.atConstantBias(BIAS_KEY)) + + plt.ioff() + plt.show() + +if __name__ == '__main__': + ImuFactorExample().run() diff --git a/python/gtsam_examples/OdometryExample.py b/python/gtsam_examples/OdometryExample.py new file mode 100644 index 000000000..0800bab4e --- /dev/null +++ b/python/gtsam_examples/OdometryExample.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python +from __future__ import print_function +import gtsam +import numpy as np + +# Create an empty nonlinear factor graph +graph = gtsam.NonlinearFactorGraph() + +# Add a prior on the first pose, setting it to the origin +# A prior factor consists of a mean and a noise model (covariance matrix) +priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin +priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) +graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) + +# Add odometry factors +odometry = gtsam.Pose2(2.0, 0.0, 0.0) +# For simplicity, we will use the same noise model for each odometry factor +odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) +# Create odometry (Between) factors between consecutive poses +graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) +graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise)) +graph.print("\nFactor Graph:\n") + +# Create the data structure to hold the initialEstimate estimate to the solution +# For illustrative purposes, these have been deliberately set to incorrect values +initial = gtsam.Values() +initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) +initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) +initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) +initial.print("\nInitial Estimate:\n") + +# optimize using Levenberg-Marquardt optimization +params = gtsam.LevenbergMarquardtParams() +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) +result = optimizer.optimize() +result.print("\nFinal Result:\n") diff --git a/python/gtsam_examples/PreintegrationExample.py b/python/gtsam_examples/PreintegrationExample.py new file mode 100644 index 000000000..7095dc59e --- /dev/null +++ b/python/gtsam_examples/PreintegrationExample.py @@ -0,0 +1,129 @@ +""" +A script validating the Preintegration of IMU measurements +""" + +import math +import matplotlib.pyplot as plt +import numpy as np + +from mpl_toolkits.mplot3d import Axes3D + +import gtsam +from gtsam_utils import plotPose3 + +IMU_FIG = 1 +POSES_FIG = 2 + +class PreintegrationExample(object): + + @staticmethod + def defaultParams(g): + """Create default parameters with Z *up* and realistic noise parameters""" + params = gtsam.PreintegrationParams.MakeSharedU(g) + kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW + kAccelSigma = 0.1 / 60 # 10 cm VRW + params.gyroscopeCovariance = kGyroSigma ** 2 * np.identity(3, np.float) + params.accelerometerCovariance = kAccelSigma ** 2 * np.identity(3, np.float) + params.integrationCovariance = 0.0000001 ** 2 * np.identity(3, np.float) + return params + + def __init__(self, twist=None, bias=None): + """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" + + # setup interactive plotting + plt.ion() + + # Setup loop as default scenario + if twist is not None: + (W, V) = twist + else: + # default = loop with forward velocity 2m/s, while pitching up + # with angular velocity 30 degree/sec (negative in FLU) + W = np.array([0, -math.radians(30), 0]) + V = np.array([2, 0, 0]) + + self.scenario = gtsam.ConstantTwistScenario(W, V) + self.dt = 1e-2 + + self.maxDim = 5 + self.labels = list('xyz') + self.colors = list('rgb') + + # Create runner + self.g = 10 # simple gravity constant + self.params = self.defaultParams(self.g) + ptr = gtsam.ScenarioPointer(self.scenario) + + if bias is not None: + self.actualBias = bias + else: + accBias = np.array([0, 0.1, 0]) + gyroBias = np.array([0, 0, 0]) + self.actualBias = gtsam.ConstantBias(accBias, gyroBias) + + self.runner = gtsam.ScenarioRunner(ptr, self.params, self.dt, self.actualBias) + + def plotImu(self, t, measuredOmega, measuredAcc): + plt.figure(IMU_FIG) + + # plot angular velocity + omega_b = self.scenario.omega_b(t) + for i, (label, color) in enumerate(zip(self.labels, self.colors)): + plt.subplot(4, 3, i + 1) + plt.scatter(t, omega_b[i], color='k', marker='.') + plt.scatter(t, measuredOmega[i], color=color, marker='.') + plt.xlabel('angular velocity ' + label) + + # plot acceleration in nav + acceleration_n = self.scenario.acceleration_n(t) + for i, (label, color) in enumerate(zip(self.labels, self.colors)): + plt.subplot(4, 3, i + 4) + plt.scatter(t, acceleration_n[i], color=color, marker='.') + plt.xlabel('acceleration in nav ' + label) + + # plot acceleration in body + acceleration_b = self.scenario.acceleration_b(t) + for i, (label, color) in enumerate(zip(self.labels, self.colors)): + plt.subplot(4, 3, i + 7) + plt.scatter(t, acceleration_b[i], color=color, marker='.') + plt.xlabel('acceleration in body ' + label) + + # plot actual specific force, as well as corrupted + actual = self.runner.actualSpecificForce(t) + for i, (label, color) in enumerate(zip(self.labels, self.colors)): + plt.subplot(4, 3, i + 10) + plt.scatter(t, actual[i], color='k', marker='.') + plt.scatter(t, measuredAcc[i], color=color, marker='.') + plt.xlabel('specific force ' + label) + + def plotGroundTruthPose(self, t): + # plot ground truth pose, as well as prediction from integrated IMU measurements + actualPose = self.scenario.pose(t) + plotPose3(POSES_FIG, actualPose, 0.3) + t = actualPose.translation() + self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim]) + ax = plt.gca() + ax.set_xlim3d(-self.maxDim, self.maxDim) + ax.set_ylim3d(-self.maxDim, self.maxDim) + ax.set_zlim3d(-self.maxDim, self.maxDim) + + plt.pause(0.01) + + def run(self): + # simulate the loop + T = 12 + for i, t in enumerate(np.arange(0, T, self.dt)): + measuredOmega = self.runner.measuredAngularVelocity(t) + measuredAcc = self.runner.measuredSpecificForce(t) + if i % 25 == 0: + self.plotImu(t, measuredOmega, measuredAcc) + self.plotGroundTruthPose(t) + pim = self.runner.integrate(t, self.actualBias, True) + predictedNavState = self.runner.predict(pim, self.actualBias) + plotPose3(POSES_FIG, predictedNavState.pose(), 0.1) + + plt.ioff() + plt.show() + +if __name__ == '__main__': + PreintegrationExample().run() diff --git a/python/gtsam_examples/SFMdata.py b/python/gtsam_examples/SFMdata.py new file mode 100644 index 000000000..21a438226 --- /dev/null +++ b/python/gtsam_examples/SFMdata.py @@ -0,0 +1,32 @@ + + # A structure-from-motion example with landmarks + # - The landmarks form a 10 meter cube + # - The robot rotates around the landmarks, always facing towards the cube + +import gtsam +import numpy as np + +def createPoints(): + # Create the set of ground-truth landmarks + points = [gtsam.Point3(10.0,10.0,10.0), + gtsam.Point3(-10.0,10.0,10.0), + gtsam.Point3(-10.0,-10.0,10.0), + gtsam.Point3(10.0,-10.0,10.0), + gtsam.Point3(10.0,10.0,-10.0), + gtsam.Point3(-10.0,10.0,-10.0), + gtsam.Point3(-10.0,-10.0,-10.0), + gtsam.Point3(10.0,-10.0,-10.0)] + return points + +def createPoses(): + # Create the set of ground-truth poses + radius = 30.0 + angles = np.linspace(0,2*np.pi,8,endpoint=False) + up = gtsam.Point3(0,0,1) + target = gtsam.Point3(0,0,0) + poses = [] + for theta in angles: + position = gtsam.Point3(radius*np.cos(theta), radius*np.sin(theta), 0.0) + camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up) + poses.append(camera.pose()) + return poses diff --git a/python/gtsam_examples/VisualISAM2Example.py b/python/gtsam_examples/VisualISAM2Example.py new file mode 100644 index 000000000..9dafa13e7 --- /dev/null +++ b/python/gtsam_examples/VisualISAM2Example.py @@ -0,0 +1,132 @@ +from __future__ import print_function + +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import numpy as np +import time # for sleep() + +import gtsam +from gtsam_examples import SFMdata +import gtsam_utils + +# shorthand symbols: +X = lambda i: int(gtsam.Symbol('x', i)) +L = lambda j: int(gtsam.Symbol('l', j)) + +def visual_ISAM2_plot(poses, points, result): + # VisualISAMPlot plots current state of ISAM2 object + # Author: Ellon Paiva + # Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert + + # Declare an id for the figure + fignum = 0 + + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + plt.cla() + + # Plot points + # Can't use data because current frame might not see all points + # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # TODO - this is slow + # gtsam.plot3DPoints(result, [], marginals) + gtsam_utils.plot3DPoints(fignum, result, 'rx') + + # Plot cameras + i = 0 + while result.exists(X(i)): + pose_i = result.atPose3(X(i)) + gtsam_utils.plotPose3(fignum, pose_i, 10) + i += 1 + + # draw + ax.set_xlim3d(-40, 40) + ax.set_ylim3d(-40, 40) + ax.set_zlim3d(-40, 40) + plt.pause(1) + +def visual_ISAM2_example(): + plt.ion() + + # Define the camera calibration parameters + K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + + # Define the camera observation noise model + measurementNoise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v + + # Create the set of ground-truth landmarks + points = SFMdata.createPoints() + + # Create the set of ground-truth poses + poses = SFMdata.createPoses() + + # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization + # and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter + # structure is available that allows the user to set various properties, such as the relinearization threshold + # and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result + # will approach the batch result. + parameters = gtsam.ISAM2Params() + parameters.relinearize_threshold = 0.01 + parameters.relinearize_skip = 1 + isam = gtsam.ISAM2(parameters) + + # Create a Factor Graph and Values to hold the new data + graph = gtsam.NonlinearFactorGraph() + initialEstimate = gtsam.Values() + + # Loop over the different poses, adding the observations to iSAM incrementally + for i, pose in enumerate(poses): + + # Add factors for each landmark observation + for j, point in enumerate(points): + camera = gtsam.PinholeCameraCal3_S2(pose, K) + measurement = camera.project(point) + graph.push_back(gtsam.GenericProjectionFactorCal3_S2(measurement, measurementNoise, X(i), L(j), K)) + + # Add an initial guess for the current pose + # Intentionally initialize the variables off from the ground truth + initialEstimate.insert(X(i), pose.compose(gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) + + # If this is the first iteration, add a prior on the first pose to set the coordinate frame + # and a prior on the first landmark to set the scale + # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before + # adding it to iSAM. + if(i == 0): + # Add a prior on pose x0 + poseNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], poseNoise)) + + # Add a prior on landmark l0 + pointNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + graph.push_back(gtsam.PriorFactorPoint3(L(0), points[0], pointNoise)) # add directly to graph + + # Add initial guesses to all observed landmarks + # Intentionally initialize the variables off from the ground truth + for j, point in enumerate(points): + initialEstimate.insert(L(j), point + gtsam.Point3(-0.25, 0.20, 0.15)) + else: + # Update iSAM with the new factors + isam.update(graph, initialEstimate) + # Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver. + # If accuracy is desired at the expense of time, update(*) can be called additional times + # to perform multiple optimizer iterations every step. + isam.update() + currentEstimate = isam.calculate_estimate() + print("****************************************************") + print("Frame", i, ":") + for j in range(i + 1): + print(X(j), ":", currentEstimate.atPose3(X(j))) + + for j in range(len(points)): + print(L(j), ":", currentEstimate.atPoint3(L(j))) + + visual_ISAM2_plot(poses, points, currentEstimate) + + # Clear the factor graph and values for the next iteration + graph.resize(0) + initialEstimate.clear() + + plt.ioff() + plt.show() + +if __name__ == '__main__': + visual_ISAM2_example() diff --git a/python/gtsam_examples/__init__.py b/python/gtsam_examples/__init__.py new file mode 100644 index 000000000..f9d032d54 --- /dev/null +++ b/python/gtsam_examples/__init__.py @@ -0,0 +1,2 @@ +from . import SFMdata +from . import VisualISAM2Example diff --git a/python/gtsam_tests/__init__.py b/python/gtsam_tests/__init__.py new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/python/gtsam_tests/__init__.py @@ -0,0 +1 @@ + diff --git a/python/gtsam_tests/testPoint2.py b/python/gtsam_tests/testPoint2.py new file mode 100644 index 000000000..80a6f6cbf --- /dev/null +++ b/python/gtsam_tests/testPoint2.py @@ -0,0 +1,13 @@ +import unittest +from gtsam import * + +#https://docs.python.org/2/library/unittest.html +class TestPoint2(unittest.TestCase): + def setUp(self): + self.point = Point2() + + def test_constructor(self): + pass + +if __name__ == '__main__': + unittest.main() diff --git a/python/gtsam_tests/testScenario.py b/python/gtsam_tests/testScenario.py new file mode 100644 index 000000000..e78121241 --- /dev/null +++ b/python/gtsam_tests/testScenario.py @@ -0,0 +1,32 @@ +import math +import unittest +import numpy as np + +import gtsam + +class TestScenario(unittest.TestCase): + def setUp(self): + pass + + def test_loop(self): + # Forward velocity 2m/s + # Pitch up with angular velocity 6 degree/sec (negative in FLU) + v = 2 + w = math.radians(6) + W = np.array([0, -w, 0]) + V = np.array([v, 0, 0]) + scenario = gtsam.ConstantTwistScenario(W, V) + + T = 30 + np.testing.assert_almost_equal(W, scenario.omega_b(T)) + np.testing.assert_almost_equal(V, scenario.velocity_b(T)) + np.testing.assert_almost_equal(np.cross(W, V), scenario.acceleration_b(T)) + + # R = v/w, so test if loop crests at 2*R + R = v / w + T30 = scenario.pose(T) + np.testing.assert_almost_equal(np.array([-math.pi, 0, -math.pi]), T30.rotation().xyz()) + self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation())) + +if __name__ == '__main__': + unittest.main() diff --git a/python/gtsam_tests/testScenarioRunner.py b/python/gtsam_tests/testScenarioRunner.py new file mode 100644 index 000000000..2e1b47202 --- /dev/null +++ b/python/gtsam_tests/testScenarioRunner.py @@ -0,0 +1,30 @@ +import math +import unittest +import numpy as np + +import gtsam + +class TestScenarioRunner(unittest.TestCase): + def setUp(self): + self.g = 10 # simple gravity constant + + def test_loop_runner(self): + # Forward velocity 2m/s + # Pitch up with angular velocity 6 degree/sec (negative in FLU) + v = 2 + w = math.radians(6) + W = np.array([0, -w, 0]) + V = np.array([v, 0, 0]) + scenario = gtsam.ConstantTwistScenario(W, V) + + dt = 0.1 + params = gtsam.PreintegrationParams.MakeSharedU(self.g) + runner = gtsam.ScenarioRunner(gtsam.ScenarioPointer(scenario), params, dt) + + # Test specific force at time 0: a is pointing up + t = 0.0 + a = w * v + np.testing.assert_almost_equal(np.array([0, 0, a + self.g]), runner.actualSpecificForce(t)) + +if __name__ == '__main__': + unittest.main() diff --git a/python/gtsam_utils/__init__.py b/python/gtsam_utils/__init__.py new file mode 100644 index 000000000..56c55aa94 --- /dev/null +++ b/python/gtsam_utils/__init__.py @@ -0,0 +1 @@ +from .plot import * diff --git a/python/gtsam_utils/plot.py b/python/gtsam_utils/plot.py new file mode 100644 index 000000000..84a388bbb --- /dev/null +++ b/python/gtsam_utils/plot.py @@ -0,0 +1,59 @@ +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D as _Axes3D + +def plotPoint3(fignum, point, linespec): + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + ax.plot([point.x()], [point.y()], [point.z()], linespec) + + +def plot3DPoints(fignum, values, linespec, marginals=None): + # PLOT3DPOINTS Plots the Point3's in a values, with optional covariances + # Finds all the Point3 objects in the given Values object and plots them. + # If a Marginals object is given, this function will also plot marginal + # covariance ellipses for each point. + + keys = values.keys() + + # Plot points and covariance matrices + for key in keys: + try: + p = values.atPoint3(key); + # if haveMarginals + # P = marginals.marginalCovariance(key); + # gtsam.plotPoint3(p, linespec, P); + # else + plotPoint3(fignum, p, linespec); + except RuntimeError: + continue + # I guess it's not a Point3 + +def plotPose3(fignum, pose, axisLength=0.1): + # get figure object + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + + # get rotation and translation (center) + gRp = pose.rotation().matrix() # rotation from pose to global + C = pose.translation().vector() + + # draw the camera axes + xAxis = C + gRp[:, 0] * axisLength + L = np.append(C[np.newaxis], xAxis[np.newaxis], axis=0) + ax.plot(L[:, 0], L[:, 1], L[:, 2], 'r-') + + yAxis = C + gRp[:, 1] * axisLength + L = np.append(C[np.newaxis], yAxis[np.newaxis], axis=0) + ax.plot(L[:, 0], L[:, 1], L[:, 2], 'g-') + + zAxis = C + gRp[:, 2] * axisLength + L = np.append(C[np.newaxis], zAxis[np.newaxis], axis=0) + ax.plot(L[:, 0], L[:, 1], L[:, 2], 'b-') + + # # plot the covariance + # if (nargin>2) && (~isempty(P)) + # pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame + # gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame + # gtsam.covarianceEllipse3D(C,gPp); + # end diff --git a/python/handwritten/CMakeLists.txt b/python/handwritten/CMakeLists.txt new file mode 100644 index 000000000..9d4f9151a --- /dev/null +++ b/python/handwritten/CMakeLists.txt @@ -0,0 +1,30 @@ +# get subdirectories list +subdirlist(SUBDIRS ${CMAKE_CURRENT_SOURCE_DIR}) + +# get the sources needed to compile gtsam python module +set(gtsam_python_srcs "") +foreach(subdir ${SUBDIRS}) + file(GLOB ${subdir}_src "${subdir}/*.cpp") + list(APPEND gtsam_python_srcs ${${subdir}_src}) +endforeach() + +# Create the library +add_library(gtsam_python SHARED exportgtsam.cpp ${gtsam_python_srcs}) +set_target_properties(gtsam_python PROPERTIES + OUTPUT_NAME gtsam_python + SKIP_BUILD_RPATH TRUE + CLEAN_DIRECT_OUTPUT 1 +) +target_link_libraries(gtsam_python + ${Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_LIBRARY} + ${PYTHON_LIBRARY} gtsam) + +# Cause the library to be output in the correct directory. +# TODO: Change below to work on different systems (currently works only with Linux) +add_custom_command( + OUTPUT ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so + DEPENDS gtsam_python + COMMAND ${CMAKE_COMMAND} -E copy $ ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so + COMMENT "Copying extension module to python/gtsam/_libgtsam_python.so" +) +add_custom_target(copy_gtsam_python_module ALL DEPENDS ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so) \ No newline at end of file diff --git a/python/handwritten/base/FastVector.cpp b/python/handwritten/base/FastVector.cpp new file mode 100644 index 000000000..1c3582813 --- /dev/null +++ b/python/handwritten/base/FastVector.cpp @@ -0,0 +1,37 @@ + /* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps FastVector instances to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/base/FastVector.h" +#include "gtsam/base/types.h" // for Key definition + +using namespace boost::python; +using namespace gtsam; + +void exportFastVectors(){ + + typedef FastVector KeyVector; + + class_("KeyVector") + .def(vector_indexing_suite()) + ; + +} \ No newline at end of file diff --git a/python/handwritten/exportgtsam.cpp b/python/handwritten/exportgtsam.cpp new file mode 100644 index 000000000..7e8c22a82 --- /dev/null +++ b/python/handwritten/exportgtsam.cpp @@ -0,0 +1,105 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports the python module + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include + +#include + +// base +void exportFastVectors(); + +// geometry +void exportPoint2(); +void exportPoint3(); +void exportRot2(); +void exportRot3(); +void exportPose2(); +void exportPose3(); +void exportPinholeBaseK(); +void exportPinholeCamera(); +void exportCal3_S2(); + +// inference +void exportSymbol(); + +// linear +void exportNoiseModels(); + +// nonlinear +void exportValues(); +void exportNonlinearFactor(); +void exportNonlinearFactorGraph(); +void exportLevenbergMarquardtOptimizer(); +void exportISAM2(); + +// slam +void exportPriorFactors(); +void exportBetweenFactors(); +void exportGenericProjectionFactor(); + +// navigation +void exportImuFactor(); +void exportScenario(); + + +// Utils (or Python wrapper specific functions) +void registerNumpyEigenConversions(); + +//-----------------------------------// + +BOOST_PYTHON_MODULE(_libgtsam_python){ + + // NOTE: We need to call import_array1() instead of import_array() to support both python 2 + // and 3. The reason is that BOOST_PYTHON_MODULE puts all its contents in a function + // returning void, and import_array() is a macro that when expanded for python 3, adds + // a 'return __null' statement to that function. For more info check files: + // boost/python/module_init.hpp and numpy/__multiarray_api.h (bottom of the file). + // Should be the first thing to be done + import_array1(); + + registerNumpyEigenConversions(); + + exportFastVectors(); + + exportPoint2(); + exportPoint3(); + exportRot2(); + exportRot3(); + exportPose2(); + exportPose3(); + exportPinholeBaseK(); + exportPinholeCamera(); + exportCal3_S2(); + + exportSymbol(); + + exportNoiseModels(); + + exportValues(); + exportNonlinearFactor(); + exportNonlinearFactorGraph(); + exportLevenbergMarquardtOptimizer(); + exportISAM2(); + + exportPriorFactors(); + exportBetweenFactors(); + exportGenericProjectionFactor(); + + exportImuFactor(); + exportScenario(); +} diff --git a/python/handwritten/geometry/Cal3_S2.cpp b/python/handwritten/geometry/Cal3_S2.cpp new file mode 100644 index 000000000..339cd6a3c --- /dev/null +++ b/python/handwritten/geometry/Cal3_S2.cpp @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Cal3_S2 class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Cal3_S2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Cal3_S2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Cal3_S2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(uncalibrate_overloads, Cal3_S2::uncalibrate, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(calibrate_overloads, Cal3_S2::calibrate, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Cal3_S2::between, 1, 3) + +// Function pointers to desambiguate Cal3_S2::calibrate calls +Point2 (Cal3_S2::*calibrate1)(const Point2 &, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp) const = &Cal3_S2::calibrate; +Vector3 (Cal3_S2::*calibrate2)(const Vector3 &) const = &Cal3_S2::calibrate; + +void exportCal3_S2(){ + +class_("Cal3_S2", init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Cal3_S2::print, print_overloads(args("s"))) + .def("equals", &Cal3_S2::equals, equals_overloads(args("q","tol"))) + .def("fx",&Cal3_S2::fx) + .def("fy",&Cal3_S2::fy) + .def("skew",&Cal3_S2::skew) + .def("px",&Cal3_S2::px) + .def("py",&Cal3_S2::py) + .def("principal_point",&Cal3_S2::principalPoint) + .def("vector",&Cal3_S2::vector) + .def("k",&Cal3_S2::K) + .def("matrix",&Cal3_S2::matrix) + .def("matrix_inverse",&Cal3_S2::matrix_inverse) + .def("uncalibrate",&Cal3_S2::uncalibrate, uncalibrate_overloads()) + .def("calibrate",calibrate1, calibrate_overloads()) + .def("calibrate",calibrate2) + .def("between",&Cal3_S2::between, between_overloads()) +; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/PinholeBaseK.cpp b/python/handwritten/geometry/PinholeBaseK.cpp new file mode 100644 index 000000000..da98783e2 --- /dev/null +++ b/python/handwritten/geometry/PinholeBaseK.cpp @@ -0,0 +1,66 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps PinholeCamera classes to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/PinholeCamera.h" +#include "gtsam/geometry/Cal3_S2.h" + + +using namespace boost::python; +using namespace gtsam; + +typedef PinholeBaseK PinholeBaseKCal3_S2; + +// Wrapper on PinholeBaseK because it has pure virtual method calibration() +struct PinholeBaseKCal3_S2Callback : PinholeBaseKCal3_S2, wrapper +{ + const Cal3_S2 & calibration () const { + return this->get_override("calibration")(); + } +}; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(project_overloads, PinholeBaseKCal3_S2::project, 2, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, PinholeBaseKCal3_S2::range, 1, 3) + +// Function pointers to desambiguate project() calls +Point2 (PinholeBaseKCal3_S2::*project1) (const Point3 &pw) const = &PinholeBaseKCal3_S2::project; +Point2 (PinholeBaseKCal3_S2::*project2) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; +Point2 (PinholeBaseKCal3_S2::*project3) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; + +// function pointers to desambiguate range() calls +double (PinholeBaseKCal3_S2::*range1) (const Point3 &point, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 3 > Dpoint) const = &PinholeBaseKCal3_S2::range; +double (PinholeBaseKCal3_S2::*range2) (const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dpose) const = &PinholeBaseKCal3_S2::range; +double (PinholeBaseKCal3_S2::*range3) (const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dother) const = &PinholeBaseKCal3_S2::range; + +void exportPinholeBaseK(){ + + class_("PinholeBaseKCal3_S2", no_init) + .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), return_value_policy()) + .def("project", project1) + .def("project", project2, project_overloads()) + .def("project", project3, project_overloads()) + .def("backproject", &PinholeBaseKCal3_S2::backproject) + .def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity) + .def("range", range1, range_overloads()) + .def("range", range2, range_overloads()) + .def("range", range3, range_overloads()) + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/PinholeCamera.cpp b/python/handwritten/geometry/PinholeCamera.cpp new file mode 100644 index 000000000..21ffcdeb7 --- /dev/null +++ b/python/handwritten/geometry/PinholeCamera.cpp @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps PinholeCamera classes to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/PinholeCamera.h" +#include "gtsam/geometry/Cal3_S2.h" + +using namespace boost::python; +using namespace gtsam; + +typedef PinholeBaseK PinholeBaseKCal3_S2; +typedef PinholeCamera PinholeCameraCal3_S2; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, PinholeCameraCal3_S2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, PinholeCameraCal3_S2::equals, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Lookat_overloads, PinholeCameraCal3_S2::Lookat, 3, 4) + +void exportPinholeCamera(){ + +class_ >("PinholeCameraCal3_S2", init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &PinholeCameraCal3_S2::print, print_overloads(args("s"))) + .def("equals", &PinholeCameraCal3_S2::equals, equals_overloads(args("q","tol"))) + .def("pose", &PinholeCameraCal3_S2::pose, return_value_policy()) + // We don't need to define calibration() here because it's already defined as virtual in the base class PinholeBaseKCal3_S2 + // .def("calibration", &PinholeCameraCal3_S2::calibration, return_value_policy()) + .def("Lookat", &PinholeCameraCal3_S2::Lookat, Lookat_overloads()) + .staticmethod("Lookat") +; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Point2.cpp b/python/handwritten/geometry/Point2.cpp new file mode 100644 index 000000000..7af3f8cf6 --- /dev/null +++ b/python/handwritten/geometry/Point2.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Point2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Point2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Point2::compose, 1, 3) + +void exportPoint2(){ + + class_("Point2", init<>()) + .def(init()) + .def(init()) + .def("identity", &Point2::identity) + .def("dist", &Point2::dist) + .def("distance", &Point2::distance) + .def("equals", &Point2::equals, equals_overloads(args("q","tol"))) + .def("norm", &Point2::norm) + .def("print", &Point2::print, print_overloads(args("s"))) + .def("unit", &Point2::unit) + .def("vector", &Point2::vector) + .def("x", &Point2::x) + .def("y", &Point2::y) + .def(self * other()) // __mult__ + .def(other() * self) // __mult__ + .def(self + self) + .def(-self) + .def(self - self) + .def(self / other()) + .def(self_ns::str(self)) + .def(repr(self)) + .def(self == self) + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp new file mode 100644 index 000000000..664b4ffda --- /dev/null +++ b/python/handwritten/geometry/Point3.cpp @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Point3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Point3.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point3::equals, 1, 2) + +void exportPoint3(){ + +class_("Point3") + .def(init<>()) + .def(init()) + .def(init()) + .def("identity", &Point3::identity) + .staticmethod("identity") + .def("add", &Point3::add) + .def("cross", &Point3::cross) + .def("dist", &Point3::dist) + .def("distance", &Point3::distance) + .def("dot", &Point3::dot) + .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) + .def("norm", &Point3::norm) + .def("normalize", &Point3::normalize) + .def("print", &Point3::print, print_overloads(args("s"))) + .def("sub", &Point3::sub) + .def("vector", &Point3::vector) + .def("x", &Point3::x) + .def("y", &Point3::y) + .def("z", &Point3::z) + .def(self * other()) + .def(other() * self) + .def(self + self) + .def(-self) + .def(self - self) + .def(self / other()) + .def(self_ns::str(self)) + .def(repr(self)) + .def(self == self) +; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Pose2.cpp b/python/handwritten/geometry/Pose2.cpp new file mode 100644 index 000000000..4f402df7e --- /dev/null +++ b/python/handwritten/geometry/Pose2.cpp @@ -0,0 +1,93 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Pose2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Pose2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose2::compose, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose2::between, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose2::transform_to, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose2::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose2::bearing, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose2::range, 1, 3) + +// Manually wrap + +void exportPose2(){ + + // double (Pose2::*range1)(const Pose2&, boost::optional, boost::optional) const + // = &Pose2::range; + // double (Pose2::*range2)(const Point2&, boost::optional, boost::optional) const + // = &Pose2::range; + + // Rot2 (Pose2::*bearing1)(const Pose2&, boost::optional, boost::optional) const + // = &Pose2::bearing; + // Rot2 (Pose2::*bearing2)(const Point2&, boost::optional, boost::optional) const + // = &Pose2::bearing; + + class_("Pose2", init<>()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Pose2::print, print_overloads(args("s"))) + + .def("equals", &Pose2::equals, equals_overloads(args("pose","tol"))) + // .def("inverse", &Pose2::inverse) + // .def("compose", &Pose2::compose, compose_overloads(args("p2", "H1", "H2"))) + // .def("between", &Pose2::between, between_overloads(args("p2", "H1", "H2"))) + // .def("dim", &Pose2::dim) + // .def("retract", &Pose2::retract) + + .def("transform_to", &Pose2::transform_to, + transform_to_overloads(args("point", "H1", "H2"))) + .def("transform_from", &Pose2::transform_from, + transform_to_overloads(args("point", "H1", "H2"))) + + .def("x", &Pose2::x) + .def("y", &Pose2::y) + .def("theta", &Pose2::theta) + // See documentation on call policy for more information + // https://wiki.python.org/moin/boost.python/CallPolicy + .def("t", &Pose2::t, return_value_policy()) + .def("r", &Pose2::r, return_value_policy()) + .def("translation", &Pose2::translation, return_value_policy()) + .def("rotation", &Pose2::rotation, return_value_policy()) + + // .def("bearing", bearing1, bearing_overloads()) + // .def("bearing", bearing2, bearing_overloads()) + + // Function overload example + // .def("range", range1, range_overloads()) + // .def("range", range2, range_overloads()) + + + .def("Expmap", &Pose2::Expmap) + .staticmethod("Expmap") + + .def(self * self) // __mult__ + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Pose3.cpp b/python/handwritten/geometry/Pose3.cpp new file mode 100644 index 000000000..dfdfe8ad1 --- /dev/null +++ b/python/handwritten/geometry/Pose3.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Pose3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Pose3.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose3::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose3::transform_to, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(translation_overloads, Pose3::translation, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose3::compose, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose3::between, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose3::bearing, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose3::range, 1, 3) + +void exportPose3(){ + + // function pointers to desambiguate transform_to() calls + Point3 (Pose3::*transform_to1)(const Point3&, OptionalJacobian< 3, 6 >, OptionalJacobian< 3, 3 > ) const = &Pose3::transform_to; + Pose3 (Pose3::*transform_to2)(const Pose3&) const = &Pose3::transform_to; + // function pointers to desambiguate compose() calls + Pose3 (Pose3::*compose1)(const Pose3 &g) const = &Pose3::compose; + Pose3 (Pose3::*compose2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const = &Pose3::compose; + // function pointers to desambiguate between() calls + Pose3 (Pose3::*between1)(const Pose3 &g) const = &Pose3::between; + Pose3 (Pose3::*between2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const = &Pose3::between; + // function pointers to desambiguate range() calls + double (Pose3::*range1)(const Point3 &, OptionalJacobian<1,6>, OptionalJacobian<1,3>) const = &Pose3::range; + double (Pose3::*range2)(const Pose3 &, OptionalJacobian<1,6>, OptionalJacobian<1,6>) const = &Pose3::range; + + class_("Pose3") + .def(init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Pose3::print, print_overloads(args("s"))) + .def("equals", &Pose3::equals, equals_overloads(args("pose","tol"))) + .def("identity", &Pose3::identity) + .staticmethod("identity") + .def("bearing", &Pose3::bearing) + .def("matrix", &Pose3::matrix) + .def("transform_from", &Pose3::transform_from, + transform_from_overloads(args("point", "H1", "H2"))) + .def("transform_to", transform_to1, + transform_to_overloads(args("point", "H1", "H2"))) + .def("transform_to", transform_to2) + .def("x", &Pose3::x) + .def("y", &Pose3::y) + .def("z", &Pose3::z) + .def("translation", &Pose3::translation, + translation_overloads()[return_value_policy()]) + .def("rotation", &Pose3::rotation, return_value_policy()) + .def(self * self) // __mult__ + .def(self * other()) // __mult__ + .def(self_ns::str(self)) // __str__ + .def(repr(self)) // __repr__ + .def("compose", compose1) + .def("compose", compose2, compose_overloads()) + .def("between", between1) + .def("between", between2, between_overloads()) + .def("range", range1, range_overloads()) + .def("range", range2, range_overloads()) + .def("bearing", &Pose3::bearing, bearing_overloads()) + ; +} \ No newline at end of file diff --git a/python/handwritten/geometry/Rot2.cpp b/python/handwritten/geometry/Rot2.cpp new file mode 100644 index 000000000..59b4ce4e8 --- /dev/null +++ b/python/handwritten/geometry/Rot2.cpp @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Rot2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Rot2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Rot2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Rot2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Rot2::compose, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(relativeBearing_overloads, Rot2::relativeBearing, 1, 3) + +void exportRot2(){ + + class_("Rot2", init<>()) + .def(init()) + .def("Expmap", &Rot2::Expmap) + .staticmethod("Expmap") + .def("Logmap", &Rot2::Logmap) + .staticmethod("Logmap") + .def("atan2", &Rot2::atan2) + .staticmethod("atan2") + .def("fromAngle", &Rot2::fromAngle) + .staticmethod("fromAngle") + .def("fromCosSin", &Rot2::fromCosSin) + .staticmethod("fromCosSin") + .def("fromDegrees", &Rot2::fromDegrees) + .staticmethod("fromDegrees") + .def("identity", &Rot2::identity) + .staticmethod("identity") + .def("relativeBearing", &Rot2::relativeBearing) + .staticmethod("relativeBearing") + .def("c", &Rot2::c) + .def("degrees", &Rot2::degrees) + .def("equals", &Rot2::equals, equals_overloads(args("q","tol"))) + .def("matrix", &Rot2::matrix) + .def("print", &Rot2::print, print_overloads(args("s"))) + .def("rotate", &Rot2::rotate) + .def("s", &Rot2::s) + .def("theta", &Rot2::theta) + .def("unrotate", &Rot2::unrotate) + .def(self * self) // __mult__ + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Rot3.cpp b/python/handwritten/geometry/Rot3.cpp new file mode 100644 index 000000000..68643fe2c --- /dev/null +++ b/python/handwritten/geometry/Rot3.cpp @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Rot3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Rot3.h" + +using namespace boost::python; +using namespace gtsam; + +static Rot3 Quaternion_0(const Vector4& q) +{ + return Rot3::Quaternion(q[0],q[1],q[2],q[3]); +} + +static Rot3 Quaternion_1(double w, double x, double y, double z) +{ + return Rot3::Quaternion(w,x,y,z); +} + +// Prototypes used to perform overloading +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html +gtsam::Rot3 (*AxisAngle_0)(const Vector3&, double) = &Rot3::AxisAngle; +gtsam::Rot3 (*AxisAngle_1)(const gtsam::Point3&, double) = &Rot3::AxisAngle; +gtsam::Rot3 (*Rodrigues_0)(const Vector3&) = &Rot3::Rodrigues; +gtsam::Rot3 (*Rodrigues_1)(double, double, double) = &Rot3::Rodrigues; +gtsam::Rot3 (*RzRyRx_0)(double, double, double) = &Rot3::RzRyRx; +gtsam::Rot3 (*RzRyRx_1)(const Vector&) = &Rot3::RzRyRx; +Vector (Rot3::*quaternion_0)() const = &Rot3::quaternion; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Rot3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Rot3::equals, 1, 2) + +void exportRot3(){ + + class_("Rot3") + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("Quaternion", Quaternion_0, arg("q"), "Creates a Rot3 from an array [w,x,y,z] representing a quaternion") + .def("Quaternion", Quaternion_1, (arg("w"),arg("x"),arg("y"),arg("z")) ) + .staticmethod("Quaternion") + .def("AxisAngle", AxisAngle_0) + .def("AxisAngle", AxisAngle_1) + .staticmethod("AxisAngle") + .def("Expmap", &Rot3::Expmap) + .staticmethod("Expmap") + .def("ExpmapDerivative", &Rot3::ExpmapDerivative) + .staticmethod("ExpmapDerivative") + .def("Logmap", &Rot3::Logmap) + .staticmethod("Logmap") + .def("LogmapDerivative", &Rot3::LogmapDerivative) + .staticmethod("LogmapDerivative") + .def("Rodrigues", Rodrigues_0) + .def("Rodrigues", Rodrigues_1) + .staticmethod("Rodrigues") + .def("Rx", &Rot3::Rx) + .staticmethod("Rx") + .def("Ry", &Rot3::Ry) + .staticmethod("Ry") + .def("Rz", &Rot3::Rz) + .staticmethod("Rz") + .def("RzRyRx", RzRyRx_0, (arg("x"),arg("y"),arg("z")), "Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis" ) + .def("RzRyRx", RzRyRx_1, arg("xyz"), "Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis" ) + .staticmethod("RzRyRx") + .def("identity", &Rot3::identity) + .staticmethod("identity") + .def("AdjointMap", &Rot3::AdjointMap) + .def("column", &Rot3::column) + .def("conjugate", &Rot3::conjugate) + .def("equals", &Rot3::equals, equals_overloads(args("q","tol"))) +#ifndef GTSAM_USE_QUATERNIONS + .def("localCayley", &Rot3::localCayley) + .def("retractCayley", &Rot3::retractCayley) +#endif + .def("matrix", &Rot3::matrix) + .def("print", &Rot3::print, print_overloads(args("s"))) + .def("r1", &Rot3::r1) + .def("r2", &Rot3::r2) + .def("r3", &Rot3::r3) + .def("rpy", &Rot3::rpy) + .def("slerp", &Rot3::slerp) + .def("transpose", &Rot3::transpose) + .def("xyz", &Rot3::xyz) + .def("quaternion", quaternion_0) + .def(self * self) + .def(self * other()) + .def(self * other()) + .def(self_ns::str(self)) // __str__ + .def(repr(self)) // __repr__ + ; + +} diff --git a/python/handwritten/inference/Symbol.cpp b/python/handwritten/inference/Symbol.cpp new file mode 100644 index 000000000..9fc5b74e7 --- /dev/null +++ b/python/handwritten/inference/Symbol.cpp @@ -0,0 +1,83 @@ + /* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Symbol class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include + +#define NO_IMPORT_ARRAY +#include + +#include // for stringstream + +#include "gtsam/inference/Symbol.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Symbol::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Symbol::equals, 1, 2) + +// Helper function to allow building a symbol from a python string and a index. +static boost::shared_ptr makeSymbol(const std::string &str, size_t j) +{ + if(str.size() > 1) + throw std::runtime_error("string argument must have one character only"); + + return boost::make_shared(str.at(0),j); +} + +// Helper function to print the symbol as "char-and-index" in python +std::string selfToString(const Symbol & self) +{ + return (std::string)self; +} + +// Helper function to convert a Symbol to int using int() cast in python +size_t selfToKey(const Symbol & self) +{ + return self.key(); +} + +// Helper function to recover symbol's unsigned char as string +std::string chrFromSelf(const Symbol & self) +{ + std::stringstream ss; + ss << self.chr(); + return ss.str(); +} + +void exportSymbol(){ + +class_ >("Symbol") + .def(init<>()) + .def(init()) + .def("__init__", make_constructor(makeSymbol)) + .def(init()) + .def("print", &Symbol::print, print_overloads(args("s"))) + .def("equals", &Symbol::equals, equals_overloads(args("q","tol"))) + .def("key", &Symbol::key) + .def("index", &Symbol::index) + .def(self < self) + .def(self == self) + .def(self == other()) + .def(self != self) + .def(self != other()) + .def("__repr__", &selfToString) + .def("__int__", &selfToKey) + .def("chr", &chrFromSelf) +; + +} \ No newline at end of file diff --git a/python/handwritten/linear/NoiseModel.cpp b/python/handwritten/linear/NoiseModel.cpp new file mode 100644 index 000000000..3453184bd --- /dev/null +++ b/python/handwritten/linear/NoiseModel.cpp @@ -0,0 +1,137 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps the noise model classes into the noiseModel module + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + + /** TODOs Summary: + * + * TODO(Ellon): Don't know yet it it's worth/needed to add 'Wrap structs' for each of the noise models. + * I think it's only worthy if we want to access virtual the virtual functions from python. + * TODO(Ellon): Wrap non-pure virtual methods of Base on BaseWrap + */ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/linear/NoiseModel.h" + +using namespace boost::python; +using namespace gtsam; +using namespace gtsam::noiseModel; + +// Wrap around pure virtual class Base. +// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the +// overloading through inheritance in Python. +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions +struct BaseCallback : Base, wrapper +{ + void print (const std::string & name="") const { + this->get_override("print")(); + } + bool equals (const Base & expected, double tol=1e-9) const { + return this->get_override("equals")(); + } + Vector whiten (const Vector & v) const { + return this->get_override("whiten")(); + } + Matrix Whiten (const Matrix & v) const { + return this->get_override("Whiten")(); + } + Vector unwhiten (const Vector & v) const { + return this->get_override("unwhiten")(); + } + double distance (const Vector & v) const { + return this->get_override("distance")(); + } + void WhitenSystem (std::vector< Matrix > &A, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A1, Matrix &A2, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A1, Matrix &A2, Matrix &A3, Vector &b) const { + this->get_override("WhitenSystem")(); + } + + // TODO(Ellon): Wrap non-pure virtual methods should go here. + // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.virtual_functions_with_default_implementations + +}; + +// Overloads for named constructors. Named constructors are static, so we declare them +// using BOOST_PYTHON_FUNCTION_OVERLOADS instead of BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html#python.default_arguments +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_SqrtInformation_overloads, Gaussian::SqrtInformation, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_Information_overloads, Gaussian::Information, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_Covariance_overloads, Gaussian::Covariance, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Sigmas_overloads, Diagonal::Sigmas, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Variances_overloads, Diagonal::Variances, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Precisions_overloads, Diagonal::Precisions, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Sigma_overloads, Isotropic::Sigma, 2, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Variance_overloads, Isotropic::Variance, 2, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Precision_overloads, Isotropic::Precision, 2, 3) + + +void exportNoiseModels(){ + + // Create a scope "noiseModel". See: http://isolation-nation.blogspot.fr/2008/09/packages-in-python-extension-modules.html + std::string noiseModel_name = extract(scope().attr("__name__") + ".noiseModel"); + object noiseModel_module(handle<>(borrowed(PyImport_AddModule(noiseModel_name.c_str())))); + scope().attr("noiseModel") = noiseModel_module; + scope noiseModel_scope = noiseModel_module; + + // Then export our classes in the noiseModel scope + class_("Base") + .def("print", pure_virtual(&Base::print)) + ; + + // NOTE: We should use "Base" in "bases<...>", and not "BaseCallback" (it was not clear at the begining) + class_, bases >("Gaussian", no_init) + .def("SqrtInformation",&Gaussian::SqrtInformation, Gaussian_SqrtInformation_overloads()) + .staticmethod("SqrtInformation") + .def("Information",&Gaussian::Information, Gaussian_Information_overloads()) + .staticmethod("Information") + .def("Covariance",&Gaussian::Covariance, Gaussian_Covariance_overloads()) + .staticmethod("Covariance") + ; + + class_, bases >("Diagonal", no_init) + .def("Sigmas",&Diagonal::Sigmas, Diagonal_Sigmas_overloads()) + .staticmethod("Sigmas") + .def("Variances",&Diagonal::Variances, Diagonal_Variances_overloads()) + .staticmethod("Variances") + .def("Precisions",&Diagonal::Precisions, Diagonal_Precisions_overloads()) + .staticmethod("Precisions") + ; + + class_, bases >("Isotropic", no_init) + .def("Sigma",&Isotropic::Sigma, Isotropic_Sigma_overloads()) + .staticmethod("Sigma") + .def("Variance",&Isotropic::Variance, Isotropic_Variance_overloads()) + .staticmethod("Variance") + .def("Precision",&Isotropic::Precision, Isotropic_Precision_overloads()) + .staticmethod("Precision") + ; + + class_, bases >("Unit", no_init) + .def("Create",&Unit::Create) + .staticmethod("Create") + ; + +} \ No newline at end of file diff --git a/python/handwritten/navigation/ImuFactor.cpp b/python/handwritten/navigation/ImuFactor.cpp new file mode 100644 index 000000000..c947ae9ee --- /dev/null +++ b/python/handwritten/navigation/ImuFactor.cpp @@ -0,0 +1,94 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps ConstantTwistScenario class to python + * @author Frank Dellaert + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/navigation/ImuFactor.h" + +using namespace boost::python; +using namespace gtsam; + +typedef gtsam::OptionalJacobian<3, 9> OptionalJacobian39; +typedef gtsam::OptionalJacobian<9, 6> OptionalJacobian96; +typedef gtsam::OptionalJacobian<9, 9> OptionalJacobian9; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(attitude_overloads, attitude, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(position_overloads, position, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(velocity_overloads, velocity, 0, 1) + +void exportImuFactor() { + class_("OptionalJacobian39", init<>()); + class_("OptionalJacobian96", init<>()); + class_("OptionalJacobian9", init<>()); + + class_("NavState", init<>()) + // TODO(frank): overload with jacobians + .def("print", &NavState::print, print_overloads()) + .def("attitude", &NavState::attitude, + attitude_overloads()[return_value_policy()]) + .def("position", &NavState::position, + position_overloads()[return_value_policy()]) + .def("velocity", &NavState::velocity, + velocity_overloads()[return_value_policy()]) + .def(repr(self)) + .def("pose", &NavState::pose); + + class_("ConstantBias", init<>()) + .def(init()) + .def(repr(self)); + + class_>( + "PreintegrationParams", init()) + .def_readwrite("gyroscopeCovariance", + &PreintegrationParams::gyroscopeCovariance) + .def_readwrite("omegaCoriolis", &PreintegrationParams::omegaCoriolis) + .def_readwrite("body_P_sensor", &PreintegrationParams::body_P_sensor) + .def_readwrite("accelerometerCovariance", + &PreintegrationParams::accelerometerCovariance) + .def_readwrite("integrationCovariance", + &PreintegrationParams::integrationCovariance) + .def_readwrite("use2ndOrderCoriolis", + &PreintegrationParams::use2ndOrderCoriolis) + .def_readwrite("n_gravity", &PreintegrationParams::n_gravity) + + .def("MakeSharedD", &PreintegrationParams::MakeSharedD) + .staticmethod("MakeSharedD") + .def("MakeSharedU", &PreintegrationParams::MakeSharedU) + .staticmethod("MakeSharedU"); + + class_( + "PreintegratedImuMeasurements", + init&, + const imuBias::ConstantBias&>()) + .def(repr(self)) + .def("predict", &PreintegratedImuMeasurements::predict) + .def("computeError", &PreintegratedImuMeasurements::computeError) + .def("resetIntegration", &PreintegratedImuMeasurements::resetIntegration) + .def("integrateMeasurement", + &PreintegratedImuMeasurements::integrateMeasurement) + .def("preintMeasCov", &PreintegratedImuMeasurements::preintMeasCov); + + // NOTE(frank): Abstract classes need boost::noncopyable + class_, boost::shared_ptr>( + "ImuFactor") + .def("error", &ImuFactor::error) + .def(init()) + .def(repr(self)); +} diff --git a/python/handwritten/navigation/Scenario.cpp b/python/handwritten/navigation/Scenario.cpp new file mode 100644 index 000000000..e11f04a57 --- /dev/null +++ b/python/handwritten/navigation/Scenario.cpp @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps ConstantTwistScenario class to python + * @author Frank Dellaert + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/navigation/ScenarioRunner.h" + +using namespace boost::python; +using namespace gtsam; + +// Create const Scenario pointer from ConstantTwistScenario +static const Scenario* ScenarioPointer(const ConstantTwistScenario& scenario) { + return static_cast(&scenario); +} + +void exportScenario() { + // NOTE(frank): Abstract classes need boost::noncopyable + class_("Scenario", no_init); + + // TODO(frank): figure out how to do inheritance + class_("ConstantTwistScenario", + init()) + .def("pose", &Scenario::pose) + .def("omega_b", &Scenario::omega_b) + .def("velocity_n", &Scenario::velocity_n) + .def("acceleration_n", &Scenario::acceleration_n) + .def("navState", &Scenario::navState) + .def("rotation", &Scenario::rotation) + .def("velocity_b", &Scenario::velocity_b) + .def("acceleration_b", &Scenario::acceleration_b); + + // NOTE(frank): https://wiki.python.org/moin/boost.python/CallPolicy + def("ScenarioPointer", &ScenarioPointer, + return_value_policy()); + + class_( + "ScenarioRunner", + init&, + double, const imuBias::ConstantBias&>()) + .def("actualSpecificForce", &ScenarioRunner::actualSpecificForce) + .def("measuredAngularVelocity", &ScenarioRunner::measuredAngularVelocity) + .def("measuredSpecificForce", &ScenarioRunner::measuredSpecificForce) + .def("imuSampleTime", &ScenarioRunner::imuSampleTime, + return_value_policy()) + .def("integrate", &ScenarioRunner::integrate) + .def("predict", &ScenarioRunner::predict) + .def("estimateCovariance", &ScenarioRunner::estimateCovariance); +} diff --git a/python/handwritten/nonlinear/ISAM2.cpp b/python/handwritten/nonlinear/ISAM2.cpp new file mode 100644 index 000000000..f80c5df99 --- /dev/null +++ b/python/handwritten/nonlinear/ISAM2.cpp @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports ISAM2 class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/ISAM2.h" +#include "gtsam/geometry/Pose3.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(update_overloads, ISAM2::update, 0, 7) + +void exportISAM2(){ + +// TODO(Ellon): Export all properties of ISAM2Params +class_("ISAM2Params") + .add_property("relinearize_skip", &ISAM2Params::getRelinearizeSkip, &ISAM2Params::setRelinearizeSkip) + .add_property("enable_relinearization", &ISAM2Params::isEnableRelinearization, &ISAM2Params::setEnableRelinearization) + .add_property("evaluate_non_linear_error", &ISAM2Params::isEvaluateNonlinearError, &ISAM2Params::setEvaluateNonlinearError) + .add_property("factorization", &ISAM2Params::getFactorization, &ISAM2Params::setFactorization) + .add_property("cache_linearized_factors", &ISAM2Params::isCacheLinearizedFactors, &ISAM2Params::setCacheLinearizedFactors) + .add_property("enable_detailed_results", &ISAM2Params::isEnableDetailedResults, &ISAM2Params::setEnableDetailedResults) + .add_property("enable_partial_linearization_check", &ISAM2Params::isEnablePartialRelinearizationCheck, &ISAM2Params::setEnablePartialRelinearizationCheck) + // TODO(Ellon): Check if it works with FastMap; Implement properly if it doesn't. + .add_property("relinearization_threshold", &ISAM2Params::getRelinearizeThreshold, &ISAM2Params::setRelinearizeThreshold) + // TODO(Ellon): Wrap the following setters/getters: + // void setOptimizationParams (OptimizationParams optimizationParams) + // OptimizationParams getOptimizationParams () const + // void setKeyFormatter (KeyFormatter keyFormatter) + // KeyFormatter getKeyFormatter () const + // GaussianFactorGraph::Eliminate getEliminationFunction () const +; + +// TODO(Ellon): Export useful methods/properties of ISAM2Result +class_("ISAM2Result") +; + +// Function pointers for overloads in ISAM2 +Values (ISAM2::*calculateEstimate_0)() const = &ISAM2::calculateEstimate; + +class_("ISAM2") + .def(init()) + // TODO(Ellon): wrap all optional values of update + .def("update",&ISAM2::update, update_overloads()) + .def("calculate_estimate", calculateEstimate_0) + .def("calculate_pose3_estimate", &ISAM2::calculateEstimate, (arg("self"), arg("key")) ) + .def("value_exists", &ISAM2::valueExists) +; + +} \ No newline at end of file diff --git a/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp new file mode 100644 index 000000000..7c7cdf3cc --- /dev/null +++ b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -0,0 +1,27 @@ +#include + +#define NO_IMPORT_ARRAY +#include + +#include + +using namespace boost::python; +using namespace gtsam; + +void exportLevenbergMarquardtOptimizer(){ + class_("LevenbergMarquardtParams", init<>()) + .def("setDiagonalDamping", &LevenbergMarquardtParams::setDiagonalDamping) + .def("setlambdaFactor", &LevenbergMarquardtParams::setlambdaFactor) + .def("setlambdaInitial", &LevenbergMarquardtParams::setlambdaInitial) + .def("setlambdaLowerBound", &LevenbergMarquardtParams::setlambdaLowerBound) + .def("setlambdaUpperBound", &LevenbergMarquardtParams::setlambdaUpperBound) + .def("setLogFile", &LevenbergMarquardtParams::setLogFile) + .def("setUseFixedLambdaFactor", &LevenbergMarquardtParams::setUseFixedLambdaFactor) + .def("setVerbosityLM", &LevenbergMarquardtParams::setVerbosityLM) + ; + + class_("LevenbergMarquardtOptimizer", + init()) + .def("optimize", &LevenbergMarquardtOptimizer::optimize, return_value_policy()) + ; +} diff --git a/python/handwritten/nonlinear/NonlinearFactor.cpp b/python/handwritten/nonlinear/NonlinearFactor.cpp new file mode 100644 index 000000000..9a130d8e9 --- /dev/null +++ b/python/handwritten/nonlinear/NonlinearFactor.cpp @@ -0,0 +1,49 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports virtual class NonlinearFactor to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/NonlinearFactor.h" + +using namespace boost::python; +using namespace gtsam; + +// Wrap around pure virtual class NonlinearFactor. +// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the +// overloading through inheritance in Python. +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions +struct NonlinearFactorCallback : NonlinearFactor, wrapper +{ + double error (const Values & values) const { + return this->get_override("error")(values); + } + size_t dim () const { + return this->get_override("dim")(); + } + boost::shared_ptr linearize(const Values & values) const { + return this->get_override("linearize")(values); + } +}; + +void exportNonlinearFactor(){ + + class_("NonlinearFactor") + ; + +} \ No newline at end of file diff --git a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp new file mode 100644 index 000000000..4ba4ba008 --- /dev/null +++ b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp @@ -0,0 +1,56 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports NonlinearFactorGraph class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/NonlinearFactorGraph.h" +#include "gtsam/nonlinear/NonlinearFactor.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, NonlinearFactorGraph::print, 0, 1); + +boost::shared_ptr getNonlinearFactor( + const NonlinearFactorGraph& graph, size_t idx) { + auto p = boost::dynamic_pointer_cast(graph.at(idx)); + if (!p) throw std::runtime_error("No NonlinearFactor at requested index"); + return p; +}; + +void exportNonlinearFactorGraph(){ + + typedef NonlinearFactorGraph::sharedFactor sharedFactor; + + void (NonlinearFactorGraph::*push_back1)(const sharedFactor&) = &NonlinearFactorGraph::push_back; + void (NonlinearFactorGraph::*add1)(const sharedFactor&) = &NonlinearFactorGraph::add; + + class_("NonlinearFactorGraph", init<>()) + .def("size",&NonlinearFactorGraph::size) + .def("push_back", push_back1) + .def("add", add1) + .def("resize", &NonlinearFactorGraph::resize) + .def("empty", &NonlinearFactorGraph::empty) + .def("print", &NonlinearFactorGraph::print, print_overloads(args("s"))) + ; + + def("getNonlinearFactor", getNonlinearFactor); + +} diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp new file mode 100644 index 000000000..84e82f376 --- /dev/null +++ b/python/handwritten/nonlinear/Values.cpp @@ -0,0 +1,80 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Values class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/Values.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" +#include "gtsam/navigation/ImuBias.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Values::print, 0, 1); + +void exportValues(){ + + typedef imuBias::ConstantBias Bias; + + bool (Values::*exists1)(Key) const = &Values::exists; + void (Values::*insert_point2)(Key, const gtsam::Point2&) = &Values::insert; + void (Values::*insert_rot2) (Key, const gtsam::Rot2&) = &Values::insert; + void (Values::*insert_pose2) (Key, const gtsam::Pose2&) = &Values::insert; + void (Values::*insert_point3)(Key, const gtsam::Point3&) = &Values::insert; + void (Values::*insert_rot3) (Key, const gtsam::Rot3&) = &Values::insert; + void (Values::*insert_pose3) (Key, const gtsam::Pose3&) = &Values::insert; + void (Values::*insert_bias) (Key, const Bias&) = &Values::insert; + void (Values::*insert_vector3) (Key, const gtsam::Vector3&) = &Values::insert; + + class_("Values", init<>()) + .def(init()) + .def("clear", &Values::clear) + .def("dim", &Values::dim) + .def("empty", &Values::empty) + .def("equals", &Values::equals) + .def("erase", &Values::erase) + .def("insert_fixed", &Values::insertFixed) + .def("print", &Values::print, print_overloads(args("s"))) + .def("size", &Values::size) + .def("swap", &Values::swap) + .def("insert", insert_point2) + .def("insert", insert_rot2) + .def("insert", insert_pose2) + .def("insert", insert_point3) + .def("insert", insert_rot3) + .def("insert", insert_pose3) + .def("insert", insert_bias) + .def("insert", insert_vector3) + .def("atPoint2", &Values::at, return_value_policy()) + .def("atRot2", &Values::at, return_value_policy()) + .def("atPose2", &Values::at, return_value_policy()) + .def("atPoint3", &Values::at, return_value_policy()) + .def("atRot3", &Values::at, return_value_policy()) + .def("atPose3", &Values::at, return_value_policy()) + .def("atConstantBias", &Values::at, return_value_policy()) + .def("atVector3", &Values::at, return_value_policy()) + .def("exists", exists1) + .def("keys", &Values::keys) + ; +} diff --git a/python/handwritten/slam/BearingFactor.cpp b/python/handwritten/slam/BearingFactor.cpp new file mode 100644 index 000000000..2d4688bde --- /dev/null +++ b/python/handwritten/slam/BearingFactor.cpp @@ -0,0 +1,16 @@ +#include + +#define NO_IMPORT_ARRAY +#include + +#include + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +template +void exportBearingFactor(const std::string& name) { + class_(name, init<>()); +} diff --git a/python/handwritten/slam/BetweenFactor.cpp b/python/handwritten/slam/BetweenFactor.cpp new file mode 100644 index 000000000..0e0949fb5 --- /dev/null +++ b/python/handwritten/slam/BetweenFactor.cpp @@ -0,0 +1,57 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps BetweenFactor for several values to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/slam/BetweenFactor.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +// template +// void exportBetweenFactor(const std::string& name){ +// class_(name, init<>()) +// .def(init()) +// ; +// } + +#define BETWEENFACTOR(T) \ + class_< BetweenFactor, bases, boost::shared_ptr< BetweenFactor > >("BetweenFactor"#T) \ + .def(init()) \ + .def("measured", &BetweenFactor::measured, return_internal_reference<>()) \ +; + +void exportBetweenFactors() +{ + BETWEENFACTOR(Point2) + BETWEENFACTOR(Rot2) + BETWEENFACTOR(Pose2) + BETWEENFACTOR(Point3) + BETWEENFACTOR(Rot3) + BETWEENFACTOR(Pose3) +} diff --git a/python/handwritten/slam/GenericProjectionFactor.cpp b/python/handwritten/slam/GenericProjectionFactor.cpp new file mode 100644 index 000000000..dd932ccd4 --- /dev/null +++ b/python/handwritten/slam/GenericProjectionFactor.cpp @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps GenericProjectionFactor for several values to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/slam/ProjectionFactor.h" +#include "gtsam/geometry/Pose3.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Cal3_S2.h" + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, GenericProjectionFactorCal3_S2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, GenericProjectionFactorCal3_S2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(evaluateError_overloads, GenericProjectionFactorCal3_S2::evaluateError, 2, 4) + +void exportGenericProjectionFactor() +{ + + class_ >("GenericProjectionFactorCal3_S2", init<>()) + .def(init &, optional >()) + .def(init &, bool, bool, optional >()) + .def("print", &GenericProjectionFactorCal3_S2::print, print_overloads(args("s"))) + .def("equals", &GenericProjectionFactorCal3_S2::equals, equals_overloads(args("q","tol"))) + .def("evaluate_error", &GenericProjectionFactorCal3_S2::evaluateError, evaluateError_overloads()) + .def("measured", &GenericProjectionFactorCal3_S2::measured, return_value_policy()) + // TODO(Ellon): Find the right return policy when returning a 'const shared_ptr<...> &' + // .def("calibration", &GenericProjectionFactorCal3_S2::calibration, return_value_policy()) + .def("verbose_cheirality", &GenericProjectionFactorCal3_S2::verboseCheirality) + .def("throw_cheirality", &GenericProjectionFactorCal3_S2::throwCheirality) + ; + +} diff --git a/python/handwritten/slam/PriorFactor.cpp b/python/handwritten/slam/PriorFactor.cpp new file mode 100644 index 000000000..3ada91f43 --- /dev/null +++ b/python/handwritten/slam/PriorFactor.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps PriorFactor for several values to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/slam/PriorFactor.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +// template< class FACTOR, class VALUE > +// void exportPriorFactor(const std::string& name){ +// class_< FACTOR >(name.c_str(), init<>()) +// .def(init< Key, VALUE&, SharedNoiseModel >()) +// ; +// } + +#define PRIORFACTOR(VALUE) \ + class_< PriorFactor, bases, boost::shared_ptr< PriorFactor > >("PriorFactor"#VALUE) \ + .def(init()) \ + .def("prior", &PriorFactor::prior, return_internal_reference<>()) \ +; + +void exportPriorFactors() +{ + PRIORFACTOR(Point2) + PRIORFACTOR(Rot2) + PRIORFACTOR(Pose2) + PRIORFACTOR(Point3) + PRIORFACTOR(Rot3) + PRIORFACTOR(Pose3) + PRIORFACTOR(Vector3) +} diff --git a/python/handwritten/utils/NumpyEigen.cpp b/python/handwritten/utils/NumpyEigen.cpp new file mode 100644 index 000000000..d7cebe7ad --- /dev/null +++ b/python/handwritten/utils/NumpyEigen.cpp @@ -0,0 +1,54 @@ + /* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief register conversion matrix between numpy and Eigen + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/base/Matrix.h" +#include "gtsam/base/Vector.h" + +using namespace boost::python; +using namespace gtsam; + +void registerNumpyEigenConversions() +{ + // NOTE: import array should be called only in the cpp defining the module + // import_array(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + +} diff --git a/python/include/numpy_eigen/NumpyEigenConverter.hpp b/python/include/numpy_eigen/NumpyEigenConverter.hpp new file mode 100755 index 000000000..4cf16a974 --- /dev/null +++ b/python/include/numpy_eigen/NumpyEigenConverter.hpp @@ -0,0 +1,334 @@ +/** + * @file NumpyEigenConverter.hpp + * @author Paul Furgale + * @date Fri Feb 4 11:17:25 2011 + * + * @brief Classes to support conversion from numpy arrays in Python + * to Eigen3 matrices in c++ + * + * + */ + +#ifndef NUMPY_EIGEN_CONVERTER_HPP +#define NUMPY_EIGEN_CONVERTER_HPP + +#include +//#include + +#include "numpy/numpyconfig.h" +#ifdef NPY_1_7_API_VERSION +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#define NPE_PY_ARRAY_OBJECT PyArrayObject +#else +//TODO Remove this as soon as support for Numpy version before 1.7 is dropped +#define NPE_PY_ARRAY_OBJECT PyObject +#endif + +#define PY_ARRAY_UNIQUE_SYMBOL NP_Eigen_AS +#include + +#include "type_traits.hpp" +#include +#include "copy_routines.hpp" + + + +/** + * @class NumpyEigenConverter + * @tparam the Eigen3 matrix type this class is specialized for + * + * adapted from http://misspent.wordpress.com/2009/09/27/how-to-write-boost-python-converters/ + * General help available http://docs.scipy.org/doc/numpy/reference/c-api.array.html + * + * To use: + * + * #include + * + * + * BOOST_PYTHON_MODULE(libmy_module_python) + * { + * // The converters will cause a segfault unless import_array() is called before the first one + * import_array(); + * NumpyEigenConverter >::register_converter(); + * NumpyEigenConverter >::register_converter(); + * } + * + */ +template +struct NumpyEigenConverter +{ + + typedef EIGEN_MATRIX_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + enum { + RowsAtCompileTime = matrix_t::RowsAtCompileTime, + ColsAtCompileTime = matrix_t::ColsAtCompileTime, + MaxRowsAtCompileTime = matrix_t::MaxRowsAtCompileTime, + MaxColsAtCompileTime = matrix_t::MaxColsAtCompileTime, + NpyType = TypeToNumPy::NpyType, + //Flags = ei_compute_matrix_flags<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::ret, + //CoeffReadCost = NumTraits::ReadCost, + Options = matrix_t::Options + //InnerStrideAtCompileTime = 1, + //OuterStrideAtCompileTime = (Options&RowMajor) ? ColsAtCompileTime : RowsAtCompileTime + }; + + static std::string castSizeOption(int option) + { + if(option == Eigen::Dynamic) + return "Dynamic"; + else + return boost::lexical_cast(option); + } + + static std::string toString() + { + return std::string() + "Eigen::Matrix<" + TypeToNumPy::typeString() + ", " + + castSizeOption(RowsAtCompileTime) + ", " + + castSizeOption(ColsAtCompileTime) + ", " + + boost::lexical_cast((int)Options) + ", " + + castSizeOption(MaxRowsAtCompileTime) + ", " + + castSizeOption(MaxColsAtCompileTime) + ">"; + } + + // The "Convert from C to Python" API + static PyObject * convert(const matrix_t & M) + { + PyObject * P = NULL; + if(RowsAtCompileTime == 1 || ColsAtCompileTime == 1) + { + // Create a 1D array + npy_intp dimensions[1]; + dimensions[0] = M.size(); + P = PyArray_SimpleNew(1, dimensions, TypeToNumPy::NpyType); + numpyTypeDemuxer< CopyEigenToNumpyVector >(&M, reinterpret_cast(P)); + } + else + { + // create a 2D array. + npy_intp dimensions[2]; + dimensions[0] = M.rows(); + dimensions[1] = M.cols(); + P = PyArray_SimpleNew(2, dimensions, TypeToNumPy::NpyType); + numpyTypeDemuxer< CopyEigenToNumpyMatrix >(&M, reinterpret_cast(P)); + } + + // incrementing the reference seems to cause a memory leak. + // boost::python::incref(P); + // This agrees with the sample code found here: + // http://mail.python.org/pipermail/cplusplus-sig/2008-October/013825.html + return P; + } + + static bool isDimensionValid(int requestedSize, int sizeAtCompileTime, int maxSizeAtCompileTime) + { + bool valid = true; + if(sizeAtCompileTime == Eigen::Dynamic) + { + // Check for dynamic fixed size + // http://eigen.tuxfamily.org/dox-devel/TutorialMatrixClass.html#TutorialMatrixOptTemplParams + if(!(maxSizeAtCompileTime == Eigen::Dynamic || requestedSize <= maxSizeAtCompileTime)) + { + valid = false; + } + } + else if(sizeAtCompileTime != requestedSize) + { + valid = false; + } + return valid; + } + + static void checkMatrixSizes(NPE_PY_ARRAY_OBJECT * obj_ptr) + { + int rows = PyArray_DIM(obj_ptr, 0); + int cols = PyArray_DIM(obj_ptr, 1); + + bool rowsValid = isDimensionValid(rows, RowsAtCompileTime, MaxRowsAtCompileTime); + bool colsValid = isDimensionValid(cols, ColsAtCompileTime, MaxColsAtCompileTime); + if(!rowsValid || !colsValid) + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + } + + static void checkRowVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr, int cols) + { + if(!isDimensionValid(cols, ColsAtCompileTime, MaxColsAtCompileTime)) + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + } + + static void checkColumnVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr, int rows) + { + // Check if the type can accomidate one column. + if(ColsAtCompileTime == Eigen::Dynamic || ColsAtCompileTime == 1) + { + if(!isDimensionValid(rows, RowsAtCompileTime, MaxRowsAtCompileTime)) + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + } + else + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + + } + + static void checkVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr) + { + int size = PyArray_DIM(obj_ptr, 0); + + // If the number of rows is fixed at 1, assume that is the sense of the vector. + // Otherwise, assume it is a column. + if(RowsAtCompileTime == 1) + { + checkRowVectorSizes(obj_ptr, size); + } + else + { + checkColumnVectorSizes(obj_ptr, size); + } + } + + + static void* convertible(PyObject *obj_ptr) + { + // Check for a null pointer. + if(!obj_ptr) + { + //THROW_TYPE_ERROR("PyObject pointer was null"); + return 0; + } + + // Make sure this is a numpy array. + if (!PyArray_Check(obj_ptr)) + { + //THROW_TYPE_ERROR("Conversion is only defined for numpy array and matrix types"); + return 0; + } + + NPE_PY_ARRAY_OBJECT * array_ptr = reinterpret_cast(obj_ptr); + + // Check the type of the array. + int npyType = getNpyType(array_ptr); + + if(!TypeToNumPy::canConvert(npyType)) + { + //THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + // << ". Mismatched types."); + return 0; + } + + + + // Check the array dimensions. + int nd = PyArray_NDIM(array_ptr); + + if(nd != 1 && nd != 2) + { + THROW_TYPE_ERROR("Conversion is only valid for arrays with 1 or 2 dimensions. Argument has " << nd << " dimensions"); + } + + if(nd == 1) + { + checkVectorSizes(array_ptr); + } + else + { + // Two-dimensional matrix type. + checkMatrixSizes(array_ptr); + } + + + return obj_ptr; + } + + + static void construct(PyObject *obj_ptr, boost::python::converter::rvalue_from_python_stage1_data *data) + { + boost::python::converter::rvalue_from_python_storage * matData = reinterpret_cast * >(data); + void* storage = matData->storage.bytes; + + // Make sure storage is 16byte aligned. With help from code from Memory.h + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + matrix_t * Mp = new (aligned) matrix_t(); + // Stash the memory chunk pointer for later use by boost.python + // This signals boost::python that the new value must be deleted eventually + data->convertible = storage; + + + // std::cout << "Creating aligned pointer " << aligned << " from storage " << storage << std::endl; + // std::cout << "matrix size: " << sizeof(matrix_t) << std::endl; + // std::cout << "referent size: " << boost::python::detail::referent_size< matrix_t & >::value << std::endl; + // std::cout << "sizeof(storage): " << sizeof(matData->storage) << std::endl; + // std::cout << "sizeof(bytes): " << sizeof(matData->storage.bytes) << std::endl; + + + + matrix_t & M = *Mp; + + if (!PyArray_Check(obj_ptr)) + { + THROW_TYPE_ERROR("construct is only defined for numpy array and matrix types"); + } + + NPE_PY_ARRAY_OBJECT * array_ptr = reinterpret_cast(obj_ptr); + + int nd = PyArray_NDIM(array_ptr); + if(nd == 1) + { + int size = PyArray_DIM(array_ptr, 0); + // This is a vector type + if(RowsAtCompileTime == 1) + { + // Row Vector + M.resize(1,size); + } + else + { + // Column Vector + M.resize(size,1); + } + numpyTypeDemuxer< CopyNumpyToEigenVector >(&M, array_ptr); + } + else + { + int rows = PyArray_DIM(array_ptr, 0); + int cols = PyArray_DIM(array_ptr, 1); + + M.resize(rows,cols); + numpyTypeDemuxer< CopyNumpyToEigenMatrix >(&M, array_ptr); + } + + + + + } + + + // The registration function. + static void register_converter() + { + boost::python::to_python_converter(); + boost::python::converter::registry::push_back( + &NumpyEigenConverter::convertible, + &NumpyEigenConverter::construct, + boost::python::type_id()); + + } + +}; + + + + +#endif /* NUMPY_EIGEN_CONVERTER_HPP */ diff --git a/python/include/numpy_eigen/README.md b/python/include/numpy_eigen/README.md new file mode 100644 index 000000000..bd5a3f44d --- /dev/null +++ b/python/include/numpy_eigen/README.md @@ -0,0 +1,6 @@ +numpy_eigen +=========== + +A boost python converter that handles the copy of matrices from the Eigen linear algebra library in C++ to numpy in Python. + +This is a minimal version based on the original from Paul Furgale (https://github.com/ethz-asl/Schweizer-Messer/tree/master/numpy_eigen) \ No newline at end of file diff --git a/python/include/numpy_eigen/boost_python_headers.hpp b/python/include/numpy_eigen/boost_python_headers.hpp new file mode 100755 index 000000000..5499d2917 --- /dev/null +++ b/python/include/numpy_eigen/boost_python_headers.hpp @@ -0,0 +1,250 @@ +/** + * @file boost_python_headers.hpp + * @author Paul Furgale + * @date Mon Dec 12 10:36:03 2011 + * + * @brief A header that specializes boost-python to work with fixed-sized Eigen types. + * + * The original version of this library did not include these specializations and this caused + * assert failures when running on Ubuntu 10.04 32-bit. More information about fixed-size + * vectorizable types in Eigen is available here: + * http://eigen.tuxfamily.org/dox-devel/TopicFixedSizeVectorizable.html + * + * This code has been tested on Ubunutu 10.04 64 and 32 bit, OSX Snow Leopard and OSX Lion. + * + * This code is derived from boost/python/converter/arg_from_python.hpp + * Copyright David Abrahams 2002. + * Distributed under the Boost Software License, Version 1.0. (See http://www.boost.org/LICENSE_1_0.txt) + * + */ +#ifndef NUMPY_EIGEN_CONVERTERS_HPP +#define NUMPY_EIGEN_CONVERTERS_HPP + +#include +#include +#include +#include +#include +#include + + +namespace boost { namespace python { namespace detail { + template + struct referent_size; + + // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types + template + struct referent_size< Eigen::Matrix& > + { + // Add 16 bytes so we can get alignment + BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix) + 16); + }; + + // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types + template + struct referent_size< Eigen::Matrix const & > + { + // Add 16 bytes so we can get alignment + BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix) + 16); + }; + + // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types + template + struct referent_size< Eigen::Matrix > + { + // Add 16 bytes so we can get alignment + BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix) + 16); + }; + + + }}} + +namespace boost { namespace python { namespace converter { + + + template + struct rvalue_from_python_data< Eigen::Matrix const &> : rvalue_from_python_storage< Eigen::Matrix const & > + { + typedef typename Eigen::Matrix T; +# if (!defined(__MWERKS__) || __MWERKS__ >= 0x3000) \ + && (!defined(__EDG_VERSION__) || __EDG_VERSION__ >= 245) \ + && (!defined(__DECCXX_VER) || __DECCXX_VER > 60590014) \ + && !defined(BOOST_PYTHON_SYNOPSIS) /* Synopsis' OpenCXX has trouble parsing this */ + // This must always be a POD struct with m_data its first member. + BOOST_STATIC_ASSERT(BOOST_PYTHON_OFFSETOF(rvalue_from_python_storage,stage1) == 0); +# endif + + // The usual constructor + rvalue_from_python_data(rvalue_from_python_stage1_data const & _stage1) + { + this->stage1 = _stage1; + } + + + // This constructor just sets m_convertible -- used by + // implicitly_convertible<> to perform the final step of the + // conversion, where the construct() function is already known. + rvalue_from_python_data(void* convertible) + { + this->stage1.convertible = convertible; + } + + // Destroys any object constructed in the storage. + ~rvalue_from_python_data() + { + // Realign the pointer and destroy + if (this->stage1.convertible == this->storage.bytes) + { + void * storage = reinterpret_cast(this->storage.bytes); + T * aligned = reinterpret_cast(reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16)); + + //std::cout << "Destroying " << (void*)aligned << std::endl; + aligned->T::~T(); + } + } + private: + typedef typename add_reference::type>::type ref_type; + }; + + + + + + // Used when T is a plain value (non-pointer, non-reference) type or + // a (non-volatile) const reference to a plain value type. + template + struct arg_rvalue_from_python< Eigen::Matrix > + { + typedef Eigen::Matrix const & T; + typedef typename boost::add_reference< + T + // We can't add_const here, or it would be impossible to pass + // auto_ptr args from Python to C++ + >::type result_type; + + arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered::converters)) + , m_source(obj) + { + + } + bool convertible() const + { + return m_data.stage1.convertible != 0; + } + + +# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196 + typename arg_rvalue_from_python:: +# endif + result_type operator()() + { + if (m_data.stage1.construct != 0) + m_data.stage1.construct(m_source, &m_data.stage1); + + // Here is the magic... + // Realign the pointer + void * storage = reinterpret_cast(m_data.storage.bytes); + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0); + } + + private: + rvalue_from_python_data m_data; + PyObject* m_source; + }; + + + // Used when T is a plain value (non-pointer, non-reference) type or + // a (non-volatile) const reference to a plain value type. + template + struct arg_rvalue_from_python< Eigen::Matrix const & > + { + typedef Eigen::Matrix const & T; + typedef typename boost::add_reference< + T + // We can't add_const here, or it would be impossible to pass + // auto_ptr args from Python to C++ + >::type result_type; + + arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered::converters)) + , m_source(obj) + { + + } + bool convertible() const + { + return m_data.stage1.convertible != 0; + } + + +# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196 + typename arg_rvalue_from_python:: +# endif + result_type operator()() + { + if (m_data.stage1.construct != 0) + m_data.stage1.construct(m_source, &m_data.stage1); + + // Here is the magic... + // Realign the pointer + void * storage = reinterpret_cast(m_data.storage.bytes); + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0); + } + + private: + rvalue_from_python_data m_data; + PyObject* m_source; + }; + + // Used when T is a plain value (non-pointer, non-reference) type or + // a (non-volatile) const reference to a plain value type. + template + struct arg_rvalue_from_python< Eigen::Matrix const > + { + typedef Eigen::Matrix const & T; + typedef typename boost::add_reference< + T + // We can't add_const here, or it would be impossible to pass + // auto_ptr args from Python to C++ + >::type result_type; + + arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered::converters)) + , m_source(obj) + { + + } + bool convertible() const + { + return m_data.stage1.convertible != 0; + } + + +# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196 + typename arg_rvalue_from_python:: +# endif + result_type operator()() + { + if (m_data.stage1.construct != 0) + m_data.stage1.construct(m_source, &m_data.stage1); + + // Here is the magic... + // Realign the pointer + void * storage = reinterpret_cast(m_data.storage.bytes); + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0); + } + + private: + rvalue_from_python_data m_data; + PyObject* m_source; + }; + + + }}} + + +#endif /* NUMPY_EIGEN_CONVERTERS_HPP */ diff --git a/python/include/numpy_eigen/copy_routines.hpp b/python/include/numpy_eigen/copy_routines.hpp new file mode 100755 index 000000000..b112a7426 --- /dev/null +++ b/python/include/numpy_eigen/copy_routines.hpp @@ -0,0 +1,149 @@ +#ifndef NUMPY_EIGEN_COPY_ROUTINES_HPP +#define NUMPY_EIGEN_COPY_ROUTINES_HPP + + +template +struct CopyNumpyToEigenMatrix +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int r = 0; r < M_->rows(); r++) + { + for(int c = 0; c < M_->cols(); c++) + { + T * p = static_cast(PyArray_GETPTR2(P_, r, c)); + (*M_)(r,c) = static_cast(*p); + } + } + } + +}; + +template +struct CopyEigenToNumpyMatrix +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int r = 0; r < M_->rows(); r++) + { + for(int c = 0; c < M_->cols(); c++) + { + T * p = static_cast(PyArray_GETPTR2(P_, r, c)); + *p = static_cast((*M_)(r,c)); + } + } + } + +}; + +template +struct CopyEigenToNumpyVector +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int i = 0; i < M_->size(); i++) + { + T * p = static_cast(PyArray_GETPTR1(P_, i)); + *p = static_cast((*M_)(i)); + } + } + +}; + + +template +struct CopyNumpyToEigenVector +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int i = 0; i < M_->size(); i++) + { + T * p = static_cast(PyArray_GETPTR1(P_, i)); + (*M_)(i) = static_cast(*p); + } + } + +}; + + + + +// Crazy syntax in this function was found here: +// http://stackoverflow.com/questions/1840253/c-template-member-function-of-template-class-called-from-template-function/1840318#1840318 +template< typename FUNCTOR_T> +inline void numpyTypeDemuxer(typename FUNCTOR_T::matrix_t * M, NPE_PY_ARRAY_OBJECT * P) +{ + FUNCTOR_T f; + + int npyType = getNpyType(P); + switch(npyType) + { + case NPY_BOOL: + f.template exec(M,P); + break; + case NPY_BYTE: + f.template exec(M,P); + break; + case NPY_UBYTE: + f.template exec(M,P); + break; + case NPY_SHORT: + f.template exec(M,P); + break; + case NPY_USHORT: + f.template exec(M,P); + break; + case NPY_INT: + f.template exec(M,P); + break; + case NPY_UINT: + f.template exec(M,P); + break; + case NPY_LONG: + f.template exec(M,P); + break; + case NPY_ULONG: + f.template exec(M,P); + break; + case NPY_LONGLONG: + f.template exec(M,P); + break; + case NPY_ULONGLONG: + f.template exec(M,P); + break; + case NPY_FLOAT: + f.template exec(M,P); + break; + case NPY_DOUBLE: + f.template exec(M,P); + break; + case NPY_LONGDOUBLE: + f.template exec(M,P); + break; + default: + THROW_TYPE_ERROR("Unsupported type: " << npyTypeToString(npyType)); + } +} + + +#endif /* NUMPY_EIGEN_COPY_ROUTINES_HPP */ diff --git a/python/include/numpy_eigen/type_traits.hpp b/python/include/numpy_eigen/type_traits.hpp new file mode 100755 index 000000000..97e4bb5a0 --- /dev/null +++ b/python/include/numpy_eigen/type_traits.hpp @@ -0,0 +1,191 @@ +#ifndef NUMPY_EIGEN_TYPE_TRAITS_HPP +#define NUMPY_EIGEN_TYPE_TRAITS_HPP + +#define THROW_TYPE_ERROR(msg) \ + { \ + std::stringstream type_error_ss; \ + type_error_ss << msg; \ + PyErr_SetString(PyExc_TypeError, type_error_ss.str().c_str()); \ + throw boost::python::error_already_set(); \ + } + + +//////////////////////////////////////////////// +// TypeToNumPy +// Defines helper functions based on the Eigen3 matrix type that +// decide what conversions can happen Eigen3 --> NumPy +// Also, converts a type to a NumPy enum. +template struct TypeToNumPy; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_INT }; + static const char * npyString() { return "NPY_INT"; } + static const char * typeString() { return "int"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_LONG; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_LONG }; + static const char * npyString() { return "NPY_LONG"; } + static const char * typeString() { return "long"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_LONG; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_UINT64 }; + static const char * npyString() { return "NPY_UINT64"; } + static const char * typeString() { return "uint64"; } + static bool canConvert(int type) + { + return type == NPY_UINT8 || type == NPY_USHORT || + type == NPY_UINT16 || type == NPY_UINT32 || + type == NPY_ULONG || type == NPY_ULONGLONG || + type == NPY_UINT64; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_UBYTE }; + static const char * npyString() { return "NPY_UBYTE"; } + static const char * typeString() { return "unsigned char"; } + static bool canConvert(int type) + { + return type == NPY_UBYTE || type == NPY_BYTE || type == NPY_CHAR; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_BYTE }; + static const char * npyString() { return "NPY_BYTE"; } + static const char * typeString() { return "char"; } + static bool canConvert(int type) + { + return type == NPY_UBYTE || type == NPY_BYTE || type == NPY_CHAR; + } +}; + + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_FLOAT }; + static const char * npyString() { return "NPY_FLOAT"; } + static const char * typeString() { return "float"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_FLOAT || type == NPY_LONG; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_DOUBLE }; + static const char * npyString() { return "NPY_DOUBLE"; } + static const char * typeString() { return "double"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_FLOAT || type == NPY_DOUBLE || type == NPY_LONG; + } +}; + + +inline int getNpyType(PyObject * obj_ptr){ + return PyArray_ObjectType(obj_ptr, 0); +} + +#ifdef NPY_1_7_API_VERSION +inline int getNpyType(PyArrayObject * obj_ptr){ + PyArray_Descr * descr = PyArray_MinScalarType(obj_ptr); + if (descr == NULL){ + THROW_TYPE_ERROR("Unsupported type: PyArray_MinScalarType returned null!"); + } + return descr->type_num; +} +#endif + +inline const char * npyTypeToString(int npyType) +{ + switch(npyType) + { + case NPY_BOOL: + return "NPY_BOOL"; + case NPY_BYTE: + return "NPY_BYTE"; + case NPY_UBYTE: + return "NPY_UBYTE"; + case NPY_SHORT: + return "NPY_SHORT"; + case NPY_USHORT: + return "NPY_USHORT"; + case NPY_INT: + return "NPY_INT"; + case NPY_UINT: + return "NPY_UINT"; + case NPY_LONG: + return "NPY_LONG"; + case NPY_ULONG: + return "NPY_ULONG"; + case NPY_LONGLONG: + return "NPY_LONGLONG"; + case NPY_ULONGLONG: + return "NPY_ULONGLONG"; + case NPY_FLOAT: + return "NPY_FLOAT"; + case NPY_DOUBLE: + return "NPY_DOUBLE"; + case NPY_LONGDOUBLE: + return "NPY_LONGDOUBLE"; + case NPY_CFLOAT: + return "NPY_CFLOAT"; + case NPY_CDOUBLE: + return "NPY_CDOUBLE"; + case NPY_CLONGDOUBLE: + return "NPY_CLONGDOUBLE"; + case NPY_OBJECT: + return "NPY_OBJECT"; + case NPY_STRING: + return "NPY_STRING"; + case NPY_UNICODE: + return "NPY_UNICODE"; + case NPY_VOID: + return "NPY_VOID"; + case NPY_NTYPES: + return "NPY_NTYPES"; + case NPY_NOTYPE: + return "NPY_NOTYPE"; + case NPY_CHAR: + return "NPY_CHAR"; + default: + return "Unknown type"; + } +} + +inline std::string npyArrayTypeString(NPE_PY_ARRAY_OBJECT * obj_ptr) +{ + std::stringstream ss; + int nd = PyArray_NDIM(obj_ptr); + ss << "numpy.array<" << npyTypeToString(getNpyType(obj_ptr)) << ">["; + if(nd > 0) + { + ss << PyArray_DIM(obj_ptr, 0); + for(int i = 1; i < nd; i++) + { + ss << ", " << PyArray_DIM(obj_ptr, i); + } + } + ss << "]"; + return ss.str(); +} + + +#endif /* NUMPY_EIGEN_TYPE_TRAITS_HPP */ diff --git a/python/setup.py.in b/python/setup.py.in new file mode 100644 index 000000000..d3b5fcde4 --- /dev/null +++ b/python/setup.py.in @@ -0,0 +1,15 @@ +from distutils.core import setup + +setup(name='gtsam', + version='${GTSAM_VERSION_STRING}', + description='GTSAM Python wrapper', + license = "BSD", + author='Frank Dellaert et. al', + author_email='frank.dellaert@gatech.edu', + maintainer_email='gtsam@lists.gatech.edu', + url='https://collab.cc.gatech.edu/borg/gtsam', + package_dir={ '': '${CMAKE_CURRENT_SOURCE_DIR}' }, + packages=['gtsam', 'gtsam_utils', 'gtsam_examples', 'gtsam_tests'], + #package_data={'gtsam' : ['_libgtsam_python.so']}, # location of .so file is relative to package_dir + data_files=[('${PY_INSTALL_FOLDER}/gtsam/', ['gtsam/_libgtsam_python.so'])], # location of .so file relative to setup.py + ) diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 48cf03f8c..bef69edb6 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -345,7 +345,6 @@ TEST(ExpressionFactor, tree) { } /* ************************************************************************* */ - TEST(ExpressionFactor, Compose1) { // Create expression @@ -600,6 +599,66 @@ TEST(Expression, testMultipleCompositions2) { EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum4_, values, fd_step, tolerance); } +/* ************************************************************************* */ +// Test multiplication with the inverse of a matrix +TEST(ExpressionFactor, MultiplyWithInverse) { + auto model = noiseModel::Isotropic::Sigma(3, 1); + + // Create expression + Vector3_ f_expr(MultiplyWithInverse<3>(), Expression(0), Vector3_(1)); + + // Check derivatives + Values values; + Matrix3 A = Vector3(1, 2, 3).asDiagonal(); + A(0, 1) = 0.1; + A(0, 2) = 0.1; + const Vector3 b(0.1, 0.2, 0.3); + values.insert(0, A); + values.insert(1, b); + ExpressionFactor factor(model, Vector3::Zero(), f_expr); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5); +} + +/* ************************************************************************* */ +// Test multiplication with the inverse of a matrix function +namespace test_operator { +Vector3 f(const Point2& a, const Vector3& b, OptionalJacobian<3, 2> H1, + OptionalJacobian<3, 3> H2) { + Matrix3 A = Vector3(1, 2, 3).asDiagonal(); + A(0, 1) = a.x(); + A(0, 2) = a.y(); + A(1, 0) = a.x(); + if (H1) *H1 << b.y(), b.z(), b.x(), 0, 0, 0; + if (H2) *H2 = A; + return A * b; +}; +} + +TEST(ExpressionFactor, MultiplyWithInverseFunction) { + auto model = noiseModel::Isotropic::Sigma(3, 1); + + using test_operator::f; + Vector3_ f_expr(MultiplyWithInverseFunction(f), + Expression(0), Vector3_(1)); + + // Check derivatives + Point2 a(1, 2); + const Vector3 b(0.1, 0.2, 0.3); + Matrix32 H1; + Matrix3 A; + const Vector Ab = f(a, b, H1, A); + CHECK(assert_equal(A * b, Ab)); + CHECK(assert_equal(numericalDerivative11( + boost::bind(f, _1, b, boost::none, boost::none), a), + H1)); + + Values values; + values.insert(0, a); + values.insert(1, b); + ExpressionFactor factor(model, Vector3::Zero(), f_expr); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5); +} + /* ************************************************************************* */ int main() { TestResult tr;