diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index da398ad23..3c1275a55 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -27,7 +27,7 @@ jobs: name: [ ubuntu-20.04-gcc-9, ubuntu-20.04-clang-9, - ubuntu-22.04-gcc-11, + ubuntu-22.04-gcc-12, ubuntu-22.04-clang-14, ] @@ -44,7 +44,7 @@ jobs: compiler: clang version: "9" - - name: ubuntu-22.04-gcc-11 + - name: ubuntu-22.04-gcc-12 os: ubuntu-22.04 compiler: gcc version: "11" diff --git a/CMakeLists.txt b/CMakeLists.txt index e3b462eec..262f38121 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,23 +1,8 @@ -cmake_minimum_required(VERSION 3.9) -if (POLICY CMP0082) - cmake_policy(SET CMP0082 NEW) # install from sub-directories immediately -endif() -if (POLICY CMP0102) -cmake_policy(SET CMP0102 NEW) # set policy on advanced variables and cmake cache -endif() -if (POLICY CMP0156) -cmake_policy(SET CMP0156 NEW) # new linker strategies -endif() +cmake_minimum_required(VERSION 3.9...3.29) if (POLICY CMP0167) cmake_policy(SET CMP0167 OLD) # Don't complain about boost endif() -# allow parent project to override options -if (POLICY CMP0077) - cmake_policy(SET CMP0077 NEW) -endif(POLICY CMP0077) - - # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 3) @@ -47,9 +32,9 @@ if(MSVC) set(CMAKE_EXE_LINKER_FLAGS ${MSVC_LINKER_FLAGS}) set(CMAKE_MODULE_LINKER_FLAGS ${MSVC_LINKER_FLAGS}) set(CMAKE_SHARED_LINKER_FLAGS ${MSVC_LINKER_FLAGS}) - set(CMAKE_STATIC_LINKER_FLAGS ${MSVC_LINKER_FLAGS}) endif() +set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake") include(GtsamMakeConfigFile) include(GNUInstallDirs) @@ -71,14 +56,7 @@ include(GtsamPrinting) ############### Decide on BOOST ###################################### # Enable or disable serialization with GTSAM_ENABLE_BOOST_SERIALIZATION option(GTSAM_ENABLE_BOOST_SERIALIZATION "Enable Boost serialization" ON) -if(GTSAM_ENABLE_BOOST_SERIALIZATION) - add_definitions(-DGTSAM_ENABLE_BOOST_SERIALIZATION) -endif() - option(GTSAM_USE_BOOST_FEATURES "Enable Features that use Boost" ON) -if(GTSAM_USE_BOOST_FEATURES) - add_definitions(-DGTSAM_USE_BOOST_FEATURES) -endif() if(GTSAM_ENABLE_BOOST_SERIALIZATION OR GTSAM_USE_BOOST_FEATURES) include(cmake/HandleBoost.cmake) diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 691068148..c3fa83234 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -129,7 +129,7 @@ protected: result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); } #define CHECK_EQUAL(expected,actual)\ -{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); } +{ if (!((expected) == (actual))) { result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); return; } } #define LONGS_EQUAL(expected,actual)\ { long actualTemp = actual; \ diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 9911b3764..733672fbe 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -22,7 +22,7 @@ In GTSAM, all properties and operations needed to use a type must be defined thr In detail, we ask that the following items are defined in the traits object (although, not all are needed for optimization): * values: - * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1. + * `inline constexpr static auto dimension = D;`, a constexpr that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1. * types: * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix`. * `ChartJacobian`, a typedef for `OptionalJacobian`. diff --git a/INSTALL.md b/INSTALL.md index 10bee196c..51f00fd3b 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -27,7 +27,7 @@ $ make install downloaded from https://www.threadingbuildingblocks.org/ - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually - achieved with MKL disabled. We therefore advise you to benchmark your problem + achieved with MKL disabled. We therefore advise you to benchmark your problem before using MKL. Tested compilers: @@ -128,12 +128,12 @@ We support several build configurations for GTSAM (case insensitive) #### CMAKE_INSTALL_PREFIX -The install folder. The default is typically `/usr/local/`. +The install folder. The default is typically `/usr/local/`. To configure to install to your home directory, you could execute: ```cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME ..``` -#### GTSAM_TOOLBOX_INSTALL_PATH +#### GTSAM_TOOLBOX_INSTALL_PATH The Matlab toolbox will be installed in a subdirectory of this folder, called 'gtsam'. @@ -142,7 +142,7 @@ of this folder, called 'gtsam'. #### GTSAM_BUILD_CONVENIENCE_LIBRARIES -This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam. +This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam. Set with the command line as follows: ```cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..``` @@ -159,6 +159,16 @@ Set with the command line as follows: ON: When enabled, libgtsam_unstable will be built and installed with the same options as libgtsam. In addition, if tests are enabled, the unit tests will be built as well. The Matlab toolbox will also be generated if the matlab toolbox is enabled, installing into a folder called `gtsam_unstable`. OFF (Default) If disabled, no `gtsam_unstable` code will be included in build or install. +## Convenience Options: + +#### GTSAM_BUILD_EXAMPLES_ALWAYS + +Whether or not to force building examples, can be true or false. + +#### GTSAM_BUILD_TESTS + +Whether or not to build tests, can be true or false. + ## Check `make check` will build and run all of the tests. Note that the tests will only be @@ -179,7 +189,7 @@ Here are some tips to get the best possible performance out of GTSAM. 1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode. 2. Enable TBB. On modern processors with multiple cores, this can easily speed up - optimization by 30-50%. Please note that this may not be true for very small + optimization by 30-50%. Please note that this may not be true for very small problems where the overhead of dispatching work to multiple threads outweighs the benefit. We recommend that you benchmark your problem with/without TBB. 3. Use `GTSAM_BUILD_WITH_MARCH_NATIVE`. A performance gain of diff --git a/cmake/FindGooglePerfTools.cmake b/cmake/FindGooglePerfTools.cmake index 01243257b..4af268528 100644 --- a/cmake/FindGooglePerfTools.cmake +++ b/cmake/FindGooglePerfTools.cmake @@ -1,42 +1,40 @@ # -*- cmake -*- -# - Find Google perftools -# Find the Google perftools includes and libraries -# This module defines -# GOOGLE_PERFTOOLS_INCLUDE_DIR, where to find heap-profiler.h, etc. -# GOOGLE_PERFTOOLS_FOUND, If false, do not try to use Google perftools. -# also defined for general use are -# TCMALLOC_LIBRARY, where to find the tcmalloc library. - -FIND_PATH(GOOGLE_PERFTOOLS_INCLUDE_DIR google/heap-profiler.h -/usr/local/include -/usr/include -) +# - Find GPerfTools (formerly Google perftools) +# Find the GPerfTools libraries +# If false, do not try to use Google perftools. +# Also defined for general use are +# - GPERFTOOLS_TCMALLOC: where to find the tcmalloc library +# - GPERFTOOLS_PROFILER: where to find the profiler library SET(TCMALLOC_NAMES ${TCMALLOC_NAMES} tcmalloc) -FIND_LIBRARY(TCMALLOC_LIBRARY +find_library(GPERFTOOLS_TCMALLOC NAMES ${TCMALLOC_NAMES} PATHS /usr/lib /usr/local/lib - ) +) +find_library(GPERFTOOLS_PROFILER + NAMES profiler + PATHS /usr/lib /usr/local/lib +) -IF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) - SET(TCMALLOC_LIBRARIES ${TCMALLOC_LIBRARY}) - SET(GOOGLE_PERFTOOLS_FOUND "YES") -ELSE (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) - SET(GOOGLE_PERFTOOLS_FOUND "NO") -ENDIF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) +IF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) + SET(TCMALLOC_LIBRARIES ${GPERFTOOLS_TCMALLOC}) + SET(GPERFTOOLS_FOUND "YES") +ELSE (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) + SET(GPERFTOOLS_FOUND "NO") +ENDIF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) -IF (GOOGLE_PERFTOOLS_FOUND) - IF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY) - MESSAGE(STATUS "Found Google perftools: ${GOOGLE_PERFTOOLS_LIBRARIES}") - ENDIF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY) -ELSE (GOOGLE_PERFTOOLS_FOUND) +IF (GPERFTOOLS_FOUND) + MESSAGE(STATUS "Found Gperftools: ${GPERFTOOLS_PROFILER}") +ELSE (GPERFTOOLS_FOUND) IF (GOOGLE_PERFTOOLS_FIND_REQUIRED) MESSAGE(FATAL_ERROR "Could not find Google perftools library") ENDIF (GOOGLE_PERFTOOLS_FIND_REQUIRED) -ENDIF (GOOGLE_PERFTOOLS_FOUND) +ENDIF (GPERFTOOLS_FOUND) MARK_AS_ADVANCED( - TCMALLOC_LIBRARY - GOOGLE_PERFTOOLS_INCLUDE_DIR - ) + GPERFTOOLS_TCMALLOC + GPERFTOOLS_PROFILER +) + +option(GTSAM_ENABLE_GPERFTOOLS "Enable/Disable Gperftools" OFF) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 476354549..cf0dfdfc7 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -108,13 +108,15 @@ endif() # Other (non-preprocessor macros) compiler flags: if(MSVC) + set(CMAKE_3_15 $) + set(CMAKE_3_25 $) # Common to all configurations, next for each configuration: - set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON /W3 /GR /EHsc /MP CACHE STRING "(User editable) Private compiler flags for all configurations.") - set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG /MDd /Zi /Ob0 /Od /RTC1 CACHE STRING "(User editable) Private compiler flags for Debug configuration.") - set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO /MD /O2 /D /Zi /d2Zi+ CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.") - set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE /MD /O2 CACHE STRING "(User editable) Private compiler flags for Release configuration.") - set(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING /MD /O2 /Zi CACHE STRING "(User editable) Private compiler flags for Profiling configuration.") - set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING /MD /O2 CACHE STRING "(User editable) Private compiler flags for Timing configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON /W3 /WX /GR /EHsc /MP CACHE STRING "(User editable) Private compiler flags for all configurations.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG $<${CMAKE_3_15}:/MDd> $<${CMAKE_3_25}:/Zi> /Ob0 /Od /RTC1 CACHE STRING "(User editable) Private compiler flags for Debug configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO $<${CMAKE_3_15}:/MD> /O2 $<${CMAKE_3_25}:/Zi> CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE $<${CMAKE_3_15}:/MD> /O2 CACHE STRING "(User editable) Private compiler flags for Release configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING $<${CMAKE_3_15}:/MD> /O2 $<${CMAKE_3_25}:/Zi> CACHE STRING "(User editable) Private compiler flags for Profiling configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING $<${CMAKE_3_15}:/MD> /O2 CACHE STRING "(User editable) Private compiler flags for Timing configuration.") else() # Common to all configurations, next for each configuration: @@ -129,13 +131,15 @@ else() endif() set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON + -Werror # Enable warnings as errors -Wall # Enable common warnings - -fPIC # ensure proper code generation for shared libraries - $<$:-Wreturn-local-addr -Werror=return-local-addr> # Error: return local address - $<$:-Wreturn-stack-address -Werror=return-stack-address> # Error: return local address - $<$:-Wno-weak-template-vtables> # TODO(dellaert): don't know how to resolve + -Wpedantic # Enable pedantic warnings + $<$:-Wextra -Wno-unused-parameter> # Enable extra warnings, but ignore no-unused-parameter (as we ifdef out chunks of code) + $<$:-Wreturn-local-addr> # Error: return local address + $<$:-Wreturn-stack-address> # Error: return local address + $<$:-Wno-weak-template-vtables> # TODO(dellaert): don't know how to resolve $<$:-Wno-weak-vtables> # TODO(dellaert): don't know how to resolve - -Wreturn-type -Werror=return-type # Error on missing return() + -Wreturn-type # Error on missing return() -Wformat -Werror=format-security # Error on wrong printf() arguments $<$:${flag_override_}> # Enforce the use of the override keyword # @@ -155,20 +159,9 @@ mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING) mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING) # Enable C++17: -if (NOT CMAKE_VERSION VERSION_LESS 3.8) - set(GTSAM_COMPILE_FEATURES_PUBLIC "cxx_std_17" CACHE STRING "CMake compile features property for all gtsam targets.") - # See: https://cmake.org/cmake/help/latest/prop_tgt/CXX_EXTENSIONS.html - set(CMAKE_CXX_EXTENSIONS OFF) - if (MSVC) - # NOTE(jlblanco): seems to be required in addition to the cxx_std_17 above? - list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC /std:c++17) - endif() -else() - # Old cmake versions: - if (NOT MSVC) - list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC $<$:-std=c++17>) - endif() -endif() +set(GTSAM_COMPILE_FEATURES_PUBLIC "cxx_std_17" CACHE STRING "CMake compile features property for all gtsam targets.") +# See: https://cmake.org/cmake/help/latest/prop_tgt/CXX_EXTENSIONS.html +set(CMAKE_CXX_EXTENSIONS OFF) # Merge all user-defined flags into the variables that are to be actually used by CMake: foreach(build_type "common" ${GTSAM_CMAKE_CONFIGURATION_TYPES}) @@ -178,6 +171,12 @@ foreach(build_type "common" ${GTSAM_CMAKE_CONFIGURATION_TYPES}) append_config_if_not_empty(GTSAM_COMPILE_DEFINITIONS_PUBLIC ${build_type}) endforeach() +# Check if timing is enabled and add appropriate definition flag +if(GTSAM_ENABLE_TIMING AND(NOT ${CMAKE_BUILD_TYPE} EQUAL "Timing")) + message(STATUS "Enabling timing for non-timing build") + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE "ENABLE_TIMING") +endif() + # Linker flags: set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") @@ -251,9 +250,9 @@ endif() # Make common binary output directory when on Windows if(WIN32) - set(RUNTIME_OUTPUT_PATH "${GTSAM_BINARY_DIR}/bin") - set(EXECUTABLE_OUTPUT_PATH "${GTSAM_BINARY_DIR}/bin") - set(LIBRARY_OUTPUT_PATH "${GTSAM_BINARY_DIR}/lib") + set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/bin") + set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/lib") + set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/lib") endif() # Set up build type list for cmake-gui diff --git a/cmake/HandleAllocators.cmake b/cmake/HandleAllocators.cmake index 63411b17b..38297bbba 100644 --- a/cmake/HandleAllocators.cmake +++ b/cmake/HandleAllocators.cmake @@ -7,7 +7,7 @@ else() list(APPEND possible_allocators BoostPool STL) set(preferred_allocator STL) endif() -if(GOOGLE_PERFTOOLS_FOUND) +if(GPERFTOOLS_FOUND) list(APPEND possible_allocators tcmalloc) endif() diff --git a/cmake/HandleEigen.cmake b/cmake/HandleEigen.cmake index 707593889..becebafb9 100644 --- a/cmake/HandleEigen.cmake +++ b/cmake/HandleEigen.cmake @@ -13,12 +13,6 @@ if(GTSAM_USE_SYSTEM_EIGEN) # Since Eigen 3.3.0 a Eigen3Config.cmake is available so use it. find_package(Eigen3 CONFIG REQUIRED) # need to find again as REQUIRED - # The actual include directory (for BUILD cmake target interface): - # Note: EIGEN3_INCLUDE_DIR points to some random location on some eigen - # versions. So here I use the target itself to get the proper include - # directory (it is generated by cmake, thus has the correct path) - get_target_property(GTSAM_EIGEN_INCLUDE_FOR_BUILD Eigen3::Eigen INTERFACE_INCLUDE_DIRECTORIES) - # check if MKL is also enabled - can have one or the other, but not both! # Note: Eigen >= v3.2.5 includes our patches if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5)) @@ -30,6 +24,8 @@ if(GTSAM_USE_SYSTEM_EIGEN) if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4)) message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.") endif() + + set(GTSAM_EIGEN_VERSION "${EIGEN3_VERSION}") else() # Use bundled Eigen include path. # Clear any variables set by FindEigen3 @@ -46,7 +42,7 @@ else() add_library(gtsam_eigen3 INTERFACE) - target_include_directories(gtsam_eigen3 INTERFACE + target_include_directories(gtsam_eigen3 SYSTEM INTERFACE $ $ ) @@ -56,11 +52,8 @@ else() list(APPEND GTSAM_EXPORTED_TARGETS gtsam_eigen3) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}") -endif() - -# Detect Eigen version: -set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h") -if (EXISTS ${EIGEN_VER_H}) + # Detect Eigen version: + set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h") file(READ "${EIGEN_VER_H}" STR_EIGEN_VERSION) # Extract the Eigen version from the Macros.h file, lines "#define EIGEN_WORLD_VERSION XX", etc... @@ -75,11 +68,9 @@ if (EXISTS ${EIGEN_VER_H}) string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${GTSAM_EIGEN_VERSION_MINOR}") set(GTSAM_EIGEN_VERSION "${GTSAM_EIGEN_VERSION_WORLD}.${GTSAM_EIGEN_VERSION_MAJOR}.${GTSAM_EIGEN_VERSION_MINOR}") +endif() - message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}") -else() - message(WARNING "Cannot determine Eigen version, missing file: `${EIGEN_VER_H}`") -endif () +message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}") if (MSVC) if (GTSAM_SHARED_LIB) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 8c56ae242..e5e2e194a 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -21,17 +21,20 @@ if (NOT MSVC) endif() # Configurable Options +option(BUILD_SHARED_LIBS "Build shared libraries" ON) if(GTSAM_UNSTABLE_AVAILABLE) option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON) option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF) endif() -option(GTSAM_FORCE_SHARED_LIB "Force gtsam to be a shared library, overriding BUILD_SHARED_LIBS" ON) +option(GTSAM_FORCE_SHARED_LIB "Force gtsam to be a shared library, overriding BUILD_SHARED_LIBS" OFF) option(GTSAM_FORCE_STATIC_LIB "Force gtsam to be a static library, overriding BUILD_SHARED_LIBS" OFF) option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) option(GTSAM_DT_MERGING "Enable/Disable merging of equal leaf nodes in DecisionTrees. This leads to significant speed up and memory savings." ON) +option(GTSAM_ENABLE_TIMING "Enable the timing tools (gttic/gttoc)" OFF) +option(GTSAM_HYBRID_TIMING "Enable the timing of hybrid factor graph machinery" OFF) option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) option(GTSAM_ENABLE_MEMORY_SANITIZER "Enable/Disable memory sanitizer" OFF) option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) @@ -46,7 +49,9 @@ option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF) option(GTSAM_SLOW_BUT_CORRECT_EXPMAP "Use slower but correct expmap for Pose2" OFF) -if (GTSAM_FORCE_SHARED_LIB) +if (GTSAM_FORCE_SHARED_LIB AND GTSAM_FORCE_STATIC_LIB) + message(FATAL_ERROR "GTSAM_FORCE_SHARED_LIB and GTSAM_FORCE_STATIC_LIB are both true. Please, to unambiguously select the desired library type to use to build GTSAM, set one of GTSAM_FORCE_SHARED_LIB=ON, GTSAM_FORCE_STATIC_LIB=ON, or BUILD_SHARED_LIBS={ON/OFF}") +elseif (GTSAM_FORCE_SHARED_LIB) message(STATUS "GTSAM is a shared library due to GTSAM_FORCE_SHARED_LIB") set(GTSAM_LIBRARY_TYPE SHARED CACHE STRING "" FORCE) set(GTSAM_SHARED_LIB 1 CACHE BOOL "" FORCE) @@ -55,10 +60,9 @@ elseif (GTSAM_FORCE_STATIC_LIB) set(GTSAM_LIBRARY_TYPE STATIC CACHE STRING "" FORCE) set(GTSAM_SHARED_LIB 0 CACHE BOOL "" FORCE) elseif (BUILD_SHARED_LIBS) - message(STATUS "GTSAM is a shared library due to BUILD_SHARED_LIBS is ON") set(GTSAM_LIBRARY_TYPE SHARED CACHE STRING "" FORCE) set(GTSAM_SHARED_LIB 1 CACHE BOOL "" FORCE) -elseif((DEFINED BUILD_SHARED_LIBS) AND (NOT BUILD_SHARED_LIBS)) +elseif(NOT BUILD_SHARED_LIBS) message(STATUS "GTSAM is a static library due to BUILD_SHARED_LIBS is OFF") set(GTSAM_LIBRARY_TYPE STATIC CACHE STRING "" FORCE) set(GTSAM_SHARED_LIB 0 CACHE BOOL "" FORCE) diff --git a/cmake/HandlePerfTools.cmake b/cmake/HandlePerfTools.cmake index 499caf80a..4b5e5c8b3 100644 --- a/cmake/HandlePerfTools.cmake +++ b/cmake/HandlePerfTools.cmake @@ -1,4 +1,4 @@ ############################################################################### # Find Google perftools -find_package(GooglePerfTools) +find_package(GooglePerfTools) \ No newline at end of file diff --git a/cmake/HandlePrintConfiguration.cmake b/cmake/HandlePrintConfiguration.cmake index fcbdc9027..17693e46c 100644 --- a/cmake/HandlePrintConfiguration.cmake +++ b/cmake/HandlePrintConfiguration.cmake @@ -91,6 +91,7 @@ print_enabled_config(${GTSAM_ENABLE_MEMORY_SANITIZER} "Build with Memory San print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") print_enabled_config(${GTSAM_DT_MERGING} "Enable branch merging in DecisionTree") +print_enabled_config(${GTSAM_ENABLE_TIMING} "Enable timing machinery") print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V43} "Allow features deprecated in GTSAM 4.3") print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp index 6ce4f76fa..436b6e283 100644 --- a/examples/CombinedImuFactorsExample.cpp +++ b/examples/CombinedImuFactorsExample.cpp @@ -48,6 +48,7 @@ #include #include #include +#include using namespace gtsam; using namespace std; diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index 342f1f220..7cbc8989f 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -49,6 +49,7 @@ #include #include +#include #include #include diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index a2ad8cf0b..bc4e6e340 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -29,6 +29,8 @@ #include #include +#include + using namespace std; using namespace gtsam; diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt index 190b73db9..d1c8cfe0d 100644 --- a/gtsam/3rdparty/cephes/CMakeLists.txt +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -89,7 +89,7 @@ set(CEPHES_SOURCES cephes/zetac.c) # Add library source files -add_library(cephes-gtsam SHARED ${CEPHES_SOURCES}) +add_library(cephes-gtsam ${GTSAM_LIBRARY_TYPE} ${CEPHES_SOURCES}) # Add include directory (aka headers) target_include_directories( @@ -103,11 +103,7 @@ set_target_properties( C_STANDARD 99) if(WIN32) - set_target_properties( - cephes-gtsam - PROPERTIES PREFIX "" - COMPILE_FLAGS /w - RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin") + set_target_properties(cephes-gtsam PROPERTIES COMPILE_FLAGS /w) endif() if(APPLE) diff --git a/gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake b/gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake index 7ea5bab94..229e39a90 100644 --- a/gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake +++ b/gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake @@ -15,7 +15,6 @@ option(GKRAND "enable GKRAND support" OFF) # Add compiler flags. if(MSVC) - set(GKlib_COPTS "/Ox") set(GKlib_COPTIONS "-DWIN32 -DMSC -D_CRT_SECURE_NO_DEPRECATE -DUSE_GKREGEX") elseif(MINGW) set(GKlib_COPTS "-DUSE_GKREGEX") diff --git a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt index 92f931b98..8a35c2f6b 100644 --- a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt @@ -3,7 +3,6 @@ include_directories(.) # Find sources. file(GLOB metis_sources *.c) # Build libmetis. -add_definitions(-fPIC) add_library(metis-gtsam ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources}) if(UNIX) target_link_libraries(metis-gtsam m) @@ -11,9 +10,8 @@ endif() if(WIN32) set_target_properties(metis-gtsam PROPERTIES - PREFIX "" COMPILE_FLAGS /w - RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin") + WINDOWS_EXPORT_ALL_SYMBOLS ON) endif() if (APPLE) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index ef4a0defd..db4a982a0 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -151,6 +151,10 @@ if (GTSAM_USE_EIGEN_MKL) target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR}) endif() +if (GTSAM_ENABLE_GPERFTOOLS AND GPERFTOOLS_FOUND) + target_link_libraries(gtsam PRIVATE ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER}) +endif() + # Add includes for source directories 'BEFORE' boost and any system include # paths so that the compiler uses GTSAM headers in our source directory instead # of any previously installed GTSAM headers. @@ -183,10 +187,7 @@ if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with PREFIX "lib") target_compile_definitions(gtsam PRIVATE GTSAM_IMPORT_STATIC) else() - set_target_properties(gtsam PROPERTIES - PREFIX "" - DEFINE_SYMBOL GTSAM_EXPORTS - RUNTIME_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/bin") + set_target_properties(gtsam PROPERTIES DEFINE_SYMBOL GTSAM_EXPORTS) endif() endif() diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h index 372f02d06..8ce44dda3 100644 --- a/gtsam/base/ConcurrentMap.h +++ b/gtsam/base/ConcurrentMap.h @@ -48,7 +48,7 @@ using ConcurrentMapBase = gtsam::FastMap; #endif -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #endif @@ -85,6 +85,8 @@ public: /** Copy constructor from the base map class */ ConcurrentMap(const Base& x) : Base(x) {} + ConcurrentMap& operator=(const ConcurrentMap& other) = default; + /** Handy 'exists' function */ bool exists(const KEY& e) const { return this->count(e); } @@ -101,7 +103,7 @@ public: #endif private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h index 414c1e83f..821575ca9 100644 --- a/gtsam/base/FastList.h +++ b/gtsam/base/FastList.h @@ -20,7 +20,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #if BOOST_VERSION >= 107400 @@ -62,6 +62,8 @@ public: /// Construct from c++11 initializer list: FastList(std::initializer_list l) : Base(l) {} + FastList& operator=(const FastList& other) = default; + #ifdef GTSAM_ALLOCATOR_BOOSTPOOL /** Copy constructor from a standard STL container */ FastList(const std::list& x) { @@ -79,7 +81,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h index e8ef3fc4f..30052908d 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -19,7 +19,7 @@ #pragma once #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #endif @@ -54,6 +54,8 @@ public: /** Copy constructor from another FastMap */ FastMap(const FastMap& x) : Base(x) {} + FastMap& operator=(const FastMap& x) = default; + /** Copy constructor from the base map class */ FastMap(const Base& x) : Base(x) {} @@ -69,7 +71,7 @@ public: bool exists(const KEY& e) const { return this->find(e) != this->end(); } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index a1be08d80..1a2627e24 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -18,7 +18,9 @@ #pragma once -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#include + +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #if BOOST_VERSION >= 107400 #include @@ -78,6 +80,8 @@ public: Base(x) { } + FastSet& operator=(const FastSet& other) = default; + #ifdef GTSAM_ALLOCATOR_BOOSTPOOL /** Copy constructor from a standard STL container */ FastSet(const std::set& x) { @@ -123,7 +127,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index bb92b6b2e..396ca16ba 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -56,9 +56,10 @@ public: GenericValue(){} /// Construct from value - GenericValue(const T& value) : - value_(value) { - } + GenericValue(const T& value) : Value(), + value_(value) {} + + GenericValue(const GenericValue& other) = default; /// Return a constant value const T& value() const { @@ -112,7 +113,7 @@ public: * Clone this value (normal clone on the heap, delete with 'delete' operator) */ std::shared_ptr clone() const override { - return std::allocate_shared(Eigen::aligned_allocator(), *this); + return std::allocate_shared(Eigen::aligned_allocator(), *this); } /// Generic Value interface version of retract @@ -173,7 +174,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -184,9 +185,8 @@ public: } #endif - // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; + constexpr static const bool NeedsToAlign = (sizeof(T) % 16) == 0; public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index ce021e10e..862ae6f4d 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -36,7 +36,7 @@ namespace gtsam { template struct LieGroup { - enum { dimension = N }; + inline constexpr static auto dimension = N; typedef OptionalJacobian ChartJacobian; typedef Eigen::Matrix Jacobian; typedef Eigen::Matrix TangentVector; @@ -183,7 +183,7 @@ struct LieGroupTraits: GetDimensionImpl { /// @name Manifold /// @{ typedef Class ManifoldType; - enum { dimension = Class::dimension }; + inline constexpr static auto dimension = Class::dimension; typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index cb30fa9c0..922b8dd2f 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -55,7 +55,7 @@ namespace internal { template struct HasManifoldPrereqs { - enum { dim = Class::dimension }; + inline constexpr static auto dim = Class::dimension; Class p, q; Eigen::Matrix v; @@ -95,7 +95,7 @@ struct ManifoldTraits: GetDimensionImpl { GTSAM_CONCEPT_ASSERT(HasManifoldPrereqs); // Dimension of the manifold - enum { dimension = Class::dimension }; + inline constexpr static auto dimension = Class::dimension; // Typedefs required by all manifold types. typedef Class ManifoldType; diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 247c53cce..1e04bc58a 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 6dba9cb05..c8dc46ed5 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -479,7 +479,7 @@ struct MultiplyWithInverse { */ template struct MultiplyWithInverseFunction { - enum { M = traits::dimension }; + inline constexpr static auto M = traits::dimension; typedef Eigen::Matrix VectorN; typedef Eigen::Matrix MatrixN; diff --git a/gtsam/base/MatrixSerialization.h b/gtsam/base/MatrixSerialization.h index 9cf730230..43c97097d 100644 --- a/gtsam/base/MatrixSerialization.h +++ b/gtsam/base/MatrixSerialization.h @@ -19,11 +19,12 @@ // \callgraph // Defined only if boost serialization is enabled -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #pragma once #include +#include #include #include #include @@ -87,6 +88,45 @@ void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) { split_free(ar, m, version); } +/******************************************************************************/ +/// Customized functions for serializing Eigen::SparseVector +template +void save(Archive& ar, const Eigen::SparseVector<_Scalar, _Options, _Index>& m, + const unsigned int /*version*/) { + _Index size = m.size(); + + std::vector> data; + for (typename Eigen::SparseVector<_Scalar, _Options, _Index>::InnerIterator + it(m); + it; ++it) + data.push_back({it.index(), it.value()}); + + ar << BOOST_SERIALIZATION_NVP(size); + ar << BOOST_SERIALIZATION_NVP(data); +} + +template +void load(Archive& ar, Eigen::SparseVector<_Scalar, _Options, _Index>& m, + const unsigned int /*version*/) { + _Index size; + ar >> BOOST_SERIALIZATION_NVP(size); + m.resize(size); + + std::vector> data; + ar >> BOOST_SERIALIZATION_NVP(data); + + for (auto&& d : data) { + m.coeffRef(d.first) = d.second; + } +} + +template +void serialize(Archive& ar, Eigen::SparseVector<_Scalar, _Options, _Index>& m, + const unsigned int version) { + split_free(ar, m, version); +} +/******************************************************************************/ + } // namespace serialization } // namespace boost #endif diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index beb376c19..bb7ba72b8 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -32,8 +32,8 @@ class ProductLieGroup: public std::pair { typedef std::pair Base; protected: - enum {dimension1 = traits::dimension}; - enum {dimension2 = traits::dimension}; + constexpr static const size_t dimension1 = traits::dimension; + constexpr static const size_t dimension2 = traits::dimension; public: /// Default constructor yields identity @@ -67,9 +67,9 @@ public: /// @name Manifold /// @{ - enum {dimension = dimension1 + dimension2}; - inline static size_t Dim() {return dimension;} - inline size_t dim() const {return dimension;} + inline constexpr static auto dimension = dimension1 + dimension2; + inline static size_t Dim() { return dimension; } + inline size_t dim() const { return dimension; } typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 1783857cb..378e91144 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -21,7 +21,7 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -386,7 +386,7 @@ namespace gtsam { template friend class SymmetricBlockMatrixBlockExpr; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index b5068ad95..f0ea79a2e 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -32,7 +32,8 @@ namespace gtsam { /** * Equals testing for basic types */ -inline bool assert_equal(const Key& expected, const Key& actual, double tol = 0.0) { +inline bool assert_equal(const Key& expected, const Key& actual) { + // TODO - why isn't tol used? if(expected != actual) { std::cout << "Not equal:\nexpected: " << expected << "\nactual: " << actual << std::endl; return false; diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index c36de42ab..e80b9d4a4 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -21,7 +21,7 @@ #include // Configuration from CMake #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #endif @@ -38,6 +38,9 @@ namespace gtsam { */ class GTSAM_EXPORT Value { public: + // todo - not sure if valid + Value() = default; + Value(const Value& other) = default; /** Clone this value in a special memory pool, must be deleted with Value::deallocate_, *not* with the 'delete' operator. */ virtual Value* clone_() const = 0; @@ -121,7 +124,7 @@ namespace gtsam { * The last two links explain why these export lines have to be in the same source module that includes * any of the archive class headers. * */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE & /*ar*/, const unsigned int /*version*/) { @@ -132,6 +135,6 @@ namespace gtsam { } /* namespace gtsam */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION BOOST_SERIALIZATION_ASSUME_ABSTRACT(gtsam::Value) #endif diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index ce7aeba7b..dbd48dfc8 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index f9a5cf079..4ef374234 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -16,6 +16,7 @@ * @author Frank Dellaert * @author Alex Hagiopol * @author Varun Agrawal + * @author Fan Jiang */ // \callgraph @@ -193,14 +194,12 @@ GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b); */ template inline double dot(const V1 &a, const V2& b) { - assert (b.size()==a.size()); return a.dot(b); } /** compatibility version for ublas' inner_prod() */ template inline double inner_prod(const V1 &a, const V2& b) { - assert (b.size()==a.size()); return a.dot(b); } diff --git a/gtsam/base/VectorSerialization.h b/gtsam/base/VectorSerialization.h index 7acf108f4..39d7e0299 100644 --- a/gtsam/base/VectorSerialization.h +++ b/gtsam/base/VectorSerialization.h @@ -17,7 +17,7 @@ */ // Defined only if boost serialization is enabled -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #pragma once #include diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index a9e9ca106..c5c4ad622 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -163,7 +163,7 @@ struct VectorSpaceImpl { template struct HasVectorSpacePrereqs { - enum { dim = Class::dimension }; + inline constexpr static auto dim = Class::dimension; Class p, q; Vector v; @@ -197,7 +197,7 @@ GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs); /// @name Manifold /// @{ - enum { dimension = Class::dimension}; + inline constexpr static auto dimension = Class::dimension; typedef Class ManifoldType; /// @} }; @@ -232,7 +232,7 @@ struct ScalarTraits : VectorSpaceImpl { /// @name Manifold /// @{ typedef Scalar ManifoldType; - enum { dimension = 1 }; + inline constexpr static auto dimension = 1; typedef Eigen::Matrix TangentVector; typedef OptionalJacobian<1, 1> ChartJacobian; @@ -305,7 +305,7 @@ struct traits > : /// @name Manifold /// @{ - enum { dimension = M*N}; + inline constexpr static auto dimension = M * N; typedef Fixed ManifoldType; typedef Eigen::Matrix TangentVector; typedef Eigen::Matrix Jacobian; @@ -377,7 +377,7 @@ struct DynamicTraits { /// @name Manifold /// @{ - enum { dimension = Eigen::Dynamic }; + inline constexpr static auto dimension = Eigen::Dynamic; typedef Eigen::VectorXd TangentVector; typedef Eigen::MatrixXd Jacobian; typedef OptionalJacobian ChartJacobian; diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 777441300..0ff8a8ae2 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -21,6 +21,8 @@ #include #include +#include + namespace gtsam { // Forward declarations @@ -219,7 +221,7 @@ namespace gtsam { friend class SymmetricBlockMatrix; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 71cef2524..9fee627fe 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -21,6 +21,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index c9f76ca9f..6ff1a44d4 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -8,7 +8,7 @@ #pragma once -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #include #include #include diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index 18612bc22..ef7ed497f 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -17,9 +17,12 @@ * @date Feb 7, 2012 */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #pragma once +#include + +#if GTSAM_ENABLE_BOOST_SERIALIZATION + #include #include #include diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 09b36cdc8..cc6dadafd 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -17,9 +17,12 @@ * @date Feb 7, 2012 */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #pragma once +#include + +#if GTSAM_ENABLE_BOOST_SERIALIZATION + #include #include #include diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 7e0f2e844..e11a167ff 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -11,7 +11,7 @@ #pragma once // Defined only if boost serialization is enabled -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include @@ -52,6 +52,10 @@ namespace boost { namespace serialization { struct U; } } namespace std { template<> struct is_trivially_default_constructible : std::false_type {}; } namespace std { template<> struct is_trivially_copy_constructible : std::false_type {}; } namespace std { template<> struct is_trivially_move_constructible : std::false_type {}; } +// QCC (The QNX GCC-based Compiler) also has this issue, but it also extends to trivial destructor. +#if defined(__QNX__) +namespace std { template<> struct is_trivially_destructible : std::false_type {}; } +#endif #endif #endif diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 1eb638b1b..02984b8c1 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1151,7 +1151,7 @@ TEST(Matrix, Matrix24IsVectorSpace) { } TEST(Matrix, RowMajorIsVectorSpace) { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES typedef Eigen::Matrix RowMajor; GTSAM_CONCEPT_ASSERT(IsVectorSpace); #endif @@ -1166,7 +1166,7 @@ TEST(Matrix, VectorIsVectorSpace) { } TEST(Matrix, RowVectorIsVectorSpace) { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES typedef Eigen::Matrix RowVector; GTSAM_CONCEPT_ASSERT(IsVectorSpace); GTSAM_CONCEPT_ASSERT(IsVectorSpace); diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index e8838a476..b14f699a3 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -18,6 +18,8 @@ #include #include +#include + using namespace std; using namespace gtsam; diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index d95b14292..46d44bb80 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -272,7 +272,7 @@ TEST(Vector, VectorIsVectorSpace) { } TEST(Vector, RowVectorIsVectorSpace) { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES typedef Eigen::Matrix RowVector; GTSAM_CONCEPT_ASSERT(IsVectorSpace); #endif diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index b43595066..9e9d1d113 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -31,7 +31,9 @@ namespace gtsam { namespace internal { - + +using ChildOrder = FastMap>; + // a static shared_ptr to TimingOutline with nullptr as the pointer const static std::shared_ptr nullTimingOutline; @@ -57,7 +59,7 @@ void TimingOutline::add(size_t usecs, size_t usecsWall) { TimingOutline::TimingOutline(const std::string& label, size_t id) : id_(id), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_( 0), lastChildOrder_(0), label_(label) { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #ifdef GTSAM_USING_NEW_BOOST_TIMERS timer_.stop(); #endif @@ -66,7 +68,7 @@ TimingOutline::TimingOutline(const std::string& label, size_t id) : /* ************************************************************************* */ size_t TimingOutline::time() const { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES size_t time = 0; bool hasChildren = false; for(const ChildMap::value_type& child: children_) { @@ -84,14 +86,13 @@ size_t TimingOutline::time() const { /* ************************************************************************* */ void TimingOutline::print(const std::string& outline) const { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES std::string formattedLabel = label_; std::replace(formattedLabel.begin(), formattedLabel.end(), '_', ' '); std::cout << outline << "-" << formattedLabel << ": " << self() << " CPU (" << n_ << " times, " << wall() << " wall, " << secs() << " children, min: " << min() << " max: " << max() << ")\n"; // Order children - typedef FastMap > ChildOrder; ChildOrder childOrder; for(const ChildMap::value_type& child: children_) { childOrder[child.second->myOrder_] = child.second; @@ -106,9 +107,57 @@ void TimingOutline::print(const std::string& outline) const { #endif } +/* ************************************************************************* */ +void TimingOutline::printCsvHeader(bool addLineBreak) const { +#ifdef GTSAM_USE_BOOST_FEATURES + // Order is (CPU time, number of times, wall time, time + children in seconds, + // min time, max time) + std::cout << label_ + " cpu time (s)" << "," << label_ + " #calls" << "," + << label_ + " wall time(s)" << "," << label_ + " subtree time (s)" + << "," << label_ + " min time (s)" << "," << label_ + "max time(s)" + << ","; + // Order children + ChildOrder childOrder; + for (const ChildMap::value_type& child : children_) { + childOrder[child.second->myOrder_] = child.second; + } + // Print children + for (const ChildOrder::value_type& order_child : childOrder) { + order_child.second->printCsvHeader(); + } + if (addLineBreak) { + std::cout << std::endl; + } + std::cout.flush(); +#endif +} + +/* ************************************************************************* */ +void TimingOutline::printCsv(bool addLineBreak) const { +#ifdef GTSAM_USE_BOOST_FEATURES + // Order is (CPU time, number of times, wall time, time + children in seconds, + // min time, max time) + std::cout << self() << "," << n_ << "," << wall() << "," << secs() << "," + << min() << "," << max() << ","; + // Order children + ChildOrder childOrder; + for (const ChildMap::value_type& child : children_) { + childOrder[child.second->myOrder_] = child.second; + } + // Print children + for (const ChildOrder::value_type& order_child : childOrder) { + order_child.second->printCsv(false); + } + if (addLineBreak) { + std::cout << std::endl; + } + std::cout.flush(); +#endif +} + void TimingOutline::print2(const std::string& outline, const double parentTotal) const { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES const int w1 = 24, w2 = 2, w3 = 6, w4 = 8, precision = 2; const double selfTotal = self(), selfMean = selfTotal / double(n_); const double childTotal = secs(); @@ -153,7 +202,7 @@ void TimingOutline::print2(const std::string& outline, /* ************************************************************************* */ const std::shared_ptr& TimingOutline::child(size_t child, const std::string& label, const std::weak_ptr& thisPtr) { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES assert(thisPtr.lock().get() == this); std::shared_ptr& result = children_[child]; if (!result) { @@ -172,7 +221,7 @@ const std::shared_ptr& TimingOutline::child(size_t child, /* ************************************************************************* */ void TimingOutline::tic() { // Disable this entire function if we are not using boost -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(timer_.is_stopped()); timer_.start(); @@ -191,7 +240,7 @@ void TimingOutline::tic() { /* ************************************************************************* */ void TimingOutline::toc() { // Disable this entire function if we are not using boost -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #ifdef GTSAM_USING_NEW_BOOST_TIMERS @@ -225,7 +274,7 @@ void TimingOutline::toc() { /* ************************************************************************* */ void TimingOutline::finishedIteration() { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES if (tIt_ > tMax_) tMax_ = tIt_; if (tMin_ == 0 || tIt_ < tMin_) @@ -240,7 +289,7 @@ void TimingOutline::finishedIteration() { /* ************************************************************************* */ size_t getTicTocID(const char *descriptionC) { // disable anything which refers to TimingOutline as well, for good measure -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES const std::string description(descriptionC); // Global (static) map from strings to ID numbers and current next ID number static size_t nextId = 0; @@ -263,7 +312,7 @@ size_t getTicTocID(const char *descriptionC) { /* ************************************************************************* */ void tic(size_t id, const char *labelC) { // disable anything which refers to TimingOutline as well, for good measure -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES const std::string label(labelC); std::shared_ptr node = // gCurrentTimer.lock()->child(id, label, gCurrentTimer); @@ -275,7 +324,7 @@ void tic(size_t id, const char *labelC) { /* ************************************************************************* */ void toc(size_t id, const char *labelC) { // disable anything which refers to TimingOutline as well, for good measure -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES const std::string label(labelC); std::shared_ptr current(gCurrentTimer.lock()); if (id != current->id_) { diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index 99c55a3d7..5a96633db 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -21,7 +21,7 @@ #include #include // for GTSAM_USE_TBB -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #include #endif @@ -107,7 +107,7 @@ // have matching gttic/gttoc statments. You may want to consider reorganizing your timing // outline to match the scope of your code. -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES // Automatically use the new Boost timers if version is recent enough. #if BOOST_VERSION >= 104800 # ifndef GTSAM_DISABLE_NEW_TIMERS @@ -165,7 +165,7 @@ namespace gtsam { ChildMap children_; ///< subtrees // disable all timers if not using boost -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #ifdef GTSAM_USING_NEW_BOOST_TIMERS boost::timer::cpu_timer timer_; #else @@ -183,7 +183,7 @@ namespace gtsam { GTSAM_EXPORT TimingOutline(const std::string& label, size_t myId); GTSAM_EXPORT size_t time() const; ///< time taken, including children double secs() const { return double(time()) / 1000000.0;} ///< time taken, in seconds, including children -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES double self() const { return double(t_) / 1000000.0;} ///< self time only, in seconds double wall() const { return double(tWall_) / 1000000.0;} ///< wall time, in seconds double min() const { return double(tMin_) / 1000000.0;} ///< min time, in seconds @@ -199,6 +199,29 @@ namespace gtsam { #endif GTSAM_EXPORT void print(const std::string& outline = "") const; GTSAM_EXPORT void print2(const std::string& outline = "", const double parentTotal = -1.0) const; + + /** + * @brief Print the CSV header. + * Order is + * (CPU time, number of times, wall time, time + children in seconds, min + * time, max time) + * + * @param addLineBreak Flag indicating if a line break should be added at + * the end. Only used at the top-leve. + */ + GTSAM_EXPORT void printCsvHeader(bool addLineBreak = false) const; + + /** + * @brief Print the times recursively from parent to child in CSV format. + * For each timing node, the output is + * (CPU time, number of times, wall time, time + children in seconds, min + * time, max time) + * + * @param addLineBreak Flag indicating if a line break should be added at + * the end. Only used at the top-leve. + */ + GTSAM_EXPORT void printCsv(bool addLineBreak = false) const; + GTSAM_EXPORT const std::shared_ptr& child(size_t child, const std::string& label, const std::weak_ptr& thisPtr); GTSAM_EXPORT void tic(); @@ -268,6 +291,14 @@ inline void tictoc_finishedIteration_() { inline void tictoc_print_() { ::gtsam::internal::gTimingRoot->print(); } +// print timing in CSV format +inline void tictoc_printCsv_(bool displayHeader = false) { + if (displayHeader) { + ::gtsam::internal::gTimingRoot->printCsvHeader(true); + } + ::gtsam::internal::gTimingRoot->printCsv(true); +} + // print mean and standard deviation inline void tictoc_print2_() { ::gtsam::internal::gTimingRoot->print2(); } diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 1b1c44acc..960440daf 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -291,7 +291,7 @@ class Basis { */ template class ManifoldEvaluationFunctor : public VectorEvaluationFunctor { - enum { M = traits::dimension }; + inline constexpr static auto M = traits::dimension; using Base = VectorEvaluationFunctor; public: diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 63fca64cc..71c3db7f0 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -20,6 +20,14 @@ namespace gtsam { +double Chebyshev2::Point(size_t N, int j, double a, double b) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); + // We add -PI so that we get values ordered from -1 to +1 + // sin(-M_PI_2 + dtheta*j); also works + return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; +} + Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { // Allocate space for weights Weights weights(N); diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 849a51104..207f4b617 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -61,13 +61,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { * @param b Upper bound of interval (default: 1) * @return double */ - static double Point(size_t N, int j, double a = -1, double b = 1) { - assert(j >= 0 && size_t(j) < N); - const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); - // We add -PI so that we get values ordered from -1 to +1 - // sin(-M_PI_2 + dtheta*j); also works - return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; - } + static double Point(size_t N, int j, double a = -1, double b = 1); /// All Chebyshev points static Vector Points(size_t N) { diff --git a/gtsam/config.h.in b/gtsam/config.h.in index a19a3f1f0..da7482016 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -26,7 +26,11 @@ // Paths to example datasets distributed with GTSAM #define GTSAM_SOURCE_TREE_DATASET_DIR "@GTSAM_SOURCE_DIR@/examples/Data" +<<<<<<< HEAD #ifndef __QNX__ +======= +#if !defined(__QNX__) +>>>>>>> 93f463ddbf8e990e6dccc622fcb8ecb67f21549a #define GTSAM_INSTALLED_DATASET_DIR "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data" #else //Set toolbox path to the path on the target. @@ -47,6 +51,9 @@ // Whether to enable merging of equal leaf nodes in the Discrete Decision Tree. #cmakedefine GTSAM_DT_MERGING +// Whether to enable timing in hybrid factor graph machinery +#cmakedefine01 GTSAM_HYBRID_TIMING + // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake) #cmakedefine GTSAM_USE_TBB @@ -93,3 +100,7 @@ #cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR #cmakedefine GTSAM_SLOW_BUT_CORRECT_EXPMAP + +// Boost flags +#cmakedefine01 GTSAM_ENABLE_BOOST_SERIALIZATION +#cmakedefine01 GTSAM_USE_BOOST_FEATURES diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 9948b0be6..a8ec66f73 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -20,12 +20,12 @@ #include #include +#include -#include +#include #include #include #include -#include #include namespace gtsam { @@ -55,28 +55,11 @@ namespace gtsam { public: using Base = DecisionTree; - /** The Real ring with addition and multiplication */ - struct Ring { - static inline double zero() { return 0.0; } - static inline double one() { return 1.0; } - static inline double add(const double& a, const double& b) { - return a + b; - } - static inline double max(const double& a, const double& b) { - return std::max(a, b); - } - static inline double mul(const double& a, const double& b) { - return a * b; - } - static inline double div(const double& a, const double& b) { - return a / b; - } - static inline double id(const double& x) { return x; } - static inline double negate(const double& x) { return -x; } - }; - AlgebraicDecisionTree(double leaf = 1.0) : Base(leaf) {} + /// Constructor which accepts root pointer + AlgebraicDecisionTree(const typename Base::NodePtr root) : Base(root) {} + // Explicitly non-explicit constructor AlgebraicDecisionTree(const Base& add) : Base(add) {} diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index bda44bb9d..efc19d9ee 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -144,7 +144,7 @@ namespace gtsam { private: using Base = DecisionTree::Node; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -471,7 +471,7 @@ namespace gtsam { private: using Base = DecisionTree::Node; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 486f798e9..7c8d2742d 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -23,7 +23,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -132,7 +132,7 @@ namespace gtsam { virtual bool isLeaf() const = 0; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -440,7 +440,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index caedab713..19deccd78 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -18,9 +18,10 @@ */ #include -#include #include #include +#include +#include #include @@ -48,7 +49,7 @@ namespace gtsam { return false; } else { const auto& f(static_cast(other)); - return ADT::equals(f, tol); + return Base::equals(other, tol) && ADT::equals(f, tol); } } @@ -62,6 +63,49 @@ namespace gtsam { return error(values.discrete()); } + /* ************************************************************************ */ + DiscreteFactor::shared_ptr DecisionTreeFactor::multiply( + const DiscreteFactor::shared_ptr& f) const { + DiscreteFactor::shared_ptr result; + if (auto tf = std::dynamic_pointer_cast(f)) { + // If f is a TableFactor, we convert `this` to a TableFactor since this + // conversion is cheaper than converting `f` to a DecisionTreeFactor. We + // then return a TableFactor. + result = std::make_shared((*tf) * TableFactor(*this)); + + } else if (auto dtf = std::dynamic_pointer_cast(f)) { + // If `f` is a DecisionTreeFactor, simply call operator*. + result = std::make_shared(this->operator*(*dtf)); + + } else { + // Simulate double dispatch in C++ + // Useful for other classes which inherit from DiscreteFactor and have + // only `operator*(DecisionTreeFactor)` defined. Thus, other classes don't + // need to be updated. + result = std::make_shared(f->operator*(*this)); + } + return result; + } + + /* ************************************************************************ */ + DiscreteFactor::shared_ptr DecisionTreeFactor::operator/( + const DiscreteFactor::shared_ptr& f) const { + if (auto tf = std::dynamic_pointer_cast(f)) { + // Check if `f` is a TableFactor. If yes, then + // convert `this` to a TableFactor which is cheaper. + return std::make_shared(tf->operator/(TableFactor(*this))); + + } else if (auto dtf = std::dynamic_pointer_cast(f)) { + // If `f` is a DecisionTreeFactor, divide normally. + return std::make_shared(this->operator/(*dtf)); + + } else { + // Else, convert `f` to a DecisionTreeFactor so we can divide + return std::make_shared( + this->operator/(f->toDecisionTreeFactor())); + } + } + /* ************************************************************************ */ double DecisionTreeFactor::safe_div(const double& a, const double& b) { // The use for safe_div is when we divide the product factor by the sum @@ -83,7 +127,7 @@ namespace gtsam { } /* ************************************************************************ */ - DecisionTreeFactor DecisionTreeFactor::apply(ADT::Unary op) const { + DecisionTreeFactor DecisionTreeFactor::apply(Unary op) const { // apply operand ADT result = ADT::apply(op); // Make a new factor @@ -91,7 +135,7 @@ namespace gtsam { } /* ************************************************************************ */ - DecisionTreeFactor DecisionTreeFactor::apply(ADT::UnaryAssignment op) const { + DecisionTreeFactor DecisionTreeFactor::apply(UnaryAssignment op) const { // apply operand ADT result = ADT::apply(op); // Make a new factor @@ -100,7 +144,7 @@ namespace gtsam { /* ************************************************************************ */ DecisionTreeFactor DecisionTreeFactor::apply(const DecisionTreeFactor& f, - ADT::Binary op) const { + Binary op) const { map cs; // new cardinalities // make unique key-cardinality map for (Key j : keys()) cs[j] = cardinality(j); @@ -118,8 +162,8 @@ namespace gtsam { } /* ************************************************************************ */ - DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine( - size_t nrFrontals, ADT::Binary op) const { + DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(size_t nrFrontals, + Binary op) const { if (nrFrontals > size()) { throw invalid_argument( "DecisionTreeFactor::combine: invalid number of frontal " @@ -146,7 +190,7 @@ namespace gtsam { /* ************************************************************************ */ DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine( - const Ordering& frontalKeys, ADT::Binary op) const { + const Ordering& frontalKeys, Binary op) const { if (frontalKeys.size() > size()) { throw invalid_argument( "DecisionTreeFactor::combine: invalid number of frontal " @@ -195,7 +239,7 @@ namespace gtsam { // Construct unordered_map with values std::vector> result; for (const auto& assignment : assignments) { - result.emplace_back(assignment, operator()(assignment)); + result.emplace_back(assignment, evaluate(assignment)); } return result; } @@ -407,11 +451,9 @@ namespace gtsam { }; /* ************************************************************************ */ - DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const { - const size_t N = maxNrAssignments; - + double DecisionTreeFactor::computeThreshold(const size_t N) const { // Set of all keys - std::set allKeys(keys().begin(), keys().end()); + std::set allKeys = this->labels(); MinHeap min_heap; auto op = [&](const Assignment& a, double p) { @@ -433,18 +475,25 @@ namespace gtsam { nrAssignments *= cardinalities_.at(k); } + // If min-heap is empty, fill it initially. + // This is because there is nothing at the top. if (min_heap.empty()) { min_heap.push(p, std::min(nrAssignments, N)); } else { - // If p is larger than the smallest element, - // then we insert into the max heap. - if (p > min_heap.top()) { - for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { + for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { + // If p is larger than the smallest element, + // then we insert into the min heap. + // We check against the top each time because the + // heap maintains the smallest element at the top. + if (p > min_heap.top()) { if (min_heap.size() == N) { min_heap.pop(); } min_heap.push(p); + } else { + // p is <= min value so move to the next one + break; } } } @@ -452,7 +501,14 @@ namespace gtsam { }; this->visitWith(op); - double threshold = min_heap.top(); + return min_heap.top(); + } + + /* ************************************************************************ */ + DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const { + const size_t N = maxNrAssignments; + + double threshold = computeThreshold(N); // Now threshold the decision tree size_t total = 0; diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 7af729f3e..4da5a7c17 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -21,11 +21,12 @@ #include #include #include +#include #include #include -#include #include +#include #include #include #include @@ -50,6 +51,11 @@ namespace gtsam { typedef std::shared_ptr shared_ptr; typedef AlgebraicDecisionTree ADT; + // Needed since we have definitions in both DiscreteFactor and DecisionTree + using Base::Binary; + using Base::Unary; + using Base::UnaryAssignment; + /// @name Standard Constructors /// @{ @@ -129,53 +135,80 @@ namespace gtsam { /// @name Standard Interface /// @{ - /// Calculate probability for given values `x`, + /// Calculate probability for given values, /// is just look up in AlgebraicDecisionTree. - double evaluate(const Assignment& values) const { + virtual double evaluate(const Assignment& values) const override { return ADT::operator()(values); } - /// Evaluate probability distribution, sugar. - double operator()(const DiscreteValues& values) const override { - return ADT::operator()(values); - } + /// Disambiguate to use DiscreteFactor version. Mainly for wrapper + using DiscreteFactor::operator(); /// Calculate error for DiscreteValues `x`, is -log(probability). double error(const DiscreteValues& values) const override; + /** + * @brief Multiply factors, DiscreteFactor::shared_ptr edition. + * + * This method accepts `DiscreteFactor::shared_ptr` and uses dynamic + * dispatch and specializations to perform the most efficient + * multiplication. + * + * While converting a DecisionTreeFactor to a TableFactor is efficient, the + * reverse is not. Hence we specialize the code to return a TableFactor if + * `f` is a TableFactor, and DecisionTreeFactor otherwise. + * + * @param f The factor to multiply with. + * @return DiscreteFactor::shared_ptr + */ + virtual DiscreteFactor::shared_ptr multiply( + const DiscreteFactor::shared_ptr& f) const override; + /// multiply two factors DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { - return apply(f, ADT::Ring::mul); + return apply(f, Ring::mul); } static double safe_div(const double& a, const double& b); - /// divide by factor f (safely) + /** + * @brief Divide by factor f (safely). + * Division of a factor \f$f(x, y)\f$ by another factor \f$g(y, z)\f$ + * results in a function which involves all keys + * \f$(\frac{f}{g})(x, y, z) = f(x, y) / g(y, z)\f$ + * + * @param f The DecisinTreeFactor to divide by. + * @return DecisionTreeFactor + */ DecisionTreeFactor operator/(const DecisionTreeFactor& f) const { return apply(f, safe_div); } + /// divide by DiscreteFactor::shared_ptr f (safely) + DiscreteFactor::shared_ptr operator/( + const DiscreteFactor::shared_ptr& f) const override; + /// Convert into a decision tree DecisionTreeFactor toDecisionTreeFactor() const override { return *this; } /// Create new factor by summing all values with the same separator values - shared_ptr sum(size_t nrFrontals) const { - return combine(nrFrontals, ADT::Ring::add); + DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override { + return combine(nrFrontals, Ring::add); } /// Create new factor by summing all values with the same separator values - shared_ptr sum(const Ordering& keys) const { - return combine(keys, ADT::Ring::add); + DiscreteFactor::shared_ptr sum(const Ordering& keys) const override { + return combine(keys, Ring::add); } /// Create new factor by maximizing over all values with the same separator. - shared_ptr max(size_t nrFrontals) const { - return combine(nrFrontals, ADT::Ring::max); + DiscreteFactor::shared_ptr max(size_t nrFrontals) const override { + return combine(nrFrontals, Ring::max); } /// Create new factor by maximizing over all values with the same separator. - shared_ptr max(const Ordering& keys) const { - return combine(keys, ADT::Ring::max); + DiscreteFactor::shared_ptr max(const Ordering& keys) const override { + return combine(keys, Ring::max); } /// @} @@ -186,21 +219,21 @@ namespace gtsam { * Apply unary operator (*this) "op" f * @param op a unary operator that operates on AlgebraicDecisionTree */ - DecisionTreeFactor apply(ADT::Unary op) const; + DecisionTreeFactor apply(Unary op) const; /** * Apply unary operator (*this) "op" f * @param op a unary operator that operates on AlgebraicDecisionTree. Takes * both the assignment and the value. */ - DecisionTreeFactor apply(ADT::UnaryAssignment op) const; + DecisionTreeFactor apply(UnaryAssignment op) const; /** * Apply binary operator (*this) "op" f * @param f the second argument for op * @param op a binary operator that operates on AlgebraicDecisionTree */ - DecisionTreeFactor apply(const DecisionTreeFactor& f, ADT::Binary op) const; + DecisionTreeFactor apply(const DecisionTreeFactor& f, Binary op) const; /** * Combine frontal variables using binary operator "op" @@ -208,7 +241,7 @@ namespace gtsam { * @param op a binary operator that operates on AlgebraicDecisionTree * @return shared pointer to newly created DecisionTreeFactor */ - shared_ptr combine(size_t nrFrontals, ADT::Binary op) const; + shared_ptr combine(size_t nrFrontals, Binary op) const; /** * Combine frontal variables in an Ordering using binary operator "op" @@ -216,7 +249,7 @@ namespace gtsam { * @param op a binary operator that operates on AlgebraicDecisionTree * @return shared pointer to newly created DecisionTreeFactor */ - shared_ptr combine(const Ordering& keys, ADT::Binary op) const; + shared_ptr combine(const Ordering& keys, Binary op) const; /// Enumerate all values into a map from values to double. std::vector> enumerate() const; @@ -224,6 +257,17 @@ namespace gtsam { /// Get all the probabilities in order of assignment values std::vector probabilities() const; + /** + * @brief Compute the probability value which is the threshold above which + * only `N` leaves are present. + * + * This is used for pruning out the smaller probabilities. + * + * @param N The number of leaves to keep post pruning. + * @return double + */ + double computeThreshold(const size_t N) const; + /** * @brief Prune the decision tree of discrete variables. * @@ -244,6 +288,12 @@ namespace gtsam { */ DecisionTreeFactor prune(size_t maxNrAssignments) const; + /** + * Get the number of non-zero values contained in this factor. + * It could be much smaller than `prod_{key}(cardinality(key))`. + */ + uint64_t nrValues() const override { return nrLeaves(); } + /// @} /// @name Wrapper support /// @{ @@ -295,7 +345,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index a5a4621aa..738b91aa5 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -147,7 +147,7 @@ class GTSAM_EXPORT DiscreteBayesNet: public BayesNet { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 5ab0c59ec..f5ad2b98a 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -19,10 +19,12 @@ #include #include #include +#include #include #include #include +#include #include #include #include @@ -42,8 +44,9 @@ template class GTSAM_EXPORT /* ************************************************************************** */ DiscreteConditional::DiscreteConditional(const size_t nrFrontals, - const DecisionTreeFactor& f) - : BaseFactor(f / (*f.sum(nrFrontals))), BaseConditional(nrFrontals) {} + const DiscreteFactor& f) + : BaseFactor((f / f.sum(nrFrontals))->toDecisionTreeFactor()), + BaseConditional(nrFrontals) {} /* ************************************************************************** */ DiscreteConditional::DiscreteConditional(size_t nrFrontals, @@ -74,6 +77,13 @@ DiscreteConditional::DiscreteConditional(const Signature& signature) /* ************************************************************************** */ DiscreteConditional DiscreteConditional::operator*( const DiscreteConditional& other) const { + // If the root is a nullptr, we have a TableDistribution + // TODO(Varun) Revisit this hack after RSS2025 submission + if (!other.root_) { + DiscreteConditional dc(other.nrFrontals(), other.toDecisionTreeFactor()); + return dc * (*this); + } + // Take union of frontal keys std::set newFrontals; for (auto&& key : this->frontals()) newFrontals.insert(key); @@ -104,7 +114,7 @@ DiscreteConditional DiscreteConditional::operator*( // Finally, add parents to keys, in order for (auto&& dk : parents) discreteKeys.push_back(dk); - ADT product = ADT::apply(other, ADT::Ring::mul); + ADT product = ADT::apply(other, Ring::mul); return DiscreteConditional(newFrontals.size(), discreteKeys, product); } @@ -148,11 +158,11 @@ void DiscreteConditional::print(const string& s, /* ************************************************************************** */ bool DiscreteConditional::equals(const DiscreteFactor& other, double tol) const { - if (!dynamic_cast(&other)) { + if (!dynamic_cast(&other)) { return false; } else { - const DecisionTreeFactor& f(static_cast(other)); - return DecisionTreeFactor::equals(f, tol); + const BaseFactor& f(static_cast(other)); + return BaseFactor::equals(f, tol); } } @@ -373,7 +383,7 @@ std::string DiscreteConditional::markdown(const KeyFormatter& keyFormatter, ss << "*\n" << std::endl; if (nrParents() == 0) { // We have no parents, call factor method. - ss << DecisionTreeFactor::markdown(keyFormatter, names); + ss << BaseFactor::markdown(keyFormatter, names); return ss.str(); } @@ -425,7 +435,7 @@ string DiscreteConditional::html(const KeyFormatter& keyFormatter, ss << "

\n"; if (nrParents() == 0) { // We have no parents, call factor method. - ss << DecisionTreeFactor::html(keyFormatter, names); + ss << BaseFactor::html(keyFormatter, names); return ss.str(); } @@ -473,7 +483,20 @@ string DiscreteConditional::html(const KeyFormatter& keyFormatter, /* ************************************************************************* */ double DiscreteConditional::evaluate(const HybridValues& x) const { - return this->evaluate(x.discrete()); + return this->operator()(x.discrete()); +} + +/* ************************************************************************* */ +DiscreteFactor::shared_ptr DiscreteConditional::max( + const Ordering& keys) const { + return BaseFactor::max(keys); +} + +/* ************************************************************************* */ +void DiscreteConditional::prune(size_t maxNrAssignments) { + // Get as DiscreteConditional so the probabilities are normalized + DiscreteConditional pruned(nrFrontals(), BaseFactor::prune(maxNrAssignments)); + this->root_ = pruned.root_; } /* ************************************************************************* */ diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index f59e29285..1bca0b09f 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -54,7 +54,7 @@ class GTSAM_EXPORT DiscreteConditional DiscreteConditional() {} /// Construct from factor, taking the first `nFrontals` keys as frontals. - DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f); + DiscreteConditional(size_t nFrontals, const DiscreteFactor& f); /** * Construct from DiscreteKeys and AlgebraicDecisionTree, taking the first @@ -168,13 +168,9 @@ class GTSAM_EXPORT DiscreteConditional static_cast(this)->print(s, formatter); } - /// Evaluate, just look up in AlgebraicDecisionTree - double evaluate(const DiscreteValues& values) const { - return ADT::operator()(values); - } - - using DecisionTreeFactor::error; ///< DiscreteValues version - using DecisionTreeFactor::operator(); ///< DiscreteValues version + using BaseFactor::error; ///< DiscreteValues version + using BaseFactor::evaluate; ///< DiscreteValues version + using BaseFactor::operator(); ///< DiscreteValues version /** * @brief restrict to given *parent* values. @@ -203,7 +199,7 @@ class GTSAM_EXPORT DiscreteConditional * @param parentsValues Known values of the parents * @return sample from conditional */ - size_t sample(const DiscreteValues& parentsValues) const; + virtual size_t sample(const DiscreteValues& parentsValues) const; /// Single parent version. size_t sample(size_t parent_value) const; @@ -218,6 +214,15 @@ class GTSAM_EXPORT DiscreteConditional */ size_t argmax(const DiscreteValues& parentsValues = DiscreteValues()) const; + /** + * @brief Create new factor by maximizing over all + * values with the same separator. + * + * @param keys The keys to sum over. + * @return DiscreteFactor::shared_ptr + */ + virtual DiscreteFactor::shared_ptr max(const Ordering& keys) const override; + /// @} /// @name Advanced Interface /// @{ @@ -271,6 +276,9 @@ class GTSAM_EXPORT DiscreteConditional */ double negLogConstant() const override; + /// Prune the conditional + virtual void prune(size_t maxNrAssignments); + /// @} protected: @@ -279,7 +287,7 @@ class GTSAM_EXPORT DiscreteConditional bool forceComplete) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/DiscreteDistribution.h b/gtsam/discrete/DiscreteDistribution.h index 4b690da15..09ea50332 100644 --- a/gtsam/discrete/DiscreteDistribution.h +++ b/gtsam/discrete/DiscreteDistribution.h @@ -40,7 +40,7 @@ class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional { /// Default constructor needed for serialization. DiscreteDistribution() {} - /// Constructor from factor. + /// Constructor from DecisionTreeFactor. explicit DiscreteDistribution(const DecisionTreeFactor& f) : Base(f.size(), f) {} diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp index 2b11046f4..68cc1df7d 100644 --- a/gtsam/discrete/DiscreteFactor.cpp +++ b/gtsam/discrete/DiscreteFactor.cpp @@ -28,6 +28,11 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +bool DiscreteFactor::equals(const DiscreteFactor& lf, double tol) const { + return Base::equals(lf, tol) && cardinalities_ == lf.cardinalities_; +} + /* ************************************************************************ */ DiscreteKeys DiscreteFactor::discreteKeys() const { DiscreteKeys result; diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 19af5bd13..6cbc00d09 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -22,6 +22,7 @@ #include #include #include +#include #include namespace gtsam { @@ -46,6 +47,11 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { using Values = DiscreteValues; ///< backwards compatibility + using Unary = std::function; + using UnaryAssignment = + std::function&, const double&)>; + using Binary = std::function; + protected: /// Map of Keys and their cardinalities. std::map cardinalities_; @@ -72,7 +78,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { /// @{ /// equals - virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0; + virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const; /// print void print( @@ -92,8 +98,21 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { size_t cardinality(Key j) const { return cardinalities_.at(j); } + /** + * @brief Calculate probability for given values. + * Calls specialized evaluation under the hood. + * + * Note: Uses Assignment as it is the base class of DiscreteValues. + * + * @param values Discrete assignment. + * @return double + */ + virtual double evaluate(const Assignment& values) const = 0; + /// Find value for given assignment of values to variables - virtual double operator()(const DiscreteValues&) const = 0; + double operator()(const DiscreteValues& values) const { + return evaluate(values); + } /// Error is just -log(value) virtual double error(const DiscreteValues& values) const; @@ -111,8 +130,40 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { /// DecisionTreeFactor virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0; + /** + * @brief Multiply in a DiscreteFactor and return the result as + * DiscreteFactor, both via shared pointers. + * + * @param df DiscreteFactor shared_ptr + * @return DiscreteFactor::shared_ptr + */ + virtual DiscreteFactor::shared_ptr multiply( + const DiscreteFactor::shared_ptr& df) const = 0; + + /// divide by DiscreteFactor::shared_ptr f (safely) + virtual DiscreteFactor::shared_ptr operator/( + const DiscreteFactor::shared_ptr& df) const = 0; + virtual DecisionTreeFactor toDecisionTreeFactor() const = 0; + /// Create new factor by summing all values with the same separator values + virtual DiscreteFactor::shared_ptr sum(size_t nrFrontals) const = 0; + + /// Create new factor by summing all values with the same separator values + virtual DiscreteFactor::shared_ptr sum(const Ordering& keys) const = 0; + + /// Create new factor by maximizing over all values with the same separator. + virtual DiscreteFactor::shared_ptr max(size_t nrFrontals) const = 0; + + /// Create new factor by maximizing over all values with the same separator. + virtual DiscreteFactor::shared_ptr max(const Ordering& keys) const = 0; + + /** + * Get the number of non-zero values contained in this factor. + * It could be much smaller than `prod_{key}(cardinality(key))`. + */ + virtual uint64_t nrValues() const = 0; + /// @} /// @name Wrapper support /// @{ @@ -145,7 +196,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 4ededbb8b..7e059c5e5 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -14,6 +14,7 @@ * @date Feb 14, 2011 * @author Duy-Nguyen Ta * @author Frank Dellaert + * @author Varun Agrawal */ #include @@ -35,13 +36,12 @@ namespace gtsam { template class FactorGraph; template class EliminateableFactorGraph; - /* ************************************************************************* */ - bool DiscreteFactorGraph::equals(const This& fg, double tol) const - { + /* ************************************************************************ */ + bool DiscreteFactorGraph::equals(const This& fg, double tol) const { return Base::equals(fg, tol); } - /* ************************************************************************* */ + /* ************************************************************************ */ KeySet DiscreteFactorGraph::keys() const { KeySet keys; for (const sharedFactor& factor : *this) { @@ -50,11 +50,11 @@ namespace gtsam { return keys; } - /* ************************************************************************* */ + /* ************************************************************************ */ DiscreteKeys DiscreteFactorGraph::discreteKeys() const { DiscreteKeys result; for (auto&& factor : *this) { - if (auto p = std::dynamic_pointer_cast(factor)) { + if (auto p = std::dynamic_pointer_cast(factor)) { DiscreteKeys factor_keys = p->discreteKeys(); result.insert(result.end(), factor_keys.begin(), factor_keys.end()); } @@ -63,26 +63,34 @@ namespace gtsam { return result; } - /* ************************************************************************* */ - DecisionTreeFactor DiscreteFactorGraph::product() const { - DecisionTreeFactor result; - for(const sharedFactor& factor: *this) - if (factor) result = (*factor) * result; + /* ************************************************************************ */ + DiscreteFactor::shared_ptr DiscreteFactorGraph::product() const { + DiscreteFactor::shared_ptr result; + for (auto it = this->begin(); it != this->end(); ++it) { + if (*it) { + if (result) { + result = result->multiply(*it); + } else { + // Assign to the first non-null factor + result = *it; + } + } + } return result; } - /* ************************************************************************* */ - double DiscreteFactorGraph::operator()( - const DiscreteValues &values) const { + /* ************************************************************************ */ + double DiscreteFactorGraph::operator()(const DiscreteValues& values) const { double product = 1.0; - for( const sharedFactor& factor: factors_ ) - product *= (*factor)(values); + for (const sharedFactor& factor : factors_) { + if (factor) product *= (*factor)(values); + } return product; } - /* ************************************************************************* */ + /* ************************************************************************ */ void DiscreteFactorGraph::print(const string& s, - const KeyFormatter& formatter) const { + const KeyFormatter& formatter) const { std::cout << s << std::endl; std::cout << "size: " << size() << std::endl; for (size_t i = 0; i < factors_.size(); i++) { @@ -111,39 +119,45 @@ namespace gtsam { // } /* ************************************************************************ */ - // Alternate eliminate function for MPE - std::pair // - EliminateForMPE(const DiscreteFactorGraph& factors, - const Ordering& frontalKeys) { + DiscreteFactor::shared_ptr DiscreteFactorGraph::scaledProduct() const { // PRODUCT: multiply all factors gttic(product); - DecisionTreeFactor product; - for (auto&& factor : factors) product = (*factor) * product; + DiscreteFactor::shared_ptr product = this->product(); gttoc(product); // Max over all the potentials by pretending all keys are frontal: - auto normalization = product.max(product.size()); + auto denominator = product->max(product->size()); // Normalize the product factor to prevent underflow. - product = product / (*normalization); + product = product->operator/(denominator); + + return product; + } + + /* ************************************************************************ */ + // Alternate eliminate function for MPE + std::pair // + EliminateForMPE(const DiscreteFactorGraph& factors, + const Ordering& frontalKeys) { + DiscreteFactor::shared_ptr product = factors.scaledProduct(); // max out frontals, this is the factor on the separator gttic(max); - DecisionTreeFactor::shared_ptr max = product.max(frontalKeys); + DiscreteFactor::shared_ptr max = product->max(frontalKeys); gttoc(max); // Ordering keys for the conditional so that frontalKeys are really in front DiscreteKeys orderedKeys; for (auto&& key : frontalKeys) - orderedKeys.emplace_back(key, product.cardinality(key)); + orderedKeys.emplace_back(key, product->cardinality(key)); for (auto&& key : max->keys()) - orderedKeys.emplace_back(key, product.cardinality(key)); + orderedKeys.emplace_back(key, product->cardinality(key)); // Make lookup with product gttic(lookup); size_t nrFrontals = frontalKeys.size(); - auto lookup = std::make_shared(nrFrontals, - orderedKeys, product); + auto lookup = std::make_shared( + nrFrontals, orderedKeys, product->toDecisionTreeFactor()); gttoc(lookup); return {std::dynamic_pointer_cast(lookup), max}; @@ -193,32 +207,21 @@ namespace gtsam { return dag.argmax(); } - DiscreteValues DiscreteFactorGraph::optimize( - const Ordering& ordering) const { + DiscreteValues DiscreteFactorGraph::optimize(const Ordering& ordering) const { gttic(DiscreteFactorGraph_optimize); DiscreteLookupDAG dag = maxProduct(ordering); return dag.argmax(); } /* ************************************************************************ */ - std::pair // + std::pair // EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { - // PRODUCT: multiply all factors - gttic(product); - DecisionTreeFactor product; - for (auto&& factor : factors) product = (*factor) * product; - gttoc(product); - - // Max over all the potentials by pretending all keys are frontal: - auto normalization = product.max(product.size()); - - // Normalize the product factor to prevent underflow. - product = product / (*normalization); + DiscreteFactor::shared_ptr product = factors.scaledProduct(); // sum out frontals, this is the factor on the separator gttic(sum); - DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys); + DiscreteFactor::shared_ptr sum = product->sum(frontalKeys); gttoc(sum); // Ordering keys for the conditional so that frontalKeys are really in front @@ -230,8 +233,9 @@ namespace gtsam { // now divide product/sum to get conditional gttic(divide); - auto conditional = - std::make_shared(product, *sum, orderedKeys); + auto conditional = std::make_shared( + product->toDecisionTreeFactor(), sum->toDecisionTreeFactor(), + orderedKeys); gttoc(divide); return {conditional, sum}; diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index d0dc282b4..f4d1a1833 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -14,6 +14,7 @@ * @date Feb 14, 2011 * @author Duy-Nguyen Ta * @author Frank Dellaert + * @author Varun Agrawal */ #pragma once @@ -48,7 +49,7 @@ class DiscreteJunctionTree; * @ingroup discrete */ GTSAM_EXPORT -std::pair +std::pair EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys); @@ -61,7 +62,7 @@ EliminateDiscrete(const DiscreteFactorGraph& factors, * @ingroup discrete */ GTSAM_EXPORT -std::pair +std::pair EliminateForMPE(const DiscreteFactorGraph& factors, const Ordering& frontalKeys); @@ -133,6 +134,7 @@ class GTSAM_EXPORT DiscreteFactorGraph /// @} + //TODO(Varun): Make compatible with TableFactor /** Add a decision-tree factor */ template void add(Args&&... args) { @@ -146,7 +148,16 @@ class GTSAM_EXPORT DiscreteFactorGraph DiscreteKeys discreteKeys() const; /** return product of all factors as a single factor */ - DecisionTreeFactor product() const; + DiscreteFactor::shared_ptr product() const; + + /** + * @brief Return product of all `factors` as a single factor, + * which is scaled by the max value to prevent underflow + * + * @param factors The factors to multiply as a DiscreteFactorGraph. + * @return DiscreteFactor::shared_ptr + */ + DiscreteFactor::shared_ptr scaledProduct() const; /** * Evaluates the factor graph given values, returns the joint probability of diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h index 44cc192ef..d200792c8 100644 --- a/gtsam/discrete/DiscreteKey.h +++ b/gtsam/discrete/DiscreteKey.h @@ -21,7 +21,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -87,7 +87,7 @@ namespace gtsam { /// Check equality to another DiscreteKeys object. bool equals(const DiscreteKeys& other, double tol = 0) const; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/DiscreteLookupDAG.cpp b/gtsam/discrete/DiscreteLookupDAG.cpp index 4d02e007b..d1108e7b7 100644 --- a/gtsam/discrete/DiscreteLookupDAG.cpp +++ b/gtsam/discrete/DiscreteLookupDAG.cpp @@ -23,6 +23,7 @@ #include #include #include +#include using std::pair; using std::vector; diff --git a/gtsam/discrete/DiscreteLookupDAG.h b/gtsam/discrete/DiscreteLookupDAG.h index f077a13d9..1c0be2f06 100644 --- a/gtsam/discrete/DiscreteLookupDAG.h +++ b/gtsam/discrete/DiscreteLookupDAG.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include #include @@ -54,6 +55,18 @@ class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional { const ADT& potentials) : DiscreteConditional(nFrontals, keys, potentials) {} + /** + * @brief Construct a new Discrete Lookup Table object + * + * @param nFrontals number of frontal variables + * @param keys a sorted list of gtsam::Keys + * @param potentials Discrete potentials as a TableFactor. + */ + DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys, + const TableFactor& potentials) + : DiscreteConditional(nFrontals, keys, + potentials.toDecisionTreeFactor()) {} + /// GTSAM-style print void print( const std::string& s = "Discrete Lookup Table: ", @@ -123,7 +136,7 @@ class GTSAM_EXPORT DiscreteLookupDAG : public BayesNet { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/discrete/Ring.h b/gtsam/discrete/Ring.h new file mode 100644 index 000000000..cf7c6424a --- /dev/null +++ b/gtsam/discrete/Ring.h @@ -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 + + * -------------------------------------------------------------------------- */ + +/** + * @file Ring.h + * @brief Real Ring definition + * @author Varun Agrawal + * @date Dec 8, 2024 + */ + +#pragma once + +#include + +/** The Real ring with addition and multiplication */ +struct Ring { + static inline double zero() { return 0.0; } + static inline double one() { return 1.0; } + static inline double add(const double& a, const double& b) { return a + b; } + static inline double max(const double& a, const double& b) { + return std::max(a, b); + } + static inline double mul(const double& a, const double& b) { return a * b; } + static inline double div(const double& a, const double& b) { + return (a == 0 || b == 0) ? 0 : (a / b); + } + static inline double id(const double& x) { return x; } + static inline double negate(const double& x) { return -x; } +}; diff --git a/gtsam/discrete/SignatureParser.cpp b/gtsam/discrete/SignatureParser.cpp index 31e425392..8e0221f01 100644 --- a/gtsam/discrete/SignatureParser.cpp +++ b/gtsam/discrete/SignatureParser.cpp @@ -38,7 +38,7 @@ std::optional static ParseConditional(const std::string& token) { } catch (...) { return std::nullopt; } - return std::move(row); + return row; } std::optional static ParseConditionalTable( @@ -62,7 +62,7 @@ std::optional
static ParseConditionalTable( } } } - return std::move(table); + return table; } std::vector static Tokenize(const std::string& str) { diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp new file mode 100644 index 000000000..e8696c5b1 --- /dev/null +++ b/gtsam/discrete/TableDistribution.cpp @@ -0,0 +1,174 @@ +/* ---------------------------------------------------------------------------- + + * 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 TableDistribution.cpp + * @date Dec 22, 2024 + * @author Varun Agrawal + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using std::pair; +using std::stringstream; +using std::vector; +namespace gtsam { + +/// Normalize sparse_table +static Eigen::SparseVector normalizeSparseTable( + const Eigen::SparseVector& sparse_table) { + return sparse_table / sparse_table.sum(); +} + +/* ************************************************************************** */ +TableDistribution::TableDistribution(const TableFactor& f) + : BaseConditional(f.keys().size(), f.discreteKeys(), ADT(nullptr)), + table_(f / (*std::dynamic_pointer_cast( + f.sum(f.keys().size())))) {} + +/* ************************************************************************** */ +TableDistribution::TableDistribution(const DiscreteKeys& keys, + const std::vector& potentials) + : BaseConditional(keys.size(), keys, ADT(nullptr)), + table_(TableFactor( + keys, normalizeSparseTable(TableFactor::Convert(keys, potentials)))) { +} + +/* ************************************************************************** */ +TableDistribution::TableDistribution(const DiscreteKeys& keys, + const std::string& potentials) + : BaseConditional(keys.size(), keys, ADT(nullptr)), + table_(TableFactor( + keys, normalizeSparseTable(TableFactor::Convert(keys, potentials)))) { +} + +/* ************************************************************************** */ +void TableDistribution::print(const string& s, + const KeyFormatter& formatter) const { + cout << s << " P( "; + for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) { + cout << formatter(*it) << " "; + } + cout << "):\n"; + table_.print("", formatter); + cout << endl; +} + +/* ************************************************************************** */ +bool TableDistribution::equals(const DiscreteFactor& other, double tol) const { + auto dtc = dynamic_cast(&other); + if (!dtc) { + return false; + } else { + const DiscreteConditional& f( + static_cast(other)); + return table_.equals(dtc->table_, tol) && + DiscreteConditional::BaseConditional::equals(f, tol); + } +} + +/* ****************************************************************************/ +DiscreteFactor::shared_ptr TableDistribution::sum(size_t nrFrontals) const { + return table_.sum(nrFrontals); +} + +/* ****************************************************************************/ +DiscreteFactor::shared_ptr TableDistribution::sum(const Ordering& keys) const { + return table_.sum(keys); +} + +/* ****************************************************************************/ +DiscreteFactor::shared_ptr TableDistribution::max(size_t nrFrontals) const { + return table_.max(nrFrontals); +} + +/* ****************************************************************************/ +DiscreteFactor::shared_ptr TableDistribution::max(const Ordering& keys) const { + return table_.max(keys); +} + +/* ****************************************************************************/ +DiscreteFactor::shared_ptr TableDistribution::operator/( + const DiscreteFactor::shared_ptr& f) const { + return table_ / f; +} + +/* ************************************************************************ */ +DiscreteValues TableDistribution::argmax() const { + uint64_t maxIdx = 0; + double maxValue = 0.0; + + Eigen::SparseVector sparseTable = table_.sparseTable(); + + for (SparseIt it(sparseTable); it; ++it) { + if (it.value() > maxValue) { + maxIdx = it.index(); + maxValue = it.value(); + } + } + + return table_.findAssignments(maxIdx); +} + +/* ****************************************************************************/ +void TableDistribution::prune(size_t maxNrAssignments) { + table_ = table_.prune(maxNrAssignments); +} + +/* ****************************************************************************/ +size_t TableDistribution::sample(const DiscreteValues& parentsValues) const { + static mt19937 rng(2); // random number generator + + DiscreteKeys parentsKeys; + for (auto&& [key, _] : parentsValues) { + parentsKeys.push_back({key, table_.cardinality(key)}); + } + + // Get the correct conditional distribution: P(F|S=parentsValues) + TableFactor pFS = table_.choose(parentsValues, parentsKeys); + + // TODO(Duy): only works for one key now, seems horribly slow this way + if (nrFrontals() != 1) { + throw std::invalid_argument( + "TableDistribution::sample can only be called on single variable " + "conditionals"); + } + Key key = firstFrontalKey(); + size_t nj = cardinality(key); + vector p(nj); + DiscreteValues frontals; + for (size_t value = 0; value < nj; value++) { + frontals[key] = value; + p[value] = pFS(frontals); // P(F=value|S=parentsValues) + if (p[value] == 1.0) { + return value; // shortcut exit + } + } + std::discrete_distribution distribution(p.begin(), p.end()); + return distribution(rng); +} + +} // namespace gtsam diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h new file mode 100644 index 000000000..72786a515 --- /dev/null +++ b/gtsam/discrete/TableDistribution.h @@ -0,0 +1,177 @@ +/* ---------------------------------------------------------------------------- + + * 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 TableDistribution.h + * @date Dec 22, 2024 + * @author Varun Agrawal + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace gtsam { + +/** + * Distribution which uses a SparseVector as the internal + * representation, similar to the TableFactor. + * + * This is primarily used in the case when we have a clique in the BayesTree + * which consists of all the discrete variables, e.g. in hybrid elimination. + * + * @ingroup discrete + */ +class GTSAM_EXPORT TableDistribution : public DiscreteConditional { + private: + TableFactor table_; + + typedef Eigen::SparseVector::InnerIterator SparseIt; + + public: + // typedefs needed to play nice with gtsam + typedef TableDistribution This; ///< Typedef to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef DiscreteConditional + BaseConditional; ///< Typedef to our conditional base class + + using Values = DiscreteValues; ///< backwards compatibility + + /// @name Standard Constructors + /// @{ + + /// Default constructor needed for serialization. + TableDistribution() {} + + /// Construct from TableFactor. + TableDistribution(const TableFactor& f); + + /** + * Construct from DiscreteKeys and std::vector. + */ + TableDistribution(const DiscreteKeys& keys, + const std::vector& potentials); + + /** + * Construct from single DiscreteKey and std::vector. + */ + TableDistribution(const DiscreteKey& key, + const std::vector& potentials) + : TableDistribution(DiscreteKeys(key), potentials) {} + + /** + * Construct from DiscreteKey and std::string. + */ + TableDistribution(const DiscreteKeys& keys, const std::string& potentials); + + /** + * Construct from single DiscreteKey and std::string. + */ + TableDistribution(const DiscreteKey& key, const std::string& potentials) + : TableDistribution(DiscreteKeys(key), potentials) {} + + /// @} + /// @name Testable + /// @{ + + /// GTSAM-style print + void print( + const std::string& s = "Table Distribution: ", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; + + /// GTSAM-style equals + bool equals(const DiscreteFactor& other, double tol = 1e-9) const override; + + /// @} + /// @name Standard Interface + /// @{ + + /// Return the underlying TableFactor + TableFactor table() const { return table_; } + + using BaseConditional::evaluate; // HybridValues version + + /// Evaluate the conditional given the values. + virtual double evaluate(const Assignment& values) const override { + return table_.evaluate(values); + } + + /// Create new factor by summing all values with the same separator values + DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override; + + /// Create new factor by summing all values with the same separator values + DiscreteFactor::shared_ptr sum(const Ordering& keys) const override; + + /// Create new factor by maximizing over all values with the same separator. + DiscreteFactor::shared_ptr max(size_t nrFrontals) const override; + + /// Create new factor by maximizing over all values with the same separator. + DiscreteFactor::shared_ptr max(const Ordering& keys) const override; + + /// divide by DiscreteFactor::shared_ptr f (safely) + DiscreteFactor::shared_ptr operator/( + const DiscreteFactor::shared_ptr& f) const override; + + /** + * @brief Return assignment that maximizes value. + * + * @return maximizing assignment for the variables. + */ + DiscreteValues argmax() const; + + /** + * sample + * @param parentsValues Known values of the parents + * @return sample from conditional + */ + virtual size_t sample(const DiscreteValues& parentsValues) const override; + + /// @} + /// @name Advanced Interface + /// @{ + + /// Prune the conditional + virtual void prune(size_t maxNrAssignments) override; + + /// Get a DecisionTreeFactor representation. + DecisionTreeFactor toDecisionTreeFactor() const override { + return table_.toDecisionTreeFactor(); + } + + /// Get the number of non-zero values. + uint64_t nrValues() const override { return table_.sparseTable().nonZeros(); } + + /// @} + + private: +#if GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional); + ar& BOOST_SERIALIZATION_NVP(table_); + } +#endif +}; +// TableDistribution + +// traits +template <> +struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index f4e023a4d..d1cedc9ef 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -62,35 +62,104 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys, : TableFactor(dkeys, DecisionTreeFactor(dkeys, dtree)) {} /** - * @brief Compute the correct ordering of the leaves in the decision tree. + * @brief Compute the indexing of the leaves in the decision tree based on the + * assignment and add the (index, leaf) pair to a SparseVector. * - * This is done by first taking all the values which have modulo 0 value with - * the cardinality of the innermost key `n`, and we go up to modulo n. + * We visit each leaf in the tree, and using the cardinalities of the keys, + * compute the correct index to add the leaf to a SparseVector which + * is then used to create the TableFactor. * * @param dt The DecisionTree - * @return std::vector + * @return Eigen::SparseVector */ -std::vector ComputeLeafOrdering(const DiscreteKeys& dkeys, - const DecisionTreeFactor& dt) { - std::vector probs = dt.probabilities(); - std::vector ordered; +static Eigen::SparseVector ComputeSparseTable( + const DiscreteKeys& dkeys, const DecisionTreeFactor& dt) { + // SparseVector needs to know the maximum possible index, + // so we compute the product of cardinalities. + size_t cardinalityProduct = 1; + for (auto&& [_, c] : dt.cardinalities()) { + cardinalityProduct *= c; + } + Eigen::SparseVector sparseTable(cardinalityProduct); + size_t nrValues = 0; + dt.visit([&nrValues](double x) { + if (x > 0) nrValues += 1; + }); + sparseTable.reserve(nrValues); - size_t n = dkeys[0].second; + KeySet allKeys(dt.keys().begin(), dt.keys().end()); - for (size_t k = 0; k < n; ++k) { - for (size_t idx = 0; idx < probs.size(); ++idx) { - if (idx % n == k) { - ordered.push_back(probs[idx]); + // Compute denominators to be used in computing sparse table indices + std::map denominators; + double denom = sparseTable.size(); + for (const DiscreteKey& dkey : dkeys) { + denom /= dkey.second; + denominators.insert(std::pair(dkey.first, denom)); + } + + /** + * @brief Functor which is called by the DecisionTree for each leaf. + * For each leaf value, we use the corresponding assignment to compute a + * corresponding index into a SparseVector. We then populate sparseTable with + * the value at the computed index. + * + * Takes advantage of the sparsity of the DecisionTree to be efficient. When + * merged branches are encountered, we enumerate over the missing keys. + * + */ + auto op = [&](const Assignment& assignment, double p) { + if (p > 0) { + // Get all the keys involved in this assignment + KeySet assignmentKeys; + for (auto&& [k, _] : assignment) { + assignmentKeys.insert(k); + } + + // Find the keys missing in the assignment + KeyVector diff; + std::set_difference(allKeys.begin(), allKeys.end(), + assignmentKeys.begin(), assignmentKeys.end(), + std::back_inserter(diff)); + + // Generate all assignments using the missing keys + DiscreteKeys extras; + for (auto&& key : diff) { + extras.push_back({key, dt.cardinality(key)}); + } + auto&& extraAssignments = DiscreteValues::CartesianProduct(extras); + + for (auto&& extra : extraAssignments) { + // Create new assignment using the extra assignment + DiscreteValues updatedAssignment(assignment); + updatedAssignment.insert(extra); + + // Generate index and add to the sparse vector. + Eigen::Index idx = 0; + // We go in reverse since a DecisionTree has the highest label first + for (auto&& it = updatedAssignment.rbegin(); + it != updatedAssignment.rend(); it++) { + idx += it->second * denominators.at(it->first); + } + sparseTable.coeffRef(idx) = p; } } - } - return ordered; + }; + + // Visit each leaf in `dt` to get the Assignment and leaf value + // to populate the sparseTable. + dt.visitWith(op); + + return sparseTable; } /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteKeys& dkeys, const DecisionTreeFactor& dtf) - : TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {} + : TableFactor(dkeys, ComputeSparseTable(dkeys, dtf)) {} + +/* ************************************************************************ */ +TableFactor::TableFactor(const DecisionTreeFactor& dtf) + : TableFactor(dtf.discreteKeys(), dtf) {} /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteConditional& c) @@ -98,7 +167,17 @@ TableFactor::TableFactor(const DiscreteConditional& c) /* ************************************************************************ */ Eigen::SparseVector TableFactor::Convert( - const std::vector& table) { + const DiscreteKeys& keys, const std::vector& table) { + size_t max_size = 1; + for (auto&& [_, cardinality] : keys.cardinalities()) { + max_size *= cardinality; + } + if (table.size() != max_size) { + throw std::runtime_error( + "The cardinalities of the keys don't match the number of values in the " + "input."); + } + Eigen::SparseVector sparse_table(table.size()); // Count number of nonzero elements in table and reserve the space. const uint64_t nnz = std::count_if(table.begin(), table.end(), @@ -113,13 +192,14 @@ Eigen::SparseVector TableFactor::Convert( } /* ************************************************************************ */ -Eigen::SparseVector TableFactor::Convert(const std::string& table) { +Eigen::SparseVector TableFactor::Convert(const DiscreteKeys& keys, + const std::string& table) { // Convert string to doubles. std::vector ys; std::istringstream iss(table); std::copy(std::istream_iterator(iss), std::istream_iterator(), std::back_inserter(ys)); - return Convert(ys); + return Convert(keys, ys); } /* ************************************************************************ */ @@ -128,12 +208,13 @@ bool TableFactor::equals(const DiscreteFactor& other, double tol) const { return false; } else { const auto& f(static_cast(other)); - return sparse_table_.isApprox(f.sparse_table_, tol); + return Base::equals(other, tol) && + sparse_table_.isApprox(f.sparse_table_, tol); } } /* ************************************************************************ */ -double TableFactor::operator()(const DiscreteValues& values) const { +double TableFactor::evaluate(const Assignment& values) const { // a b c d => D * (C * (B * (a) + b) + c) + d uint64_t idx = 0, card = 1; for (auto it = sorted_dkeys_.rbegin(); it != sorted_dkeys_.rend(); ++it) { @@ -173,14 +254,67 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const { return toDecisionTreeFactor() * f; } +/* ************************************************************************ */ +DiscreteFactor::shared_ptr TableFactor::multiply( + const DiscreteFactor::shared_ptr& f) const { + DiscreteFactor::shared_ptr result; + if (auto tf = std::dynamic_pointer_cast(f)) { + // If `f` is a TableFactor, we can simply call `operator*`. + result = std::make_shared(this->operator*(*tf)); + + } else if (auto dtf = std::dynamic_pointer_cast(f)) { + // If `f` is a DecisionTreeFactor, we convert to a TableFactor which is + // cheaper than converting `this` to a DecisionTreeFactor. + result = std::make_shared(this->operator*(TableFactor(*dtf))); + + } else { + // Simulate double dispatch in C++ + // Useful for other classes which inherit from DiscreteFactor and have + // only `operator*(DecisionTreeFactor)` defined. Thus, other classes don't + // need to be updated to know about TableFactor. + // Those classes can be specialized to use TableFactor + // if efficiency is a problem. + result = std::make_shared( + f->operator*(this->toDecisionTreeFactor())); + } + return result; +} + +/* ************************************************************************ */ +DiscreteFactor::shared_ptr TableFactor::operator/( + const DiscreteFactor::shared_ptr& f) const { + if (auto tf = std::dynamic_pointer_cast(f)) { + return std::make_shared(this->operator/(*tf)); + } else if (auto dtf = std::dynamic_pointer_cast(f)) { + return std::make_shared( + this->operator/(TableFactor(f->discreteKeys(), *dtf))); + } else { + TableFactor divisor(f->toDecisionTreeFactor()); + return std::make_shared(this->operator/(divisor)); + } +} + /* ************************************************************************ */ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { DiscreteKeys dkeys = discreteKeys(); - std::vector table; - for (auto i = 0; i < sparse_table_.size(); i++) { - table.push_back(sparse_table_.coeff(i)); + + // If no keys, then return empty DecisionTreeFactor + if (dkeys.size() == 0) { + AlgebraicDecisionTree tree; + // We can have an empty sparse_table_ or one with a single value. + if (sparse_table_.size() != 0) { + tree = AlgebraicDecisionTree(sparse_table_.coeff(0)); + } + return DecisionTreeFactor(dkeys, tree); } - DecisionTreeFactor f(dkeys, table); + + std::vector table(sparse_table_.size(), 0.0); + for (SparseIt it(sparse_table_); it; ++it) { + table[it.index()] = it.value(); + } + + AlgebraicDecisionTree tree(dkeys, table); + DecisionTreeFactor f(dkeys, tree); return f; } @@ -249,7 +383,8 @@ void TableFactor::print(const string& s, const KeyFormatter& formatter) const { for (auto&& kv : assignment) { cout << "(" << formatter(kv.first) << ", " << kv.second << ")"; } - cout << " | " << it.value() << " | " << it.index() << endl; + cout << " | " << std::setw(10) << std::left << it.value() << " | " + << it.index() << endl; } cout << "number of nnzs: " << sparse_table_.nonZeros() << endl; } diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index f0ecd66a3..1cb9eda8b 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -17,8 +17,10 @@ #pragma once +#include #include #include +#include #include #include @@ -30,6 +32,12 @@ #include #include +#if GTSAM_ENABLE_BOOST_SERIALIZATION +#include + +#include +#endif + namespace gtsam { class DiscreteConditional; @@ -79,40 +87,24 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { return DiscreteKey(keys_[i], cardinalities_.at(keys_[i])); } - /// Convert probability table given as doubles to SparseVector. - /// Example) {0, 1, 1, 0, 0, 1, 0} -> values: {1, 1, 1}, indices: {1, 2, 5} - static Eigen::SparseVector Convert(const std::vector& table); + public: + /** + * Convert probability table given as doubles to SparseVector. + * Example: {0, 1, 1, 0, 0, 1, 0} -> values: {1, 1, 1}, indices: {1, 2, 5} + */ + static Eigen::SparseVector Convert(const DiscreteKeys& keys, + const std::vector& table); /// Convert probability table given as string to SparseVector. - static Eigen::SparseVector Convert(const std::string& table); + static Eigen::SparseVector Convert(const DiscreteKeys& keys, + const std::string& table); - public: // typedefs needed to play nice with gtsam typedef TableFactor This; typedef DiscreteFactor Base; ///< Typedef to base class typedef std::shared_ptr shared_ptr; typedef Eigen::SparseVector::InnerIterator SparseIt; typedef std::vector> AssignValList; - using Unary = std::function; - using UnaryAssignment = - std::function&, const double&)>; - using Binary = std::function; - - public: - /** The Real ring with addition and multiplication */ - struct Ring { - static inline double zero() { return 0.0; } - static inline double one() { return 1.0; } - static inline double add(const double& a, const double& b) { return a + b; } - static inline double max(const double& a, const double& b) { - return std::max(a, b); - } - static inline double mul(const double& a, const double& b) { return a * b; } - static inline double div(const double& a, const double& b) { - return (a == 0 || b == 0) ? 0 : (a / b); - } - static inline double id(const double& x) { return x; } - }; /// @name Standard Constructors /// @{ @@ -129,11 +121,11 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { /** Constructor from doubles */ TableFactor(const DiscreteKeys& keys, const std::vector& table) - : TableFactor(keys, Convert(table)) {} + : TableFactor(keys, Convert(keys, table)) {} /** Constructor from string */ TableFactor(const DiscreteKeys& keys, const std::string& table) - : TableFactor(keys, Convert(table)) {} + : TableFactor(keys, Convert(keys, table)) {} /// Single-key specialization template @@ -146,6 +138,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { /// Constructor from DecisionTreeFactor TableFactor(const DiscreteKeys& keys, const DecisionTreeFactor& dtf); + TableFactor(const DecisionTreeFactor& dtf); /// Constructor from DecisionTree/AlgebraicDecisionTree TableFactor(const DiscreteKeys& keys, const DecisionTree& dtree); @@ -169,14 +162,11 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { // /// @name Standard Interface // /// @{ - /// Calculate probability for given values `x`, - /// is just look up in TableFactor. - double evaluate(const DiscreteValues& values) const { - return operator()(values); - } + /// Getter for the underlying sparse vector + Eigen::SparseVector sparseTable() const { return sparse_table_; } - /// Evaluate probability distribution, sugar. - double operator()(const DiscreteValues& values) const override; + /// Evaluate probability distribution, is just look up in TableFactor. + double evaluate(const Assignment& values) const override; /// Calculate error for DiscreteValues `x`, is -log(probability). double error(const DiscreteValues& values) const override; @@ -189,6 +179,23 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { /// multiply with DecisionTreeFactor DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + /** + * @brief Multiply factors, DiscreteFactor::shared_ptr edition. + * + * This method accepts `DiscreteFactor::shared_ptr` and uses dynamic + * dispatch and specializations to perform the most efficient + * multiplication. + * + * While converting a DecisionTreeFactor to a TableFactor is efficient, the + * reverse is not. + * Hence we specialize the code to return a TableFactor always. + * + * @param f The factor to multiply with. + * @return DiscreteFactor::shared_ptr + */ + virtual DiscreteFactor::shared_ptr multiply( + const DiscreteFactor::shared_ptr& f) const override; + static double safe_div(const double& a, const double& b); /// divide by factor f (safely) @@ -196,30 +203,34 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { return apply(f, safe_div); } + /// divide by DiscreteFactor::shared_ptr f (safely) + DiscreteFactor::shared_ptr operator/( + const DiscreteFactor::shared_ptr& f) const override; + /// Convert into a decisiontree DecisionTreeFactor toDecisionTreeFactor() const override; /// Create a TableFactor that is a subset of this TableFactor - TableFactor choose(const DiscreteValues assignments, + TableFactor choose(const DiscreteValues parentAssignments, DiscreteKeys parent_keys) const; /// Create new factor by summing all values with the same separator values - shared_ptr sum(size_t nrFrontals) const { + DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override { return combine(nrFrontals, Ring::add); } /// Create new factor by summing all values with the same separator values - shared_ptr sum(const Ordering& keys) const { + DiscreteFactor::shared_ptr sum(const Ordering& keys) const override { return combine(keys, Ring::add); } /// Create new factor by maximizing over all values with the same separator. - shared_ptr max(size_t nrFrontals) const { + DiscreteFactor::shared_ptr max(size_t nrFrontals) const override { return combine(nrFrontals, Ring::max); } /// Create new factor by maximizing over all values with the same separator. - shared_ptr max(const Ordering& keys) const { + DiscreteFactor::shared_ptr max(const Ordering& keys) const override { return combine(keys, Ring::max); } @@ -324,6 +335,12 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { */ TableFactor prune(size_t maxNrAssignments) const; + /** + * Get the number of non-zero values contained in this factor. + * It could be much smaller than `prod_{key}(cardinality(key))`. + */ + uint64_t nrValues() const override { return sparse_table_.nonZeros(); } + /// @} /// @name Wrapper support /// @{ @@ -359,6 +376,19 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { double error(const HybridValues& values) const override; /// @} + + private: +#if GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(sparse_table_); + ar& BOOST_SERIALIZATION_NVP(denominators_); + ar& BOOST_SERIALIZATION_NVP(sorted_dkeys_); + } +#endif }; // traits diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 55c8f9e22..40f1822cf 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -61,14 +61,14 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor { DecisionTreeFactor(const std::vector& keys, string table); DecisionTreeFactor(const gtsam::DiscreteConditional& c); - + void print(string s = "DecisionTreeFactor\n", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::DecisionTreeFactor& other, double tol = 1e-9) const; size_t cardinality(gtsam::Key j) const; - + double operator()(const gtsam::DiscreteValues& values) const; gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const; size_t cardinality(gtsam::Key j) const; @@ -168,6 +168,43 @@ virtual class DiscreteDistribution : gtsam::DiscreteConditional { std::vector pmf() const; }; +#include +virtual class TableFactor : gtsam::DiscreteFactor { + TableFactor(); + TableFactor(const gtsam::DiscreteKeys& keys, + const gtsam::TableFactor& potentials); + TableFactor(const gtsam::DiscreteKeys& keys, std::vector& table); + TableFactor(const gtsam::DiscreteKeys& keys, string spec); + TableFactor(const gtsam::DiscreteKeys& keys, + const gtsam::DecisionTreeFactor& dtf); + TableFactor(const gtsam::DecisionTreeFactor& dtf); + + void print(string s = "TableFactor\n", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + + double evaluate(const gtsam::DiscreteValues& values) const; + double error(const gtsam::DiscreteValues& values) const; +}; + +#include +virtual class TableDistribution : gtsam::DiscreteConditional { + TableDistribution(); + TableDistribution(const gtsam::TableFactor& f); + TableDistribution(const gtsam::DiscreteKey& key, std::vector spec); + TableDistribution(const gtsam::DiscreteKeys& keys, std::vector spec); + TableDistribution(const gtsam::DiscreteKeys& keys, string spec); + TableDistribution(const gtsam::DiscreteKey& key, string spec); + + void print(string s = "Table Distribution\n", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + + gtsam::TableFactor table() const; + double evaluate(const gtsam::DiscreteValues& values) const; + size_t nrValues() const; +}; + #include class DiscreteBayesNet { DiscreteBayesNet(); diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 526001b51..c664fe6b5 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -124,14 +125,6 @@ struct traits
: public Testable
{}; GTSAM_CONCEPT_TESTABLE_INST(DT) -struct Ring { - static inline int zero() { return 0; } - static inline int one() { return 1; } - static inline int id(const int& a) { return a; } - static inline int add(const int& a, const int& b) { return a + b; } - static inline int mul(const int& a, const int& b) { return a * b; } -}; - /* ************************************************************************** */ // Check that creating decision trees respects key order. TEST(DecisionTree, ConstructorOrder) { diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index a41d06c2b..ec9185ecb 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -30,6 +30,12 @@ using namespace std; using namespace gtsam; +/** Convert Signature into CPT */ +DecisionTreeFactor create(const Signature& signature) { + DecisionTreeFactor p(signature.discreteKeys(), signature.cpt()); + return p; +} + /* ************************************************************************* */ TEST(DecisionTreeFactor, ConstructorsMatch) { // Declare two keys @@ -105,21 +111,45 @@ TEST(DecisionTreeFactor, multiplication) { CHECK(assert_equal(expected2, actual)); } +/* ************************************************************************* */ +TEST(DecisionTreeFactor, Divide) { + DiscreteKey A(0, 2), S(1, 2); + DecisionTreeFactor pA = create(A % "99/1"), pS = create(S % "50/50"); + DecisionTreeFactor joint = pA * pS; + + DecisionTreeFactor s = joint / pA; + + // Factors are not equal due to difference in keys + EXPECT(assert_inequal(pS, s)); + + // The underlying data should be the same + using ADT = AlgebraicDecisionTree; + EXPECT(assert_equal(ADT(pS), ADT(s))); + + KeySet keys(joint.keys()); + keys.insert(pA.keys().begin(), pA.keys().end()); + EXPECT(assert_inequal(KeySet(pS.keys()), keys)); + +} + /* ************************************************************************* */ TEST(DecisionTreeFactor, sum_max) { DiscreteKey v0(0, 3), v1(1, 2); DecisionTreeFactor f1(v0 & v1, "1 2 3 4 5 6"); DecisionTreeFactor expected(v1, "9 12"); - DecisionTreeFactor::shared_ptr actual = f1.sum(1); + auto actual = std::dynamic_pointer_cast(f1.sum(1)); + CHECK(actual); CHECK(assert_equal(expected, *actual, 1e-5)); DecisionTreeFactor expected2(v1, "5 6"); - DecisionTreeFactor::shared_ptr actual2 = f1.max(1); + auto actual2 = std::dynamic_pointer_cast(f1.max(1)); + CHECK(actual2); CHECK(assert_equal(expected2, *actual2)); DecisionTreeFactor f2(v1 & v0, "1 2 3 4 5 6"); - DecisionTreeFactor::shared_ptr actual22 = f2.sum(1); + auto actual22 = std::dynamic_pointer_cast(f2.sum(1)); + CHECK(actual22); } /* ************************************************************************* */ @@ -140,11 +170,46 @@ TEST(DecisionTreeFactor, enumerate) { EXPECT(actual == expected); } +namespace pruning_fixture { + +DiscreteKey A(1, 2), B(2, 2), C(3, 2); +DecisionTreeFactor f(A& B& C, "1 5 3 7 2 6 4 8"); + +DiscreteKey D(4, 2); +DecisionTreeFactor factor( + D& C & B & A, + "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " + "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0"); + +} // namespace pruning_fixture + +/* ************************************************************************* */ +// Check if computing the correct threshold works. +TEST(DecisionTreeFactor, ComputeThreshold) { + using namespace pruning_fixture; + + // Only keep the leaves with the top 5 values. + double threshold = f.computeThreshold(5); + EXPECT_DOUBLES_EQUAL(4.0, threshold, 1e-9); + + // Check for more extreme pruning where we only keep the top 2 leaves + threshold = f.computeThreshold(2); + EXPECT_DOUBLES_EQUAL(7.0, threshold, 1e-9); + + threshold = factor.computeThreshold(5); + EXPECT_DOUBLES_EQUAL(0.99995287, threshold, 1e-9); + + threshold = factor.computeThreshold(3); + EXPECT_DOUBLES_EQUAL(1.0, threshold, 1e-9); + + threshold = factor.computeThreshold(6); + EXPECT_DOUBLES_EQUAL(0.61247742, threshold, 1e-9); +} + /* ************************************************************************* */ // Check pruning of the decision tree works as expected. TEST(DecisionTreeFactor, Prune) { - DiscreteKey A(1, 2), B(2, 2), C(3, 2); - DecisionTreeFactor f(A & B & C, "1 5 3 7 2 6 4 8"); + using namespace pruning_fixture; // Only keep the leaves with the top 5 values. size_t maxNrAssignments = 5; @@ -160,12 +225,6 @@ TEST(DecisionTreeFactor, Prune) { DecisionTreeFactor expected2(A & B & C, "0 0 0 7 0 0 0 8"); EXPECT(assert_equal(expected2, pruned2)); - DiscreteKey D(4, 2); - DecisionTreeFactor factor( - D & C & B & A, - "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " - "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0"); - DecisionTreeFactor expected3(D & C & B & A, "0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 " "0.999952870000 1.0 1.0 1.0 1.0"); @@ -188,12 +247,6 @@ void maybeSaveDotFile(const DecisionTreeFactor& f, const string& filename) { #endif } -/** Convert Signature into CPT */ -DecisionTreeFactor create(const Signature& signature) { - DecisionTreeFactor p(signature.discreteKeys(), signature.cpt()); - return p; -} - /* ************************************************************************* */ // test Asia Joint TEST(DecisionTreeFactor, joint) { diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp index 2482a86a2..b91e1bd8a 100644 --- a/gtsam/discrete/tests/testDiscreteConditional.cpp +++ b/gtsam/discrete/tests/testDiscreteConditional.cpp @@ -46,7 +46,7 @@ TEST(DiscreteConditional, constructors) { DecisionTreeFactor f2( X & Y & Z, "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75"); DiscreteConditional actual2(1, f2); - DecisionTreeFactor expected2 = f2 / *f2.sum(1); + DecisionTreeFactor expected2 = f2 / f2.sum(1)->toDecisionTreeFactor(); EXPECT(assert_equal(expected2, static_cast(actual2))); std::vector probs{0.2, 0.5, 0.3, 0.6, 0.4, 0.7, 0.25, 0.55, 0.35, 0.65, 0.45, 0.75}; @@ -70,7 +70,7 @@ TEST(DiscreteConditional, constructors_alt_interface) { DecisionTreeFactor f2( X & Y & Z, "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75"); DiscreteConditional actual2(1, f2); - DecisionTreeFactor expected2 = f2 / *f2.sum(1); + DecisionTreeFactor expected2 = f2 / f2.sum(1)->toDecisionTreeFactor(); EXPECT(assert_equal(expected2, static_cast(actual2))); } diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 341eb63e3..0c1dd7a2a 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -94,7 +94,7 @@ TEST_UNSAFE( DiscreteFactorGraph, DiscreteFactorGraphEvaluationTest) { EXPECT_DOUBLES_EQUAL( 1.944, graph(values), 1e-9); // Check if graph product works - DecisionTreeFactor product = graph.product(); + DecisionTreeFactor product = graph.product()->toDecisionTreeFactor(); EXPECT_DOUBLES_EQUAL( 1.944, product(values), 1e-9); } @@ -113,12 +113,13 @@ TEST(DiscreteFactorGraph, test) { const Ordering frontalKeys{0}; const auto [conditional, newFactorPtr] = EliminateDiscrete(graph, frontalKeys); - DecisionTreeFactor newFactor = *newFactorPtr; + DecisionTreeFactor newFactor = + *std::dynamic_pointer_cast(newFactorPtr); // Normalize newFactor by max for comparison with expected - auto normalization = newFactor.max(newFactor.size()); + auto denominator = newFactor.max(newFactor.size())->toDecisionTreeFactor(); - newFactor = newFactor / *normalization; + newFactor = newFactor / denominator; // Check Conditional CHECK(conditional); @@ -130,9 +131,10 @@ TEST(DiscreteFactorGraph, test) { CHECK(&newFactor); DecisionTreeFactor expectedFactor(B & A, "10 6 6 10"); // Normalize by max. - normalization = expectedFactor.max(expectedFactor.size()); - // Ensure normalization is correct. - expectedFactor = expectedFactor / *normalization; + denominator = + expectedFactor.max(expectedFactor.size())->toDecisionTreeFactor(); + // Ensure denominator is correct. + expectedFactor = expectedFactor / denominator; EXPECT(assert_equal(expectedFactor, newFactor)); // Test using elimination tree diff --git a/gtsam/discrete/tests/testSerializationDiscrete.cpp b/gtsam/discrete/tests/testSerializationDiscrete.cpp index df7df0b7e..b118a00f6 100644 --- a/gtsam/discrete/tests/testSerializationDiscrete.cpp +++ b/gtsam/discrete/tests/testSerializationDiscrete.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include using namespace std; @@ -32,6 +33,7 @@ BOOST_CLASS_EXPORT_GUID(Tree::Leaf, "gtsam_DecisionTreeStringInt_Leaf") BOOST_CLASS_EXPORT_GUID(Tree::Choice, "gtsam_DecisionTreeStringInt_Choice") BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor"); +BOOST_CLASS_EXPORT_GUID(TableFactor, "gtsam_TableFactor"); using ADT = AlgebraicDecisionTree; BOOST_CLASS_EXPORT_GUID(ADT, "gtsam_AlgebraicDecisionTree"); @@ -79,6 +81,19 @@ TEST(DiscreteSerialization, DecisionTreeFactor) { EXPECT(equalsBinary(f)); } +/* ************************************************************************* */ +// Check serialization for TableFactor +TEST(DiscreteSerialization, TableFactor) { + using namespace serializationTestHelpers; + + DiscreteKey A(Symbol('x', 1), 3); + TableFactor tf(A, "1 2 2"); + + EXPECT(equalsObj(tf)); + EXPECT(equalsXML(tf)); + EXPECT(equalsBinary(tf)); +} + /* ************************************************************************* */ // Check serialization for DiscreteConditional & DiscreteDistribution TEST(DiscreteSerialization, DiscreteConditional) { diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index 0f7d7a615..76a0f2b5c 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -134,14 +134,77 @@ TEST(TableFactor, constructors) { EXPECT(assert_equal(expected, f4)); // Test for 9=3x3 values. - DiscreteKey V(0, 3), W(1, 3); + DiscreteKey V(0, 3), W(1, 3), O(100, 3); DiscreteConditional conditional5(V | W = "1/2/3 5/6/7 9/10/11"); TableFactor f5(conditional5); - // GTSAM_PRINT(f5); - TableFactor expected_f5( - X & Y, - "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667"); + + std::string expected_values = + "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667"; + TableFactor expected_f5(V & W, expected_values); EXPECT(assert_equal(expected_f5, f5, 1e-6)); + + TableFactor f5_with_wrong_keys(V & O, expected_values); + EXPECT(assert_inequal(f5_with_wrong_keys, f5, 1e-9)); +} + +/* ************************************************************************* */ +// Check conversion from DecisionTreeFactor. +TEST(TableFactor, Conversion) { + /* This is the DecisionTree we are using + Choice(m2) + 0 Choice(m1) + 0 0 Leaf 0 + 0 1 Choice(m0) + 0 1 0 Leaf 0 + 0 1 1 Leaf 0.14649446 // 3 + 1 Choice(m1) + 1 0 Choice(m0) + 1 0 0 Leaf 0 + 1 0 1 Leaf 0.14648756 // 5 + 1 1 Choice(m0) + 1 1 0 Leaf 0.14649446 // 6 + 1 1 1 Leaf 0.23918345 // 7 + */ + DiscreteKeys dkeys = {{0, 2}, {1, 2}, {2, 2}}; + DecisionTreeFactor dtf( + dkeys, std::vector{0, 0, 0, 0.14649446, 0, 0.14648756, 0.14649446, + 0.23918345}); + + TableFactor tf(dtf.discreteKeys(), dtf); + + EXPECT(assert_equal(dtf, tf.toDecisionTreeFactor())); + + // Test for correct construction when keys are not in reverse order. + // This is possible in conditionals e.g. P(x1 | x0) + DiscreteKey X(1, 2), Y(0, 2); + DiscreteConditional dtf2( + X, {Y}, std::vector{0.33333333, 0.6, 0.66666667, 0.4}); + + TableFactor tf2(dtf2); + // GTSAM_PRINT(dtf2); + // GTSAM_PRINT(tf2); + // GTSAM_PRINT(tf2.toDecisionTreeFactor()); + + // Check for ADT equality since the order of keys is irrelevant + EXPECT(assert_equal>(dtf2, + tf2.toDecisionTreeFactor())); +} + +/* ************************************************************************* */ +TEST(TableFactor, Empty) { + DiscreteKey X(1, 2); + + auto single = TableFactor({X}, "1 1").sum(1); + // Should not throw a segfault + auto expected_single = DecisionTreeFactor(X, "1 1").sum(1); + EXPECT(assert_equal(expected_single->toDecisionTreeFactor(), + single->toDecisionTreeFactor())); + + auto empty = TableFactor({X}, "0 0").sum(1); + // Should not throw a segfault + auto expected_empty = DecisionTreeFactor(X, "0 0").sum(1); + EXPECT(assert_equal(expected_empty->toDecisionTreeFactor(), + empty->toDecisionTreeFactor())); } /* ************************************************************************* */ @@ -242,15 +305,18 @@ TEST(TableFactor, sum_max) { TableFactor f1(v0 & v1, "1 2 3 4 5 6"); TableFactor expected(v1, "9 12"); - TableFactor::shared_ptr actual = f1.sum(1); + auto actual = std::dynamic_pointer_cast(f1.sum(1)); + CHECK(actual); CHECK(assert_equal(expected, *actual, 1e-5)); TableFactor expected2(v1, "5 6"); - TableFactor::shared_ptr actual2 = f1.max(1); + auto actual2 = std::dynamic_pointer_cast(f1.max(1)); + CHECK(actual2); CHECK(assert_equal(expected2, *actual2)); TableFactor f2(v1 & v0, "1 2 3 4 5 6"); - TableFactor::shared_ptr actual22 = f2.sum(1); + auto actual22 = std::dynamic_pointer_cast(f2.sum(1)); + CHECK(actual22); } /* ************************************************************************* */ diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 19194bb7a..9c4e42edf 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -21,7 +21,7 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -55,9 +55,9 @@ private: R range_; public: - enum { dimB = traits::dimension }; - enum { dimR = traits::dimension }; - enum { dimension = dimB + dimR }; + constexpr static const size_t dimB = traits::dimension; + constexpr static const size_t dimR = traits::dimension; + constexpr static const size_t dimension = dimB + dimR; /// @name Standard Constructors /// @{ @@ -148,7 +148,7 @@ public: /// @{ private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// Serialization function template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { @@ -162,9 +162,7 @@ private: /// @} // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { - NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0 - }; + inline constexpr static auto NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0; public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index 1b66eb336..63efb0d49 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -73,7 +73,7 @@ class GTSAM_EXPORT Cal3 { double u0_ = 0.0f, v0_ = 0.0f; ///< principal point public: - enum { dimension = 5 }; + inline constexpr static auto dimension = 5; ///< shared pointer to calibration object using shared_ptr = std::shared_ptr; @@ -184,7 +184,7 @@ class GTSAM_EXPORT Cal3 { /// @{ private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 081688d48..71b5fba1d 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -37,7 +37,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3f { // Note: u0 and v0 are constants and not optimized. public: - enum { dimension = 3 }; + inline constexpr static auto dimension = 3; using shared_ptr = std::shared_ptr; /// @name Constructors @@ -145,7 +145,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3f { /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index a7ca08afc..8c1bbe302 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -36,7 +36,7 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { using Base = Cal3DS2_Base; public: - enum { dimension = 9 }; + inline constexpr static auto dimension = 9; ///< shared pointer to stereo calibration object using shared_ptr = std::shared_ptr; @@ -104,7 +104,7 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index b0ef0368b..986c9842b 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -46,7 +46,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 { double tol_ = 1e-5; ///< tolerance value when calibrating public: - enum { dimension = 9 }; + inline constexpr static auto dimension = 9; ///< shared pointer to stereo calibration object using shared_ptr = std::shared_ptr; @@ -156,7 +156,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 { /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index 3f1cac148..f10a31b65 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -55,7 +55,7 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 { double tol_ = 1e-5; ///< tolerance value when calibrating public: - enum { dimension = 9 }; + inline constexpr static auto dimension = 9; ///< shared pointer to fisheye calibration object using shared_ptr = std::shared_ptr; @@ -184,7 +184,7 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 { /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 309b6dca1..cef2b8f9b 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -50,7 +50,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { double xi_ = 0.0f; ///< mirror parameter public: - enum { dimension = 10 }; + inline constexpr static auto dimension = 10; ///< shared pointer to stereo calibration object using shared_ptr = std::shared_ptr; @@ -138,7 +138,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 885be614f..8a587af9b 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -33,7 +33,7 @@ namespace gtsam { */ class GTSAM_EXPORT Cal3_S2 : public Cal3 { public: - enum { dimension = 5 }; + inline constexpr static auto dimension = 5; ///< shared pointer to calibration object using shared_ptr = std::shared_ptr; @@ -132,7 +132,7 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 { /// @{ private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 4fee1a022..58c7dd37a 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -32,7 +32,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 { double b_ = 1.0f; ///< Stereo baseline. public: - enum { dimension = 6 }; + inline constexpr static auto dimension = 6; ///< shared pointer to stereo calibration object using shared_ptr = std::shared_ptr; @@ -143,7 +143,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 { /// @{ private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Cal3f.cpp b/gtsam/geometry/Cal3f.cpp index 319155dc9..b8e365eed 100644 --- a/gtsam/geometry/Cal3f.cpp +++ b/gtsam/geometry/Cal3f.cpp @@ -28,6 +28,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/Cal3f.h b/gtsam/geometry/Cal3f.h index f053f3d11..b6cf4435b 100644 --- a/gtsam/geometry/Cal3f.h +++ b/gtsam/geometry/Cal3f.h @@ -34,7 +34,7 @@ namespace gtsam { */ class GTSAM_EXPORT Cal3f : public Cal3 { public: - enum { dimension = 1 }; + inline constexpr static auto dimension = 1; using shared_ptr = std::shared_ptr; /// @name Constructors @@ -118,7 +118,7 @@ class GTSAM_EXPORT Cal3f : public Cal3 { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index dca15feb2..d364318c9 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -25,7 +25,7 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -230,7 +230,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -252,9 +252,7 @@ class GTSAM_EXPORT CalibratedCamera: public PinholeBase { public: - enum { - dimension = 6 - }; + inline constexpr static auto dimension = 6; /// @name Standard Constructors /// @{ @@ -408,7 +406,7 @@ private: /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 26d4952c8..36c1e1f54 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -26,6 +26,7 @@ #include #include +#include namespace gtsam { @@ -471,7 +472,7 @@ class CameraSet : public std::vector> { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index d514c4f19..74b736685 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -82,7 +82,7 @@ class EssentialMatrix { /// @name Manifold /// @{ - enum { dimension = 5 }; + inline constexpr static auto dimension = 5; inline static size_t Dim() { return dimension;} inline size_t dim() const { return dimension;} @@ -180,7 +180,7 @@ class EssentialMatrix { /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 5e8c26e62..837ba7263 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -74,6 +74,20 @@ Matrix3 FundamentalMatrix::matrix() const { V_.transpose().matrix(); } +Vector3 FundamentalMatrix::epipolarLine(const Point2& p, + OptionalJacobian<3, 7> H) { + Vector3 point(p.x(), p.y(), 1); // Create a point in homogeneous coordinates + Vector3 line = matrix() * point; // Compute the epipolar line + + if (H) { + // Compute the Jacobian if requested + throw std::runtime_error( + "FundamentalMatrix::epipolarLine: Jacobian not implemented yet."); + } + + return line; // Return the epipolar line +} + void FundamentalMatrix::print(const std::string& s) const { std::cout << s << matrix() << std::endl; } @@ -116,6 +130,20 @@ Matrix3 SimpleFundamentalMatrix::matrix() const { return Ka().transpose().inverse() * E_.matrix() * Kb().inverse(); } +Vector3 SimpleFundamentalMatrix::epipolarLine(const Point2& p, + OptionalJacobian<3, 7> H) { + Vector3 point(p.x(), p.y(), 1); // Create a point in homogeneous coordinates + Vector3 line = matrix() * point; // Compute the epipolar line + + if (H) { + // Compute the Jacobian if requested + throw std::runtime_error( + "SimpleFundamentalMatrix::epipolarLine: Jacobian not implemented yet."); + } + + return line; // Return the epipolar line +} + void SimpleFundamentalMatrix::print(const std::string& s) const { std::cout << s << " E:\n" << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_ diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index c114c2b5b..0752ced7d 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -7,6 +7,7 @@ #pragma once +#include #include #include #include @@ -86,6 +87,9 @@ class GTSAM_EXPORT FundamentalMatrix { /// Return the fundamental matrix representation Matrix3 matrix() const; + /// Computes the epipolar line in a (left) for a given point in b (right) + Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {}); + /// @name Testable /// @{ /// Print the FundamentalMatrix @@ -98,7 +102,7 @@ class GTSAM_EXPORT FundamentalMatrix { /// @name Manifold /// @{ - enum { dimension = 7 }; // 3 for U, 1 for s, 3 for V + inline constexpr static auto dimension = 7; // 3 for U, 1 for s, 3 for V inline static size_t Dim() { return dimension; } inline size_t dim() const { return dimension; } @@ -161,6 +165,9 @@ class GTSAM_EXPORT SimpleFundamentalMatrix { /// F = Ka^(-T) * E * Kb^(-1) Matrix3 matrix() const; + /// Computes the epipolar line in a (left) for a given point in b (right) + Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {}); + /// @name Testable /// @{ /// Print the SimpleFundamentalMatrix @@ -172,7 +179,7 @@ class GTSAM_EXPORT SimpleFundamentalMatrix { /// @name Manifold /// @{ - enum { dimension = 7 }; // 5 for E, 1 for fa, 1 for fb + inline constexpr static auto dimension = 7; // 5 for E, 1 for fa, 1 for fb inline static size_t Dim() { return dimension; } inline size_t dim() const { return dimension; } diff --git a/gtsam/geometry/Line3.cpp b/gtsam/geometry/Line3.cpp index f5cf344f5..5a4b22075 100644 --- a/gtsam/geometry/Line3.cpp +++ b/gtsam/geometry/Line3.cpp @@ -97,7 +97,6 @@ Line3 transformTo(const Pose3 &wTc, const Line3 &wL, Rot3 cRw = wRc.inverse(); Rot3 cRl = cRw * wL.R_; - Vector2 w_ab; Vector3 t = ((wL.R_).transpose() * wTc.translation()); Vector2 c_ab(wL.a_ - t[0], wL.b_ - t[1]); diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h index def49d268..5ae50e138 100644 --- a/gtsam/geometry/Line3.h +++ b/gtsam/geometry/Line3.h @@ -47,7 +47,7 @@ class GTSAM_EXPORT Line3 { double a_, b_; // Intersection of line with the world x-y plane rotated by R_ // Also the closest point on line to origin public: - enum { dimension = 4 }; + inline constexpr static auto dimension = 4; /** Default constructor is the Z axis **/ Line3() : diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 07c8445fe..3d84086ac 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -39,9 +39,7 @@ private: double d_; ///< The perpendicular distance to this plane public: - enum { - dimension = 3 - }; + inline constexpr static auto dimension = 3; /// @name Constructors /// @{ diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 8329664fd..f8599572d 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -51,9 +51,7 @@ private: public: - enum { - dimension = 6 + DimK - }; ///< Dimension depends on calibration + inline constexpr static auto dimension = 6 + DimK; ///< Dimension depends on calibration /// @name Standard Constructors /// @{ @@ -326,7 +324,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index df6ec5c08..252d8bc6f 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -212,7 +212,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -245,9 +245,7 @@ private: public: - enum { - dimension = 6 - }; ///< There are 6 DOF to optimize for + inline constexpr static auto dimension = 6; ///< There are 6 DOF to optimize for /// @name Standard Constructors /// @{ @@ -427,7 +425,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h index 55269f9d6..45cdb11e4 100644 --- a/gtsam/geometry/PinholeSet.h +++ b/gtsam/geometry/PinholeSet.h @@ -64,7 +64,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 76beea11a..66d57f785 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -19,7 +19,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index ef91108eb..79fd6b9bf 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -69,6 +69,16 @@ Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1, p.x() * q.y() - p.y() * q.x()); } +Point3 doubleCross(const Point3 &p, const Point3 &q, // + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) { + if (H1) *H1 = q.dot(p) * I_3x3 + p * q.transpose() - 2 * q * p.transpose(); + if (H2) { + const Matrix3 W = skewSymmetric(p); + *H2 = W * W; + } + return gtsam::cross(p, gtsam::cross(p, q)); +} + double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { if (H1) *H1 << q.x(), q.y(), q.z(); diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 8da83817d..9bc943a43 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -26,7 +26,7 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -55,11 +55,16 @@ GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {}); /// normalize, with optional Jacobian GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {}); -/// cross product @return this x q +/// cross product @return p x q GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q, OptionalJacobian<3, 3> H_p = {}, OptionalJacobian<3, 3> H_q = {}); +/// double cross product @return p x (p x q) +GTSAM_EXPORT Point3 doubleCross(const Point3& p, const Point3& q, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}); + /// dot product GTSAM_EXPORT double dot(const Point3& p, const Point3& q, OptionalJacobian<1, 3> H_p = {}, diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 05678dc27..640481406 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index aad81493f..da7bc922e 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -60,7 +60,10 @@ public: } /** copy constructor */ - Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {} + Pose2(const Pose2& pose) = default; + // : r_(pose.r_), t_(pose.t_) {} + + Pose2& operator=(const Pose2& other) = default; /** * construct from (x,y,theta) @@ -81,9 +84,13 @@ public: Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} /** Constructor from 3*3 matrix */ - Pose2(const Matrix &T) : - r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { - assert(T.rows() == 3 && T.cols() == 3); + Pose2(const Matrix &T) + : r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { +#ifndef NDEBUG + if (T.rows() != 3 || T.cols() != 3) { + throw; + } +#endif } /// @} @@ -335,7 +342,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION // +#if GTSAM_ENABLE_BOOST_SERIALIZATION // // Serialization function friend class boost::serialization::access; template diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 894314491..6d9256f93 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -11,23 +11,21 @@ /** * @file Pose3.cpp - * @brief 3D Pose + * @brief 3D Pose manifold SO(3) x R^3 and group SE(3) */ -#include -#include -#include #include +#include +#include +#include +#include #include #include -#include #include namespace gtsam { -using std::vector; - /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3) @@ -45,6 +43,20 @@ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR, return Pose3(R, t); } +// Pose2 constructor Jacobian is always the same. +static const Matrix63 Hpose2 = (Matrix63() << // + 0., 0., 0., // + 0., 0., 0.,// + 0., 0., 1.,// + 1., 0., 0.,// + 0., 1., 0.,// + 0., 0., 0.).finished(); + +Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) { + if (H) *H << Hpose2; + return Pose3(p); +} + /* ************************************************************************* */ Pose3 Pose3::inverse() const { Rot3 Rt = R_.inverse(); @@ -114,7 +126,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) { /* ************************************************************************* */ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { + OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { if (Hxi) { Hxi->setZero(); for (int i = 0; i < 6; ++i) { @@ -161,27 +173,31 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { return Pose3(interpolate(R_, T.R_, t), - interpolate(t_, T.t_, t)); + interpolate(t_, T.t_, t)); } /* ************************************************************************* */ -/** Modified from Murray94book version (which assumes w and v normalized?) */ +// Expmap is implemented in so3::ExpmapFunctor::expmap, based on Ethan Eade's +// elegant Lie group document, at https://www.ethaneade.org/lie.pdf. Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { - if (Hxi) *Hxi = ExpmapDerivative(xi); + // Get angular velocity omega and translational velocity v from twist xi + const Vector3 w = xi.head<3>(), v = xi.tail<3>(); - // get angular velocity omega and translational velocity v from twist xi - Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); + // Compute rotation using Expmap + Matrix3 Jr; + Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr); - Rot3 R = Rot3::Expmap(omega); - double theta2 = omega.dot(omega); - if (theta2 > std::numeric_limits::epsilon()) { - Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis - Vector3 omega_cross_v = omega.cross(v); // points towards axis - Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; - return Pose3(R, t); - } else { - return Pose3(R, v); + // Compute translation and optionally its Jacobian Q in w + // The Jacobian in v is the right Jacobian Jr of SO(3), which we already have. + Matrix3 Q; + const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr); + + if (Hxi) { + *Hxi << Jr, Z_3x3, // + Q, Jr; } + + return Pose3(R, t); } /* ************************************************************************* */ @@ -240,55 +256,55 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { } /* ************************************************************************* */ -Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { +Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, + double nearZeroThreshold) { const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - const Matrix3 V = skewSymmetric(v); - const Matrix3 W = skewSymmetric(w); - Matrix3 Q; - -#ifdef NUMERICAL_EXPMAP_DERIV - Matrix3 Qj = Z_3x3; - double invFac = 1.0; - Q = Z_3x3; - Matrix3 Wj = I_3x3; - for (size_t j=1; j<10; ++j) { - Qj = Qj*W + Wj*V; - invFac = -invFac/(j+1); - Q = Q + invFac*Qj; - Wj = Wj*W; - } -#else - // The closed-form formula in Barfoot14tro eq. (102) - double phi = w.norm(); - const Matrix3 WVW = W * V * W; - if (std::abs(phi) > nearZeroThreshold) { - const double s = sin(phi), c = cos(phi); - const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, - phi5 = phi4 * phi; - // Invert the sign of odd-order terms to have the right Jacobian - Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) + - (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) - - 0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) * - (WVW * W + W * WVW); - } else { - Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) - - 1. / 24. * (W * W * V + V * W * W - 3 * WVW) + - 1. / 120. * (WVW * W + W * WVW); - } -#endif - + ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); return Q; } +/* ************************************************************************* */ +// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas +// t_parallel = w * w.dot(v); // translation parallel to axis +// w_cross_v = w.cross(v); // translation orthogonal to axis +// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2; +// but functor does not need R, deals automatically with the case where theta2 +// is near zero, and also gives us the machinery for the Jacobians. +Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, + OptionalJacobian<3, 3> Q, + OptionalJacobian<3, 3> J, + double nearZeroThreshold) { + const double theta2 = w.dot(w); + bool nearZero = (theta2 <= nearZeroThreshold); + + // Instantiate functor for Dexp-related operations: + so3::DexpFunctor local(w, nearZero); + + // Call applyLeftJacobian which is faster than local.leftJacobian() * v if you + // don't need Jacobians, and returns Jacobian of t with respect to w if asked. + Matrix3 H; + Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr); + + // We return Jacobians for use in Expmap, so we multiply with X, that + // translates from left to right for our right expmap convention: + if (Q) { + Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); + *Q = X * H; + } + + if (J) { + *J = local.rightJacobian(); // = X * local.leftJacobian(); + } + + return t; +} + /* ************************************************************************* */ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { - const Vector3 w = xi.head<3>(); - const Matrix3 Jw = Rot3::ExpmapDerivative(w); - const Matrix3 Q = ComputeQforExpmapDerivative(xi); Matrix6 J; - J << Jw, Z_3x3, Q, Jw; + Expmap(xi, J); return J; } @@ -311,7 +327,6 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const { } /* ************************************************************************* */ - const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const { if (Hself) { *Hself << I_3x3, Z_3x3; @@ -329,14 +344,14 @@ Matrix4 Pose3::matrix() const { /* ************************************************************************* */ Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself, - OptionalJacobian<6, 6> HaTb) const { + OptionalJacobian<6, 6> HaTb) const { const Pose3& wTa = *this; return wTa.compose(aTb, Hself, HaTb); } /* ************************************************************************* */ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself, - OptionalJacobian<6, 6> HwTb) const { + OptionalJacobian<6, 6> HwTb) const { if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap(); if (HwTb) *HwTb = I_6x6; const Pose3& wTa = *this; @@ -345,7 +360,7 @@ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself, /* ************************************************************************* */ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself, - OptionalJacobian<3, 3> Hpoint) const { + OptionalJacobian<3, 3> Hpoint) const { // Only get matrix once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 R = R_.matrix(); @@ -369,7 +384,7 @@ Matrix Pose3::transformFrom(const Matrix& points) const { /* ************************************************************************* */ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, - OptionalJacobian<3, 3> Hpoint) const { + OptionalJacobian<3, 3> Hpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); @@ -475,7 +490,7 @@ std::optional Pose3::Align(const Point3Pairs &abPointPairs) { std::optional Pose3::Align(const Matrix& a, const Matrix& b) { if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) { throw std::invalid_argument( - "Pose3:Align expects 3*N matrices of equal shape."); + "Pose3:Align expects 3*N matrices of equal shape."); } Point3Pairs abPointPairs; for (Eigen::Index j = 0; j < a.cols(); j++) { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 8a807cc23..e4896ae26 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -11,7 +11,7 @@ /** *@file Pose3.h - *@brief 3D Pose + * @brief 3D Pose manifold SO(3) x R^3 and group SE(3) */ // \callgraph @@ -55,9 +55,9 @@ public: Pose3() : R_(traits::Identity()), t_(traits::Identity()) {} /** Copy constructor */ - Pose3(const Pose3& pose) : - R_(pose.R_), t_(pose.t_) { - } + Pose3(const Pose3& pose) = default; + + Pose3& operator=(const Pose3& other) = default; /** Construct from R,t */ Pose3(const Rot3& R, const Point3& t) : @@ -78,6 +78,9 @@ public: OptionalJacobian<6, 3> HR = {}, OptionalJacobian<6, 3> Ht = {}); + /** Construct from Pose2 in the xy plane, with derivative. */ + static Pose3 FromPose2(const Pose2& p, OptionalJacobian<6,3> H = {}); + /** * Create Pose3 by aligning two point pairs * A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point @@ -217,10 +220,27 @@ public: * (see Chirikjian11book2, pg 44, eq 10.95. * The closed-form formula is identical to formula 102 in Barfoot14tro where * Q_l of the SE3 Expmap left derivative matrix is given. + * This is the Jacobian of ExpmapTranslation and computed there. */ static Matrix3 ComputeQforExpmapDerivative( const Vector6& xi, double nearZeroThreshold = 1e-5); + /** + * Compute the translation part of the exponential map, with Jacobians. + * @param w 3D angular velocity + * @param v 3D velocity + * @param Q Optionally, compute 3x3 Jacobian wrpt w + * @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3) + * @param nearZeroThreshold threshold for small values + * @note This function returns Jacobians Q and J corresponding to the bottom + * blocks of the SE(3) exponential, and translated from left to right from the + * applyLeftJacobian Jacobians. + */ + static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v, + OptionalJacobian<3, 3> Q = {}, + OptionalJacobian<3, 3> J = {}, + double nearZeroThreshold = 1e-5); + using LieGroup::inverse; // version with derivative /** @@ -389,7 +409,7 @@ public: friend std::ostream &operator<<(std::ostream &os, const Pose3& p); private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index f9ed2a383..36f891505 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -45,9 +45,7 @@ struct traits { /// @} /// @name Basic manifold traits /// @{ - enum { - dimension = 3 - }; + inline constexpr static auto dimension = 3; typedef OptionalJacobian<3, 3> ChartJacobian; typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 6bb97ff6c..faa0e79a2 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -52,11 +52,15 @@ namespace gtsam { Rot2() : c_(1.0), s_(0.0) {} /** copy constructor */ - Rot2(const Rot2& r) : Rot2(r.c_, r.s_) {} + Rot2(const Rot2& r) = default; + + Rot2& operator=(const Rot2& other) = default; /// Constructor from angle in radians == exponential map at identity Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {} + // Rot2& operator=(const gtsam::Rot2& other) = default; + /// Named constructor from angle in radians static Rot2 fromAngle(double theta) { return Rot2(theta); @@ -213,7 +217,7 @@ namespace gtsam { static Rot2 ClosestTo(const Matrix2& M); private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index a936bd02a..10ba5894e 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -23,6 +23,7 @@ #include #include +#include #include using namespace std; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 7e05ee4da..71f4cbacd 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -159,7 +159,11 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. inline static Rot3 RzRyRx(const Vector& xyz, OptionalJacobian<3, 3> H = {}) { - assert(xyz.size() == 3); +#ifndef NDEBUG + if (xyz.size() != 3) { + throw; + } +#endif Rot3 out; if (H) { Vector3 Hx, Hy, Hz; @@ -528,7 +532,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 2585c37be..41f7ce810 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -18,13 +18,14 @@ * @date December 2014 */ +#include +#include #include +#include #include #include - #include -#include #include namespace gtsam { @@ -32,6 +33,15 @@ namespace gtsam { //****************************************************************************** namespace so3 { +static constexpr double one_6th = 1.0 / 6.0; +static constexpr double one_12th = 1.0 / 12.0; +static constexpr double one_24th = 1.0 / 24.0; +static constexpr double one_60th = 1.0 / 60.0; +static constexpr double one_120th = 1.0 / 120.0; +static constexpr double one_180th = 1.0 / 180.0; +static constexpr double one_720th = 1.0 / 720.0; +static constexpr double one_1260th = 1.0 / 1260.0; + GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) { Matrix99 H; auto R = Q.matrix(); @@ -41,7 +51,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) { return H; } -GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) { +GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, + OptionalJacobian<9, 9> H) { Matrix3 MR = M * R.matrix(); if (H) *H = Dcompose(R); return MR; @@ -51,82 +62,128 @@ void ExpmapFunctor::init(bool nearZeroApprox) { nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); if (!nearZero) { - sin_theta = std::sin(theta); + const double sin_theta = std::sin(theta); + A = sin_theta / theta; const double s2 = std::sin(theta / 2.0); - one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + const double one_minus_cos = + 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + B = one_minus_cos / theta2; + } else { + // Taylor expansion at 0 + A = 1.0 - theta2 * one_6th; + B = 0.5 - theta2 * one_24th; } } ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) - : theta2(omega.dot(omega)), theta(std::sqrt(theta2)) { - const double wx = omega.x(), wy = omega.y(), wz = omega.z(); - W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; + : theta2(omega.dot(omega)), + theta(std::sqrt(theta2)), + W(skewSymmetric(omega)), + WW(W * W) { init(nearZeroApprox); - if (!nearZero) { - K = W / theta; - KK = K * K; - } } ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) - : theta2(angle * angle), theta(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; + : theta2(angle * angle), + theta(angle), + W(skewSymmetric(axis * angle)), + WW(W * W) { init(nearZeroApprox); - if (!nearZero) { - KK = K * K; - } } -SO3 ExpmapFunctor::expmap() const { - if (nearZero) - return SO3(I_3x3 + W); - else - return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK); -} +SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { - if (nearZero) { - dexp_ = I_3x3 - 0.5 * W; + if (!nearZero) { + C = (1 - A) / theta2; + D = (1.0 - A / (2.0 * B)) / theta2; + E = (2.0 * B - A) / theta2; + F = (3.0 * C - B) / theta2; } else { - a = one_minus_cos / theta; - b = 1.0 - sin_theta / theta; - dexp_ = I_3x3 - a * K + b * KK; + // Taylor expansion at 0 + // TODO(Frank): flipping signs here does not trigger any tests: harden! + C = one_6th - theta2 * one_120th; + D = one_12th + theta2 * one_720th; + E = one_12th - theta2 * one_180th; + F = one_60th - theta2 * one_1260th; } } +Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const { + // Wv = omega x v + const Vector3 Wv = gtsam::cross(omega, v); + if (H) { + // Apply product rule to (B Wv) + // - E * omega.transpose() is 1x3 Jacobian of B with respect to omega + // - skewSymmetric(v) is 3x3 Jacobian of Wv = gtsam::cross(omega, v) + *H = - Wv * E * omega.transpose() - B * skewSymmetric(v); + } + return B * Wv; +} + +Vector3 DexpFunctor::doubleCrossC(const Vector3& v, + OptionalJacobian<3, 3> H) const { + // WWv = omega x (omega x * v) + Matrix3 doubleCrossJacobian; + const Vector3 WWv = + gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr); + if (H) { + // Apply product rule to (C WWv) + // - F * omega.transpose() is 1x3 Jacobian of C with respect to omega + // doubleCrossJacobian is 3x3 Jacobian of WWv = gtsam::doubleCross(omega, v) + *H = - WWv * F * omega.transpose() + C * doubleCrossJacobian; + } + return C * WWv; +} + +// Multiplies v with left Jacobian through vector operations only. 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; + Matrix3 D_BWv_w, D_CWWv_w; + const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr); + const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr); + if (H1) *H1 = -D_BWv_w + D_CWWv_w; + if (H2) *H2 = rightJacobian(); + return v - BWv + CWWv; } 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; + const Matrix3 invJr = rightJacobianInverse(); + const Vector3 c = invJr * v; if (H1) { - Matrix3 D_dexpv_omega; - applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping - *H1 = -invDexp * D_dexpv_omega; + Matrix3 H; + applyDexp(c, H); // get derivative H of forward mapping + *H1 = -invJr * H; } - if (H2) *H2 = invDexp; + if (H2) *H2 = invJr; + return c; +} + +Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, + OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + Matrix3 D_BWv_w, D_CWWv_w; + const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr); + const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr); + if (H1) *H1 = D_BWv_w + D_CWWv_w; + if (H2) *H2 = leftJacobian(); + return v + BWv + CWWv; +} + +Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v, + OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + const Matrix3 invJl = leftJacobianInverse(); + const Vector3 c = invJl * v; + if (H1) { + Matrix3 H; + applyLeftJacobian(c, H); // get derivative H of forward mapping + *H1 = -invJl * H; + } + if (H2) *H2 = invJl; return c; } @@ -168,12 +225,7 @@ SO3 SO3::ChordalMean(const std::vector& rotations) { template <> GTSAM_EXPORT Matrix3 SO3::Hat(const Vector3& xi) { - // skew symmetric matrix X = xi^ - Matrix3 Y = Z_3x3; - Y(0, 1) = -xi(2); - Y(0, 2) = +xi(1); - Y(1, 2) = -xi(0); - return Y - Y.transpose(); + return skewSymmetric(xi); } //****************************************************************************** diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index cd67bfb8c..36329a19d 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -26,7 +26,6 @@ #include #include -#include #include namespace gtsam { @@ -99,7 +98,7 @@ template <> GTSAM_EXPORT Vector9 SO3::vec(OptionalJacobian<9, 3> H) const; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION template /** Serialization function */ void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) { @@ -133,16 +132,17 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); // functor also implements dedicated methods to apply dexp and/or inv(dexp). /// Functor implementing Exponential map -class GTSAM_EXPORT ExpmapFunctor { - protected: - const double theta2; - Matrix3 W, K, KK; +/// Math is based on Ethan Eade's elegant Lie group document, at +/// https://www.ethaneade.org/lie.pdf. +struct GTSAM_EXPORT ExpmapFunctor { + const double theta2, theta; + const Matrix3 W, WW; bool nearZero; - double theta, sin_theta, one_minus_cos; // only defined if !nearZero - void init(bool nearZeroApprox = false); + // Ethan Eade's constants: + double A; // A = sin(theta) / theta + double B; // B = (1 - cos(theta)) - public: /// Constructor with element of Lie algebra so(3) explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -151,34 +151,75 @@ class GTSAM_EXPORT ExpmapFunctor { /// Rodrigues formula SO3 expmap() const; + + protected: + void init(bool nearZeroApprox = false); }; /// Functor that implements Exponential map *and* its derivatives -class DexpFunctor : public ExpmapFunctor { +/// Math extends Ethan theme of elegant I + aW + bWW expressions. +/// See https://www.ethaneade.org/lie.pdf expmap (82) and left Jacobian (83). +struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Vector3 omega; - double a, b; - Matrix3 dexp_; - public: + // Ethan's C constant used in Jacobians + double C; // (1 - A) / theta^2 + + // Constant used in inverse Jacobians + double D; // (1 - A/2B) / theta2 + + // Constants used in cross and doubleCross + double E; // (2B - A) / theta2 + double F; // (3C - B) / theta2 + /// Constructor with element of Lie algebra so(3) - GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); + explicit 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_; } + // Expmap(xi + dxi) \approx Expmap(xi) * Expmap(dexp * dxi) + // This maps a perturbation dxi=(w,v) in the tangent space to + // a perturbation on the manifold Expmap(dexp * xi) + Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; } + + // Compute the left Jacobian for Exponential map in SO(3) + Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; } + + /// Differential of expmap == right Jacobian + inline Matrix3 dexp() const { return rightJacobian(); } + + /// Inverse of right Jacobian + Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; } + + // Inverse of left Jacobian + Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; } + + /// Synonym for rightJacobianInverse + inline Matrix3 invDexp() const { return rightJacobianInverse(); } + + /// Computes B * (omega x v). + Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; + + /// Computes C * (omega x (omega x v)). + Vector3 doubleCrossC(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; /// Multiplies with dexp(), with optional derivatives - GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, + Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; /// Multiplies with dexp().inverse(), with optional derivatives - GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v, - OptionalJacobian<3, 3> H1 = {}, + Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; + + /// Multiplies with leftJacobian(), with optional derivatives + Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; + + /// Multiplies with leftJacobianInverse(), with optional derivatives + Vector3 applyLeftJacobianInverse(const Vector3& v, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; }; } // namespace so3 diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp index 1c4920af8..e2d6c2a69 100644 --- a/gtsam/geometry/SO4.cpp +++ b/gtsam/geometry/SO4.cpp @@ -107,7 +107,6 @@ SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) { } // Build expX = exp(xi^) - Matrix4 expX; using std::cos; using std::sin; const auto X2 = X * X; diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index 834cab746..43b74f7aa 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -78,7 +78,7 @@ GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = {}); */ GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = {}); -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION template /** Serialization function */ void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) { diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 7e1762b3f..e8e1f641d 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -24,7 +24,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -33,6 +33,7 @@ #include #include #include +#include namespace gtsam { @@ -53,7 +54,7 @@ constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; } template class SO : public LieGroup, internal::DimensionSO(N)> { public: - enum { dimension = internal::DimensionSO(N) }; + inline constexpr static auto dimension = internal::DimensionSO(N); using MatrixNN = Eigen::Matrix; using VectorN2 = Eigen::Matrix; using MatrixDD = Eigen::Matrix; @@ -325,7 +326,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /// @name Serialization /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION template friend void save(Archive&, SO&, const unsigned int); template @@ -379,7 +380,7 @@ template <> GTSAM_EXPORT typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ template void serialize( diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index cf2360b08..e569755ef 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -45,7 +45,6 @@ static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs, static double calculateScale(const Point3Pairs &d_abPointPairs, const Rot3 &aRb) { double x = 0, y = 0; - Point3 da, db; for (const auto& [da, db] : d_abPointPairs) { const Vector3 da_prime = aRb * db; y += da.transpose() * da_prime; diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index 05bd0431e..8f7f30be8 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -203,7 +203,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup { private: - #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + #if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index ef20aa7fe..786295d7b 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -26,7 +26,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -41,7 +41,7 @@ namespace gtsam { */ class GTSAM_EXPORT EmptyCal { public: - enum { dimension = 0 }; + inline constexpr static auto dimension = 0; EmptyCal() {} virtual ~EmptyCal() = default; using shared_ptr = std::shared_ptr; @@ -54,7 +54,7 @@ class GTSAM_EXPORT EmptyCal { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -73,7 +73,7 @@ class GTSAM_EXPORT EmptyCal { */ class GTSAM_EXPORT SphericalCamera { public: - enum { dimension = 6 }; + inline constexpr static auto dimension = 6; using Measurement = Unit3; using MeasurementVector = std::vector; @@ -223,7 +223,7 @@ class GTSAM_EXPORT SphericalCamera { static size_t Dim() { return 6; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index da71dd070..315e4d267 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -61,9 +61,7 @@ private: public: - enum { - dimension = 6 - }; + inline constexpr static auto dimension = 6; /// @name Standard Constructors /// @{ @@ -179,7 +177,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int /*version*/) { diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 4383e212e..26c14750b 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -20,7 +20,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -37,7 +37,7 @@ private: double uL_, uR_, v_; public: - enum { dimension = 3 }; + inline constexpr static auto dimension = 3; /// @name Standard Constructors /// @{ @@ -150,7 +150,7 @@ private: /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 18bc5d9f0..65a304d62 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -53,9 +53,7 @@ private: public: - enum { - dimension = 2 - }; + inline constexpr static auto dimension = 2; /// @name Constructors /// @{ @@ -200,7 +198,7 @@ private: /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 42d2fe550..a8af78f2f 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -360,6 +360,7 @@ class Rot3 { // Standard Interface static gtsam::Rot3 Expmap(gtsam::Vector v); static gtsam::Vector Logmap(const gtsam::Rot3& p); + gtsam::Rot3 expmap(const gtsam::Vector& v); gtsam::Vector logmap(const gtsam::Rot3& p); gtsam::Matrix matrix() const; gtsam::Matrix transpose() const; diff --git a/gtsam/geometry/tests/testBearingRange.cpp b/gtsam/geometry/tests/testBearingRange.cpp index ff2a9a6a4..df4a4dc51 100644 --- a/gtsam/geometry/tests/testBearingRange.cpp +++ b/gtsam/geometry/tests/testBearingRange.cpp @@ -56,7 +56,7 @@ TEST(BearingRange, 3D) { EXPECT(assert_equal(expected, actual)); } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION using namespace serializationTestHelpers; /* ************************************************************************* */ TEST(BearingRange, Serialization2D) { diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index dcfb99105..b3b751ea1 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -154,6 +154,17 @@ TEST( Point3, cross2) { } } +/* ************************************************************************* */ +TEST(Point3, doubleCross) { + Matrix aH1, aH2; + std::function f = + [](const Point3& p, const Point3& q) { return doubleCross(p, q); }; + const Point3 omega(1, 2, 3), theta(4, 5, 6); + doubleCross(omega, theta, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); +} + //************************************************************************* TEST (Point3, normalize) { Matrix actualH; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index c9851f761..b04fb1982 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -835,17 +835,7 @@ TEST(Pose3, Align2) { } /* ************************************************************************* */ -TEST( Pose3, ExpmapDerivative1) { - Matrix6 actualH; - Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; - Pose3::Expmap(w,actualH); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - EXPECT(assert_equal(expectedH, actualH)); -} - -/* ************************************************************************* */ -TEST(Pose3, ExpmapDerivative2) { +TEST(Pose3, ExpmapDerivative) { // 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) @@ -879,26 +869,71 @@ TEST(Pose3, ExpmapDerivative2) { } } -TEST( Pose3, ExpmapDerivativeQr) { - Vector6 w = Vector6::Random(); - w.head<3>().normalize(); - w.head<3>() = w.head<3>() * 0.9e-2; - Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>(); - EXPECT(assert_equal(expectedQr, actualQr, 1e-6)); +//****************************************************************************** +namespace test_cases { +std::vector small{{0, 0, 0}, // + {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //, + {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}}; +std::vector large{{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, + {0, 0, 1}, {.1, .2, .3}, {1, -2, 3}}; +auto omegas = [](bool nearZero) { return nearZero ? small : large; }; +std::vector vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, + {.4, .3, .2}, {4, 5, 6}, {-10, -20, 30}}; +} // namespace test_cases + +//****************************************************************************** +TEST(Pose3, ExpmapDerivatives) { + for (bool nearZero : {true, false}) { + for (const Vector3& w : test_cases::omegas(nearZero)) { + for (Vector3 v : test_cases::vs) { + const Vector6 xi = (Vector6() << w, v).finished(); + const Matrix6 expectedH = + numericalDerivative21 >( + &Pose3::Expmap, xi, {}); + Matrix actualH; + Pose3::Expmap(xi, actualH); + EXPECT(assert_equal(expectedH, actualH)); + } + } + } } -/* ************************************************************************* */ -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); - EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5)); - Matrix expectedH = numericalDerivative21 >(&Pose3::Logmap, p, {}); - EXPECT(assert_equal(expectedH, actualH)); +//****************************************************************************** +// Check logmap for all small values, as we don't want wrapping. +TEST(Pose3, Logmap) { + static constexpr bool nearZero = true; + for (const Vector3& w : test_cases::omegas(nearZero)) { + for (Vector3 v : test_cases::vs) { + const Vector6 xi = (Vector6() << w, v).finished(); + Pose3 pose = Pose3::Expmap(xi); + EXPECT(assert_equal(xi, Pose3::Logmap(pose))); + } + } +} + +//****************************************************************************** +// Check logmap derivatives for all values +TEST(Pose3, LogmapDerivatives) { + for (bool nearZero : {true, false}) { + for (const Vector3& w : test_cases::omegas(nearZero)) { + for (Vector3 v : test_cases::vs) { + const Vector6 xi = (Vector6() << w, v).finished(); + Pose3 pose = Pose3::Expmap(xi); + const Matrix6 expectedH = + numericalDerivative21 >( + &Pose3::Logmap, pose, {}); + Matrix actualH; + Pose3::Logmap(pose, actualH); +#ifdef GTSAM_USE_QUATERNIONS + // TODO(Frank): Figure out why quaternions are not as accurate. + // Hint: 6 cases fail on Ubuntu 22.04, but none on MacOS. + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +#else + EXPECT(assert_equal(expectedH, actualH)); +#endif + } + } + } } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 27c143d31..c6fd52161 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -304,18 +304,77 @@ TEST(SO3, JacobianLogmap) { } //****************************************************************************** -TEST(SO3, ApplyDexp) { - Matrix aH1, aH2; - for (bool nearZeroApprox : {true, false}) { +namespace test_cases { +std::vector small{{0, 0, 0}, // + {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //, + {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}}; +std::vector large{ + {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}}; +auto omegas = [](bool nearZero) { return nearZero ? small : large; }; +std::vector vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}}; +} // namespace test_cases + +//****************************************************************************** +TEST(SO3, JacobianInverses) { + Matrix HR, HL; + for (bool nearZero : {true, false}) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + EXPECT(assert_equal(local.rightJacobian().inverse(), + local.rightJacobianInverse())); + EXPECT(assert_equal(local.leftJacobian().inverse(), + local.leftJacobianInverse())); + } + } +} + +//****************************************************************************** +TEST(SO3, CrossB) { + Matrix aH1; + for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v); + return so3::DexpFunctor(omega, nearZero).crossB(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)}) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + for (const Vector3& v : test_cases::vs) { + local.crossB(v, aH1); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + } + } + } +} + +//****************************************************************************** +TEST(SO3, DoubleCrossC) { + Matrix aH1; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).doubleCrossC(v); + }; + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + for (const Vector3& v : test_cases::vs) { + local.doubleCrossC(v, aH1); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + } + } + } +} + +//****************************************************************************** +TEST(SO3, ApplyDexp) { + Matrix aH1, aH2; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).applyDexp(v); + }; + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + for (const Vector3& v : test_cases::vs) { EXPECT(assert_equal(Vector3(local.dexp() * v), local.applyDexp(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); @@ -329,22 +388,63 @@ TEST(SO3, ApplyDexp) { //****************************************************************************** TEST(SO3, ApplyInvDexp) { Matrix aH1, aH2; - for (bool nearZeroApprox : {true, false}) { + for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v); + return so3::DexpFunctor(omega, nearZero).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))); + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + Matrix invJr = local.rightJacobianInverse(); + for (const Vector3& v : test_cases::vs) { + EXPECT( + assert_equal(Vector3(invJr * 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)); + EXPECT(assert_equal(invJr, aH2)); + } + } + } +} + +//****************************************************************************** +TEST(SO3, ApplyLeftJacobian) { + Matrix aH1, aH2; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v); + }; + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + for (const Vector3& v : test_cases::vs) { + EXPECT(assert_equal(Vector3(local.leftJacobian() * v), + local.applyLeftJacobian(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(local.leftJacobian(), aH2)); + } + } + } +} + +//****************************************************************************** +TEST(SO3, ApplyLeftJacobianInverse) { + Matrix aH1, aH2; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v); + }; + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + Matrix invJl = local.leftJacobianInverse(); + for (const Vector3& v : test_cases::vs) { + EXPECT(assert_equal(Vector3(invJl * v), + local.applyLeftJacobianInverse(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(invJl, aH2)); } } } diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 0c4c2c725..855e50a42 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -499,7 +499,7 @@ TEST(Unit3, CopyAssign) { } /* ************************************************************************* */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION TEST(actualH, Serialization) { Unit3 p(0, 1, 0); EXPECT(serializationTestHelpers::equalsObj(p)); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index d82da3459..749824845 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -20,7 +20,7 @@ #pragma once -#include "gtsam/geometry/Point3.h" +#include #include #include #include @@ -317,7 +317,11 @@ typename CAMERA::MeasurementVector undistortMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { const size_t nrMeasurements = measurements.size(); - assert(nrMeasurements == cameras.size()); +#ifndef NDEBUG + if (nrMeasurements != cameras.size()) { + throw; + } +#endif typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); for (size_t ii = 0; ii < nrMeasurements; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that @@ -618,7 +622,7 @@ struct GTSAM_EXPORT TriangulationParameters { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -683,7 +687,7 @@ class TriangulationResult : public std::optional { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 623b82eea..8668bedd6 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -55,12 +56,15 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const { joint = joint * (*conditional); } - // Prune the joint. NOTE: again, possibly quite expensive. - const DecisionTreeFactor pruned = joint.prune(maxNrLeaves); - - // Create a the result starting with the pruned joint. + // Create the result starting with the pruned joint. HybridBayesNet result; - result.emplace_shared(pruned.size(), pruned); + result.emplace_shared(joint); + // Prune the joint. NOTE: imperative and, again, possibly quite expensive. + result.back()->asDiscrete()->prune(maxNrLeaves); + + // Get pruned discrete probabilities so + // we can prune HybridGaussianConditionals. + DiscreteConditional pruned = *result.back()->asDiscrete(); /* To prune, we visitWith every leaf in the HybridGaussianConditional. * For each leaf, using the assignment we can check the discrete decision tree @@ -126,7 +130,14 @@ HybridValues HybridBayesNet::optimize() const { for (auto &&conditional : *this) { if (conditional->isDiscrete()) { - discrete_fg.push_back(conditional->asDiscrete()); + if (auto dtc = conditional->asDiscrete()) { + // The number of keys should be small so should not + // be expensive to convert to DiscreteConditional. + discrete_fg.push_back(DiscreteConditional(dtc->nrFrontals(), + dtc->toDecisionTreeFactor())); + } else { + discrete_fg.push_back(conditional->asDiscrete()); + } } } diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 96afb87d6..3e07c71ce 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -268,7 +268,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 193635a21..31d256d6f 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,22 @@ bool HybridBayesTree::equals(const This& other, double tol) const { return Base::equals(other, tol); } +/* ************************************************************************* */ +DiscreteValues HybridBayesTree::discreteMaxProduct( + const DiscreteFactorGraph& dfg) const { + DiscreteFactor::shared_ptr product = dfg.scaledProduct(); + + // Check type of product, and get as TableFactor for efficiency. + TableFactor p; + if (auto tf = std::dynamic_pointer_cast(product)) { + p = *tf; + } else { + p = TableFactor(product->toDecisionTreeFactor()); + } + DiscreteValues assignment = TableDistribution(p).argmax(); + return assignment; +} + /* ************************************************************************* */ HybridValues HybridBayesTree::optimize() const { DiscreteFactorGraph discrete_fg; @@ -52,8 +69,9 @@ HybridValues HybridBayesTree::optimize() const { // The root should be discrete only, we compute the MPE if (root_conditional->isDiscrete()) { - discrete_fg.push_back(root_conditional->asDiscrete()); - mpe = discrete_fg.optimize(); + auto discrete = root_conditional->asDiscrete(); + discrete_fg.push_back(discrete); + mpe = discreteMaxProduct(discrete_fg); } else { throw std::runtime_error( "HybridBayesTree root is not discrete-only. Please check elimination " @@ -179,16 +197,17 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { /* ************************************************************************* */ void HybridBayesTree::prune(const size_t maxNrLeaves) { - auto discreteProbs = this->roots_.at(0)->conditional()->asDiscrete(); + auto prunedDiscreteProbs = + this->roots_.at(0)->conditional()->asDiscrete(); - DecisionTreeFactor prunedDiscreteProbs = discreteProbs->prune(maxNrLeaves); - discreteProbs->root_ = prunedDiscreteProbs.root_; + // Imperative pruning + prunedDiscreteProbs->prune(maxNrLeaves); /// Helper struct for pruning the hybrid bayes tree. struct HybridPrunerData { /// The discrete decision tree after pruning. - DecisionTreeFactor prunedDiscreteProbs; - HybridPrunerData(const DecisionTreeFactor& prunedDiscreteProbs, + DiscreteConditional::shared_ptr prunedDiscreteProbs; + HybridPrunerData(const DiscreteConditional::shared_ptr& prunedDiscreteProbs, const HybridBayesTree::sharedNode& parentClique) : prunedDiscreteProbs(prunedDiscreteProbs) {} @@ -210,9 +229,11 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) { if (conditional->isHybrid()) { auto hybridGaussianCond = conditional->asHybrid(); - // Imperative - clique->conditional() = std::make_shared( - hybridGaussianCond->prune(parentData.prunedDiscreteProbs)); + if (!hybridGaussianCond->pruned()) { + // Imperative + clique->conditional() = std::make_shared( + hybridGaussianCond->prune(*parentData.prunedDiscreteProbs)); + } } return parentData; } diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 0fe9ca6ea..ec29f7b1e 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -115,7 +115,11 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /// Helper method to compute the max product assignment + /// given a DiscreteFactorGraph + DiscreteValues discreteMaxProduct(const DiscreteFactorGraph& dfg) const; + +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index f3df725ef..3cf5b80e5 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -166,12 +166,13 @@ class GTSAM_EXPORT HybridConditional } /** - * @brief Return conditional as a DiscreteConditional + * @brief Return conditional as a DiscreteConditional or specified type T. * @return nullptr if not a DiscreteConditional * @return DiscreteConditional::shared_ptr */ - DiscreteConditional::shared_ptr asDiscrete() const { - return std::dynamic_pointer_cast(inner_); + template + typename T::shared_ptr asDiscrete() const { + return std::dynamic_pointer_cast(inner_); } /// Get the type-erased pointer to the inner type @@ -217,7 +218,7 @@ class GTSAM_EXPORT HybridConditional /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 8d6fef3f4..9fc280322 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -140,7 +140,7 @@ class GTSAM_EXPORT HybridFactor : public Factor { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 1bec42810..78e1f5324 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -120,7 +120,7 @@ struct HybridGaussianConditional::Helper { /* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKeys &discreteParents, Helper &&helper) + const DiscreteKeys &discreteParents, Helper &&helper, bool pruned) : BaseFactor(discreteParents, FactorValuePairs( [&](const GaussianFactorValuePair @@ -130,7 +130,8 @@ HybridGaussianConditional::HybridGaussianConditional( }, std::move(helper.pairs))), BaseConditional(*helper.nrFrontals), - negLogConstant_(helper.minNegLogConstant) {} + negLogConstant_(helper.minNegLogConstant), + pruned_(pruned) {} HybridGaussianConditional::HybridGaussianConditional( const DiscreteKey &discreteParent, @@ -166,8 +167,9 @@ HybridGaussianConditional::HybridGaussianConditional( : HybridGaussianConditional(discreteParents, Helper(conditionals)) {} HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKeys &discreteParents, const FactorValuePairs &pairs) - : HybridGaussianConditional(discreteParents, Helper(pairs)) {} + const DiscreteKeys &discreteParents, const FactorValuePairs &pairs, + bool pruned) + : HybridGaussianConditional(discreteParents, Helper(pairs), pruned) {} /* *******************************************************************************/ const HybridGaussianConditional::Conditionals @@ -302,7 +304,7 @@ std::set DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) { /* *******************************************************************************/ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune( - const DecisionTreeFactor &discreteProbs) const { + const DiscreteConditional &discreteProbs) const { // Find keys in discreteProbs.keys() but not in this->keys(): std::set mine(this->keys().begin(), this->keys().end()); std::set theirs(discreteProbs.keys().begin(), @@ -331,7 +333,7 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune( FactorValuePairs prunedConditionals = factors().apply(pruner); return std::make_shared(discreteKeys(), - prunedConditionals); + prunedConditionals, true); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index c485fafce..3b95e0277 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -68,6 +69,9 @@ class GTSAM_EXPORT HybridGaussianConditional ///< Take advantage of the neg-log space so everything is a minimization double negLogConstant_; + /// Flag to indicate if the conditional has been pruned. + bool pruned_ = false; + public: /// @name Constructors /// @{ @@ -150,9 +154,10 @@ class GTSAM_EXPORT HybridGaussianConditional * * @param discreteParents the discrete parents. Will be placed last. * @param conditionalPairs Decision tree of GaussianFactor/scalar pairs. + * @param pruned Flag indicating if conditional has been pruned. */ HybridGaussianConditional(const DiscreteKeys &discreteParents, - const FactorValuePairs &pairs); + const FactorValuePairs &pairs, bool pruned = false); /// @} /// @name Testable @@ -231,7 +236,10 @@ class GTSAM_EXPORT HybridGaussianConditional * @return Shared pointer to possibly a pruned HybridGaussianConditional */ HybridGaussianConditional::shared_ptr prune( - const DecisionTreeFactor &discreteProbs) const; + const DiscreteConditional &discreteProbs) const; + + /// Return true if the conditional has already been pruned. + bool pruned() const { return pruned_; } /// @} @@ -241,12 +249,12 @@ class GTSAM_EXPORT HybridGaussianConditional /// Private constructor that uses helper struct above. HybridGaussianConditional(const DiscreteKeys &discreteParents, - Helper &&helper); + Helper &&helper, bool pruned = false); /// Check whether `given` has values for all frontal keys. bool allFrontalsGiven(const VectorValues &given) const; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 6d1064647..efbba9e51 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -176,7 +176,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { // Private constructor using ConstructorHelper above. HybridGaussianFactor(const ConstructorHelper &helper); -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 8832cbb34..cf56b52ed 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -20,12 +20,13 @@ #include #include -#include #include #include #include #include #include +#include +#include #include #include #include @@ -241,29 +242,29 @@ continuousElimination(const HybridGaussianFactorGraph &factors, /* ************************************************************************ */ /** * @brief Take negative log-values, shift them so that the minimum value is 0, - * and then exponentiate to create a DecisionTreeFactor (not normalized yet!). + * and then exponentiate to create a TableFactor (not normalized yet!). * * @param errors DecisionTree of (unnormalized) errors. - * @return DecisionTreeFactor::shared_ptr + * @return TableFactor::shared_ptr */ -static DecisionTreeFactor::shared_ptr DiscreteFactorFromErrors( +static TableFactor::shared_ptr DiscreteFactorFromErrors( const DiscreteKeys &discreteKeys, const AlgebraicDecisionTree &errors) { double min_log = errors.min(); AlgebraicDecisionTree potentials( errors, [&min_log](const double x) { return exp(-(x - min_log)); }); - return std::make_shared(discreteKeys, potentials); + return std::make_shared(discreteKeys, potentials); } /* ************************************************************************ */ -static std::pair> -discreteElimination(const HybridGaussianFactorGraph &factors, - const Ordering &frontalKeys) { +static DiscreteFactorGraph CollectDiscreteFactors( + const HybridGaussianFactorGraph &factors) { DiscreteFactorGraph dfg; for (auto &f : factors) { if (auto df = dynamic_pointer_cast(f)) { dfg.push_back(df); + } else if (auto gmf = dynamic_pointer_cast(f)) { // Case where we have a HybridGaussianFactor with no continuous keys. // In this case, compute a discrete factor from the remaining error. @@ -282,16 +283,73 @@ discreteElimination(const HybridGaussianFactorGraph &factors, } else if (auto hc = dynamic_pointer_cast(f)) { auto dc = hc->asDiscrete(); if (!dc) throwRuntimeError("discreteElimination", dc); - dfg.push_back(hc->asDiscrete()); +#if GTSAM_HYBRID_TIMING + gttic_(ConvertConditionalToTableFactor); +#endif + if (auto dtc = std::dynamic_pointer_cast(dc)) { + /// Get the underlying TableFactor + dfg.push_back(dtc->table()); + } else { + // Convert DiscreteConditional to TableFactor + auto tdc = std::make_shared(*dc); + dfg.push_back(tdc); + } +#if GTSAM_HYBRID_TIMING + gttoc_(ConvertConditionalToTableFactor); +#endif } else { throwRuntimeError("discreteElimination", f); } } - // NOTE: This does sum-product. For max-product, use EliminateForMPE. - auto result = EliminateDiscrete(dfg, frontalKeys); + return dfg; +} - return {std::make_shared(result.first), result.second}; +/* ************************************************************************ */ +static std::pair> +discreteElimination(const HybridGaussianFactorGraph &factors, + const Ordering &frontalKeys) { + DiscreteFactorGraph dfg = CollectDiscreteFactors(factors); + +#if GTSAM_HYBRID_TIMING + gttic_(EliminateDiscrete); +#endif + // Check if separator is empty. + // This is the same as checking if the number of frontal variables + // is the same as the number of variables in the DiscreteFactorGraph. + // If the separator is empty, we have a clique of all the discrete variables + // so we can use the TableFactor for efficiency. + if (frontalKeys.size() == dfg.keys().size()) { + // Get product factor + DiscreteFactor::shared_ptr product = dfg.scaledProduct(); + +#if GTSAM_HYBRID_TIMING + gttic_(EliminateDiscreteFormDiscreteConditional); +#endif + // Check type of product, and get as TableFactor for efficiency. + TableFactor p; + if (auto tf = std::dynamic_pointer_cast(product)) { + p = *tf; + } else { + p = TableFactor(product->toDecisionTreeFactor()); + } + auto conditional = std::make_shared(p); +#if GTSAM_HYBRID_TIMING + gttoc_(EliminateDiscreteFormDiscreteConditional); +#endif + + DiscreteFactor::shared_ptr sum = product->sum(frontalKeys); + + return {std::make_shared(conditional), sum}; + + } else { + // Perform sum-product. + auto result = EliminateDiscrete(dfg, frontalKeys); + return {std::make_shared(result.first), result.second}; + } +#if GTSAM_HYBRID_TIMING + gttoc_(EliminateDiscrete); +#endif } /* ************************************************************************ */ @@ -319,8 +377,19 @@ static std::shared_ptr createDiscreteFactor( } }; +#if GTSAM_HYBRID_TIMING + gttic_(DiscreteBoundaryErrors); +#endif AlgebraicDecisionTree errors(eliminationResults, calculateError); - return DiscreteFactorFromErrors(discreteSeparator, errors); +#if GTSAM_HYBRID_TIMING + gttoc_(DiscreteBoundaryErrors); + gttic_(DiscreteBoundaryResult); +#endif + auto result = DiscreteFactorFromErrors(discreteSeparator, errors); +#if GTSAM_HYBRID_TIMING + gttoc_(DiscreteBoundaryResult); +#endif + return result; } /* *******************************************************************************/ @@ -360,12 +429,18 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const { // the discrete separator will be *all* the discrete keys. DiscreteKeys discreteSeparator = GetDiscreteKeys(*this); +#if GTSAM_HYBRID_TIMING + gttic_(HybridCollectProductFactor); +#endif // Collect all the factors to create a set of Gaussian factor graphs in a // decision tree indexed by all discrete keys involved. Just like any hybrid // factor, every assignment also has a scalar error, in this case the sum of // all errors in the graph. This error is assignment-specific and accounts for // any difference in noise models used. HybridGaussianProductFactor productFactor = collectProductFactor(); +#if GTSAM_HYBRID_TIMING + gttoc_(HybridCollectProductFactor); +#endif // Check if a factor is null auto isNull = [](const GaussianFactor::shared_ptr &ptr) { return !ptr; }; @@ -393,8 +468,14 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const { return {conditional, conditional->negLogConstant(), factor, scalar}; }; +#if GTSAM_HYBRID_TIMING + gttic_(HybridEliminate); +#endif // Perform elimination! const ResultTree eliminationResults(productFactor, eliminate); +#if GTSAM_HYBRID_TIMING + gttoc_(HybridEliminate); +#endif // If there are no more continuous parents we create a DiscreteFactor with the // error for each discrete choice. Otherwise, create a HybridGaussianFactor diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index 28116df45..f99d95c01 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -104,7 +104,13 @@ void HybridGaussianISAM::updateInternal( elimination_ordering, function, std::cref(index)); if (maxNrLeaves) { +#if GTSAM_HYBRID_TIMING + gttic_(HybridBayesTreePrune); +#endif bayesTree->prune(*maxNrLeaves); +#if GTSAM_HYBRID_TIMING + gttoc_(HybridBayesTreePrune); +#endif } // Re-add into Bayes tree data structures diff --git a/gtsam/hybrid/HybridGaussianProductFactor.cpp b/gtsam/hybrid/HybridGaussianProductFactor.cpp index 280059f54..9a34ea1e9 100644 --- a/gtsam/hybrid/HybridGaussianProductFactor.cpp +++ b/gtsam/hybrid/HybridGaussianProductFactor.cpp @@ -33,7 +33,7 @@ static Y add(const Y& y1, const Y& y2) { GaussianFactorGraph result = y1.first; result.push_back(y2.first); return {result, y1.second + y2.second}; -}; +} /* *******************************************************************************/ HybridGaussianProductFactor operator+(const HybridGaussianProductFactor& a, diff --git a/gtsam/hybrid/HybridGaussianProductFactor.h b/gtsam/hybrid/HybridGaussianProductFactor.h index 60a58a3a5..f5e75aa86 100644 --- a/gtsam/hybrid/HybridGaussianProductFactor.h +++ b/gtsam/hybrid/HybridGaussianProductFactor.h @@ -119,7 +119,7 @@ class GTSAM_EXPORT HybridGaussianProductFactor ///@} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 22d3c7dd2..b9bdcbed2 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -22,6 +22,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index 29e467d86..3b4856dfb 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -15,6 +15,7 @@ * @author Varun Agrawal */ +#include #include #include #include @@ -65,7 +66,14 @@ void HybridNonlinearISAM::reorderRelinearize() { // Obtain the new linearization point const Values newLinPoint = estimate(); - auto discreteProbs = *(isam_.roots().at(0)->conditional()->asDiscrete()); + DiscreteConditional::shared_ptr discreteProbabilities; + + auto discreteRoot = isam_.roots().at(0)->conditional(); + if (discreteRoot->asDiscrete()) { + discreteProbabilities = discreteRoot->asDiscrete(); + } else { + discreteProbabilities = discreteRoot->asDiscrete(); + } isam_.clear(); @@ -73,7 +81,7 @@ void HybridNonlinearISAM::reorderRelinearize() { HybridNonlinearFactorGraph pruned_factors; for (auto&& factor : factors_) { if (auto nf = std::dynamic_pointer_cast(factor)) { - pruned_factors.push_back(nf->prune(discreteProbs)); + pruned_factors.push_back(nf->prune(*discreteProbabilities)); } else { pruned_factors.push_back(factor); } diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 14bef5fbb..c98485fea 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -79,8 +80,8 @@ TEST(GaussianMixture, GaussianMixtureModel) { double midway = mu1 - mu0; auto eliminationResult = gmm.toFactorGraph({{Z(0), Vector1(midway)}}).eliminateSequential(); - auto pMid = *eliminationResult->at(0)->asDiscrete(); - EXPECT(assert_equal(DiscreteConditional(m, "60/40"), pMid)); + auto pMid = eliminationResult->at(0)->asDiscrete(); + EXPECT(assert_equal(TableDistribution(m, "60 40"), *pMid)); // Everywhere else, the result should be a sigmoid. for (const double shift : {-4, -2, 0, 2, 4}) { @@ -90,7 +91,8 @@ TEST(GaussianMixture, GaussianMixtureModel) { // Workflow 1: convert HBN to HFG and solve auto eliminationResult1 = gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential(); - auto posterior1 = *eliminationResult1->at(0)->asDiscrete(); + auto posterior1 = + *eliminationResult1->at(0)->asDiscrete(); EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); // Workflow 2: directly specify HFG and solve @@ -99,7 +101,8 @@ TEST(GaussianMixture, GaussianMixtureModel) { m, std::vector{Gaussian(mu0, sigma, z), Gaussian(mu1, sigma, z)}); hfg1.push_back(mixing); auto eliminationResult2 = hfg1.eliminateSequential(); - auto posterior2 = *eliminationResult2->at(0)->asDiscrete(); + auto posterior2 = + *eliminationResult2->at(0)->asDiscrete(); EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8); } } @@ -133,13 +136,13 @@ TEST(GaussianMixture, GaussianMixtureModel2) { // Eliminate the graph! auto eliminationResultMax = gfg.eliminateSequential(); - // Equality of posteriors asserts that the elimination is correct (same ratios - // for all modes) + // Equality of posteriors asserts that the elimination is correct + // (same ratios for all modes) EXPECT(assert_equal(expectedDiscretePosterior, eliminationResultMax->discretePosterior(vv))); - auto pMax = *eliminationResultMax->at(0)->asDiscrete(); - EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4)); + auto pMax = *eliminationResultMax->at(0)->asDiscrete(); + EXPECT(assert_equal(TableDistribution(m, "42 58"), pMax, 1e-4)); // Everywhere else, the result should be a bell curve like function. for (const double shift : {-4, -2, 0, 2, 4}) { @@ -149,7 +152,8 @@ TEST(GaussianMixture, GaussianMixtureModel2) { // Workflow 1: convert HBN to HFG and solve auto eliminationResult1 = gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential(); - auto posterior1 = *eliminationResult1->at(0)->asDiscrete(); + auto posterior1 = + *eliminationResult1->at(0)->asDiscrete(); EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); // Workflow 2: directly specify HFG and solve @@ -158,10 +162,12 @@ TEST(GaussianMixture, GaussianMixtureModel2) { m, std::vector{Gaussian(mu0, sigma0, z), Gaussian(mu1, sigma1, z)}); hfg.push_back(mixing); auto eliminationResult2 = hfg.eliminateSequential(); - auto posterior2 = *eliminationResult2->at(0)->asDiscrete(); + auto posterior2 = + *eliminationResult2->at(0)->asDiscrete(); EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8); } } + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 135da5dc7..989694b26 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -454,7 +455,8 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) { } size_t maxNrLeaves = 3; - auto prunedDecisionTree = joint.prune(maxNrLeaves); + DiscreteConditional prunedDecisionTree(joint); + prunedDecisionTree.prune(maxNrLeaves); #ifdef GTSAM_DT_MERGING EXPECT_LONGS_EQUAL(maxNrLeaves + 2 /*2 zero leaves*/, diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 5b27e2b41..ef2ae9c41 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -16,6 +16,7 @@ */ #include +#include #include #include #include @@ -464,14 +465,14 @@ TEST(HybridEstimation, EliminateSequentialRegression) { // Create expected discrete conditional on m0. DiscreteKey m(M(0), 2); - DiscreteConditional expected(m % "0.51341712/1"); // regression + TableDistribution expected(m, "0.51341712 1"); // regression // Eliminate into BN using one ordering const Ordering ordering1{X(0), X(1), M(0)}; HybridBayesNet::shared_ptr bn1 = fg->eliminateSequential(ordering1); // Check that the discrete conditional matches the expected. - auto dc1 = bn1->back()->asDiscrete(); + auto dc1 = bn1->back()->asDiscrete(); EXPECT(assert_equal(expected, *dc1, 1e-9)); // Eliminate into BN using a different ordering @@ -479,7 +480,7 @@ TEST(HybridEstimation, EliminateSequentialRegression) { HybridBayesNet::shared_ptr bn2 = fg->eliminateSequential(ordering2); // Check that the discrete conditional matches the expected. - auto dc2 = bn2->back()->asDiscrete(); + auto dc2 = bn2->back()->asDiscrete(); EXPECT(assert_equal(expected, *dc2, 1e-9)); } diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp index 350bc9184..8bb83cac4 100644 --- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp @@ -261,7 +261,8 @@ TEST(HybridGaussianConditional, Prune) { potentials[i] = 1; const DecisionTreeFactor decisionTreeFactor(keys, potentials); // Prune the HybridGaussianConditional - const auto pruned = hgc.prune(decisionTreeFactor); + const auto pruned = + hgc.prune(DiscreteConditional(keys.size(), decisionTreeFactor)); // Check that the pruned HybridGaussianConditional has 1 conditional EXPECT_LONGS_EQUAL(1, pruned->nrComponents()); } @@ -271,7 +272,8 @@ TEST(HybridGaussianConditional, Prune) { 0, 0, 0.5, 0}; const DecisionTreeFactor decisionTreeFactor(keys, potentials); - const auto pruned = hgc.prune(decisionTreeFactor); + const auto pruned = + hgc.prune(DiscreteConditional(keys.size(), decisionTreeFactor)); // Check that the pruned HybridGaussianConditional has 2 conditionals EXPECT_LONGS_EQUAL(2, pruned->nrComponents()); @@ -286,7 +288,8 @@ TEST(HybridGaussianConditional, Prune) { 0, 0, 0.5, 0}; const DecisionTreeFactor decisionTreeFactor(keys, potentials); - const auto pruned = hgc.prune(decisionTreeFactor); + const auto pruned = + hgc.prune(DiscreteConditional(keys.size(), decisionTreeFactor)); // Check that the pruned HybridGaussianConditional has 3 conditionals EXPECT_LONGS_EQUAL(3, pruned->nrComponents()); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 31e36101b..c8735c40a 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -114,10 +115,10 @@ TEST(HybridGaussianFactorGraph, hybridEliminationOneFactor) { EXPECT(HybridConditional::CheckInvariants(*result.first, values)); // Check that factor is discrete and correct - auto factor = std::dynamic_pointer_cast(result.second); + auto factor = std::dynamic_pointer_cast(result.second); CHECK(factor); // regression test - EXPECT(assert_equal(DecisionTreeFactor{m1, "1 1"}, *factor, 1e-5)); + EXPECT(assert_equal(TableFactor{m1, "1 1"}, *factor, 1e-5)); } /* ************************************************************************* */ @@ -329,7 +330,7 @@ TEST(HybridBayesNet, Switching) { // Check the remaining factor for x1 CHECK(factor_x1); - auto phi_x1 = std::dynamic_pointer_cast(factor_x1); + auto phi_x1 = std::dynamic_pointer_cast(factor_x1); CHECK(phi_x1); EXPECT_LONGS_EQUAL(1, phi_x1->keys().size()); // m0 // We can't really check the error of the decision tree factor phi_x1, because @@ -650,7 +651,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) { mode, std::vector{conditional0, conditional1}); // Add prior on mode. - expectedBayesNet.emplace_shared(mode, "74/26"); + expectedBayesNet.emplace_shared(mode, "74 26"); // Test elimination const auto posterior = fg.eliminateSequential(); @@ -700,11 +701,11 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) { m1, std::vector{conditional0, conditional1}); // Add prior on m1. - expectedBayesNet.emplace_shared(m1, "1/1"); + expectedBayesNet.emplace_shared(m1, "0.188638 0.811362"); // Test elimination const auto posterior = fg.eliminateSequential(); - // EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01)); + EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01)); EXPECT(ratioTest(bn, measurements, *posterior)); @@ -736,7 +737,9 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) { mode, std::vector{conditional0, conditional1}); // Add prior on mode. - expectedBayesNet.emplace_shared(mode, "23/77"); + // Since this is the only discrete conditional, it is added as a + // TableDistribution. + expectedBayesNet.emplace_shared(mode, "23 77"); // Test elimination const auto posterior = fg.eliminateSequential(); diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 04b44f904..54964f6f7 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -141,7 +142,7 @@ TEST(HybridGaussianISAM, IncrementalInference) { expectedRemainingGraph->eliminateMultifrontal(discreteOrdering); // Test the probability values with regression tests. - auto discrete = isam[M(1)]->conditional()->asDiscrete(); + auto discrete = isam[M(1)]->conditional()->asDiscrete(); EXPECT(assert_equal(0.095292, (*discrete)({{M(0), 0}, {M(1), 0}}), 1e-5)); EXPECT(assert_equal(0.282758, (*discrete)({{M(0), 1}, {M(1), 0}}), 1e-5)); EXPECT(assert_equal(0.314175, (*discrete)({{M(0), 0}, {M(1), 1}}), 1e-5)); @@ -221,16 +222,12 @@ TEST(HybridGaussianISAM, ApproxInference) { 1 1 1 Leaf 0.5 */ - auto discreteConditional_m0 = *dynamic_pointer_cast( + auto discreteConditional_m0 = *dynamic_pointer_cast( incrementalHybrid[M(0)]->conditional()->inner()); EXPECT(discreteConditional_m0.keys() == KeyVector({M(0), M(1), M(2)})); - // Get the number of elements which are greater than 0. - auto count = [](const double &value, int count) { - return value > 0 ? count + 1 : count; - }; // Check that the number of leaves after pruning is 5. - EXPECT_LONGS_EQUAL(5, discreteConditional_m0.fold(count, 0)); + EXPECT_LONGS_EQUAL(5, discreteConditional_m0.nrValues()); // Check that the hybrid nodes of the bayes net match those of the pre-pruning // bayes net, at the same positions. @@ -477,7 +474,9 @@ TEST(HybridGaussianISAM, NonTrivial) { // Test if the optimal discrete mode assignment is (1, 1, 1). DiscreteFactorGraph discreteGraph; - discreteGraph.push_back(discreteTree); + // discreteTree is a TableDistribution, so we convert to + // DecisionTreeFactor for the DiscreteFactorGraph + discreteGraph.push_back(discreteTree->toDecisionTreeFactor()); DiscreteValues optimal_assignment = discreteGraph.optimize(); DiscreteValues expected_assignment; diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp index 747a1b688..a4de6a17b 100644 --- a/gtsam/hybrid/tests/testHybridMotionModel.cpp +++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -143,8 +144,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel) { // Since no measurement on x1, we hedge our bets // Importance sampling run with 100k samples gives 50.051/49.949 // approximateDiscreteMarginal(hbn, hybridMotionModel, given); - DiscreteConditional expected(m1, "50/50"); - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()))); + TableDistribution expected(m1, "50 50"); + EXPECT( + assert_equal(expected, *(bn->at(2)->asDiscrete()))); } { @@ -160,8 +162,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel) { // Since we have a measurement on x1, we get a definite result // Values taken from an importance sampling run with 100k samples: // approximateDiscreteMarginal(hbn, hybridMotionModel, given); - DiscreteConditional expected(m1, "44.3854/55.6146"); - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); + TableDistribution expected(m1, "44.3854 55.6146"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), + 0.02)); } } @@ -248,8 +251,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) { // Values taken from an importance sampling run with 100k samples: // approximateDiscreteMarginal(hbn, hybridMotionModel, given); - DiscreteConditional expected(m1, "48.3158/51.6842"); - EXPECT(assert_equal(expected, *(eliminated->at(2)->asDiscrete()), 0.002)); + TableDistribution expected(m1, "48.3158 51.6842"); + EXPECT(assert_equal( + expected, *(eliminated->at(2)->asDiscrete()), 0.02)); } { @@ -263,8 +267,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) { // Values taken from an importance sampling run with 100k samples: // approximateDiscreteMarginal(hbn, hybridMotionModel, given); - DiscreteConditional expected(m1, "55.396/44.604"); - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); + TableDistribution expected(m1, "55.396 44.604"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), + 0.02)); } } @@ -340,8 +345,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel3) { // Values taken from an importance sampling run with 100k samples: // approximateDiscreteMarginal(hbn, hybridMotionModel, given); - DiscreteConditional expected(m1, "51.7762/48.2238"); - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); + TableDistribution expected(m1, "51.7762 48.2238"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), + 0.02)); } { @@ -355,8 +361,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel3) { // Values taken from an importance sampling run with 100k samples: // approximateDiscreteMarginal(hbn, hybridMotionModel, given); - DiscreteConditional expected(m1, "49.0762/50.9238"); - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.005)); + TableDistribution expected(m1, "49.0762 50.9238"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), + 0.05)); } } @@ -381,8 +388,9 @@ TEST(HybridGaussianFactorGraph, TwoStateModel4) { // Values taken from an importance sampling run with 100k samples: // approximateDiscreteMarginal(hbn, hybridMotionModel, given); - DiscreteConditional expected(m1, "8.91527/91.0847"); - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); + TableDistribution expected(m1, "8.91527 91.0847"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), + 0.01)); } /* ************************************************************************* */ @@ -537,8 +545,8 @@ TEST(HybridGaussianFactorGraph, DifferentCovariances) { DiscreteValues dv0{{M(1), 0}}; DiscreteValues dv1{{M(1), 1}}; - DiscreteConditional expected_m1(m1, "0.5/0.5"); - DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); + TableDistribution expected_m1(m1, "0.5 0.5"); + TableDistribution actual_m1 = *(hbn->at(2)->asDiscrete()); EXPECT(assert_equal(expected_m1, actual_m1)); } diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 07f70e95c..3df03021b 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -368,10 +369,9 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { EXPECT_LONGS_EQUAL(1, hybridGaussianConditional->nrParents()); // This is now a discreteFactor - auto discreteFactor = dynamic_pointer_cast(factorOnModes); + auto discreteFactor = dynamic_pointer_cast(factorOnModes); CHECK(discreteFactor); EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size()); - EXPECT(discreteFactor->root_->isLeaf() == false); } /**************************************************************************** @@ -513,9 +513,10 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) { // P(m1) EXPECT(hybridBayesNet->at(4)->frontals() == KeyVector{M(1)}); EXPECT_LONGS_EQUAL(0, hybridBayesNet->at(4)->nrParents()); - EXPECT( - dynamic_pointer_cast(hybridBayesNet->at(4)->inner()) - ->equals(*discreteBayesNet.at(1))); + TableDistribution dtc = + *hybridBayesNet->at(4)->asDiscrete(); + EXPECT(DiscreteConditional(dtc.nrFrontals(), dtc.toDecisionTreeFactor()) + .equals(*discreteBayesNet.at(1))); } /**************************************************************************** @@ -1062,8 +1063,8 @@ TEST(HybridNonlinearFactorGraph, DifferentCovariances) { DiscreteValues dv0{{M(1), 0}}; DiscreteValues dv1{{M(1), 1}}; - DiscreteConditional expected_m1(m1, "0.5/0.5"); - DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); + TableDistribution expected_m1(m1, "0.5 0.5"); + TableDistribution actual_m1 = *(hbn->at(2)->asDiscrete()); EXPECT(assert_equal(expected_m1, actual_m1)); } diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 67cec8319..b32860cff 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -265,16 +266,12 @@ TEST(HybridNonlinearISAM, ApproxInference) { 1 1 1 Leaf 0.5 */ - auto discreteConditional_m0 = *dynamic_pointer_cast( + auto discreteConditional_m0 = *dynamic_pointer_cast( bayesTree[M(0)]->conditional()->inner()); EXPECT(discreteConditional_m0.keys() == KeyVector({M(0), M(1), M(2)})); - // Get the number of elements which are greater than 0. - auto count = [](const double &value, int count) { - return value > 0 ? count + 1 : count; - }; // Check that the number of leaves after pruning is 5. - EXPECT_LONGS_EQUAL(5, discreteConditional_m0.fold(count, 0)); + EXPECT_LONGS_EQUAL(5, discreteConditional_m0.nrValues()); // Check that the hybrid nodes of the bayes net match those of the pre-pruning // bayes net, at the same positions. @@ -520,12 +517,13 @@ TEST(HybridNonlinearISAM, NonTrivial) { // The final discrete graph should not be empty since we have eliminated // all continuous variables. - auto discreteTree = bayesTree[M(3)]->conditional()->asDiscrete(); + auto discreteTree = + bayesTree[M(3)]->conditional()->asDiscrete(); EXPECT_LONGS_EQUAL(3, discreteTree->size()); // Test if the optimal discrete mode assignment is (1, 1, 1). DiscreteFactorGraph discreteGraph; - discreteGraph.push_back(discreteTree); + discreteGraph.push_back(discreteTree->toDecisionTreeFactor()); DiscreteValues optimal_assignment = discreteGraph.optimize(); DiscreteValues expected_assignment; diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 3be96b751..9aabe309b 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -44,6 +45,7 @@ BOOST_CLASS_EXPORT_GUID(HybridFactor, "gtsam_HybridFactor"); BOOST_CLASS_EXPORT_GUID(JacobianFactor, "gtsam_JacobianFactor"); BOOST_CLASS_EXPORT_GUID(GaussianConditional, "gtsam_GaussianConditional"); BOOST_CLASS_EXPORT_GUID(DiscreteConditional, "gtsam_DiscreteConditional"); +BOOST_CLASS_EXPORT_GUID(TableDistribution, "gtsam_TableDistribution"); BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor"); using ADT = AlgebraicDecisionTree; diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 95d475a02..fdfa94d2b 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -27,7 +27,7 @@ #include #include - +#include namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index d0ebd916e..03f79c8cf 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -262,7 +262,7 @@ namespace gtsam { template friend class EliminatableClusterTree; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index adffa2f14..0ccb04e90 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -208,7 +208,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 318624608..2a5f0b455 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -19,6 +19,7 @@ #include #endif #include +#include namespace gtsam { diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index f9da36b7b..31e451714 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -233,7 +233,7 @@ namespace gtsam { // Cast to derived type (const) (casts down to derived conditional type, then up to factor type) const FACTOR& asFactor() const { return static_cast(static_cast(*this)); } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index e9acc12e1..16449d0f4 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -19,6 +19,7 @@ #include #include +#include #include #include diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 074151e16..19741b22e 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -21,7 +21,7 @@ #pragma once -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -193,7 +193,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// @name Serialization /// @{ /** Serialization function */ diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index b6046d0bb..ca7321359 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -29,7 +29,7 @@ #include // for Eigen::aligned_allocator -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #endif @@ -420,7 +420,7 @@ class FactorGraph { inline bool exists(size_t idx) const { return idx < size() && at(idx); } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 0f79c2a9a..6a8d6918a 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -25,6 +25,8 @@ #include #include +#include + namespace gtsam { template diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 927fb7669..16ac7bccc 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -142,7 +142,7 @@ class GTSAM_EXPORT LabeledSymbol { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index abdf11c5f..e051b9ae9 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -92,7 +92,11 @@ public: return nKeys_; } Key intToKey(int32_t value) const { - assert(value >= 0); +#ifndef NDEBUG + if (value < 0) { + throw; + } +#endif return intKeyBMap_.right.find(value)->second; } diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index cb2ca752d..95eb17a1c 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index c3df1b8a0..fced5d4f2 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -252,7 +252,7 @@ private: static Ordering ColamdConstrained( const VariableIndex& variableIndex, std::vector& cmember); -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 9f2133707..a62c22fac 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -21,7 +21,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -124,7 +124,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 207ded0ce..2f872b25d 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -87,9 +87,11 @@ class GTSAM_EXPORT VariableIndex { const FactorIndices& operator[](Key variable) const { KeyMap::const_iterator item = index_.find(variable); if(item == index_.end()) - throw std::invalid_argument("Requested non-existent variable from VariableIndex"); + throw std::invalid_argument("Requested non-existent variable '" + + DefaultKeyFormatter(variable) + + "' from VariableIndex"); else - return item->second; + return item->second; } /// Return true if no factors associated with a variable @@ -190,7 +192,7 @@ protected: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 46d49ca4f..aaec039e9 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -69,6 +69,8 @@ struct GTSAM_EXPORT ConjugateGradientParameters epsilon_abs(p.epsilon_abs), blas_kernel(GTSAM) {} + ConjugateGradientParameters& operator=(const ConjugateGradientParameters& other) = default; + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 inline size_t getMinIterations() const { return minIterations; } inline size_t getMaxIterations() const { return maxIterations; } diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index e858bbe43..538d1532e 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -267,7 +267,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 27270fece..14b1ce87f 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -278,7 +278,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index c5133c6b2..253bbff0f 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -179,7 +179,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 6a7848f51..67313c086 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -400,7 +400,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 3c3050f90..1172dc281 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -25,13 +25,14 @@ #include #include #include +#include #include #include #include #include +#include #include -#include "gtsam/base/Vector.h" using namespace std; @@ -378,7 +379,7 @@ GaussianFactor::shared_ptr HessianFactor::negate() const { shared_ptr result = std::make_shared(*this); // Negate the information matrix of the result result->info_.negate(); - return std::move(result); + return result; } /* ************************************************************************* */ diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 8cbabcd5d..189020712 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -23,7 +23,6 @@ #include #include - namespace gtsam { // Forward declarations @@ -241,14 +240,18 @@ namespace gtsam { * use, for example, begin() + 2 to get the 3rd variable in this factor. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif return info_.aboveDiagonalBlock(j - begin(), size()); } /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::constBlock linearTerm() const { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif // get the last column (except the bottom right block) return info_.aboveDiagonalRange(0, size(), size(), size() + 1); } @@ -256,7 +259,9 @@ namespace gtsam { /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::Block linearTerm() { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif return info_.aboveDiagonalRange(0, size(), size(), size() + 1); } @@ -325,7 +330,9 @@ namespace gtsam { * @param other the HessianFactor to be updated */ void updateHessian(HessianFactor* other) const { - assert(other); +#ifndef NDEBUG + if(!other) throw; +#endif updateHessian(other->keys_, &other->info_); } @@ -363,7 +370,7 @@ namespace gtsam { friend class NonlinearFactorGraph; friend class NonlinearClusterTree; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 3cfb5ce7b..51d513e33 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -32,6 +32,7 @@ #include #include +#include #include #include diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 407ed1e27..a9933374f 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -24,7 +24,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #endif @@ -117,6 +117,8 @@ namespace gtsam { /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ explicit JacobianFactor(const HessianFactor& hf); + JacobianFactor& operator=(const JacobianFactor& jf) = default; + /** default constructor for I/O */ JacobianFactor(); @@ -421,7 +423,7 @@ namespace gtsam { // be very selective on who can access these private methods: template friend class ExpressionFactor; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -468,7 +470,7 @@ struct traits : public Testable { } // \ namespace gtsam -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1) #endif diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index ded6bc5e3..ebc4d583e 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -20,26 +20,44 @@ * @author Frank Dellaert */ + #include + +#include #include #include -#include -#include +#ifndef NDEBUG +#include +#endif -using namespace std; +// In the code below we often get a covariance matrix Q, and we need to create a +// JacobianFactor with cost 0.5 * |Ax - b|^T Q^{-1} |Ax - b|. Factorizing Q as +// Q = L L^T +// and hence +// Q^{-1} = L^{-T} L^{-1} = M^T M, with M = L^{-1} +// We then have +// 0.5 * |Ax - b|^T Q^{-1} |Ax - b| +// = 0.5 * |Ax - b|^T M^T M |Ax - b| +// = 0.5 * |MAx - Mb|^T |MAx - Mb| +// The functor below efficiently multiplies with M by calling L.solve(). +namespace { +using Matrix = gtsam::Matrix; +struct InverseL : public Eigen::LLT { + InverseL(const Matrix& Q) : Eigen::LLT(Q) {} + Matrix operator*(const Matrix& A) const { return matrixL().solve(A); } +}; +} // namespace namespace gtsam { - /* ************************************************************************* */ -// Auxiliary function to solve factor graph and return pointer to root conditional -KalmanFilter::State // -KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const { - +// Auxiliary function to solve factor graph and return pointer to root +// conditional +KalmanFilter::State KalmanFilter::solve( + const GaussianFactorGraph& factorGraph) const { // Eliminate the graph using the provided Eliminate function Ordering ordering(factorGraph.keys()); - const auto bayesNet = // - factorGraph.eliminateSequential(ordering, function_); + const auto bayesNet = factorGraph.eliminateSequential(ordering, function_); // As this is a filter, all we need is the posterior P(x_t). // This is the last GaussianConditional in the resulting BayesNet @@ -49,9 +67,8 @@ KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const { /* ************************************************************************* */ // Auxiliary function to create a small graph for predict or update and solve -KalmanFilter::State // -KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const { - +KalmanFilter::State KalmanFilter::fuse( + const State& p, GaussianFactor::shared_ptr newFactor) const { // Create a factor graph GaussianFactorGraph factorGraph; factorGraph.push_back(p); @@ -63,45 +80,54 @@ KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const { /* ************************************************************************* */ KalmanFilter::State KalmanFilter::init(const Vector& x0, - const SharedDiagonal& P0) const { - + const SharedDiagonal& P0) const { // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - factorGraph.emplace_shared(0, I_, x0, P0); // |x-x0|^2_diagSigma + factorGraph.emplace_shared(0, I_, x0, + P0); // |x-x0|^2_P0 return solve(factorGraph); } /* ************************************************************************* */ -KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const { - +KalmanFilter::State KalmanFilter::init(const Vector& x0, + const Matrix& P0) const { // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - factorGraph.emplace_shared(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0) + + // Perform Cholesky decomposition of P0 = LL^T + InverseL M(P0); + + // Premultiply I and x0 with M=L^{-1} + const Matrix A = M * I_; // = M + const Vector b = M * x0; // = M*x0 + factorGraph.emplace_shared(0, A, b); return solve(factorGraph); } /* ************************************************************************* */ -void KalmanFilter::print(const string& s) const { - cout << "KalmanFilter " << s << ", dim = " << n_ << endl; +void KalmanFilter::print(const std::string& s) const { + std::cout << "KalmanFilter " << s << ", dim = " << n_ << std::endl; } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F, - const Matrix& B, const Vector& u, const SharedDiagonal& model) const { - + const Matrix& B, const Vector& u, + const SharedDiagonal& model) const { // The factor related to the motion model is defined as - // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T + // f2(x_{k},x_{k+1}) = + // (F*x_{k} + B*u - x_{k+1}) * Q^-1 * (F*x_{k} + B*u - x_{k+1})^T Key k = step(p); return fuse(p, - std::make_shared(k, -F, k + 1, I_, B * u, model)); + std::make_shared(k, -F, k + 1, I_, B * u, model)); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, - const Matrix& B, const Vector& u, const Matrix& Q) const { - + const Matrix& B, const Vector& u, + const Matrix& Q) const { #ifndef NDEBUG - DenseIndex n = F.cols(); + const DenseIndex n = dim(); + assert(F.cols() == n); assert(F.rows() == n); assert(B.rows() == n); assert(B.cols() == u.size()); @@ -109,50 +135,52 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, assert(Q.cols() == n); #endif - // The factor related to the motion model is defined as - // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T - // See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u: - // TODO: starts to seem more elaborate than straight-up KF equations? - Matrix M = Q.inverse(), Ft = trans(F); - Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M; - Vector b = B * u, g2 = M * b, g1 = -Ft * g2; - double f = dot(b, g2); - Key k = step(p); - return fuse(p, - std::make_shared(k, k + 1, G11, G12, g1, G22, g2, f)); + // Perform Cholesky decomposition of Q = LL^T + InverseL M(Q); + + // Premultiply -F, I, and B * u with M=L^{-1} + const Matrix A1 = -(M * F); + const Matrix A2 = M * I_; + const Vector b = M * (B * u); + return predict2(p, A1, A2, b); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0, - const Matrix& A1, const Vector& b, const SharedDiagonal& model) const { + const Matrix& A1, const Vector& b, + const SharedDiagonal& model) const { // Nhe factor related to the motion model is defined as - // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2 + // f2(x_{k},x_{k+1}) = |A0*x_{k} + A1*x_{k+1} - b|^2 Key k = step(p); return fuse(p, std::make_shared(k, A0, k + 1, A1, b, model)); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, - const Vector& z, const SharedDiagonal& model) const { + const Vector& z, + const SharedDiagonal& model) const { // The factor related to the measurements would be defined as - // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T - // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T + // f2 = (h(x_{k}) - z_{k}) * R^-1 * (h(x_{k}) - z_{k})^T + // = (x_{k} - z_{k}) * R^-1 * (x_{k} - z_{k})^T Key k = step(p); return fuse(p, std::make_shared(k, H, z, model)); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, - const Vector& z, const Matrix& Q) const { + const Vector& z, + const Matrix& Q) const { Key k = step(p); - Matrix M = Q.inverse(), Ht = trans(H); - Matrix G = Ht * M * H; - Vector g = Ht * M * z; - double f = dot(z, M * z); - return fuse(p, std::make_shared(k, G, g, f)); + + // Perform Cholesky decomposition of Q = LL^T + InverseL M(Q); + + // Pre-multiply H and z with M=L^{-1}, respectively + const Matrix A = M * H; + const Vector b = M * z; + return fuse(p, std::make_shared(k, A, b)); } /* ************************************************************************* */ -} // \namespace gtsam - +} // namespace gtsam diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 65fac877a..2bf0549fa 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -11,10 +11,10 @@ /** * @file KalmanFilter.h - * @brief Simple linear Kalman filter. Implemented using factor graphs, i.e., does Cholesky-based SRIF, really. + * @brief Simple linear Kalman filter implemented using factor graphs, i.e., + * performs Cholesky or QR-based SRIF (Square-Root Information Filter). * @date Sep 3, 2011 - * @author Stephen Williams - * @author Frank Dellaert + * @authors Stephen Williams, Frank Dellaert */ #pragma once @@ -32,120 +32,186 @@ namespace gtsam { /** * Kalman Filter class * - * Knows how to maintain a Gaussian density under linear-Gaussian motion and - * measurement models. It uses the square-root information form, as usual in GTSAM. + * Maintains a Gaussian density under linear-Gaussian motion and + * measurement models using the square-root information form. * - * The filter is functional, in that it does not have state: you call init() to create - * an initial state, then predict() and update() that create new states out of an old state. + * The filter is functional; it does not maintain internal state. Instead: + * - Use `init()` to create an initial filter state, + * - Call `predict()` and `update()` to create new states. */ class GTSAM_EXPORT KalmanFilter { - -public: - + public: /** - * This Kalman filter is a Square-root Information filter - * The type below allows you to specify the factorization variant. + * @enum Factorization + * @brief Specifies the factorization variant to use. */ - enum Factorization { - QR, CHOLESKY - }; + enum Factorization { QR, CHOLESKY }; /** - * The Kalman filter state is simply a GaussianDensity + * @typedef State + * @brief The Kalman filter state, represented as a shared pointer to a + * GaussianDensity. */ typedef GaussianDensity::shared_ptr State; -private: - - const size_t n_; /** dimensionality of state */ - const Matrix I_; /** identity matrix of size n*n */ - const GaussianFactorGraph::Eliminate function_; /** algorithm */ - - State solve(const GaussianFactorGraph& factorGraph) const; - State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const; - -public: - - // Constructor - KalmanFilter(size_t n, Factorization method = - KALMANFILTER_DEFAULT_FACTORIZATION) : - n_(n), I_(Matrix::Identity(n_, n_)), function_( - method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) : - GaussianFactorGraph::Eliminate(EliminateCholesky)) { - } + private: + const size_t n_; ///< Dimensionality of the state. + const Matrix I_; ///< Identity matrix of size \f$ n \times n \f$. + const GaussianFactorGraph::Eliminate + function_; ///< Elimination algorithm used. /** - * Create initial state, i.e., prior density at time k=0 - * In Kalman Filter notation, these are x_{0|0} and P_{0|0} - * @param x0 estimate at time 0 - * @param P0 covariance at time 0, given as a diagonal Gaussian 'model' + * Solve the factor graph. + * @param factorGraph The Gaussian factor graph to solve. + * @return The resulting Kalman filter state. + */ + State solve(const GaussianFactorGraph& factorGraph) const; + + /** + * Fuse two states. + * @param p The prior state. + * @param newFactor The new factor to incorporate. + * @return The resulting fused state. + */ + State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const; + + public: + /** + * Constructor. + * @param n Dimensionality of the state. + * @param method Factorization method (default: QR unless compile-flag set). + */ + KalmanFilter(size_t n, + Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION) + : n_(n), + I_(Matrix::Identity(n_, n_)), + function_(method == QR + ? GaussianFactorGraph::Eliminate(EliminateQR) + : GaussianFactorGraph::Eliminate(EliminateCholesky)) {} + + /** + * Create the initial state (prior density at time \f$ k=0 \f$). + * + * In Kalman Filter notation: + * - \f$ x_{0|0} \f$: Initial state estimate. + * - \f$ P_{0|0} \f$: Initial covariance matrix. + * + * @param x0 Estimate of the state at time 0 (\f$ x_{0|0} \f$). + * @param P0 Covariance matrix (\f$ P_{0|0} \f$), given as a diagonal Gaussian + * model. + * @return Initial Kalman filter state. */ State init(const Vector& x0, const SharedDiagonal& P0) const; - /// version of init with a full covariance matrix + /** + * Create the initial state with a full covariance matrix. + * @param x0 Initial state estimate. + * @param P0 Full covariance matrix. + * @return Initial Kalman filter state. + */ State init(const Vector& x0, const Matrix& P0) const; - /// print + /** + * Print the Kalman filter details. + * @param s Optional string prefix. + */ void print(const std::string& s = "") const; - /** Return step index k, starts at 0, incremented at each predict. */ - static Key step(const State& p) { - return p->firstFrontalKey(); - } + /** + * Return the step index \f$ k \f$ (starts at 0, incremented at each predict + * step). + * @param p The current state. + * @return Step index. + */ + static Key step(const State& p) { return p->firstFrontalKey(); } /** - * Predict the state P(x_{t+1}|Z^t) - * In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} - * Details and parameters: - * In a linear Kalman Filter, the motion model is f(x_{t}) = F*x_{t} + B*u_{t} + w - * where F is the state transition model/matrix, B is the control input model, - * and w is zero-mean, Gaussian white noise with covariance Q. + * Predict the next state \f$ P(x_{k+1}|Z^k) \f$. + * + * In Kalman Filter notation: + * - \f$ x_{k+1|k} \f$: Predicted state. + * - \f$ P_{k+1|k} \f$: Predicted covariance. + * + * Motion model: + * \f[ + * x_{k+1} = F \cdot x_k + B \cdot u_k + w + * \f] + * where \f$ w \f$ is zero-mean Gaussian noise with covariance \f$ Q \f$. + * + * @param p Previous state (\f$ x_k \f$). + * @param F State transition matrix (\f$ F \f$). + * @param B Control input matrix (\f$ B \f$). + * @param u Control vector (\f$ u_k \f$). + * @param modelQ Noise model (\f$ Q \f$, diagonal Gaussian). + * @return Predicted state (\f$ x_{k+1|k} \f$). */ State predict(const State& p, const Matrix& F, const Matrix& B, - const Vector& u, const SharedDiagonal& modelQ) const; + const Vector& u, const SharedDiagonal& modelQ) const; - /* - * Version of predict with full covariance - * Q is normally derived as G*w*G^T where w models uncertainty of some - * physical property, such as velocity or acceleration, and G is derived from physics. - * This version allows more realistic models than a diagonal covariance matrix. + /** + * Predict the next state with a full covariance matrix. + * + *@note Q is normally derived as G*w*G^T where w models uncertainty of some + * physical property, such as velocity or acceleration, and G is derived from + * physics. This version allows more realistic models than a diagonal matrix. + * + * @param p Previous state. + * @param F State transition matrix. + * @param B Control input matrix. + * @param u Control vector. + * @param Q Full covariance matrix (\f$ Q \f$). + * @return Predicted state. */ State predictQ(const State& p, const Matrix& F, const Matrix& B, - const Vector& u, const Matrix& Q) const; + const Vector& u, const Matrix& Q) const; /** - * Predict the state P(x_{t+1}|Z^t) - * In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} - * After the call, that is the density that can be queried. - * Details and parameters: - * This version of predict takes GaussianFactor motion model [A0 A1 b] - * with an optional noise model. + * Predict the next state using a GaussianFactor motion model. + * @param p Previous state. + * @param A0 Factor matrix. + * @param A1 Factor matrix. + * @param b Constant term vector. + * @param model Noise model (optional). + * @return Predicted state. */ State predict2(const State& p, const Matrix& A0, const Matrix& A1, - const Vector& b, const SharedDiagonal& model) const; + const Vector& b, const SharedDiagonal& model = nullptr) const; /** - * Update Kalman filter with a measurement - * For the Kalman Filter, the measurement function, h(x_{t}) = z_{t} - * will be of the form h(x_{t}) = H*x_{t} + v - * where H is the observation model/matrix, and v is zero-mean, - * Gaussian white noise with covariance R. + * Update the Kalman filter with a measurement. + * + * Observation model: + * \f[ + * z_k = H \cdot x_k + v + * \f] + * where \f$ v \f$ is zero-mean Gaussian noise with covariance R. * In this version, R is restricted to diagonal Gaussians (model parameter) + * + * @param p Previous state. + * @param H Observation matrix. + * @param z Measurement vector. + * @param model Noise model (diagonal Gaussian). + * @return Updated state. */ State update(const State& p, const Matrix& H, const Vector& z, - const SharedDiagonal& model) const; + const SharedDiagonal& model) const; - /* - * Version of update with full covariance - * Q is normally derived as G*w*G^T where w models uncertainty of some - * physical property, such as velocity or acceleration, and G is derived from physics. - * This version allows more realistic models than a diagonal covariance matrix. + /** + * Update the Kalman filter with a measurement using a full covariance matrix. + * @param p Previous state. + * @param H Observation matrix. + * @param z Measurement vector. + * @param R Full covariance matrix. + * @return Updated state. */ State updateQ(const State& p, const Matrix& H, const Vector& z, - const Matrix& Q) const; + const Matrix& R) const; + + /** + * Return the dimensionality of the state. + * @return Dimensionality of the state. + */ + size_t dim() const { return n_; } }; -} // \namespace gtsam - -/* ************************************************************************* */ - +} // namespace gtsam diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index a8902e11b..989557d87 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -24,7 +24,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #include @@ -129,7 +129,7 @@ class GTSAM_EXPORT Base { void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -161,7 +161,7 @@ class GTSAM_EXPORT Null : public Base { static shared_ptr Create(); private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -195,7 +195,7 @@ class GTSAM_EXPORT Fair : public Base { double modelParameter() const { return c_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -230,7 +230,7 @@ class GTSAM_EXPORT Huber : public Base { double modelParameter() const { return k_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -270,7 +270,7 @@ class GTSAM_EXPORT Cauchy : public Base { double modelParameter() const { return k_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -306,7 +306,7 @@ class GTSAM_EXPORT Tukey : public Base { double modelParameter() const { return c_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -341,7 +341,7 @@ class GTSAM_EXPORT Welsch : public Base { double modelParameter() const { return c_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -380,7 +380,7 @@ class GTSAM_EXPORT GemanMcClure : public Base { double c_; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -420,7 +420,7 @@ class GTSAM_EXPORT DCS : public Base { double c_; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -460,7 +460,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { double modelParameter() const { return k_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -496,7 +496,7 @@ class GTSAM_EXPORT AsymmetricTukey : public Base { double modelParameter() const { return c_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -532,7 +532,7 @@ class GTSAM_EXPORT AsymmetricCauchy : public Base { double modelParameter() const { return k_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -577,7 +577,7 @@ class GTSAM_EXPORT Custom : public Base { inline Custom() = default; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 884c87270..2f693bbc4 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -63,7 +64,7 @@ std::optional checkIfDiagonal(const Matrix& M) { Vector diagonal(n); for (j = 0; j < n; j++) diagonal(j) = M(j, j); - return std::move(diagonal); + return diagonal; } } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 34fa63e4c..47a84654a 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -24,7 +24,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #include @@ -141,7 +141,7 @@ namespace gtsam { virtual double weight(const Vector& v) const { return 1.0; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -280,7 +280,7 @@ namespace gtsam { double negLogConstant() const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -375,7 +375,7 @@ namespace gtsam { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -520,7 +520,7 @@ namespace gtsam { shared_ptr unit() const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -593,7 +593,7 @@ namespace gtsam { inline double sigma() const { return sigma_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -648,7 +648,7 @@ namespace gtsam { void unwhitenInPlace(Eigen::Block& /*v*/) const override {} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -739,7 +739,7 @@ namespace gtsam { const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise); private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 20d4c955b..68f21d723 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -17,6 +17,9 @@ */ #include + +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index 834fc8d12..9520b826b 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -25,7 +25,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #include @@ -71,7 +71,7 @@ vector Subgraph::edgeIndices() const { return eid; } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /****************************************************************************/ void Subgraph::save(const std::string &fn) const { std::ofstream os(fn.c_str()); diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h index f9ddd4c9a..df77ea9de 100644 --- a/gtsam/linear/SubgraphBuilder.h +++ b/gtsam/linear/SubgraphBuilder.h @@ -21,7 +21,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #include #endif @@ -51,7 +51,7 @@ class GTSAM_EXPORT Subgraph { friend std::ostream &operator<<(std::ostream &os, const Edge &edge); private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(Archive &ar, const unsigned int /*version*/) { @@ -87,7 +87,7 @@ class GTSAM_EXPORT Subgraph { friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph); private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(Archive &ar, const unsigned int /*version*/) { diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 96f4847b5..53ea94d6e 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -25,6 +25,7 @@ #include #include +#include using std::cout; using std::endl; diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 7fbd43ffc..35a381745 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -106,7 +106,7 @@ namespace gtsam { template explicit VectorValues(const CONTAINER& c) : values_(c.begin(), c.end()) {} - /** Implicit copy constructor to specialize the explicit constructor from any container. */ + /** Copy constructor to specialize the explicit constructor from any container. */ VectorValues(const VectorValues& c) : values_(c.values_) {} /** Create from a pair of iterators over pair. */ @@ -119,6 +119,9 @@ namespace gtsam { /// Constructor from Vector, with Scatter VectorValues(const Vector& c, const Scatter& scatter); + // We override the copy constructor; expicitly declare operator= + VectorValues& operator=(const VectorValues& other) = default; + /** Create a VectorValues with the same structure as \c other, but filled with zeros. */ static VectorValues Zero(const VectorValues& other); @@ -371,7 +374,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 05439bafc..496fcde3d 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -120,7 +120,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -139,9 +139,6 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN { PreintegratedAhrsMeasurements _PIM_; - /** Default constructor - only use for serialization */ - AHRSFactor() {} - public: // Provide access to the Matrix& version of evaluateError: @@ -154,6 +151,9 @@ public: typedef std::shared_ptr shared_ptr; #endif + /** Default constructor - only use for serialization */ + AHRSFactor() {} + /** * Constructor * @param rot_i previous rot key @@ -208,7 +208,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 2fada724d..e342e451e 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -73,7 +73,7 @@ class AttitudeFactor { const Unit3& bRef() const { return bMeasured_; } #endif -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -138,7 +138,7 @@ class GTSAM_EXPORT Rot3AttitudeFactor : public NoiseModelFactorN, } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -220,7 +220,7 @@ class GTSAM_EXPORT Pose3AttitudeFactor : public NoiseModelFactorN, } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h index 70cae8d36..545928ca9 100644 --- a/gtsam/navigation/BarometricFactor.h +++ b/gtsam/navigation/BarometricFactor.h @@ -97,7 +97,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index a2eb9e0c0..64ee86be9 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -21,7 +21,7 @@ **/ #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 0ffb386a0..b4c3ae8dc 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -169,7 +169,7 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -281,7 +281,7 @@ class GTSAM_EXPORT CombinedImuFactor OptionalMatrixType H6) const override; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index cd89dd464..6af184360 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -97,7 +97,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -166,7 +166,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 6df87eda6..02a020c42 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -20,7 +20,7 @@ #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -142,7 +142,7 @@ private: /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 88e45d318..5697d54d4 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -23,6 +23,7 @@ /* External or standard includes */ #include +#include namespace gtsam { diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 7254838fd..b19989a77 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -142,7 +142,7 @@ public: #endif private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -244,7 +244,7 @@ public: private: /** Serialization function */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { @@ -316,7 +316,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index ba9a08d78..ea0b30c75 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -132,7 +132,7 @@ class MagPoseFactor: public NoiseModelFactorN { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function. friend class boost::serialization::access; template diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index 40691c445..43228044a 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -113,7 +113,7 @@ public: /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 3e2817752..bd482132e 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -77,10 +77,13 @@ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { } //------------------------------------------------------------------------------ -Matrix7 NavState::matrix() const { +Matrix5 NavState::matrix() const { Matrix3 R = this->R(); - Matrix7 T; - T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0; + + Matrix5 T = Matrix5::Identity(); + T.block<3, 3>(0, 0) = R; + T.block<3, 1>(0, 3) = t_; + T.block<3, 1>(0, 4) = v_; return T; } @@ -103,6 +106,160 @@ bool NavState::equals(const NavState& other, double tol) const { && equal_with_abs_tol(v_, other.v_, tol); } +//------------------------------------------------------------------------------ +NavState NavState::inverse() const { + Rot3 Rt = R_.inverse(); + return NavState(Rt, Rt * (-t_), Rt * -(v_)); +} + +//------------------------------------------------------------------------------ +NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { + // Get angular velocity w and components rho (for t) and nu (for v) from xi + Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>(); + + // Compute rotation using Expmap + Matrix3 Jr; + Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr); + + // Compute translations and optionally their Jacobians Q in w + // The Jacobians with respect to rho and nu are equal to Jr + Matrix3 Qt, Qv; + Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr); + Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr); + + if (Hxi) { + *Hxi << Jr, Z_3x3, Z_3x3, // + Qt, Jr, Z_3x3, // + Qv, Z_3x3, Jr; + } + + return NavState(R, t, v); +} + +//------------------------------------------------------------------------------ +Vector9 NavState::Logmap(const NavState& state, OptionalJacobian<9, 9> Hstate) { + if (Hstate) *Hstate = LogmapDerivative(state); + + const Vector3 phi = Rot3::Logmap(state.rotation()); + const Vector3& p = state.position(); + const Vector3& v = state.velocity(); + const double t = phi.norm(); + if (t < 1e-8) { + Vector9 log; + log << phi, p, v; + return log; + + } else { + const Matrix3 W = skewSymmetric(phi / t); + + const double Tan = tan(0.5 * t); + const Vector3 Wp = W * p; + const Vector3 Wv = W * v; + const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp); + const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv); + Vector9 log; + // Order is ω, p, v + log << phi, rho, nu; + return log; + } +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::AdjointMap() const { + const Matrix3 R = R_.matrix(); + Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R; + Matrix3 B = skewSymmetric(v_.x(), v_.y(), v_.z()) * R; + // Eqn 2 in Barrau20icra + Matrix9 adj; + adj << R, Z_3x3, Z_3x3, A, R, Z_3x3, B, Z_3x3, R; + return adj; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_state, + OptionalJacobian<9, 9> H_xib) const { + const Matrix9 Ad = AdjointMap(); + + // Jacobians + if (H_state) *H_state = -Ad * adjointMap(xi_b); + if (H_xib) *H_xib = Ad; + + return Ad * xi_b; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::adjointMap(const Vector9& xi) { + Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); + Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); + Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8)); + Matrix9 adj; + adj << w_hat, Z_3x3, Z_3x3, v_hat, w_hat, Z_3x3, a_hat, Z_3x3, w_hat; + return adj; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi, + OptionalJacobian<9, 9> H_y) { + if (Hxi) { + Hxi->setZero(); + for (int i = 0; i < 9; ++i) { + Vector9 dxi; + dxi.setZero(); + dxi(i) = 1.0; + Matrix9 Gi = adjointMap(dxi); + Hxi->col(i) = Gi * y; + } + } + + const Matrix9& ad_xi = adjointMap(xi); + if (H_y) *H_y = ad_xi; + + return ad_xi * y; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { + Matrix9 J; + Expmap(xi, J); + return J; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::LogmapDerivative(const NavState& state) { + const Vector9 xi = Logmap(state); + const Vector3 w = xi.head<3>(); + Vector3 rho = xi.segment<3>(3); + Vector3 nu = xi.tail<3>(); + + Matrix3 Qt, Qv; + const Rot3 R = Rot3::Expmap(w); + Pose3::ExpmapTranslation(w, rho, Qt); + Pose3::ExpmapTranslation(w, nu, Qv); + const Matrix3 Jw = Rot3::LogmapDerivative(w); + const Matrix3 Qt2 = -Jw * Qt * Jw; + const Matrix3 Qv2 = -Jw * Qv * Jw; + + Matrix9 J; + J << Jw, Z_3x3, Z_3x3, + Qt2, Jw, Z_3x3, + Qv2, Z_3x3, Jw; + return J; +} + + +//------------------------------------------------------------------------------ +NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, + ChartJacobian Hxi) { + return Expmap(xi, Hxi); +} + +//------------------------------------------------------------------------------ +Vector9 NavState::ChartAtOrigin::Local(const NavState& state, + ChartJacobian Hstate) { + return Logmap(state, Hstate); +} + //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { @@ -142,15 +299,16 @@ Vector9 NavState::localCoordinates(const NavState& g, // Matrix3 D_xi_R; xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV; if (H1) { - *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // - D_dt_R, -I_3x3, Z_3x3, // - D_dv_R, Z_3x3, -I_3x3; + *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // + D_dt_R, -I_3x3, Z_3x3, // + D_dv_R, Z_3x3, -I_3x3; } if (H2) { - *H2 << D_xi_R, Z_3x3, Z_3x3, // - Z_3x3, dR.matrix(), Z_3x3, // - Z_3x3, Z_3x3, dR.matrix(); + *H2 << D_xi_R, Z_3x3, Z_3x3, // + Z_3x3, dR.matrix(), Z_3x3, // + Z_3x3, Z_3x3, dR.matrix(); } + return xi; } @@ -213,7 +371,8 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, //------------------------------------------------------------------------------ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, OptionalJacobian<9, 9> H) const { - auto [nRb, n_t, n_v] = (*this); + Rot3 nRb = R_; + Point3 n_t = t_, n_v = v_; const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index f778a7123..e246cbe27 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -12,7 +12,7 @@ /** * @file NavState.h * @brief Navigation state composing of attitude, position, and velocity - * @author Frank Dellaert + * @authors Frank Dellaert, Varun Agrawal, Fan Jiang * @date July 2015 **/ @@ -25,14 +25,18 @@ namespace gtsam { /// Velocity is currently typedef'd to Vector3 -typedef Vector3 Velocity3; +using Velocity3 = Vector3; /** * Navigation state: Pose (rotation, translation) + velocity - * NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold + * Following Barrau20icra, this class belongs to the Lie group SE_2(3). + * This group is also called "double direct isometries”. + * + * NOTE: While Barrau20icra follow a R,v,t order, + * we use a R,t,v order to maintain backwards compatibility. */ -class GTSAM_EXPORT NavState { -private: +class GTSAM_EXPORT NavState : public LieGroup { + private: // TODO(frank): // - should we rename t_ to p_? if not, we should rename dP do dT @@ -42,11 +46,8 @@ private: public: - enum { - dimension = 9 - }; + inline constexpr static auto dimension = 9; - typedef std::pair PositionAndVelocity; /// @name Constructors /// @{ @@ -69,11 +70,14 @@ public: } /// Named constructor with derivatives static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v, - OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2, - OptionalJacobian<9, 3> H3); + OptionalJacobian<9, 3> H1 = {}, + OptionalJacobian<9, 3> H2 = {}, + OptionalJacobian<9, 3> H3 = {}); + /// Named constructor with derivatives static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel, - OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2); + OptionalJacobian<9, 6> H1 = {}, + OptionalJacobian<9, 3> H2 = {}); /// @} /// @name Component Access @@ -111,9 +115,8 @@ public: Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const; /// Return matrix group representation, in MATLAB notation: - /// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1] - /// With this embedding in GL(3), matrix product agrees with compose - Matrix7 matrix() const; + /// nTb = [nRb n_t n_v; 0_1x3 1 0; 0_1x3 0 1] + Matrix5 matrix() const; /// @} /// @name Testable @@ -130,7 +133,29 @@ public: bool equals(const NavState& other, double tol = 1e-8) const; /// @} - /// @name Manifold + /// @name Group + /// @{ + + /// identity for group operation + static NavState Identity() { + return NavState(); + } + + /// inverse transformation with derivatives + NavState inverse() const; + + using LieGroup::inverse; // version with derivative + + /// compose syntactic sugar + NavState operator*(const NavState& T) const { + return NavState(R_ * T.R_, t_ + R_ * T.t_, v_ + R_ * T.v_); + } + + /// Syntactic sugar + const Rot3& rotation() const { return attitude(); }; + + /// @} + /// @name Lie Group /// @{ // Tangent space sugar. @@ -164,6 +189,59 @@ public: OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = {}) const; + /** + * Exponential map at identity - create a NavState from canonical coordinates + * \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ + */ + static NavState Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {}); + + /** + * Log map at identity - return the canonical coordinates \f$ + * [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ of this NavState + */ + static Vector9 Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose = {}); + + /** + * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) + * frame to the world spatial frame. + */ + Matrix9 AdjointMap() const; + + /** + * Apply this NavState's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a + * body-fixed velocity, transforming it to the spatial frame + * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ + * Note that H_xib = AdjointMap() + */ + Vector9 Adjoint(const Vector9& xi_b, + OptionalJacobian<9, 9> H_this = {}, + OptionalJacobian<9, 9> H_xib = {}) const; + + /** + * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 + * but for the NavState [ad(w,v)] = [w^, zero3; v^, w^] + */ + static Matrix9 adjointMap(const Vector9& xi); + + /** + * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives + */ + static Vector9 adjoint(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi = {}, + OptionalJacobian<9, 9> H_y = {}); + + /// Derivative of Expmap + static Matrix9 ExpmapDerivative(const Vector9& xi); + + /// Derivative of Logmap + static Matrix9 LogmapDerivative(const NavState& xi); + + // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP + struct GTSAM_EXPORT ChartAtOrigin { + static NavState Retract(const Vector9& xi, ChartJacobian Hxi = {}); + static Vector9 Local(const NavState& state, ChartJacobian Hstate = {}); + }; + /// @} /// @name Dynamics /// @{ @@ -171,8 +249,9 @@ public: /// Integrate forward in time given angular velocity and acceleration in body frame /// Uses second order integration for position, returns derivatives except dt. NavState update(const Vector3& b_acceleration, const Vector3& b_omega, - const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, - OptionalJacobian<9, 3> G2) const; + const double dt, OptionalJacobian<9, 9> F = {}, + OptionalJacobian<9, 3> G1 = {}, + OptionalJacobian<9, 3> G2 = {}) const; /// Compute tangent space contribution due to Coriolis forces Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, @@ -190,7 +269,7 @@ public: private: /// @{ /// serialization -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { @@ -203,8 +282,10 @@ private: }; // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors -template<> -struct traits : internal::Manifold { -}; +template <> +struct traits : public internal::LieGroup {}; + +template <> +struct traits : public internal::LieGroup {}; } // namespace gtsam diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 49b1aa4c1..a887ef321 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -85,7 +85,7 @@ struct GTSAM_EXPORT PreintegratedRotationParams { std::optional getBodyPSensor() const { return body_P_sensor; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -227,7 +227,7 @@ class GTSAM_EXPORT PreintegratedRotation { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 7f998987b..dd86b829c 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -20,9 +20,11 @@ * @author Varun Agrawal **/ -#include "PreintegrationBase.h" +#include #include +#include + using namespace std; namespace gtsam { diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index e4a790b9d..de3a380cf 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -173,7 +173,7 @@ class GTSAM_EXPORT PreintegrationBase { OptionalJacobian<9, 6> H5 = {}) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/PreintegrationCombinedParams.h b/gtsam/navigation/PreintegrationCombinedParams.h index 6be05c082..b384a36e7 100644 --- a/gtsam/navigation/PreintegrationCombinedParams.h +++ b/gtsam/navigation/PreintegrationCombinedParams.h @@ -84,7 +84,7 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index 8435d0bad..6ea9e73b3 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -71,7 +71,7 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { protected: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index e16ad9310..7f5c63d50 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -127,7 +127,7 @@ public: /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 831788073..adb8bf2bb 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -77,6 +77,9 @@ virtual class PreintegratedRotationParams { std::optional getOmegaCoriolis() const; std::optional getBodyPSensor() const; + + // enabling serialization functionality + void serialize() const; }; #include @@ -148,6 +151,9 @@ virtual class ImuFactor: gtsam::NonlinearFactor { gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i, const gtsam::Pose3& pose_j, gtsam::Vector vel_j, const gtsam::imuBias::ConstantBias& bias); + + // enable serialization functionality + void serialize() const; }; #include @@ -170,22 +176,26 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { gtsam::Matrix getBiasAccCovariance() const ; gtsam::Matrix getBiasOmegaCovariance() const ; gtsam::Matrix getBiasAccOmegaInit() const; - + + // enabling serialization functionality + void serialize() const; }; class PreintegratedCombinedMeasurements { -// Constructors - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, - const gtsam::imuBias::ConstantBias& bias); + // Constructors + PreintegratedCombinedMeasurements( + const gtsam::PreintegrationCombinedParams* params); + PreintegratedCombinedMeasurements( + const gtsam::PreintegrationCombinedParams* params, + const gtsam::imuBias::ConstantBias& bias); // Testable void print(string s = "Preintegrated Measurements:") const; bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, - double tol); + double tol); // Standard Interface - void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega, - double deltaT); + void integrateMeasurement(gtsam::Vector measuredAcc, + gtsam::Vector measuredOmega, double deltaT); void resetIntegration(); void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); @@ -197,7 +207,10 @@ class PreintegratedCombinedMeasurements { gtsam::imuBias::ConstantBias biasHat() const; gtsam::Vector biasHatVector() const; gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; + const gtsam::imuBias::ConstantBias& bias) const; + + // enable serialization functionality + void serialize() const; }; virtual class CombinedImuFactor: gtsam::NoiseModelFactor { @@ -211,6 +224,9 @@ virtual class CombinedImuFactor: gtsam::NoiseModelFactor { const gtsam::Pose3& pose_j, gtsam::Vector vel_j, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j); + + // enable serialization functionality + void serialize() const; }; #include @@ -237,6 +253,9 @@ class PreintegratedAhrsMeasurements { // Standard Interface void integrateMeasurement(gtsam::Vector measuredOmega, double deltaT); void resetIntegration() ; + + // enable serialization functionality + void serialize() const; }; virtual class AHRSFactor : gtsam::NonlinearFactor { @@ -253,6 +272,9 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { gtsam::Rot3 predict(const gtsam::Rot3& rot_i, gtsam::Vector bias, const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis) const; + + // enable serialization functionality + void serialize() const; }; #include @@ -266,6 +288,9 @@ virtual class Rot3AttitudeFactor : gtsam::NoiseModelFactor { bool equals(const gtsam::NonlinearFactor& expected, double tol) const; gtsam::Unit3 nRef() const; gtsam::Unit3 bMeasured() const; + + // enable serialization functionality + void serialize() const; }; virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor { @@ -280,6 +305,9 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor { bool equals(const gtsam::NonlinearFactor& expected, double tol) const; gtsam::Unit3 nRef() const; gtsam::Unit3 bMeasured() const; + + // enable serialization functionality + void serialize() const; }; #include @@ -294,6 +322,9 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ // Standard Interface gtsam::Point3 measurementIn() const; + + // enable serialization functionality + void serialize() const; }; virtual class GPSFactor2 : gtsam::NonlinearFactor { @@ -307,6 +338,9 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor { // Standard Interface gtsam::Point3 measurementIn() const; + + // enable serialization functionality + void serialize() const; }; #include @@ -324,6 +358,9 @@ virtual class BarometricFactor : gtsam::NonlinearFactor { const double& measurementIn() const; double heightOut(double n) const; double baroOut(const double& meters) const; + + // enable serialization functionality + void serialize() const; }; #include diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index b8f081518..427531415 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -12,13 +12,16 @@ /** * @file testNavState.cpp * @brief Unit tests for NavState - * @author Frank Dellaert + * @authors Frank Dellaert, Varun Agrawal, Fan Jiang * @date July 2015 */ #include -#include + +#include #include +#include +#include #include @@ -26,6 +29,9 @@ using namespace std::placeholders; using namespace std; using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(NavState) +GTSAM_CONCEPT_LIE_INST(NavState) + static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3); static const Point3 kPosition(1.0, 2.0, 3.0); static const Pose3 kPose(kAttitude, kPosition); @@ -36,6 +42,16 @@ static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04); static const Vector3 kGravity(0, 0, 9.81); static const Vector9 kZeroXi = Vector9::Zero(); +static const Point3 V(3, 0.4, -2.2); +static const Point3 P(0.2, 0.7, -2); +static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0); +static const Point3 V2(-6.5, 3.5, 6.2); +static const Point3 P2(3.5, -8.2, 4.2); +static const NavState T(R, P2, V2); +static const NavState T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2); +static const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), + Point3(1, 2, 3)); + /* ************************************************************************* */ TEST(NavState, Constructor) { std::function create = @@ -46,14 +62,14 @@ TEST(NavState, Constructor) { assert_equal(kState1, NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3))); EXPECT( - assert_equal( - numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); +assert_equal( + numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); EXPECT( - assert_equal( - numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); +assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); EXPECT( - assert_equal( - numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); +assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); } /* ************************************************************************* */ @@ -64,7 +80,7 @@ TEST(NavState, Constructor2) { Matrix aH1, aH2; EXPECT( assert_equal(kState1, - NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2))); + NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1)); EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2)); } @@ -127,8 +143,8 @@ TEST( NavState, Manifold ) { Point3 dt = Point3(xi.segment<3>(3)); Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3); NavState state2 = NavState(kState1.attitude() * drot, - kState1.position() + kState1.attitude() * dt, - kState1.velocity() + kState1.attitude() * dvel); + kState1.position() + kState1.attitude() * dt, + kState1.velocity() + kState1.attitude() * dvel); EXPECT(assert_equal(state2, kState1.retract(xi))); EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); @@ -169,6 +185,143 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); } +/* ************************************************************************* */ +TEST(NavState, Equals) { + NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), Point3(1, 2, 3)); + NavState pose2 = T3; + EXPECT(T3.equals(pose2)); + NavState origin; + EXPECT(!T3.equals(origin)); +} + +/* ************************************************************************* */ +TEST(NavState, Compose) { + NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); + NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0}); + NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0}, + {3.0, -1.0, 1.0}); + + auto ab_c = (nav_state_a * nav_state_b) * nav_state_c; + auto a_bc = nav_state_a * (nav_state_b * nav_state_c); + CHECK(assert_equal(ab_c, a_bc)); + + Matrix actual = (T2 * T2).matrix(); + + Matrix expected = T2.matrix() * T2.matrix(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix actualDcompose1, actualDcompose2; + T2.compose(T2, actualDcompose1, actualDcompose2); + + Matrix numericalH1 = + numericalDerivative21(testing::compose, T2, T2); + + EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3)); + + Matrix numericalH2 = + numericalDerivative22(testing::compose, T2, T2); + EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-4)); +} + +/* ************************************************************************* */ +// Check compose and its push-forward, another case +TEST(NavState, Compose2) { + const NavState& T1 = T; + Matrix actual = (T1 * T2).matrix(); + Matrix expected = T1.matrix() * T2.matrix(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix actualDcompose1, actualDcompose2; + T1.compose(T2, actualDcompose1, actualDcompose2); + + Matrix numericalH1 = + numericalDerivative21(testing::compose, T1, T2); + EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3)); + + Matrix numericalH2 = + numericalDerivative22(testing::compose, T1, T2); + EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, Inverse) { + NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); + NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0}); + NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0}, + {3.0, -1.0, 1.0}); + + auto a_inv = nav_state_a.inverse(); + auto a_a_inv = nav_state_a * a_inv; + CHECK(assert_equal(a_a_inv, NavState())); + + auto b_inv = nav_state_b.inverse(); + auto b_b_inv = nav_state_b * b_inv; + CHECK(assert_equal(b_b_inv, NavState())); + + Matrix actualDinverse; + Matrix actual = T.inverse(actualDinverse).matrix(); + Matrix expected = T.matrix().inverse(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix numericalH = numericalDerivative11(testing::inverse, T); + EXPECT(assert_equal(numericalH, actualDinverse, 5e-3)); + EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3)); +} + +/* ************************************************************************* */ +TEST(NavState, InverseDerivatives) { + Rot3 R = Rot3::Rodrigues(0.3, 0.4, -0.5); + Vector3 v(3.5, -8.2, 4.2); + Point3 p(3.5, -8.2, 4.2); + NavState T(R, p, v); + + Matrix numericalH = numericalDerivative11(testing::inverse, T); + Matrix actualDinverse; + T.inverse(actualDinverse); + EXPECT(assert_equal(numericalH, actualDinverse, 5e-3)); + EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3)); +} + +/* ************************************************************************* */ +TEST(NavState, Compose_Inverse) { + NavState actual = T * T.inverse(); + NavState expected; + EXPECT(assert_equal(actual, expected, 1e-8)); +} + +/* ************************************************************************* */ +TEST(NavState, Between) { + NavState s1, s2(Rot3(), Point3(1, 2, 3), Velocity3(0, 0, 0)); + + NavState actual = s1.compose(s2); + EXPECT(assert_equal(s2, actual)); + + NavState between = s2.between(s1); + NavState expected_between(Rot3(), Point3(-1, -2, -3), Velocity3(0, 0, 0)); + EXPECT(assert_equal(expected_between, between)); + + NavState expected = T2.inverse() * T3; + Matrix actualDBetween1, actualDBetween2; + actual = T2.between(T3, actualDBetween1, actualDBetween2); + EXPECT(assert_equal(expected, actual)); + + Matrix numericalH1 = + numericalDerivative21(testing::between, T2, T3); + EXPECT(assert_equal(numericalH1, actualDBetween1, 5e-3)); + + Matrix numericalH2 = + numericalDerivative22(testing::between, T2, T3); + EXPECT(assert_equal(numericalH2, actualDBetween2, 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, interpolate) { + EXPECT(assert_equal(T2, interpolate(T2, T3, 0.0))); + EXPECT(assert_equal(T3, interpolate(T2, T3, 1.0))); +} + /* ************************************************************************* */ static const double dt = 2.0; std::function coriolis = @@ -189,7 +342,7 @@ TEST(NavState, Coriolis) { TEST(NavState, Coriolis2) { Matrix9 aH; NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), - Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0)); + Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0)); // first-order state2.coriolis(dt, kOmegaCoriolis, false, aH); @@ -200,10 +353,10 @@ TEST(NavState, Coriolis2) { } TEST(NavState, Coriolis3) { - /** Consider a massless planet with an attached nav frame at - * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with + /** Consider a massless planet with an attached nav frame at + * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with * velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously - * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and + * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and * second order Coriolis corrections are as expected. */ @@ -216,9 +369,9 @@ TEST(NavState, Coriolis3) { bRn = nRb.inverse(); // Get expected first and second order corrections in the nav frame - Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, + Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2), - n_dV1e = dt * n_aCorr1, + n_dV1e = dt * n_aCorr1, n_dV2e = dt * (n_aCorr1 + n_aCorr2); // Get expected first and second order corrections in the body frame @@ -271,6 +424,262 @@ TEST(NavState, Stream) EXPECT(os.str() == expected); } +/* ************************************************************************* */ +TEST(NavState, Print) { + NavState state(Rot3::Identity(), Point3(1, 2, 3), Vector3(1, 2, 3)); + + // Generate the expected output + std::string R = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n"; + std::string p = "p: 1 2 3\n"; + std::string v = "v: 1 2 3\n"; + std::string expected = R + p + v; + + EXPECT(assert_print_equal(expected, state)); +} + +/* ************************************************************************* */ +#ifndef GTSAM_POSE3_EXPMAP +TEST(NavState, Retract_first_order) { + NavState id; + Vector v = Z_9x1; + v(0) = 0.3; + EXPECT(assert_equal(NavState(R, Point3(0, 0, 0), Vector3(0, 0, 0)), + id.retract(v), 1e-2)); + v(3) = 0.2; + v(4) = 0.7; + v(5) = -2; + v(6) = 3; + v(7) = 0.4; + v(8) = -2.2; + EXPECT(assert_equal(NavState(R, P, V), id.retract(v), 1e-2)); +} +#endif + +/* ************************************************************************* */ +TEST(NavState, RetractExpmap) { + Vector xi = Z_9x1; + xi(0) = 0.3; + NavState pose = NavState::Expmap(xi), + expected(R, Point3(0, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expected, pose, 1e-2)); + EXPECT(assert_equal(xi, NavState::Logmap(pose), 1e-2)); +} + +/* ************************************************************************* */ +TEST(NavState, Expmap_A_Full) { + NavState id; + Vector xi = Z_9x1; + xi(0) = 0.3; + EXPECT(assert_equal(expmap_default(id, xi), + NavState(R, Point3(0, 0, 0), Point3(0, 0, 0)))); + xi(3) = -0.2; + xi(4) = -0.394742; + xi(5) = 2.08998; + xi(6) = 0.2; + xi(7) = 0.394742; + xi(8) = -2.08998; + + NavState expected(R, -P, P); + EXPECT(assert_equal(expected, expmap_default(id, xi), 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, Expmap_b) { + NavState p1(Rot3(), Point3(-100, 0, 0), Point3(100, 0, 0)); + NavState p2 = p1.retract( + (Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished()); + NavState expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0), + Point3(100.0, 0.0, 0.0)); + EXPECT(assert_equal(expected, p2)); +} + +/* ************************************************************************* */ +// test case for screw motion in the plane +namespace screwNavState { +double a = 0.3, c = cos(a), s = sin(a), w = 0.3; +Vector xi = (Vector(9) << 0.0, 0.0, w, w, 0.0, 1.0, w, 0.0, 1.0).finished(); +Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1); +Point3 expectedV(0.29552, 0.0446635, 1); +Point3 expectedP(0.29552, 0.0446635, 1); +NavState expected(expectedR, expectedV, expectedP); +} // namespace screwNavState + +/* ************************************************************************* */ +// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) +TEST(NavState, Adjoint_full) { + NavState expected = T * NavState::Expmap(screwNavState::xi) * T.inverse(); + Vector xiPrime = T.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected, NavState::Expmap(xiPrime), 1e-6)); + + NavState expected2 = T2 * NavState::Expmap(screwNavState::xi) * T2.inverse(); + Vector xiPrime2 = T2.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected2, NavState::Expmap(xiPrime2), 1e-6)); + + NavState expected3 = T3 * NavState::Expmap(screwNavState::xi) * T3.inverse(); + Vector xiPrime3 = T3.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected3, NavState::Expmap(xiPrime3), 1e-6)); +} + +/* ************************************************************************* */ +TEST(NavState, Adjoint_compose_full) { + // To debug derivatives of compose, assert that + // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 + const NavState& T1 = T; + Vector x = + (Vector(9) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8).finished(); + NavState expected = T1 * NavState::Expmap(x) * T2; + Vector y = T2.inverse().Adjoint(x); + NavState actual = T1 * T2 * NavState::Expmap(y); + EXPECT(assert_equal(expected, actual, 1e-6)); +} + +/* ************************************************************************* */ +TEST(NavState, Retract_LocalCoordinates) { + Vector9 d; + d << 1, 2, 3, 4, 5, 6, 7, 8, 9; + d /= 10; + const Rot3 R = Rot3::Retract(d.head<3>()); + NavState t = NavState::Retract(d); + EXPECT(assert_equal(d, NavState::LocalCoordinates(t))); +} +/* ************************************************************************* */ +TEST(NavState, retract_localCoordinates) { + Vector9 d12; + d12 << 1, 2, 3, 4, 5, 6, 7, 8, 9; + d12 /= 10; + NavState t1 = T, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} +/* ************************************************************************* */ +TEST(NavState, expmap_logmap) { + Vector d12 = Vector9::Constant(0.1); + NavState t1 = T, t2 = t1.expmap(d12); + EXPECT(assert_equal(d12, t1.logmap(t2))); +} + +/* ************************************************************************* */ +TEST(NavState, retract_localCoordinates2) { + NavState t1 = T; + NavState t2 = T3; + NavState origin; + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); + // NOTE(FRANK): d12 !== -d21 for arbitrary retractions. +} +/* ************************************************************************* */ +TEST(NavState, manifold_expmap) { + NavState t1 = T; + NavState t2 = T3; + NavState origin; + Vector d12 = t1.logmap(t2); + EXPECT(assert_equal(t2, t1.expmap(d12))); + Vector d21 = t2.logmap(t1); + EXPECT(assert_equal(t1, t2.expmap(d21))); + + // Check that log(t1,t2)=-log(t2,t1) + EXPECT(assert_equal(d12, -d21)); +} + +/* ************************************************************************* */ +TEST(NavState, subgroups) { + // Frank - Below only works for correct "Agrawal06iros style expmap + // lines in canonical coordinates correspond to Abelian subgroups in SE(3) + Vector d = + (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); + // exp(-d)=inverse(exp(d)) + EXPECT(assert_equal(NavState::Expmap(-d), NavState::Expmap(d).inverse())); + // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) + NavState T2 = NavState::Expmap(2 * d); + NavState T3 = NavState::Expmap(3 * d); + NavState T5 = NavState::Expmap(5 * d); + EXPECT(assert_equal(T5, T2 * T3)); + EXPECT(assert_equal(T5, T3 * T2)); +} + +/* ************************************************************************* */ +TEST(NavState, adjointMap) { + Matrix res = NavState::adjointMap(screwNavState::xi); + Matrix wh = skewSymmetric(screwNavState::xi(0), screwNavState::xi(1), + screwNavState::xi(2)); + Matrix vh = skewSymmetric(screwNavState::xi(3), screwNavState::xi(4), + screwNavState::xi(5)); + Matrix rh = skewSymmetric(screwNavState::xi(6), screwNavState::xi(7), + screwNavState::xi(8)); + Matrix9 expected; + expected << wh, Z_3x3, Z_3x3, vh, wh, Z_3x3, rh, Z_3x3, wh; + EXPECT(assert_equal(expected, res, 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, ExpmapDerivative1) { + Matrix9 actualH; + Vector9 w; + w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; + NavState::Expmap(w, actualH); + + std::function f = [](const Vector9& w) { + return NavState::Expmap(w); + }; + Matrix expectedH = + numericalDerivative21 >( + &NavState::Expmap, w, {}); + EXPECT(assert_equal(expectedH, actualH)); +} + +/* ************************************************************************* */ +TEST(NavState, LogmapDerivative) { + Matrix9 actualH; + Vector9 w; + w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; + NavState p = NavState::Expmap(w); + EXPECT(assert_equal(w, NavState::Logmap(p, actualH), 1e-5)); + + std::function f = [](const NavState& p) { + return NavState::Logmap(p); + }; + Matrix expectedH = + numericalDerivative21 >( + &NavState::Logmap, p, {}); + EXPECT(assert_equal(expectedH, actualH)); +} + +//****************************************************************************** +TEST(NavState, Invariants) { + NavState 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(NavState, LieGroupDerivatives) { + NavState 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); +} + +//****************************************************************************** +TEST(NavState, ChartDerivatives) { + NavState id; + if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { + CHECK_CHART_DERIVATIVES(id, id); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T3); + } +} /* ************************************************************************* */ int main() { diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index c4015db37..5f8a19ff0 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -88,7 +88,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index 4f1c6fb54..e82a513ca 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -16,6 +16,7 @@ */ #include +#include #include using namespace std; diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 8ce9b361e..1038dd522 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -17,6 +17,7 @@ #pragma once #include +#include #include #include diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 71ef1d840..b09bcb3a5 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -26,6 +26,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index b3e34d079..df89f5b44 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -149,7 +149,7 @@ protected: noiseModel_->WhitenSystem(Ab.matrix(), b); } - return std::move(factor); + return factor; } /// @return a deep copy of this factor @@ -200,7 +200,7 @@ protected: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// Save to an archive: just saves the base class template void save(Archive& ar, const unsigned int /*version*/) const { @@ -224,9 +224,9 @@ private: #endif // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; - public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + inline constexpr static auto NeedsToAlign = (sizeof(T) % 16) == 0; + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // ExpressionFactor @@ -287,7 +287,7 @@ private: return expression(keys); } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int /*version*/) { diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 5bf643c12..072f40b58 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -23,6 +23,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 128de5847..ca253179b 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -119,7 +119,7 @@ class FunctorizedFactor : public NoiseModelFactorN { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -231,7 +231,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 20874e2dc..d0b27f3f3 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -25,6 +25,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index e9a9696eb..bb0210237 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -33,6 +33,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 47857a2a2..0eb2deef5 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -29,6 +29,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index d3abd95fd..f71b4b7da 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -340,7 +340,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { void updateDelta(bool forceFullSolve = false) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp index d4b050f84..64ab7c66e 100644 --- a/gtsam/nonlinear/ISAM2Clique.cpp +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -22,6 +22,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h index fde9dd3f2..39bce432a 100644 --- a/gtsam/nonlinear/ISAM2Clique.h +++ b/gtsam/nonlinear/ISAM2Clique.h @@ -146,7 +146,7 @@ class GTSAM_EXPORT ISAM2Clique void restoreFromOriginals(const Vector& originalValues, VectorValues* delta) const; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 99682fb77..f6711f0f0 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -26,7 +26,7 @@ #include #include #include -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #include #endif @@ -123,7 +123,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, auto currentState = static_cast(state_.get()); bool verbose = (params_.verbosityLM >= LevenbergMarquardtParams::TRYLAMBDA); -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #ifdef GTSAM_USING_NEW_BOOST_TIMERS boost::timer::cpu_timer lamda_iteration_timer; lamda_iteration_timer.start(); @@ -222,7 +222,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, } // if (systemSolvedSuccessfully) if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES // do timing #ifdef GTSAM_USING_NEW_BOOST_TIMERS double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall; diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index dfbf221c3..f63ecb0c6 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -163,7 +163,7 @@ public: void initializeLinearizationPoint(const Values& linearizationPoint); private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index f81486b30..538a95208 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -180,7 +180,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -273,7 +273,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -344,7 +344,7 @@ class NonlinearEquality2 : public NoiseModelFactorN { GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 7312bf6d9..5eabfac91 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -19,6 +19,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 725117748..1af01f60d 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -22,13 +22,14 @@ #pragma once #include +#include #include #include #include #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -302,7 +303,7 @@ public: shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -433,7 +434,7 @@ class NoiseModelFactorN public detail::NoiseModelFactorAliases { public: /// N is the number of variables (N-way factor) - enum { N = sizeof...(ValueTypes) }; + inline constexpr static auto N = sizeof...(ValueTypes); using NoiseModelFactor::unwhitenedError; @@ -714,7 +715,7 @@ protected: } } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index bf56d835f..8e3443dc7 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -250,7 +250,7 @@ namespace gtsam { std::shared_ptr linearizeToHessianFactor( const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const; -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index ab2a66a77..0979be3df 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -105,7 +105,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -118,7 +118,7 @@ namespace gtsam { #endif // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; + inline constexpr static auto NeedsToAlign = (sizeof(T) % 16) == 0; public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 6af9f2b6a..1caaa9db1 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -28,6 +28,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 2fe64d9c9..7e950479a 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -29,7 +29,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -393,7 +393,7 @@ namespace gtsam { return filter(key_value.key) && (dynamic_cast*>(&key_value.value)); } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index de0c9721e..25587f511 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -26,6 +26,7 @@ #include // operator typeid #include #include +#include class ExpressionFactorBinaryTest; // Forward declare for testing diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 09c234630..b684b1aad 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -127,6 +127,7 @@ virtual class NonlinearFactor : gtsam::Factor { // NonlinearFactor bool equals(const gtsam::NonlinearFactor& other, double tol) const; double error(const gtsam::Values& c) const; + double error(const gtsam::HybridValues& c) const; size_t dim() const; bool active(const gtsam::Values& c) const; gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; @@ -715,6 +716,9 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { void print(string s = "BatchFixedLagSmoother:\n") const; gtsam::LevenbergMarquardtParams params() const; + + gtsam::NonlinearFactorGraph getFactors() const; + template diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 6f5fc53f5..67815e262 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -115,7 +115,7 @@ TEST(Expression, Unary3) { // Simple test class that implements the `VectorSpace` protocol. class Class : public Point3 { public: - enum {dimension = 3}; + inline constexpr static auto dimension = 3; using Point3::Point3; const Vector3& vector() const { return *this; } inline static Class Identity() { return Class(0,0,0); } diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index d6f693a23..a96ede430 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -32,7 +32,6 @@ using namespace std; using namespace gtsam; using namespace gtsam::serializationTestHelpers; - /* ************************************************************************* */ // Create GUIDs for Noisemodels BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") @@ -153,9 +152,15 @@ TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { // Deserialize XML PriorFactor factor_deserialized_xml = PriorFactor(); + #if !defined(__QNX__) deserializeFromXMLFile(GTSAM_SOURCE_TREE_DATASET_DIR "/../../gtsam/nonlinear/tests/priorFactor.xml", factor_deserialized_xml); + #else + bool c = deserializeFromXMLFile( + "priorFactor.xml", + factor_deserialized_xml); + #endif EXPECT(assert_equal(factor, factor_deserialized_xml)); #endif } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index a2b265a56..06eb46d4c 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -54,7 +54,7 @@ int TestValueData::DestructorCount = 0; class TestValue { TestValueData data_; public: - enum {dimension = 0}; + inline constexpr static auto dimension = 0; void print(const std::string& str = "") const {} bool equals(const TestValue& other, double tol = 1e-9) const { return true; } size_t dim() const { return 0; } diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h index 01b2baf18..97f23ed8f 100644 --- a/gtsam/sam/BearingFactor.h +++ b/gtsam/sam/BearingFactor.h @@ -75,7 +75,7 @@ struct BearingFactor : public ExpressionFactorN { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index d5467be47..aa08fea17 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -92,7 +92,7 @@ class BearingRangeFactor private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h index 193a89a5a..0ccc54afd 100644 --- a/gtsam/sam/RangeFactor.h +++ b/gtsam/sam/RangeFactor.h @@ -80,7 +80,7 @@ class RangeFactor : public ExpressionFactorN { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { @@ -164,7 +164,7 @@ class RangeFactorWithTransform : public ExpressionFactorN { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; /** Serialization function */ template diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h index 7161ab2f9..dbe336979 100644 --- a/gtsam/sfm/SfmData.h +++ b/gtsam/sfm/SfmData.h @@ -125,7 +125,7 @@ struct GTSAM_EXPORT SfmData { /// @name Serialization /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; /** Serialization function */ template diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index 2b494ed7c..bdfa79cef 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -160,7 +160,7 @@ struct GTSAM_EXPORT SfmTrack : SfmTrack2d { /// @name Serialization /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 830f1c719..6c9f91e90 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -37,6 +37,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index c8b249ddc..0338762be 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -236,6 +236,8 @@ class EssentialTransferFactorK /** * @brief Constructor that accepts a vector of point triplets. * + * @note Calibrations are assumed all different, keys are derived from edges. + * * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a) * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b) * @param triplets A vector of triplets containing (pa, pb, pc) @@ -251,6 +253,24 @@ class EssentialTransferFactorK TransferEdges(edge1, edge2), triplets_(triplets) {} + /** + * @brief Constructor that accepts a vector of point triplets. + * + * @note Calibrations are assumed all same, using given key `keyK`. + * + * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a) + * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b) + * @param keyK Calibration key for all views. + * @param triplets A vector of triplets containing (pa, pb, pc) + * @param model An optional SharedNoiseModel + */ + EssentialTransferFactorK(EdgeKey edge1, EdgeKey edge2, Key keyK, + const std::vector& triplets, + const SharedNoiseModel& model = nullptr) + : Base(model, edge1, edge2, keyK, keyK, keyK), + TransferEdges(edge1, edge2), + triplets_(triplets) {} + /// Transfer points pa and pb to view c and evaluate error. Vector2 TransferError(const Matrix3& Eca, const K& Ka, const Point2& pa, const Matrix3& Ecb, const K& Kb, const Point2& pb, diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index cfb9cff20..fe06fbaae 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -78,7 +78,7 @@ class TranslationFactor : public NoiseModelFactorN { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { @@ -134,7 +134,7 @@ class BilinearAngleTranslationFactor OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override { // Ideally we should use a positive real valued scalar datatype for scale. - const double abs_scale = abs(scale[0]); + const double abs_scale = std::abs(scale[0]); const Point3 predicted = (Tb - Ta) * abs_scale; if (H1) { *H1 = -Matrix3::Identity() * abs_scale; @@ -149,7 +149,7 @@ class BilinearAngleTranslationFactor } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index a15b73ea1..24f24f1bb 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -98,8 +98,11 @@ virtual class EssentialTransferFactor : gtsam::NoiseModelFactor { template virtual class EssentialTransferFactorK : gtsam::NoiseModelFactor { EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, - const std::vector>& triplets, - const gtsam::noiseModel::Base* model = nullptr); + const std::vector>& triplets, + const gtsam::noiseModel::Base* model = nullptr); + EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, size_t keyK, + const std::vector>& triplets, + const gtsam::noiseModel::Base* model = nullptr); }; #include diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index ced3d2ce7..b34bdf832 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -154,7 +154,7 @@ TEST(TransferFactor, MultipleTuples) { } //************************************************************************* -// Test for EssentialTransferFactor +// Test for EssentialTransferFactorK TEST(EssentialTransferFactor, Jacobians) { using namespace fixture; diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index 3b031b334..11a0b1530 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -106,7 +106,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 6eaa5c01b..297703d4a 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -135,7 +135,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -147,10 +147,10 @@ namespace gtsam { } #endif - // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 }; - public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html + inline constexpr static auto NeedsToAlign = (sizeof(VALUE) % 16) == 0; + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // \class BetweenFactor /// traits @@ -176,7 +176,7 @@ namespace gtsam { private: /** Serialization function */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index a496971e2..85d1f6255 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -83,7 +83,7 @@ struct BoundingConstraint1: public NoiseModelFactorN { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -161,7 +161,7 @@ struct BoundingConstraint2: public NoiseModelFactorN { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 45e54aa5d..6b57a8f27 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -91,7 +91,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index b19bb09ab..5d38cb234 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -38,7 +38,6 @@ class EssentialMatrixFactor : public NoiseModelFactorN { typedef EssentialMatrixFactor This; public: - // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -71,7 +70,9 @@ class EssentialMatrixFactor : public NoiseModelFactorN { const SharedNoiseModel& model, std::shared_ptr K) : Base(model, key) { - assert(K); +#ifndef NDEBUG + if (!K) throw; +#endif vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB)); } @@ -93,8 +94,8 @@ class EssentialMatrixFactor : public NoiseModelFactorN { } /// vector of errors returns 1D vector - Vector evaluateError( - const EssentialMatrix& E, OptionalMatrixType H) const override { + Vector evaluateError(const EssentialMatrix& E, + OptionalMatrixType H) const override { Vector error(1); error << E.error(vA_, vB_, H); return error; @@ -118,7 +119,6 @@ class EssentialMatrixFactor2 typedef EssentialMatrixFactor2 This; public: - // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -178,9 +178,9 @@ class EssentialMatrixFactor2 * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError( - const EssentialMatrix& E, const double& d, - OptionalMatrixType DE, OptionalMatrixType Dd) const override { + Vector evaluateError(const EssentialMatrix& E, const double& d, + OptionalMatrixType DE, + OptionalMatrixType Dd) const override { // We have point x,y in image 1 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d // We then convert to second camera by P2 = 1R2'*(P1-1T2) @@ -241,7 +241,6 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { Rot3 cRb_; ///< Rotation from body to camera frame public: - // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -292,9 +291,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError( - const EssentialMatrix& E, const double& d, - OptionalMatrixType DE, OptionalMatrixType Dd) const override { + Vector evaluateError(const EssentialMatrix& E, const double& d, + OptionalMatrixType DE, + OptionalMatrixType Dd) const override { if (!DE) { // Convert E from body to camera frame EssentialMatrix cameraE = cRb_ * E; @@ -304,8 +303,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { // Version with derivatives Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E); - // Using the pointer version of evaluateError since the Base class (EssentialMatrixFactor2) - // does not have the matrix reference version of evaluateError + // Using the pointer version of evaluateError since the Base class + // (EssentialMatrixFactor2) does not have the matrix reference version of + // evaluateError Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd); *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5) return e; @@ -327,7 +327,7 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { * Even with a prior, we can only optimize 2 DoF in the calibration. So the * prior should have a noise model with very low sigma in the remaining * dimensions. This has been tested to work on Cal3_S2. With Cal3Bundler, it - * works only with a strong prior (low sigma noisemodel) on all degrees of + * works only with a strong prior (low sigma noise model) on all degrees of * freedom. */ template @@ -343,13 +343,12 @@ class EssentialMatrixFactor4 typedef Eigen::Matrix JacobianCalibration; public: - - // Provide access to the Matrix& version of evaluateError: + // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; /** * Constructor - * @param keyE Essential Matrix (from camera B to A) variable key + * @param keyE Essential Matrix aEb variable key * @param keyK Calibration variable key (common for both cameras) * @param pA point in first camera, in pixel coordinates * @param pB point in second camera, in pixel coordinates @@ -357,7 +356,7 @@ class EssentialMatrixFactor4 * coordinates */ EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model) + const SharedNoiseModel& model = nullptr) : Base(model, keyE, keyK), pA_(pA), pB_(pB) {} /// @return a deep copy of this factor @@ -385,32 +384,32 @@ class EssentialMatrixFactor4 * @param H2 optional jacobian of error w.r.t K * @return * Vector 1D vector of algebraic error */ - Vector evaluateError( - const EssentialMatrix& E, const CALIBRATION& K, - OptionalMatrixType H1, OptionalMatrixType H2) const override { + Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& K, + OptionalMatrixType HE, + OptionalMatrixType HK) const override { // converting from pixel coordinates to normalized coordinates cA and cB JacobianCalibration cA_H_K; // dcA/dK JacobianCalibration cB_H_K; // dcB/dK - Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, OptionalNone); - Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, OptionalNone); + Point2 cA = K.calibrate(pA_, HK ? &cA_H_K : 0, OptionalNone); + Point2 cB = K.calibrate(pB_, HK ? &cB_H_K : 0, OptionalNone); // convert to homogeneous coordinates Vector3 vA = EssentialMatrix::Homogeneous(cA); Vector3 vB = EssentialMatrix::Homogeneous(cB); - if (H2) { + if (HK) { // compute the jacobian of error w.r.t K // error function f = vA.T * E * vB // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]] - *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K + + *HK = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K + vA.transpose() * E.matrix().leftCols<2>() * cB_H_K; } Vector error(1); - error << E.error(vA, vB, H1); + error << E.error(vA, vB, HE); return error; } @@ -420,4 +419,108 @@ class EssentialMatrixFactor4 }; // EssentialMatrixFactor4 +/** + * Binary factor that optimizes for E and two calibrations Ka and Kb using the + * algebraic epipolar error (Ka^-1 pA)'E (Kb^-1 pB). The calibrations are + * assumed different for the two images, but if you use the same key for Ka and + * Kb, the sum of the two K Jacobians equals that of the K Jacobian for + * EssentialMatrix4. If you know there is a single global calibration, use + * that factor instead. + * + * Note: see the comment about priors from EssentialMatrixFactor4: even stronger + * caveats about having priors on calibration apply here. + */ +template +class EssentialMatrixFactor5 + : public NoiseModelFactorN { + private: + Point2 pA_, pB_; ///< points in pixel coordinates + + typedef NoiseModelFactorN Base; + typedef EssentialMatrixFactor5 This; + + static constexpr int DimK = FixedDimension::value; + typedef Eigen::Matrix JacobianCalibration; + + public: + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /** + * Constructor + * @param keyE Essential Matrix aEb variable key + * @param keyKa Calibration variable key for camera A + * @param keyKb Calibration variable key for camera B + * @param pA point in first camera, in pixel coordinates + * @param pB point in second camera, in pixel coordinates + * @param model noise model is about dot product in ideal, homogeneous + * coordinates + */ + EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA, + const Point2& pB, + const SharedNoiseModel& model = nullptr) + : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + Base::print(s); + std::cout << " EssentialMatrixFactor5 with measurements\n (" + << pA_.transpose() << ")' and (" << pB_.transpose() << ")'" + << std::endl; + } + + /** + * @brief Calculate the algebraic epipolar error pA' (Ka^-1)' E Kb pB. + * + * @param E essential matrix for key keyE + * @param Ka calibration for camera A for key keyKa + * @param Kb calibration for camera B for key keyKb + * @param H1 optional jacobian of error w.r.t E + * @param H2 optional jacobian of error w.r.t Ka + * @param H3 optional jacobian of error w.r.t Kb + * @return * Vector 1D vector of algebraic error + */ + Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& Ka, + const CALIBRATION& Kb, OptionalMatrixType HE, + OptionalMatrixType HKa, + OptionalMatrixType HKb) const override { + // converting from pixel coordinates to normalized coordinates cA and cB + JacobianCalibration cA_H_Ka; // dcA/dKa + JacobianCalibration cB_H_Kb; // dcB/dKb + Point2 cA = Ka.calibrate(pA_, HKa ? &cA_H_Ka : 0, OptionalNone); + Point2 cB = Kb.calibrate(pB_, HKb ? &cB_H_Kb : 0, OptionalNone); + + // Convert to homogeneous coordinates. + Vector3 vA = EssentialMatrix::Homogeneous(cA); + Vector3 vB = EssentialMatrix::Homogeneous(cB); + + if (HKa) { + // Compute the jacobian of error w.r.t Ka. + *HKa = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_Ka; + } + + if (HKb) { + // Compute the jacobian of error w.r.t Kb. + *HKb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb; + } + + Vector error(1); + error << E.error(vA, vB, HE); + + return error; + } + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; +// EssentialMatrixFactor5 + } // namespace gtsam diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp index e70f64d96..982430d81 100644 --- a/gtsam/slam/FrobeniusFactor.cpp +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -61,7 +61,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { return noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), isoModel); } else { - return std::move(isoModel); + return isoModel; } } diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index a000a1514..37efa5f24 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -49,7 +49,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t n, */ template class FrobeniusPrior : public NoiseModelFactorN { - enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime; using MatrixNN = typename Rot::MatrixNN; Eigen::Matrix vecM_; ///< vectorized matrix to approximate @@ -79,7 +79,7 @@ class FrobeniusPrior : public NoiseModelFactorN { */ template class FrobeniusFactor : public NoiseModelFactorN { - enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime; public: @@ -111,7 +111,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN { Rot R12_; ///< measured rotation between R1 and R2 Eigen::Matrix R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1 - enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime; public: diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 581624b38..06c832eb4 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -36,7 +36,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -182,7 +182,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -283,7 +283,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h index 4e82ca1d9..6b25dd261 100644 --- a/gtsam/slam/KarcherMeanFactor.h +++ b/gtsam/slam/KarcherMeanFactor.h @@ -45,7 +45,7 @@ template T FindKarcherMean(std::initializer_list &&rotations); * */ template class KarcherMeanFactor : public NonlinearFactor { // Compile time dimension: can be -1 - enum { D = traits::dimension }; + inline constexpr static auto D = traits::dimension; // Runtime dimension: always >=0 size_t d_; diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h new file mode 100644 index 000000000..d53cd0ae1 --- /dev/null +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -0,0 +1,298 @@ +/** + * ProjectionFactor, but with pose2 (robot on the floor) + * + * This factor is useful for high-school robotics competitions, + * which run robots on the floor, with use fixed maps and fiducial + * markers. + * + * @see https://www.firstinspires.org/ + * + * @file PlanarProjectionFactor.h + * @brief for planar smoothing + * @date Dec 2, 2024 + * @author joel@truher.org + */ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace gtsam { + + /** + * @class PlanarProjectionFactorBase + * @brief Camera projection for robot on the floor. Measurement is camera pixels. + */ + class PlanarProjectionFactorBase { + protected: + PlanarProjectionFactorBase() {} + + /** + * @param measured pixels in the camera frame + */ + PlanarProjectionFactorBase(const Point2& measured) : measured_(measured) {} + + /** + * Predict the projection of the landmark in camera pixels. + * + * @param landmark point3 of the target + * @param wTb "world to body": planar pose2 of vehicle body frame in world frame + * @param bTc "body to camera": camera pose in body frame, oriented parallel to pose2 zero i.e. +x + * @param calib camera calibration with distortion + * @param Hlandmark jacobian + * @param HwTb jacobian + * @param HbTc jacobian + * @param Hcalib jacobian + */ + Point2 predict( + const Point3& landmark, + const Pose2& wTb, + const Pose3& bTc, + const Cal3DS2& calib, + OptionalJacobian<2, 3> Hlandmark = {}, // (x, y, z) + OptionalJacobian<2, 3> HwTb = {}, // (x, y, theta) + OptionalJacobian<2, 6> HbTc = {}, // (rx, ry, rz, x, y, theta) + OptionalJacobian<2, 9> Hcalib = {} + ) const { +#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION + try { +#endif + Matrix63 Hp; // 6x3 + Matrix66 H0; // 6x6 + Pose3 wTc = Pose3::FromPose2(wTb, HwTb ? &Hp : nullptr).compose(bTc, HwTb ? &H0 : nullptr); + PinholeCamera camera = PinholeCamera(wTc, calib); + if (HwTb || HbTc) { + // Dpose is for pose3 (R,t) + Matrix26 Dpose; + Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib); + if (HbTc) + *HbTc = Dpose; + if (HwTb) + *HwTb = Dpose * H0 * Hp; + return result; + } else { + return camera.project(landmark, {}, {}, {}); + } +#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION + } catch (CheiralityException& e) { + std::cout << "****** CHIRALITY EXCEPTION ******\n"; + if (Hlandmark) Hlandmark->setZero(); + if (HwTb) HwTb->setZero(); + if (HbTc) HbTc->setZero(); + if (Hcalib) Hcalib->setZero(); + // return a large error + return Matrix::Constant(2, 1, 2.0 * calib.fx()); + } +#endif + } + + Point2 measured_; // pixel measurement + }; + + + /** + * @class PlanarProjectionFactor1 + * @brief One variable: the pose. + * Landmark, camera offset, camera calibration are constant. + * This is intended for localization within a known map. + */ + class PlanarProjectionFactor1 + : public PlanarProjectionFactorBase, public NoiseModelFactorN { + public: + typedef NoiseModelFactorN Base; + using Base::evaluateError; + PlanarProjectionFactor1() {} + + ~PlanarProjectionFactor1() override {} + + /// @return a deep copy of this factor + NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this))); + } + + + /** + * @brief constructor for known landmark, offset, and calibration + * @param poseKey index of the robot pose2 in the z=0 plane + * @param landmark point3 in the world + * @param measured corresponding point2 in the camera frame + * @param bTc "body to camera": constant camera offset from pose + * @param calib constant camera calibration + * @param model stddev of the measurements, ~one pixel? + */ + PlanarProjectionFactor1( + Key poseKey, + const Point3& landmark, + const Point2& measured, + const Pose3& bTc, + const Cal3DS2& calib, + const SharedNoiseModel& model = {}) + : PlanarProjectionFactorBase(measured), + NoiseModelFactorN(model, poseKey), + landmark_(landmark), + bTc_(bTc), + calib_(calib) {} + + /** + * @param wTb "world to body": estimated pose2 + * @param HwTb jacobian + */ + Vector evaluateError(const Pose2& wTb, OptionalMatrixType HwTb) const override { + return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_; + } + + private: + Point3 landmark_; // landmark + Pose3 bTc_; // "body to camera": camera offset to robot pose + Cal3DS2 calib_; // camera calibration + }; + + template<> + struct traits : + public Testable {}; + + /** + * @class PlanarProjectionFactor2 + * @brief Two unknowns: the pose and the landmark. + * Camera offset and calibration are constant. + * This is similar to GeneralSFMFactor, used for SLAM. + */ + class PlanarProjectionFactor2 + : public PlanarProjectionFactorBase, public NoiseModelFactorN { + public: + typedef NoiseModelFactorN Base; + using Base::evaluateError; + + PlanarProjectionFactor2() {} + + ~PlanarProjectionFactor2() override {} + + /// @return a deep copy of this factor + NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this))); + } + + /** + * @brief constructor for variable landmark, known offset and calibration + * @param poseKey index of the robot pose2 in the z=0 plane + * @param landmarkKey index of the landmark point3 + * @param measured corresponding point in the camera frame + * @param bTc "body to camera": constant camera offset from pose + * @param calib constant camera calibration + * @param model stddev of the measurements, ~one pixel? + */ + PlanarProjectionFactor2( + Key poseKey, + Key landmarkKey, + const Point2& measured, + const Pose3& bTc, + const Cal3DS2& calib, + const SharedNoiseModel& model = {}) + : PlanarProjectionFactorBase(measured), + NoiseModelFactorN(model, landmarkKey, poseKey), + bTc_(bTc), + calib_(calib) {} + + /** + * @param wTb "world to body": estimated pose2 + * @param landmark estimated landmark + * @param HwTb jacobian + * @param Hlandmark jacobian + */ + Vector evaluateError( + const Pose2& wTb, + const Point3& landmark, + OptionalMatrixType HwTb, + OptionalMatrixType Hlandmark) const override { + return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_; + } + + private: + Pose3 bTc_; // "body to camera": camera offset to robot pose + Cal3DS2 calib_; // camera calibration + }; + + template<> + struct traits : + public Testable {}; + + /** + * @class PlanarProjectionFactor3 + * @brief Three unknowns: the pose, the camera offset, and the camera calibration. + * Landmark is constant. + * This is intended to be used for camera calibration. + */ + class PlanarProjectionFactor3 : public PlanarProjectionFactorBase, public NoiseModelFactorN { + public: + typedef NoiseModelFactorN Base; + using Base::evaluateError; + + PlanarProjectionFactor3() {} + + ~PlanarProjectionFactor3() override {} + + /// @return a deep copy of this factor + NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this))); + } + + /** + * @brief constructor for variable pose, offset, and calibration, known landmark. + * @param poseKey index of the robot pose2 in the z=0 plane + * @param offsetKey index of camera offset from pose + * @param calibKey index of camera calibration + * @param landmark point3 in the world + * @param measured corresponding point2 in the camera frame + * @param model stddev of the measurements, ~one pixel? + */ + PlanarProjectionFactor3( + Key poseKey, + Key offsetKey, + Key calibKey, + const Point3& landmark, + const Point2& measured, + const SharedNoiseModel& model = {}) + : PlanarProjectionFactorBase(measured), + NoiseModelFactorN(model, poseKey, offsetKey, calibKey), + landmark_(landmark) {} + + /** + * @param wTb "world to body": estimated pose2 + * @param bTc "body to camera": pose3 offset from pose2 +x + * @param calib calibration + * @param HwTb pose jacobian + * @param HbTc offset jacobian + * @param Hcalib calibration jacobian + */ + Vector evaluateError( + const Pose2& wTb, + const Pose3& bTc, + const Cal3DS2& calib, + OptionalMatrixType HwTb, + OptionalMatrixType HbTc, + OptionalMatrixType Hcalib) const override { + return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_; + } + + private: + Point3 landmark_; // landmark + }; + + template<> + struct traits : + public Testable {}; + +} // namespace gtsam diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 8ce90988b..47ea4c69f 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -91,7 +91,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index c5257f45c..378610825 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -90,7 +90,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index f40d8f5bb..baea29eed 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -188,7 +188,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 0ae6ea88a..21617e27a 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -122,7 +122,7 @@ public: Key local_key() const { return this->key3(); } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 5d6ec4445..e641bf5d1 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -30,7 +30,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif #include @@ -135,7 +135,11 @@ protected: /// Add a bunch of measurements, together with the camera keys. void add(const ZVector& measurements, const KeyVector& cameraKeys) { - assert(measurements.size() == cameraKeys.size()); +#ifndef NDEBUG + if (measurements.size() != cameraKeys.size()) { + throw std::runtime_error("Number of measurements and camera keys do not match"); + } +#endif for (size_t i = 0; i < measurements.size(); i++) { this->add(measurements[i], cameraKeys[i]); } @@ -446,7 +450,7 @@ protected: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION/// +#if GTSAM_ENABLE_BOOST_SERIALIZATION/// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/slam/SmartFactorParams.h b/gtsam/slam/SmartFactorParams.h index 4523d0e0e..de3245038 100644 --- a/gtsam/slam/SmartFactorParams.h +++ b/gtsam/slam/SmartFactorParams.h @@ -118,7 +118,7 @@ struct SmartProjectionParams { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index c54efecc4..3c8843b39 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -466,7 +466,7 @@ protected: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index b88c1c628..855ae8c08 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -148,7 +148,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 6007c84e2..dd4237299 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -349,7 +349,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 5adafcf3a..5fd171851 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -175,7 +175,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index 3b8486a59..f36e9b385 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -187,7 +187,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 97dcfcae7..1761a6cc3 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -24,6 +24,38 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { void serialize() const; }; +#include +virtual class PlanarProjectionFactor1 : gtsam::NoiseModelFactor { + PlanarProjectionFactor1( + size_t poseKey, + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::Pose3& bTc, + const gtsam::Cal3DS2& calib, + const gtsam::noiseModel::Base* model); + void serialize() const; +}; +virtual class PlanarProjectionFactor2 : gtsam::NoiseModelFactor { + PlanarProjectionFactor2( + size_t poseKey, + size_t landmarkKey, + const gtsam::Point2& measured, + const gtsam::Pose3& bTc, + const gtsam::Cal3DS2& calib, + const gtsam::noiseModel::Base* model); + void serialize() const; +}; +virtual class PlanarProjectionFactor3 : gtsam::NoiseModelFactor { + PlanarProjectionFactor3( + size_t poseKey, + size_t offsetKey, + size_t calibKey, + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::noiseModel::Base* model); + void serialize() const; +}; + #include template virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { @@ -107,7 +139,7 @@ typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorPoseCal3Unified; -template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { GeneralSFMFactor2(const gtsam::Point2& measured, @@ -221,15 +253,55 @@ 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); + EssentialMatrixFactor(size_t key, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model); void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::EssentialMatrixFactor& other, double tol) const; gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E) const; }; +virtual class EssentialMatrixFactor2 : gtsam::NoiseModelFactor { + EssentialMatrixFactor2(size_t key1, size_t key2, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, double d) const; +}; + +virtual class EssentialMatrixFactor3 : gtsam::EssentialMatrixFactor2 { + EssentialMatrixFactor3(size_t key1, size_t key2, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::Rot3& cRb, const gtsam::noiseModel::Base* model); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, double d) const; +}; + +template +virtual class EssentialMatrixFactor4 : gtsam::NoiseModelFactor { + EssentialMatrixFactor4(size_t keyE, size_t keyK, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model = nullptr); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, const CALIBRATION& K) const; +}; + +template +virtual class EssentialMatrixFactor5 : gtsam::NoiseModelFactor { + EssentialMatrixFactor5(size_t keyE, size_t keyKa, size_t keyKb, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model = nullptr); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, + const CALIBRATION& Ka, const CALIBRATION& Kb) const; +}; + #include virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor { EssentialMatrixConstraint(size_t key1, size_t key2, const gtsam::EssentialMatrix &measuredE, diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index af713d37f..15de253cd 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -14,8 +14,8 @@ #include #include #include -#include #include +#include #include using namespace std::placeholders; @@ -103,7 +103,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { std::function)> f = - std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2); + std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), + std::placeholders::_2); Expression E_(key); // leaf expression Expression expr(f, E_); // unary expression @@ -118,8 +119,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { // Check evaluation Vector expected(1); expected << 0; - vector Hactual(1); - Vector actual = factor.unwhitenedError(values, Hactual); + vector actualH(1); + Vector actual = factor.unwhitenedError(values, actualH); EXPECT(assert_equal(expected, actual, 1e-7)); } } @@ -129,10 +130,11 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { std::function)> f = - std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2); + std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), + std::placeholders::_2); std::function, - OptionalJacobian<5, 2>)> + OptionalJacobian<5, 3>, + OptionalJacobian<5, 2>)> g; Expression R_(key); Expression d_(trueDirection); @@ -151,8 +153,8 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { // Check evaluation Vector expected(1); expected << 0; - vector Hactual(1); - Vector actual = factor.unwhitenedError(values, Hactual); + vector actualH(1); + Vector actual = factor.unwhitenedError(values, actualH); EXPECT(assert_equal(expected, actual, 1e-7)); } } @@ -213,9 +215,9 @@ TEST(EssentialMatrixFactor2, factor) { const Point2 pi = PinholeBase::Project(P2); Point2 expected(pi - pB(i)); - Matrix Hactual1, Hactual2; + Matrix actualH1, actualH2; double d(baseline / P1.z()); - Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); + Vector actual = factor.evaluateError(trueE, d, actualH1, actualH2); EXPECT(assert_equal(expected, actual, 1e-7)); Values val; @@ -278,9 +280,9 @@ TEST(EssentialMatrixFactor3, factor) { const Point2 pi = camera2.project(P1); Point2 expected(pi - pB(i)); - Matrix Hactual1, Hactual2; + Matrix actualH1, actualH2; double d(baseline / P1.z()); - Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); + Vector actual = factor.evaluateError(bodyE, d, actualH1, actualH2); EXPECT(assert_equal(expected, actual, 1e-7)); Values val; @@ -531,6 +533,48 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { 1e-6); } +//************************************************************************* +TEST(EssentialMatrixFactor5, factor) { + Key keyE(1), keyKa(2), keyKb(3); + for (size_t i = 0; i < 5; i++) { + EssentialMatrixFactor5 factor(keyE, keyKa, keyKb, pA(i), pB(i), + model1); + + // Check evaluation + Vector1 expected; + expected << 0; + Vector actual = factor.evaluateError(trueE, trueK, trueK); + EXPECT(assert_equal(expected, actual, 1e-7)); + + Values truth; + truth.insert(keyE, trueE); + truth.insert(keyKa, trueK); + truth.insert(keyKb, trueK); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, truth, 1e-6, 1e-7); + } +} + +//************************************************************************* +// Test that if we use the same keys for Ka and Kb the sum of the two K +// Jacobians equals that of the single K Jacobian for EssentialMatrix4 +TEST(EssentialMatrixFactor5, SameKeys) { + Key keyE(1), keyK(2); + for (size_t i = 0; i < 5; i++) { + EssentialMatrixFactor4 factor4(keyE, keyK, pA(i), pB(i), model1); + EssentialMatrixFactor5 factor5(keyE, keyK, keyK, pA(i), pB(i), + model1); + + Values truth; + truth.insert(keyE, trueE); + truth.insert(keyK, trueK); + + // Check Jacobians + Matrix H0, H1, H2; + factor4.evaluateError(trueE, trueK, nullptr, &H0); + factor5.evaluateError(trueE, trueK, trueK, nullptr, &H1, &H2); + EXPECT(assert_equal(H0, H1 + H2, 1e-9)); + } +} } // namespace example1 //************************************************************************* diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp new file mode 100644 index 000000000..5f65c9b5e --- /dev/null +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -0,0 +1,437 @@ +/** + * @file testPlanarProjectionFactor.cpp + * @date Dec 3, 2024 + * @author joel@truher.org + * @brief unit tests for PlanarProjectionFactor + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::X; +using symbol_shorthand::C; +using symbol_shorthand::K; +using symbol_shorthand::L; + +/* ************************************************************************* */ +TEST(PlanarProjectionFactor1, Error1) { + // Example: center projection and Jacobian + Point3 landmark(1, 0, 0); + Point2 measured(200, 200); + Pose3 offset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model); + Pose2 pose(0, 0, 0); + Matrix H; + CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6)); + CHECK(assert_equal((Matrix23() << // + 0, 200, 200, // + 0, 0, 0).finished(), H, 1e-6)); +} + +/* ************************************************************************* */ +TEST(PlanarProjectionFactor1, Error2) { + // Example: upper left corner projection and Jacobian + Point3 landmark(1, 1, 1); + Point2 measured(0, 0); + Pose3 offset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model); + Pose2 pose(0, 0, 0); + Matrix H; + CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6)); + CHECK(assert_equal((Matrix23() << // + -200, 200, 400, // + -200, 0, 200).finished(), H, 1e-6)); +} + +/* ************************************************************************* */ +TEST(PlanarProjectionFactor1, Error3) { + // Example: upper left corner projection and Jacobian with distortion + Point3 landmark(1, 1, 1); + Point2 measured(0, 0); + Pose3 offset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); // note distortion + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model); + Pose2 pose(0, 0, 0); + Matrix H; + CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6)); + CHECK(assert_equal((Matrix23() << // + -360, 280, 640, // + -360, 80, 440).finished(), H, 1e-6)); +} + +/* ************************************************************************* */ +TEST(PlanarProjectionFactor1, Jacobian) { + // Verify Jacobians with numeric derivative + std::default_random_engine rng(42); + std::uniform_real_distribution dist(-0.3, 0.3); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + // center of the random camera poses + Pose3 centerOffset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + + for (int i = 0; i < 1000; ++i) { + Point3 landmark(2 + dist(rng), dist(rng), dist(rng)); + Point2 measured(200 + 100 * dist(rng), 200 + 100 * dist(rng)); + Pose3 offset = centerOffset.compose( + Pose3( + Rot3::Ypr(dist(rng), dist(rng), dist(rng)), + Point3(dist(rng), dist(rng), dist(rng)))); + Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); + PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model); + Pose2 pose(dist(rng), dist(rng), dist(rng)); + Matrix H1; + factor.evaluateError(pose, H1); + auto expectedH1 = numericalDerivative11( + [&factor](const Pose2& p) { + return factor.evaluateError(p, {});}, + pose); + CHECK(assert_equal(expectedH1, H1, 5e-6)); + } +} + +/* ************************************************************************* */ +TEST(PlanarProjectionFactor1, Solve) { + // Example localization + + SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + // pose model is wide, so the solver finds the right answer. + SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(10, 10, 10)); + + // landmarks + Point3 l0(1, 0.1, 1); + Point3 l1(1, -0.1, 1); + + // camera pixels + Point2 p0(180, 0); + Point2 p1(220, 0); + + // body + Pose2 x0(0, 0, 0); + + // camera z looking at +x with (xy) antiparallel to (yz) + Pose3 c0( + Rot3(0, 0, 1, // + -1, 0, 0, // + 0, -1, 0), // + Vector3(0, 0, 0)); + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + + NonlinearFactorGraph graph; + graph.add(PlanarProjectionFactor1(X(0), l0, p0, c0, calib, pxModel)); + graph.add(PlanarProjectionFactor1(X(0), l1, p1, c0, calib, pxModel)); + graph.add(PriorFactor(X(0), x0, xNoise)); + + Values initialEstimate; + initialEstimate.insert(X(0), x0); + + // run the optimizer + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); + + // verify that the optimizer found the right pose. + CHECK(assert_equal(x0, result.at(X(0)), 2e-3)); + + // covariance + Marginals marginals(graph, result); + Matrix cov = marginals.marginalCovariance(X(0)); + CHECK(assert_equal((Matrix33() << // + 0.000012, 0.000000, 0.000000, // + 0.000000, 0.001287, -.001262, // + 0.000000, -.001262, 0.001250).finished(), cov, 3e-6)); + + // pose stddev + Vector3 sigma = cov.diagonal().cwiseSqrt(); + CHECK(assert_equal((Vector3() << // + 0.0035, + 0.0359, + 0.0354 + ).finished(), sigma, 1e-4)); + +} + +/* ************************************************************************* */ +TEST(PlanarProjectionFactor3, Error1) { + // Example: center projection and Jacobian + Point3 landmark(1, 0, 0); + Point2 measured(200, 200); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model); + Pose2 pose(0, 0, 0); + Pose3 offset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + Matrix H1; + Matrix H2; + Matrix H3; + CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, offset, calib, H1, H2, H3), 1e-6)); + CHECK(assert_equal((Matrix23() < dist(-0.3, 0.3); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + // center of the random camera poses + Pose3 centerOffset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + + for (int i = 0; i < 1000; ++i) { + Point3 landmark(2 + dist(rng), dist(rng), dist(rng)); + Point2 measured(200 + 100 * dist(rng), 200 + 100 * dist(rng)); + Pose3 offset = centerOffset.compose( + Pose3( + Rot3::Ypr(dist(rng), dist(rng), dist(rng)), + Point3(dist(rng), dist(rng), dist(rng)))); + Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); + + PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model); + + Pose2 pose(dist(rng), dist(rng), dist(rng)); + + // actual H + Matrix H1, H2, H3; + factor.evaluateError(pose, offset, calib, H1, H2, H3); + + Matrix expectedH1 = numericalDerivative31( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c, {}, {}, {});}, + pose, offset, calib); + Matrix expectedH2 = numericalDerivative32( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c, {}, {}, {});}, + pose, offset, calib); + Matrix expectedH3 = numericalDerivative33( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c, {}, {}, {});}, + pose, offset, calib); + CHECK(assert_equal(expectedH1, H1, 5e-6)); + CHECK(assert_equal(expectedH2, H2, 5e-6)); + CHECK(assert_equal(expectedH3, H3, 5e-6)); + } +} + +/* ************************************************************************* */ +TEST(PlanarProjectionFactor3, SolveOffset) { + // Example localization + SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01)); + // offset model is wide, so the solver finds the right answer. + SharedNoiseModel cNoise = noiseModel::Diagonal::Sigmas(Vector6(10, 10, 10, 10, 10, 10)); + SharedNoiseModel kNoise = noiseModel::Diagonal::Sigmas(Vector9(0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001)); + + // landmarks + Point3 l0(1, 0, 1); + Point3 l1(1, 0, 0); + Point3 l2(1, -1, 1); + Point3 l3(2, 2, 1); + + // camera pixels + Point2 p0(200, 200); + Point2 p1(200, 400); + Point2 p2(400, 200); + Point2 p3(0, 200); + + // body + Pose2 x0(0, 0, 0); + + // camera z looking at +x with (xy) antiparallel to (yz) + Pose3 c0( + Rot3(0, 0, 1, // + -1, 0, 0, // + 0, -1, 0), // + Vector3(0, 0, 1)); // note z offset + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + + NonlinearFactorGraph graph; + graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l0, p0, pxModel)); + graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l1, p1, pxModel)); + graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l2, p2, pxModel)); + graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l3, p3, pxModel)); + graph.add(PriorFactor(X(0), x0, xNoise)); + graph.add(PriorFactor(C(0), c0, cNoise)); + graph.add(PriorFactor(K(0), calib, kNoise)); + + Values initialEstimate; + initialEstimate.insert(X(0), x0); + initialEstimate.insert(C(0), c0); + initialEstimate.insert(K(0), calib); + + // run the optimizer + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); + + // verify that the optimizer found the right pose. + CHECK(assert_equal(x0, result.at(X(0)), 2e-3)); + + // verify the camera is pointing at +x + Pose3 cc0 = result.at(C(0)); + CHECK(assert_equal(c0, cc0, 5e-3)); + + // verify the calibration + CHECK(assert_equal(calib, result.at(K(0)), 2e-3)); + + Marginals marginals(graph, result); + Matrix x0cov = marginals.marginalCovariance(X(0)); + + // narrow prior => ~zero cov + CHECK(assert_equal(Matrix33::Zero(), x0cov, 1e-4)); + + Matrix c0cov = marginals.marginalCovariance(C(0)); + + // invert the camera offset to get covariance in body coordinates + Matrix66 HcTb = cc0.inverse().AdjointMap().inverse(); + Matrix c0cov2 = HcTb * c0cov * HcTb.transpose(); + + // camera-frame stddev + Vector6 c0sigma = c0cov.diagonal().cwiseSqrt(); + CHECK(assert_equal((Vector6() << // + 0.009, + 0.011, + 0.004, + 0.012, + 0.012, + 0.011 + ).finished(), c0sigma, 1e-3)); + + // body frame stddev + Vector6 bTcSigma = c0cov2.diagonal().cwiseSqrt(); + CHECK(assert_equal((Vector6() << // + 0.004, + 0.009, + 0.011, + 0.012, + 0.012, + 0.012 + ).finished(), bTcSigma, 1e-3)); + + // narrow prior => ~zero cov + CHECK(assert_equal(Matrix99::Zero(), marginals.marginalCovariance(K(0)), 3e-3)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 3102191dc..4bba7cb69 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -101,7 +101,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index 275b9560d..2b9e2f7ad 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -62,7 +62,7 @@ namespace gtsam { bool equals(const This& other, double tol = 1e-9) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index 5cc4e9cfc..3cc3e60cc 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -126,7 +126,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index 190609ecb..fd19740de 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -151,7 +151,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 4122031d0..bbcd59f5e 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -145,7 +145,7 @@ namespace gtsam { /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 1f7b4a7c7..aaffe54a8 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -107,9 +107,7 @@ if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with COMPILE_DEFINITIONS GTSAM_UNSTABLE_IMPORT_STATIC) else() set_target_properties(gtsam_unstable PROPERTIES - PREFIX "" - DEFINE_SYMBOL GTSAM_UNSTABLE_EXPORTS - RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin") + DEFINE_SYMBOL GTSAM_UNSTABLE_EXPORTS) endif() endif() diff --git a/gtsam_unstable/base/Dummy.h b/gtsam_unstable/base/Dummy.h index 548bce344..f26585ac1 100644 --- a/gtsam_unstable/base/Dummy.h +++ b/gtsam_unstable/base/Dummy.h @@ -29,6 +29,8 @@ namespace gtsam { size_t id; Dummy(); ~Dummy(); + Dummy(const Dummy& other) = default; + Dummy& operator=(const Dummy& other) = default; void print(const std::string& s="") const ; unsigned char dummyTwoVar(unsigned char a) const ; }; diff --git a/gtsam_unstable/base/FixedVector.h b/gtsam_unstable/base/FixedVector.h index b1fff90ef..ad6e4322d 100644 --- a/gtsam_unstable/base/FixedVector.h +++ b/gtsam_unstable/base/FixedVector.h @@ -37,6 +37,8 @@ public: /** copy constructors */ FixedVector(const FixedVector& v) : Base(v) {} + FixedVector& operator=(const FixedVector& other) = default; + /** Convert from a variable-size vector to a fixed size vector */ FixedVector(const Vector& v) : Base(v) {} diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index 2bd9e6dfd..585ca8103 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -26,7 +26,7 @@ void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const { } /* ************************************************************************* */ -double AllDiff::operator()(const DiscreteValues& values) const { +double AllDiff::evaluate(const Assignment& values) const { std::set taken; // record values taken by keys for (Key dkey : keys_) { size_t value = values.at(dkey); // get the value for that key diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index d7a63eae0..1180abad4 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -45,7 +45,7 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { } /// Calculate value = expensive ! - double operator()(const DiscreteValues& values) const override; + double evaluate(const Assignment& values) const override; /// Convert into a decisiontree, can be *very* expensive ! DecisionTreeFactor toDecisionTreeFactor() const override; diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index 18b335092..e96bfdfde 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -47,7 +47,7 @@ class BinaryAllDiff : public Constraint { } /// Calculate value - double operator()(const DiscreteValues& values) const override { + double evaluate(const Assignment& values) const override { return (double)(values.at(keys_[0]) != values.at(keys_[1])); } diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index 3526a282d..328fabbea 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -68,7 +68,8 @@ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor { /* * Ensure Arc-consistency by checking every possible value of domain j. * @param j domain to be checked - * @param (in/out) domains all domains, but only domains->at(j) will be checked. + * @param (in/out) domains all domains, but only domains->at(j) will be + * checked. * @return true if domains->at(j) was changed, false otherwise. */ virtual bool ensureArcConsistency(Key j, Domains* domains) const = 0; @@ -78,6 +79,39 @@ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor { /// Partially apply known values, domain version virtual shared_ptr partiallyApply(const Domains&) const = 0; + + /// Multiply factors, DiscreteFactor::shared_ptr edition + DiscreteFactor::shared_ptr multiply( + const DiscreteFactor::shared_ptr& df) const override { + return std::make_shared( + this->operator*(df->toDecisionTreeFactor())); + } + + /// divide by DiscreteFactor::shared_ptr f (safely) + DiscreteFactor::shared_ptr operator/( + const DiscreteFactor::shared_ptr& df) const override { + return this->toDecisionTreeFactor() / df; + } + + /// Get the number of non-zero values contained in this factor. + uint64_t nrValues() const override { return 1; }; + + DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override { + return toDecisionTreeFactor().sum(nrFrontals); + } + + DiscreteFactor::shared_ptr sum(const Ordering& keys) const override { + return toDecisionTreeFactor().sum(keys); + } + + DiscreteFactor::shared_ptr max(size_t nrFrontals) const override { + return toDecisionTreeFactor().max(nrFrontals); + } + + DiscreteFactor::shared_ptr max(const Ordering& keys) const override { + return toDecisionTreeFactor().max(keys); + } + /// @} /// @name Wrapper support /// @{ diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index bbbc87667..74f621dc7 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -30,7 +30,7 @@ string Domain::base1Str() const { } /* ************************************************************************* */ -double Domain::operator()(const DiscreteValues& values) const { +double Domain::evaluate(const Assignment& values) const { return contains(values.at(key())); } diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 7f7b717c2..6ce846201 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -49,7 +49,7 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { /// Erase a value, non const :-( void erase(size_t value) { values_.erase(value); } - size_t nrValues() const { return values_.size(); } + uint64_t nrValues() const override { return values_.size(); } bool isSingleton() const { return nrValues() == 1; } @@ -82,7 +82,7 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { bool contains(size_t value) const { return values_.count(value) > 0; } /// Calculate value - double operator()(const DiscreteValues& values) const override; + double evaluate(const Assignment& values) const override; /// Convert into a decisiontree DecisionTreeFactor toDecisionTreeFactor() const override; diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index 0de4d32c4..53be25d55 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -14,6 +14,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 6b78f38f5..220bc9c06 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -22,7 +22,7 @@ void SingleValue::print(const string& s, const KeyFormatter& formatter) const { } /* ************************************************************************* */ -double SingleValue::operator()(const DiscreteValues& values) const { +double SingleValue::evaluate(const Assignment& values) const { return (double)(values.at(keys_[0]) == value_); } diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 3f7f22d6a..3df1209b8 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -55,7 +55,7 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { } /// Calculate value - double operator()(const DiscreteValues& values) const override; + double evaluate(const Assignment& values) const override; /// Convert into a decisiontree DecisionTreeFactor toDecisionTreeFactor() const override; diff --git a/gtsam_unstable/discrete/tests/testCSP.cpp b/gtsam_unstable/discrete/tests/testCSP.cpp index 2b9a20ca6..6806bfe58 100644 --- a/gtsam_unstable/discrete/tests/testCSP.cpp +++ b/gtsam_unstable/discrete/tests/testCSP.cpp @@ -124,7 +124,7 @@ TEST(CSP, allInOne) { EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9); // Just for fun, create the product and check it - DecisionTreeFactor product = csp.product(); + DecisionTreeFactor product = csp.product()->toDecisionTreeFactor(); // product.dot("product"); DecisionTreeFactor expectedProduct(ID & AZ & UT, "0 1 0 0 0 0 1 0"); EXPECT(assert_equal(expectedProduct, product)); diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp index bd94da50d..731b8b3b3 100644 --- a/gtsam_unstable/discrete/tests/testScheduler.cpp +++ b/gtsam_unstable/discrete/tests/testScheduler.cpp @@ -113,7 +113,7 @@ TEST(schedulingExample, test) { EXPECT(assert_equal(expected, (DiscreteFactorGraph)s)); // Do brute force product and output that to file - DecisionTreeFactor product = s.product(); + DecisionTreeFactor product = s.product()->toDecisionTreeFactor(); // product.dot("scheduling", false); // Do exact inference @@ -140,7 +140,11 @@ TEST(schedulingExample, test) { /* ************************************************************************* */ TEST(schedulingExample, smallFromFile) { +<<<<<<< HEAD #ifndef __QNX__ +======= + #if !defined(__QNX__) +>>>>>>> 93f463ddbf8e990e6dccc622fcb8ecb67f21549a string path(TOPSRCDIR "/gtsam_unstable/discrete/examples/"); #else string path(""); //Same Directory diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h index d4ebcba19..2dc0d552b 100644 --- a/gtsam_unstable/dynamics/DynamicsPriors.h +++ b/gtsam_unstable/dynamics/DynamicsPriors.h @@ -12,6 +12,8 @@ #include #include +#include + namespace gtsam { // Indices of relevant variables in the PoseRTV tangent vector: diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 02da899b7..eea019cd2 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { /** diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index ee09600e5..241b6b497 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { /** diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index cf21c315b..9457c99e8 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -7,6 +7,8 @@ #include #include +#include + namespace gtsam { using namespace std; diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index 0e0d9ae27..95b7cfff3 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -157,7 +157,7 @@ public: /// @} private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 2bb70e1b5..dbdd27259 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { namespace dynamics { diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 9f2ec89d2..9751b30a7 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -51,7 +51,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp index 9dfed612c..23afd158b 100644 --- a/gtsam_unstable/geometry/BearingS2.cpp +++ b/gtsam_unstable/geometry/BearingS2.cpp @@ -9,6 +9,8 @@ #include +#include + namespace gtsam { using namespace std; diff --git a/gtsam_unstable/geometry/BearingS2.h b/gtsam_unstable/geometry/BearingS2.h index cb1d055df..0c3f40df8 100644 --- a/gtsam_unstable/geometry/BearingS2.h +++ b/gtsam_unstable/geometry/BearingS2.h @@ -90,7 +90,7 @@ private: /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION // +#if GTSAM_ENABLE_BOOST_SERIALIZATION // // Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index a3c907646..471ada35b 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -39,7 +39,7 @@ class GTSAM_UNSTABLE_EXPORT Event { Point3 location_; ///< Location at time event was generated public: - enum { dimension = 4 }; + inline constexpr static auto dimension = 4; /// Default Constructor Event() : time_(0), location_(0, 0, 0) {} diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 45b27efe3..d1486209e 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -19,7 +19,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -175,7 +175,7 @@ private: /// @name Advanced Interface /// @{ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index 9cd9f78f5..c5ef0d70f 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -6,8 +6,9 @@ */ #include -#include "gtsam/base/OptionalJacobian.h" -#include "gtsam/base/Vector.h" +#include +#include +#include #include diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index a77715fc6..464f46928 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -43,6 +43,7 @@ public: Pose3Upright(const Rot2& bearing, const Point3& t); Pose3Upright(double x, double y, double z, double theta); Pose3Upright(const Pose2& pose, double z); + Pose3Upright& operator=(const Pose3Upright& x) = default; /// Down-converts from a full Pose3 Pose3Upright(const Pose3& fullpose); @@ -130,7 +131,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION // +#if GTSAM_ENABLE_BOOST_SERIALIZATION // // Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h new file mode 100644 index 000000000..1e367e55c --- /dev/null +++ b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h @@ -0,0 +1,358 @@ +/* ---------------------------------------------------------------------------- + + * 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 BayesTreeMarginalizationHelper.h + * @brief Helper functions for marginalizing variables from a Bayes Tree. + * + * @author Jeffrey (Zhiwei Wang) + * @date Oct 28, 2024 + */ + +// \callgraph +#pragma once + +#include +#include +#include +#include +#include +#include +#include "gtsam_unstable/dllexport.h" + +namespace gtsam { + +/** + * This class provides helper functions for marginalizing variables from a Bayes Tree. + */ +template +class GTSAM_UNSTABLE_EXPORT BayesTreeMarginalizationHelper { + +public: + using Clique = typename BayesTree::Clique; + using sharedClique = typename BayesTree::sharedClique; + + /** + * This function identifies variables that need to be re-eliminated before + * performing marginalization. + * + * Re-elimination is necessary for a clique containing marginalizable + * variables if: + * + * 1. Some non-marginalizable variables appear before marginalizable ones + * in that clique; + * 2. Or it has a child node depending on a marginalizable variable AND the + * subtree rooted at that child contains non-marginalizables. + * + * In addition, for any descendant node depending on a marginalizable + * variable, if the subtree rooted at that descendant contains + * non-marginalizable variables (i.e., it lies on a path from one of the + * aforementioned cliques that require re-elimination to a node containing + * non-marginalizable variables at the leaf side), then it also needs to + * be re-eliminated. + * + * @param[in] bayesTree The Bayes tree + * @param[in] marginalizableKeys Keys to be marginalized + * @return Set of additional keys that need to be re-eliminated + */ + static std::unordered_set + gatherAdditionalKeysToReEliminate( + const BayesTree& bayesTree, + const KeyVector& marginalizableKeys) { + const bool debug = ISDEBUG("BayesTreeMarginalizationHelper"); + + std::unordered_set additionalCliques = + gatherAdditionalCliquesToReEliminate(bayesTree, marginalizableKeys); + + std::unordered_set additionalKeys; + for (const Clique* clique : additionalCliques) { + addCliqueToKeySet(clique, &additionalKeys); + } + + if (debug) { + std::cout << "BayesTreeMarginalizationHelper: Additional keys to re-eliminate: "; + for (const Key& key : additionalKeys) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << std::endl; + } + + return additionalKeys; + } + + protected: + /** + * This function identifies cliques that need to be re-eliminated before + * performing marginalization. + * See the docstring of @ref gatherAdditionalKeysToReEliminate(). + */ + static std::unordered_set + gatherAdditionalCliquesToReEliminate( + const BayesTree& bayesTree, + const KeyVector& marginalizableKeys) { + std::unordered_set additionalCliques; + std::unordered_set marginalizableKeySet( + marginalizableKeys.begin(), marginalizableKeys.end()); + CachedSearch cachedSearch; + + // Check each clique that contains a marginalizable key + for (const Clique* clique : + getCliquesContainingKeys(bayesTree, marginalizableKeySet)) { + if (additionalCliques.count(clique)) { + // The clique has already been visited. This can happen when an + // ancestor of the current clique also contain some marginalizable + // varaibles and it's processed beore the current. + continue; + } + + if (needsReelimination(clique, marginalizableKeySet, &cachedSearch)) { + // Add the current clique + additionalCliques.insert(clique); + + // Then add the dependent cliques + gatherDependentCliques(clique, marginalizableKeySet, &additionalCliques, + &cachedSearch); + } + } + return additionalCliques; + } + + /** + * Gather the cliques containing any of the given keys. + * + * @param[in] bayesTree The Bayes tree + * @param[in] keysOfInterest Set of keys of interest + * @return Set of cliques that contain any of the given keys + */ + static std::unordered_set getCliquesContainingKeys( + const BayesTree& bayesTree, + const std::unordered_set& keysOfInterest) { + std::unordered_set cliques; + for (const Key& key : keysOfInterest) { + cliques.insert(bayesTree[key].get()); + } + return cliques; + } + + /** + * A struct to cache the results of the below two functions. + */ + struct CachedSearch { + std::unordered_map wholeMarginalizableCliques; + std::unordered_map wholeMarginalizableSubtrees; + }; + + /** + * Check if all variables in the clique are marginalizable. + * + * Note we use a cache map to avoid repeated searches. + */ + static bool isWholeCliqueMarginalizable( + const Clique* clique, + const std::unordered_set& marginalizableKeys, + CachedSearch* cache) { + auto it = cache->wholeMarginalizableCliques.find(clique); + if (it != cache->wholeMarginalizableCliques.end()) { + return it->second; + } else { + bool ret = true; + for (Key key : clique->conditional()->frontals()) { + if (!marginalizableKeys.count(key)) { + ret = false; + break; + } + } + cache->wholeMarginalizableCliques.insert({clique, ret}); + return ret; + } + } + + /** + * Check if all variables in the subtree are marginalizable. + * + * Note we use a cache map to avoid repeated searches. + */ + static bool isWholeSubtreeMarginalizable( + const Clique* subtree, + const std::unordered_set& marginalizableKeys, + CachedSearch* cache) { + auto it = cache->wholeMarginalizableSubtrees.find(subtree); + if (it != cache->wholeMarginalizableSubtrees.end()) { + return it->second; + } else { + bool ret = true; + if (isWholeCliqueMarginalizable(subtree, marginalizableKeys, cache)) { + for (const sharedClique& child : subtree->children) { + if (!isWholeSubtreeMarginalizable(child.get(), marginalizableKeys, cache)) { + ret = false; + break; + } + } + } else { + ret = false; + } + cache->wholeMarginalizableSubtrees.insert({subtree, ret}); + return ret; + } + } + + /** + * Check if a clique contains variables that need reelimination due to + * elimination ordering conflicts. + * + * @param[in] clique The clique to check + * @param[in] marginalizableKeys Set of keys to be marginalized + * @return true if any variables in the clique need re-elimination + */ + static bool needsReelimination( + const Clique* clique, + const std::unordered_set& marginalizableKeys, + CachedSearch* cache) { + bool hasNonMarginalizableAhead = false; + + // Check each frontal variable in order + for (Key key : clique->conditional()->frontals()) { + if (marginalizableKeys.count(key)) { + // If we've seen non-marginalizable variables before this one, + // we need to reeliminate + if (hasNonMarginalizableAhead) { + return true; + } + + // Check if any child depends on this marginalizable key and the + // subtree rooted at that child contains non-marginalizables. + for (const sharedClique& child : clique->children) { + if (hasDependency(child.get(), key) && + !isWholeSubtreeMarginalizable(child.get(), marginalizableKeys, cache)) { + return true; + } + } + } else { + hasNonMarginalizableAhead = true; + } + } + return false; + } + + /** + * Gather all dependent nodes that lie on a path from the root clique + * to a clique containing a non-marginalizable variable at the leaf side. + * + * @param[in] rootClique The root clique + * @param[in] marginalizableKeys Set of keys to be marginalized + */ + static void gatherDependentCliques( + const Clique* rootClique, + const std::unordered_set& marginalizableKeys, + std::unordered_set* additionalCliques, + CachedSearch* cache) { + std::vector dependentChildren; + dependentChildren.reserve(rootClique->children.size()); + for (const sharedClique& child : rootClique->children) { + if (additionalCliques->count(child.get())) { + // This child has already been visited. This can happen if the + // child itself contains a marginalizable variable and it's + // processed before the current rootClique. + continue; + } + if (hasDependency(child.get(), marginalizableKeys)) { + dependentChildren.push_back(child.get()); + } + } + gatherDependentCliquesFromChildren( + dependentChildren, marginalizableKeys, additionalCliques, cache); + } + + /** + * A helper function for the above gatherDependentCliques(). + */ + static void gatherDependentCliquesFromChildren( + const std::vector& dependentChildren, + const std::unordered_set& marginalizableKeys, + std::unordered_set* additionalCliques, + CachedSearch* cache) { + std::deque descendants( + dependentChildren.begin(), dependentChildren.end()); + while (!descendants.empty()) { + const Clique* descendant = descendants.front(); + descendants.pop_front(); + + // If the subtree rooted at this descendant contains non-marginalizables, + // it must lie on a path from the root clique to a clique containing + // non-marginalizables at the leaf side. + if (!isWholeSubtreeMarginalizable(descendant, marginalizableKeys, cache)) { + additionalCliques->insert(descendant); + + // Add children of the current descendant to the set descendants. + for (const sharedClique& child : descendant->children) { + if (additionalCliques->count(child.get())) { + // This child has already been visited. + continue; + } else { + descendants.push_back(child.get()); + } + } + } + } + } + + /** + * Add all frontal variables from a clique to a key set. + * + * @param[in] clique Clique to add keys from + * @param[out] additionalKeys Pointer to the output key set + */ + static void addCliqueToKeySet( + const Clique* clique, + std::unordered_set* additionalKeys) { + for (Key key : clique->conditional()->frontals()) { + additionalKeys->insert(key); + } + } + + /** + * Check if the clique depends on the given key. + * + * @param[in] clique Clique to check + * @param[in] key Key to check for dependencies + * @return true if clique depends on the key + */ + static bool hasDependency( + const Clique* clique, Key key) { + auto& conditional = clique->conditional(); + if (std::find(conditional->beginParents(), + conditional->endParents(), key) + != conditional->endParents()) { + return true; + } else { + return false; + } + } + + /** + * Check if the clique depends on any of the given keys. + */ + static bool hasDependency( + const Clique* clique, const std::unordered_set& keys) { + auto& conditional = clique->conditional(); + for (auto it = conditional->beginParents(); + it != conditional->endParents(); ++it) { + if (keys.count(*it)) { + return true; + } + } + + return false; + } +}; +// BayesTreeMarginalizationHelper + +}/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 36cb6165f..8dfb15aa2 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -21,6 +21,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 52e56260d..0cd9ecbac 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -20,32 +20,11 @@ */ #include +#include #include namespace gtsam { -/* ************************************************************************* */ -void recursiveMarkAffectedKeys(const Key& key, - const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { - - // Check if the separator keys of the current clique contain the specified key - if (std::find(clique->conditional()->beginParents(), - clique->conditional()->endParents(), key) - != clique->conditional()->endParents()) { - - // Mark the frontal keys of the current clique - for(Key i: clique->conditional()->frontals()) { - additionalKeys.insert(i); - } - - // Recursively mark all of the children - for(const ISAM2Clique::shared_ptr& child: clique->children) { - recursiveMarkAffectedKeys(key, child, additionalKeys); - } - } - // If the key was not found in the separator/parents, then none of its children can have it either -} - /* ************************************************************************* */ void IncrementalFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { @@ -114,14 +93,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( std::cout << std::endl; } - // Mark additional keys between the marginalized keys and the leaves - std::set additionalKeys; - for(Key key: marginalizableKeys) { - ISAM2Clique::shared_ptr clique = isam_[key]; - for(const ISAM2Clique::shared_ptr& child: clique->children) { - recursiveMarkAffectedKeys(key, child, additionalKeys); - } - } + std::unordered_set additionalKeys = + BayesTreeMarginalizationHelper::gatherAdditionalKeysToReEliminate( + isam_, marginalizableKeys); KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); // Update iSAM2 diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index 2d404005d..375c49341 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -17,6 +17,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 1ff45ef5f..7aa596be1 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -59,7 +59,7 @@ public: const Values& linearizationPoint() const { return lin_points_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -148,7 +148,7 @@ public: Vector error_vector(const Values& c) const; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; /** Serialization function */ template @@ -279,7 +279,7 @@ public: private: /** Serialization function */ -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 3454c352a..cd2ba593b 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -49,6 +49,41 @@ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullini return assert_equal(expected, actual); } +/* ************************************************************************* */ +void PrintSymbolicTreeHelper( + const ISAM2Clique::shared_ptr& clique, const std::string indent = "") { + + // Print the current clique + std::cout << indent << "P( "; + for(Key key: clique->conditional()->frontals()) { + std::cout << DefaultKeyFormatter(key) << " "; + } + if (clique->conditional()->nrParents() > 0) + std::cout << "| "; + for(Key key: clique->conditional()->parents()) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << ")" << std::endl; + + // Recursively print all of the children + for(const ISAM2Clique::shared_ptr& child: clique->children) { + PrintSymbolicTreeHelper(child, indent + " "); + } +} + +/* ************************************************************************* */ +void PrintSymbolicTree(const ISAM2& isam, + const std::string& label) { + std::cout << label << std::endl; + if (!isam.roots().empty()) { + for(const ISAM2::sharedClique& root: isam.roots()) { + PrintSymbolicTreeHelper(root); + } + } else + std::cout << "{Empty Tree}" << std::endl; +} + + /* ************************************************************************* */ TEST( IncrementalFixedLagSmoother, Example ) { @@ -64,7 +99,7 @@ TEST( IncrementalFixedLagSmoother, Example ) // Create a Fixed-Lag Smoother typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps; - IncrementalFixedLagSmoother smoother(7.0, ISAM2Params()); + IncrementalFixedLagSmoother smoother(12.0, ISAM2Params()); // Create containers to keep the full graph Values fullinit; @@ -158,6 +193,9 @@ TEST( IncrementalFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; + // Add the odometry factor twice to ensure the removeFactor test below works, + // where we need to keep the connectivity of the graph. + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newTimestamps[key2] = double(i); @@ -188,6 +226,7 @@ TEST( IncrementalFixedLagSmoother, Example ) newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newTimestamps[key2] = double(i); + ++i; fullgraph.push_back(newFactors); fullinit.insert(newValues); @@ -210,6 +249,10 @@ TEST( IncrementalFixedLagSmoother, Example ) const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors(); + std::cout << "fullgraph.size() = " << fullgraph.size() << std::endl; + std::cout << "smootherFactorsBeforeRemove.size() = " + << smootherFactorsBeforeRemove.size() << std::endl; + // remove factor smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove); @@ -231,6 +274,67 @@ TEST( IncrementalFixedLagSmoother, Example ) } } } + + { + SETDEBUG("BayesTreeMarginalizationHelper", true); + PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree Before marginalization test:"); + + // Do pressure test on marginalization. Enlarge max_i to enhance the test. + const int max_i = 500; + while(i <= max_i) { + Key key_0 = MakeKey(i); + Key key_1 = MakeKey(i-1); + Key key_2 = MakeKey(i-2); + Key key_3 = MakeKey(i-3); + Key key_4 = MakeKey(i-4); + Key key_5 = MakeKey(i-5); + Key key_6 = MakeKey(i-6); + Key key_7 = MakeKey(i-7); + Key key_8 = MakeKey(i-8); + Key key_9 = MakeKey(i-9); + Key key_10 = MakeKey(i-10); + + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; + + // To make a complex graph + newFactors.push_back(BetweenFactor(key_1, key_0, Point2(1.0, 0.0), odometerNoise)); + if (i % 2 == 0) + newFactors.push_back(BetweenFactor(key_2, key_1, Point2(1.0, 0.0), odometerNoise)); + if (i % 3 == 0) + newFactors.push_back(BetweenFactor(key_3, key_2, Point2(1.0, 0.0), odometerNoise)); + if (i % 4 == 0) + newFactors.push_back(BetweenFactor(key_4, key_3, Point2(1.0, 0.0), odometerNoise)); + if (i % 5 == 0) + newFactors.push_back(BetweenFactor(key_5, key_4, Point2(1.0, 0.0), odometerNoise)); + if (i % 6 == 0) + newFactors.push_back(BetweenFactor(key_6, key_5, Point2(1.0, 0.0), odometerNoise)); + if (i % 7 == 0) + newFactors.push_back(BetweenFactor(key_7, key_6, Point2(1.0, 0.0), odometerNoise)); + if (i % 8 == 0) + newFactors.push_back(BetweenFactor(key_8, key_7, Point2(1.0, 0.0), odometerNoise)); + if (i % 9 == 0) + newFactors.push_back(BetweenFactor(key_9, key_8, Point2(1.0, 0.0), odometerNoise)); + if (i % 10 == 0) + newFactors.push_back(BetweenFactor(key_10, key_9, Point2(1.0, 0.0), odometerNoise)); + + newValues.insert(key_0, Point2(double(i)+0.1, -0.1)); + newTimestamps[key_0] = double(i); + + fullgraph.push_back(newFactors); + fullinit.insert(newValues); + + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); + + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key_0)); + PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree marginalization test: i = " + std::to_string(i)); + + ++i; + } + } } /* ************************************************************************* */ diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index f4718f7a9..21ed91604 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp index ee93977a0..7575e6154 100644 --- a/gtsam_unstable/partition/tests/testFindSeparator.cpp +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -105,11 +105,19 @@ TEST ( Partition, edgePartitionByMetis2 ) graph.push_back(std::make_shared(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D, 20)); graph.push_back(std::make_shared(3, 4, 3, NODE_POSE_3D, NODE_POSE_3D, 1)); //QNX Testing: fix tiebreaker to match +<<<<<<< HEAD #ifndef __QNX__ std::vector keys{0, 1, 2, 3, 4}; #else //Anything where 2 is before 0 will work. std::vector keys{2, 0, 1, 3, 4}; +======= + #if !defined(__QNX__) + std::vector keys{0, 1, 2, 3, 4}; + #else + //Anything where 2 is before 0 will work. + std::vector keys{2, 0, 3, 1, 4}; +>>>>>>> 93f463ddbf8e990e6dccc622fcb8ecb67f21549a #endif WorkSpace workspace(6); diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index f1337c0e8..6cdce415e 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -415,7 +415,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index f2b669f4f..1c278b111 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -94,7 +94,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 0accb8f42..b759ceb56 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -704,7 +704,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index a94c95335..56f2e2cee 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -573,7 +573,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index b91ed2afe..c9c4141b1 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -113,7 +113,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 8fda5456b..95fde2f75 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -411,7 +411,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 64a1366a2..f47c56bc9 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -113,7 +113,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index a49b2090c..638b77440 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -133,7 +133,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 7abbbe89a..01f087df0 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -140,7 +140,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 8664002e8..f71df8425 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -137,7 +137,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -269,7 +269,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; /// Serialization function template diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 4714f2b6f..b23a4c3ee 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -35,9 +35,9 @@ public: } /// Copy constructor - Mechanization_bRn2(const Mechanization_bRn2& other) : - bRn_(other.bRn_), x_g_(other.x_g_), x_a_(other.x_a_) { - } + Mechanization_bRn2(const Mechanization_bRn2& other) = default; + + Mechanization_bRn2& operator=(const Mechanization_bRn2& other) = default; /// gravity in the body frame Vector3 b_g(double g_e) const { diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 8e2ea0cf7..28e9c0215 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -213,7 +213,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 7722e5d82..645d0305a 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -20,6 +20,8 @@ #include #include +#include + namespace gtsam { /** @@ -140,7 +142,7 @@ namespace gtsam { const std::vector& indices() const { return indices_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index aa383967d..1e0526e22 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -119,7 +119,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index 4a201ad89..60df7e948 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -101,7 +101,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index be454cb70..911445fa5 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -92,7 +92,7 @@ class PoseToPointFactor : public NoiseModelFactorN { const POINT& measured() const { return measured_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 38467c6cc..126900bc2 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -171,7 +171,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index df63330df..cea74842f 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -151,7 +151,7 @@ class ProjectionFactorPPPC private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index c5eb58f14..778713733 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -194,7 +194,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter inline bool throwCheirality() const { return throwCheirality_; } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h index d24415c3b..13bd5eba0 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.h +++ b/gtsam_unstable/slam/RelativeElevationFactor.h @@ -65,7 +65,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 4a2047ee1..f28ce642b 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -453,7 +453,7 @@ class SmartProjectionPoseFactorRollingShutter } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 52d6eda05..7b06e8827 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -30,7 +30,7 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif @@ -504,7 +504,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp index c4a473154..6d78bdb7a 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp @@ -18,6 +18,8 @@ #include +#include + namespace gtsam { SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP( diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 189c501bb..7482682fa 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -285,7 +285,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp index 57913385a..52129daa8 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp @@ -21,6 +21,8 @@ #include +#include + namespace gtsam { SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor( diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 77a15c246..93d8877a7 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -138,7 +138,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor Base::Cameras cameras(const Values& values) const override; private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 2a257ff57..cbc8eba52 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -216,7 +216,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index c47cf3b7d..352a9ff37 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -414,7 +414,7 @@ namespace gtsam { private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index 1eb43cec8..7a9f77d09 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -32,7 +32,7 @@ from gtsam import ( K = gtsam.symbol_shorthand.K # Methods to compare -methods = ["SimpleF", "Fundamental", "Essential+Ks", "Calibrated"] +methods = ["SimpleF", "Fundamental", "Essential+Ks", "Essential+K", "Calibrated", "Binary+Ks", "Binary+K"] # Formatter function for printing keys @@ -76,8 +76,8 @@ def simulate_data(points, poses, cal, rng, noise_std): return measurements -# Function to compute ground truth matrices def compute_ground_truth(method, poses, cal): + """Function to compute ground truth edge variables.""" E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1])) E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2])) F1 = FundamentalMatrix(cal.K(), E1, cal.K()) @@ -90,58 +90,80 @@ def compute_ground_truth(method, poses, cal): SF1 = SimpleFundamentalMatrix(E1, f, f, c, c) SF2 = SimpleFundamentalMatrix(E2, f, f, c, c) return SF1, SF2 - elif method == "Essential+Ks" or method == "Calibrated": - return E1, E2 else: - raise ValueError(f"Unknown method {method}") + return E1, E2 def build_factor_graph(method, num_cameras, measurements, cal): """build the factor graph""" graph = NonlinearFactorGraph() + # Determine the FactorClass based on the method if method == "Fundamental": FactorClass = gtsam.TransferFactorFundamentalMatrix elif method == "SimpleF": FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix - elif method == "Essential+Ks": + elif method in ["Essential+Ks", "Essential+K"]: FactorClass = gtsam.EssentialTransferFactorKCal3f - # add priors on all calibrations: - for i in range(num_cameras): - model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) - graph.addPriorCal3f(K(i), cal, model) + elif method == "Binary+K": + FactorClass = gtsam.EssentialMatrixFactor4Cal3f + elif method == "Binary+Ks": + FactorClass = gtsam.EssentialMatrixFactor5Cal3f elif method == "Calibrated": FactorClass = gtsam.EssentialTransferFactorCal3f - # No priors on calibration needed else: raise ValueError(f"Unknown method {method}") + # Add priors on calibrations if necessary + if method in ["Essential+Ks", "Binary+Ks"]: + for i in range(num_cameras): + model = gtsam.noiseModel.Isotropic.Sigma(1, 1000.0) + graph.addPriorCal3f(K(i), cal, model) + elif method in ["Essential+K", "Binary+K"]: + model = gtsam.noiseModel.Isotropic.Sigma(1, 1000.0) + graph.addPriorCal3f(K(0), cal, model) + z = measurements # shorthand for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera c = (a + 2) % num_cameras # Camera after next - - # Vectors to collect tuples for each factor - tuples1 = [] - tuples2 = [] - tuples3 = [] - - # Collect data for the three factors - for j in range(len(measurements[0])): - tuples1.append((z[a][j], z[b][j], z[c][j])) - tuples2.append((z[a][j], z[c][j], z[b][j])) - tuples3.append((z[c][j], z[b][j], z[a][j])) - - # Add transfer factors between views a, b, and c. - if method in ["Calibrated"]: - graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal)) - graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal)) - graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal)) + if method in ["Binary+Ks", "Binary+K"]: + # Add binary Essential Matrix factors + ab, ac = EdgeKey(a, b).key(), EdgeKey(a, c).key() + for j in range(len(measurements[0])): + if method == "Binary+Ks": + graph.add(FactorClass(ab, K(a), K(b), z[a][j], z[b][j])) + graph.add(FactorClass(ac, K(a), K(c), z[a][j], z[c][j])) + else: # Binary+K + graph.add(FactorClass(ab, K(0), z[a][j], z[b][j])) + graph.add(FactorClass(ac, K(0), z[a][j], z[c][j])) else: - graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1)) - graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2)) - graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3)) + # Add transfer factors between views a, b, and c + + # Vectors to collect tuples for each factor + tuples1 = [] + tuples2 = [] + tuples3 = [] + + for j in range(len(measurements[0])): + tuples1.append((z[a][j], z[b][j], z[c][j])) + tuples2.append((z[a][j], z[c][j], z[b][j])) + tuples3.append((z[c][j], z[b][j], z[a][j])) + + # Add transfer factors between views a, b, and c. + if method in ["Calibrated"]: + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal)) + graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal)) + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal)) + elif method == "Essential+K": + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), K(0), tuples1)) + graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), K(0), tuples2)) + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), K(0), tuples3)) + else: + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1)) + graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2)) + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3)) return graph @@ -159,22 +181,25 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): initialEstimate.insert(EdgeKey(a, b).key(), F1) initialEstimate.insert(EdgeKey(a, c).key(), F2) total_dimension += F1.dim() + F2.dim() - elif method in ["Essential+Ks", "Calibrated"]: + elif method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "Calibrated"]: E1, E2 = ground_truth for a in range(num_cameras): - b = (a + 1) % num_cameras # Next camera - c = (a + 2) % num_cameras # Camera after next + b = (a + 1) % num_cameras + c = (a + 2) % num_cameras + # initialEstimate.insert(EdgeKey(a, b).key(), E1.retract(0.1 * np.ones((5, 1)))) + # initialEstimate.insert(EdgeKey(a, c).key(), E2.retract(0.1 * np.ones((5, 1)))) initialEstimate.insert(EdgeKey(a, b).key(), E1) initialEstimate.insert(EdgeKey(a, c).key(), E2) total_dimension += E1.dim() + E2.dim() - else: - raise ValueError(f"Unknown method {method}") - if method == "Essential+Ks": - # Insert initial calibrations + # Insert initial calibrations + if method in ["Essential+Ks", "Binary+Ks"]: for i in range(num_cameras): initialEstimate.insert(K(i), cal) total_dimension += cal.dim() + elif method in ["Essential+K", "Binary+K"]: + initialEstimate.insert(K(0), cal) + total_dimension += cal.dim() print(f"Total dimension of the problem: {total_dimension}") return initialEstimate @@ -183,8 +208,9 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): def optimize(graph, initialEstimate, method): """optimize the graph""" params = LevenbergMarquardtParams() - params.setlambdaInitial(1e10) # Initialize lambda to a high value - params.setlambdaUpperBound(1e10) + if method not in ["Calibrated", "Binary+K", "Binary+Ks"]: + params.setlambdaInitial(1e10) # Initialize lambda to a high value + params.setlambdaUpperBound(1e10) # params.setAbsoluteErrorTol(0.1) params.setVerbosityLM("SUMMARY") optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params) @@ -205,7 +231,7 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): key_ab = EdgeKey(a, b).key() key_ac = EdgeKey(a, c).key() - if method in ["Essential+Ks", "Calibrated"]: + if method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "Calibrated"]: E_est_ab = result.atEssentialMatrix(key_ab) E_est_ac = result.atEssentialMatrix(key_ac) @@ -218,15 +244,18 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): SF_est_ac = result.atSimpleFundamentalMatrix(key_ac).matrix() F_est_ab = FundamentalMatrix(SF_est_ab) F_est_ac = FundamentalMatrix(SF_est_ac) - elif method == "Essential+Ks": - # Retrieve calibrations from result: + elif method in ["Essential+Ks", "Binary+Ks"]: + # Retrieve calibrations from result for each camera cal_a = result.atCal3f(K(a)) cal_b = result.atCal3f(K(b)) cal_c = result.atCal3f(K(c)) - - # Convert estimated EssentialMatrices to FundamentalMatrices F_est_ab = FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K()) F_est_ac = FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K()) + elif method in ["Essential+K", "Binary+K"]: + # Use single shared calibration + cal_shared = result.atCal3f(K(0)) + F_est_ab = FundamentalMatrix(cal_shared.K(), E_est_ab, cal_shared.K()) + F_est_ac = FundamentalMatrix(cal_shared.K(), E_est_ac, cal_shared.K()) elif method == "Calibrated": # Use ground truth calibration F_est_ab = FundamentalMatrix(cal.K(), E_est_ab, cal.K()) @@ -347,6 +376,8 @@ def main(): # Compute final error final_error = graph.error(result) + if method in ["Binary+K", "Binary+Ks"]: + final_error *= cal.f() * cal.f() # Store results results[method]["distances"].extend(distances) diff --git a/python/gtsam/tests/test_DecisionTreeFactor.py b/python/gtsam/tests/test_DecisionTreeFactor.py index 12308bb3c..a78d9c94a 100644 --- a/python/gtsam/tests/test_DecisionTreeFactor.py +++ b/python/gtsam/tests/test_DecisionTreeFactor.py @@ -13,9 +13,10 @@ Author: Frank Dellaert import unittest +from gtsam.utils.test_case import GtsamTestCase + from gtsam import (DecisionTreeFactor, DiscreteDistribution, DiscreteValues, Ordering) -from gtsam.utils.test_case import GtsamTestCase class TestDecisionTreeFactor(GtsamTestCase): diff --git a/python/gtsam/tests/test_DiscreteBayesTree.py b/python/gtsam/tests/test_DiscreteBayesTree.py index 2a9b6ea09..e08491fab 100644 --- a/python/gtsam/tests/test_DiscreteBayesTree.py +++ b/python/gtsam/tests/test_DiscreteBayesTree.py @@ -19,8 +19,8 @@ from gtsam.utils.test_case import GtsamTestCase import gtsam from gtsam import (DiscreteBayesNet, DiscreteBayesTreeClique, - DiscreteConditional, DiscreteFactorGraph, - DiscreteValues, Ordering) + DiscreteConditional, DiscreteFactorGraph, DiscreteValues, + Ordering) class TestDiscreteBayesNet(GtsamTestCase): diff --git a/python/gtsam/tests/test_DiscreteConditional.py b/python/gtsam/tests/test_DiscreteConditional.py index 241a5f0be..6c9eb9aec 100644 --- a/python/gtsam/tests/test_DiscreteConditional.py +++ b/python/gtsam/tests/test_DiscreteConditional.py @@ -13,9 +13,10 @@ Author: Varun Agrawal import unittest -from gtsam import DecisionTreeFactor, DiscreteConditional, DiscreteKeys from gtsam.utils.test_case import GtsamTestCase +from gtsam import DecisionTreeFactor, DiscreteConditional, DiscreteKeys + # Some DiscreteKeys for binary variables: A = 0, 2 B = 1, 2 diff --git a/python/gtsam/tests/test_DiscreteFactorGraph.py b/python/gtsam/tests/test_DiscreteFactorGraph.py index d725ceac8..3053087b4 100644 --- a/python/gtsam/tests/test_DiscreteFactorGraph.py +++ b/python/gtsam/tests/test_DiscreteFactorGraph.py @@ -14,9 +14,12 @@ Author: Frank Dellaert import unittest import numpy as np -from gtsam import DecisionTreeFactor, DiscreteConditional, DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering, Symbol from gtsam.utils.test_case import GtsamTestCase +from gtsam import (DecisionTreeFactor, DiscreteConditional, + DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering, + Symbol) + OrderingType = Ordering.OrderingType diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 6d609deb0..6edab3449 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -13,14 +13,14 @@ Author: Fan Jiang, Varun Agrawal, Frank Dellaert import unittest import numpy as np -from gtsam.symbol_shorthand import C, M, X, Z -from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam import (DiscreteConditional, GaussianConditional, - HybridBayesNet, HybridGaussianConditional, - HybridGaussianFactor, HybridGaussianFactorGraph, - HybridValues, JacobianFactor, noiseModel) +from gtsam import (DiscreteConditional, GaussianConditional, HybridBayesNet, + HybridGaussianConditional, HybridGaussianFactor, + HybridGaussianFactorGraph, HybridValues, JacobianFactor, + TableDistribution, noiseModel) +from gtsam.symbol_shorthand import C, M, X, Z +from gtsam.utils.test_case import GtsamTestCase DEBUG_MARGINALS = False @@ -51,7 +51,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): self.assertEqual(len(hybridCond.keys()), 2) discrete_conditional = hbn.at(hbn.size() - 1).inner() - self.assertIsInstance(discrete_conditional, DiscreteConditional) + self.assertIsInstance(discrete_conditional, TableDistribution) def test_optimize(self): """Test construction of hybrid factor graph.""" diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py new file mode 100644 index 000000000..4f5c57b0a --- /dev/null +++ b/python/gtsam/tests/test_Serialization.py @@ -0,0 +1,64 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Serialization and deep copy tests. + +Author: Varun Agrawal +""" +import unittest + +import numpy as np +from gtsam.symbol_shorthand import B, V, X +from gtsam.utils.test_case import GtsamTestCase + +import gtsam + + +class TestDeepCopy(GtsamTestCase): + """Tests for deep copy of various GTSAM objects.""" + + def test_PreintegratedImuMeasurements(self): + """ + Test the deep copy of `PreintegratedImuMeasurements`. + """ + params = gtsam.PreintegrationParams.MakeSharedD(9.81) + pim = gtsam.PreintegratedImuMeasurements(params) + + self.assertDeepCopyEquality(pim) + + def test_ImuFactor(self): + """ + Test the deep copy of `ImuFactor`. + """ + params = gtsam.PreintegrationParams.MakeSharedD(9.81) + params.setAccelerometerCovariance(1e-7 * np.eye(3)) + params.setGyroscopeCovariance(1e-8 * np.eye(3)) + params.setIntegrationCovariance(1e-9 * np.eye(3)) + priorBias = gtsam.imuBias.ConstantBias(np.zeros(3), np.zeros(3)) + pim = gtsam.PreintegratedImuMeasurements(params, priorBias) + + # Preintegrate a single measurement for serialization to work. + pim.integrateMeasurement(measuredAcc=np.zeros(3), + measuredOmega=np.zeros(3), + deltaT=0.005) + + factor = gtsam.ImuFactor(0, 1, 2, 3, 4, pim) + + self.assertDeepCopyEquality(factor) + + def test_PreintegratedCombinedMeasurements(self): + """ + Test the deep copy of `PreintegratedCombinedMeasurements`. + """ + params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81])) + pim = gtsam.PreintegratedCombinedMeasurements(params) + + self.assertDeepCopyEquality(pim) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam/tests/test_pickle.py b/python/gtsam/tests/test_pickle.py index a6a5745bc..e51617b00 100644 --- a/python/gtsam/tests/test_pickle.py +++ b/python/gtsam/tests/test_pickle.py @@ -9,24 +9,26 @@ Unit tests to check pickling. Author: Ayush Baid """ -from gtsam import Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3, SfmTrack, Unit3 - from gtsam.utils.test_case import GtsamTestCase +from gtsam import (Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3, + Pose3, Rot3, SfmTrack, Unit3) + + class TestPickle(GtsamTestCase): """Tests pickling on some of the classes.""" def test_cal3Bundler_roundtrip(self): obj = Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70) self.assertEqualityOnPickleRoundtrip(obj) - + def test_pinholeCameraCal3Bundler_roundtrip(self): obj = PinholeCameraCal3Bundler( Pose3(Rot3.RzRyRx(0, 0.1, -0.05), Point3(1, 1, 0)), Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70), ) self.assertEqualityOnPickleRoundtrip(obj) - + def test_rot3_roundtrip(self): obj = Rot3.RzRyRx(0, 0.05, 0.1) self.assertEqualityOnPickleRoundtrip(obj) diff --git a/python/gtsam/utils/test_case.py b/python/gtsam/utils/test_case.py index 50af004f4..74eaff1db 100644 --- a/python/gtsam/utils/test_case.py +++ b/python/gtsam/utils/test_case.py @@ -10,6 +10,7 @@ Author: Frank Dellaert """ import pickle import unittest +from copy import deepcopy class GtsamTestCase(unittest.TestCase): @@ -28,8 +29,8 @@ class GtsamTestCase(unittest.TestCase): else: equal = actual.equals(expected, tol) if not equal: - raise self.failureException( - "Values are not equal:\n{}!={}".format(actual, expected)) + raise self.failureException("Values are not equal:\n{}!={}".format( + actual, expected)) def assertEqualityOnPickleRoundtrip(self, obj: object, tol=1e-9) -> None: """ Performs a round-trip using pickle and asserts equality. @@ -41,3 +42,14 @@ class GtsamTestCase(unittest.TestCase): """ roundTripObj = pickle.loads(pickle.dumps(obj)) self.gtsamAssertEquals(roundTripObj, obj) + + def assertDeepCopyEquality(self, obj): + """Perform assertion by checking if a + deep copied version of `obj` is equal to itself. + + Args: + obj: The object to check is deep-copyable. + """ + # If deep copy failed, then this will throw an error + obj2 = deepcopy(obj) + self.gtsamAssertEquals(obj, obj2) diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 612dd1891..78fc26708 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -160,7 +160,7 @@ namespace simulated2D { /// Default constructor GenericPrior() { } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -210,7 +210,7 @@ namespace simulated2D { /// Default constructor GenericOdometry() { } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template @@ -261,7 +261,7 @@ namespace simulated2D { /// Default constructor GenericMeasurement() { } -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; template diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 4c2ac99b1..845c28e5a 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -688,7 +688,7 @@ public: } private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 721ed51c0..3b507d806 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -25,6 +25,7 @@ #include +#include using namespace std; using namespace gtsam; diff --git a/tests/testSerializationSlam.cpp b/tests/testSerializationSlam.cpp index c4170b108..ed4c9e246 100644 --- a/tests/testSerializationSlam.cpp +++ b/tests/testSerializationSlam.cpp @@ -634,7 +634,7 @@ TEST(SubgraphSolver, Solves) { KeyInfo keyInfo(Ab); std::map lambda; system.build(Ab, keyInfo, lambda); - + // Create a perturbed (non-zero) RHS const auto xbar = system.Rc1().optimize(); // merely for use in zero below auto values_y = VectorValues::Zero(xbar); diff --git a/wrap/pybind11/docs/requirements.txt b/wrap/pybind11/docs/requirements.txt index 293db6a06..8dffd36b5 100644 --- a/wrap/pybind11/docs/requirements.txt +++ b/wrap/pybind11/docs/requirements.txt @@ -130,9 +130,9 @@ imagesize==1.4.1 \ --hash=sha256:0d8d18d08f840c19d0ee7ca1fd82490fdc3729b7ac93f49870406ddde8ef8d8b \ --hash=sha256:69150444affb9cb0d5cc5a92b3676f0b2fb7cd9ae39e947a5e11a36b4497cd4a # via sphinx -jinja2==3.1.4 \ - --hash=sha256:4a3aee7acbbe7303aede8e9648d13b8bf88a429282aa6122a993f0ac800cb369 \ - --hash=sha256:bc5dd2abb727a5319567b7a813e6a2e7318c39f4f487cfe6c89c6f9c7d25197d +jinja2==3.1.5 \ + --hash=sha256:8fefff8dc3034e27bb80d67c671eb8a9bc424c0ef4c0826edbff304cceff43bb \ + --hash=sha256:aba0f4dc9ed8013c424088f68a5c226f7d6097ed89b246d7749c2ec4175c6adb # via sphinx markupsafe==2.1.5 \ --hash=sha256:00e046b6dd71aa03a41079792f8473dc494d564611a8f89bbbd7cb93295ebdcf \