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

fixing spacing issue

release/4.3a0
Jai Moraes 2025-01-10 10:58:17 -05:00
parent 26b301ddb3 93f463ddbf
commit 8489ebcab5
355 changed files with 5433 additions and 1427 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.github/workflows/build-linux.yml vendored
View File

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

26
CMakeLists.txt
View File

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

2
CppUnitLite/Test.h
View File

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

2
GTSAM-Concepts.md
View File

@ -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<double,n,1>`.
* `ChartJacobian`, a typedef for `OptionalJacobian<dimension, dimension>`.

10
INSTALL.md
View File

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

56
cmake/FindGooglePerfTools.cmake
View File

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

55
cmake/GtsamBuildTypes.cmake
View File

@ -108,13 +108,15 @@ endif()
# Other (non-preprocessor macros) compiler flags:
if(MSVC)
set(CMAKE_3_15 $<VERSION_LESS:${CMAKE_VERSION},3.15>)
set(CMAKE_3_25 $<VERSION_LESS:${CMAKE_VERSION},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
$<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr -Werror=return-local-addr> # Error: return local address
$<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address -Werror=return-stack-address> # Error: return local address
$<$<CXX_COMPILER_ID:Clang>:-Wno-weak-template-vtables> # TODO(dellaert): don't know how to resolve
-Wpedantic # Enable pedantic warnings
$<$<COMPILE_LANGUAGE:CXX>:-Wextra -Wno-unused-parameter> # Enable extra warnings, but ignore no-unused-parameter (as we ifdef out chunks of code)
$<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr> # Error: return local address
$<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address> # Error: return local address
$<$<CXX_COMPILER_ID:Clang>:-Wno-weak-template-vtables> # TODO(dellaert): don't know how to resolve
$<$<CXX_COMPILER_ID:Clang>:-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
$<$<COMPILE_LANGUAGE:CXX>:${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 $<$<COMPILE_LANGUAGE:CXX>:-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

2
cmake/HandleAllocators.cmake
View File

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

23
cmake/HandleEigen.cmake
View File

@ -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
$<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
$<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
)
@ -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)

12
cmake/HandleGeneralOptions.cmake
View File

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

1
cmake/HandlePrintConfiguration.cmake
View File

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

1
examples/CombinedImuFactorsExample.cpp
View File

@ -48,6 +48,7 @@
#include <cstring>
#include <fstream>
#include <iostream>
#include <cassert>
using namespace gtsam;
using namespace std;

1
examples/ImuFactorsExample.cpp
View File

@ -49,6 +49,7 @@
#include <gtsam/slam/dataset.h>
#include <cstring>
#include <cassert>
#include <fstream>
#include <iostream>

2
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -29,6 +29,8 @@
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/geometry/Point2.h>
#include <cassert>
using namespace std;
using namespace gtsam;

8
gtsam/3rdparty/cephes/CMakeLists.txt vendored
View File

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

1
gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake vendored
View File

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

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

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

9
gtsam/CMakeLists.txt
View File

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

6
gtsam/base/ConcurrentMap.h
View File

@ -48,7 +48,7 @@ using ConcurrentMapBase = gtsam::FastMap<KEY, VALUE>;
#endif
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#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<class Archive>

6
gtsam/base/FastList.h
View File

@ -20,7 +20,7 @@
#include <gtsam/base/FastDefaultAllocator.h>
#include <list>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/version.hpp>
#if BOOST_VERSION >= 107400
@ -62,6 +62,8 @@ public:
/// Construct from c++11 initializer list:
FastList(std::initializer_list<VALUE> l) : Base(l) {}
FastList& operator=(const FastList& other) = default;
#ifdef GTSAM_ALLOCATOR_BOOSTPOOL
/** Copy constructor from a standard STL container */
FastList(const std::list<VALUE>& 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<class ARCHIVE>

6
gtsam/base/FastMap.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <gtsam/base/FastDefaultAllocator.h>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/map.hpp>
#endif
@ -54,6 +54,8 @@ public:
/** Copy constructor from another FastMap */
FastMap(const FastMap<KEY,VALUE>& x) : Base(x) {}
FastMap& operator=(const FastMap<KEY,VALUE>& 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<class ARCHIVE>

8
gtsam/base/FastSet.h
View File

@ -18,7 +18,9 @@
#pragma once
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <gtsam/config.h>
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/version.hpp>
#if BOOST_VERSION >= 107400
#include <boost/serialization/library_version_type.hpp>
@ -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<VALUE>& 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<class ARCHIVE>

14
gtsam/base/GenericValue.h
View File

@ -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<Value> clone() const override {
return std::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
return std::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *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<class ARCHIVE>
@ -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)
};

4
gtsam/base/Lie.h
View File

@ -36,7 +36,7 @@ namespace gtsam {
template <class Class, int N>
struct LieGroup {
enum { dimension = N };
inline constexpr static auto dimension = N;
typedef OptionalJacobian<N, N> ChartJacobian;
typedef Eigen::Matrix<double, N, N> Jacobian;
typedef Eigen::Matrix<double, N, 1> TangentVector;
@ -183,7 +183,7 @@ struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
/// @name Manifold
/// @{
typedef Class ManifoldType;
enum { dimension = Class::dimension };
inline constexpr static auto dimension = Class::dimension;
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;

4
gtsam/base/Manifold.h
View File

@ -55,7 +55,7 @@ namespace internal {
template<class Class>
struct HasManifoldPrereqs {
enum { dim = Class::dimension };
inline constexpr static auto dim = Class::dimension;
Class p, q;
Eigen::Matrix<double, dim, 1> v;
@ -95,7 +95,7 @@ struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
GTSAM_CONCEPT_ASSERT(HasManifoldPrereqs<Class>);
// Dimension of the manifold
enum { dimension = Class::dimension };
inline constexpr static auto dimension = Class::dimension;
// Typedefs required by all manifold types.
typedef Class ManifoldType;

1
gtsam/base/Matrix.cpp
View File

@ -24,6 +24,7 @@
#include <Eigen/LU>
#include <cstdarg>
#include <cassert>
#include <cstring>
#include <iomanip>
#include <list>

2
gtsam/base/Matrix.h
View File

@ -479,7 +479,7 @@ struct MultiplyWithInverse {
*/
template <typename T, int N>
struct MultiplyWithInverseFunction {
enum { M = traits<T>::dimension };
inline constexpr static auto M = traits<T>::dimension;
typedef Eigen::Matrix<double, N, 1> VectorN;
typedef Eigen::Matrix<double, N, N> MatrixN;

42
gtsam/base/MatrixSerialization.h
View File

@ -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 <gtsam/base/Matrix.h>
#include <Eigen/Sparse>
#include <boost/serialization/array.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_free.hpp>
@ -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 <class Archive, typename _Scalar, int _Options, typename _Index>
void save(Archive& ar, const Eigen::SparseVector<_Scalar, _Options, _Index>& m,
const unsigned int /*version*/) {
_Index size = m.size();
std::vector<std::pair<Eigen::Index, _Scalar>> 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 <class Archive, typename _Scalar, int _Options, typename _Index>
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<std::pair<Eigen::Index, _Scalar>> data;
ar >> BOOST_SERIALIZATION_NVP(data);
for (auto&& d : data) {
m.coeffRef(d.first) = d.second;
}
}
template <class Archive, typename _Scalar, int _Options, typename _Index>
void serialize(Archive& ar, Eigen::SparseVector<_Scalar, _Options, _Index>& m,
const unsigned int version) {
split_free(ar, m, version);
}
/******************************************************************************/
} // namespace serialization
} // namespace boost
#endif

10
gtsam/base/ProductLieGroup.h
View File

@ -32,8 +32,8 @@ class ProductLieGroup: public std::pair<G, H> {
typedef std::pair<G, H> Base;
protected:
enum {dimension1 = traits<G>::dimension};
enum {dimension2 = traits<H>::dimension};
constexpr static const size_t dimension1 = traits<G>::dimension;
constexpr static const size_t dimension2 = traits<H>::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<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;

4
gtsam/base/SymmetricBlockMatrix.h
View File

@ -21,7 +21,7 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/base/types.h>
#include <gtsam/dllexport.h>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#endif
#include <cassert>
@ -386,7 +386,7 @@ namespace gtsam {
template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr;
private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>

3
gtsam/base/TestableAssertions.h
View File

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

9
gtsam/base/Value.h
View File

@ -21,7 +21,7 @@
#include <gtsam/config.h> // Configuration from CMake
#include <gtsam/base/Vector.h>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/assume_abstract.hpp>
#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<class ARCHIVE>
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

1
gtsam/base/Vector.cpp
View File

@ -24,6 +24,7 @@
#include <iostream>
#include <fstream>
#include <sstream>
#include <cassert>
#include <iomanip>
#include <cmath>
#include <cstdio>

3
gtsam/base/Vector.h
View File

@ -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<class V1, class V2>
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<class V1, class V2>
inline double inner_prod(const V1 &a, const V2& b) {
assert (b.size()==a.size());
return a.dot(b);
}

2
gtsam/base/VectorSerialization.h
View File

@ -17,7 +17,7 @@
*/
// Defined only if boost serialization is enabled
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#pragma once
#include <gtsam/base/Vector.h>

10
gtsam/base/VectorSpace.h
View File

@ -163,7 +163,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
template<class Class>
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<Class>);
/// @name Manifold
/// @{
enum { dimension = Class::dimension};
inline constexpr static auto dimension = Class::dimension;
typedef Class ManifoldType;
/// @}
};
@ -232,7 +232,7 @@ struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
/// @name Manifold
/// @{
typedef Scalar ManifoldType;
enum { dimension = 1 };
inline constexpr static auto dimension = 1;
typedef Eigen::Matrix<double, 1, 1> TangentVector;
typedef OptionalJacobian<1, 1> ChartJacobian;
@ -305,7 +305,7 @@ struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
/// @name Manifold
/// @{
enum { dimension = M*N};
inline constexpr static auto dimension = M * N;
typedef Fixed ManifoldType;
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef Eigen::Matrix<double, dimension, dimension> 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<dimension, dimension> ChartJacobian;

4
gtsam/base/VerticalBlockMatrix.h
View File

@ -21,6 +21,8 @@
#include <gtsam/base/MatrixSerialization.h>
#include <gtsam/base/FastVector.h>
#include <cassert>
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<class ARCHIVE>

1
gtsam/base/cholesky.cpp
View File

@ -21,6 +21,7 @@
#include <gtsam/base/timing.h>
#include <cmath>
#include <cassert>
using namespace std;

2
gtsam/base/concepts.h
View File

@ -8,7 +8,7 @@
#pragma once
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
#include <boost/concept_check.hpp>
#include <boost/concept/assert.hpp>
#include <boost/concept/requires.hpp>

5
gtsam/base/serialization.h
View File

@ -17,9 +17,12 @@
* @date Feb 7, 2012
*/
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#pragma once
#include <gtsam/config.h>
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <Eigen/Core>
#include <fstream>
#include <sstream>

5
gtsam/base/serializationTestHelpers.h
View File

@ -17,9 +17,12 @@
* @date Feb 7, 2012
*/
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#pragma once
#include <gtsam/config.h>
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <iostream>
#include <sstream>
#include <string>

6
gtsam/base/std_optional_serialization.h
View File

@ -11,7 +11,7 @@
#pragma once
// Defined only if boost serialization is enabled
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <optional>
#include <boost/config.hpp>
@ -52,6 +52,10 @@ namespace boost { namespace serialization { struct U; } }
namespace std { template<> struct is_trivially_default_constructible<boost::serialization::U> : std::false_type {}; }
namespace std { template<> struct is_trivially_copy_constructible<boost::serialization::U> : std::false_type {}; }
namespace std { template<> struct is_trivially_move_constructible<boost::serialization::U> : 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<boost::serialization::U> : std::false_type {}; }
#endif
#endif
#endif

4
gtsam/base/tests/testMatrix.cpp
View File

@ -1151,7 +1151,7 @@ TEST(Matrix, Matrix24IsVectorSpace) {
}
TEST(Matrix, RowMajorIsVectorSpace) {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
typedef Eigen::Matrix<double, 2, 3, Eigen::RowMajor> RowMajor;
GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowMajor>);
#endif
@ -1166,7 +1166,7 @@ TEST(Matrix, VectorIsVectorSpace) {
}
TEST(Matrix, RowVectorIsVectorSpace) {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
typedef Eigen::Matrix<double, 1, -1> RowVector;
GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowVector>);
GTSAM_CONCEPT_ASSERT(IsVectorSpace<Vector5>);

2
gtsam/base/tests/testNumericalDerivative.cpp
View File

@ -18,6 +18,8 @@
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <cassert>
using namespace std;
using namespace gtsam;

2
gtsam/base/tests/testVector.cpp
View File

@ -272,7 +272,7 @@ TEST(Vector, VectorIsVectorSpace) {
}
TEST(Vector, RowVectorIsVectorSpace) {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
typedef Eigen::Matrix<double,1,-1> RowVector;
GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowVector>);
#endif

73
gtsam/base/timing.cpp
View File

@ -32,6 +32,8 @@
namespace gtsam {
namespace internal {
using ChildOrder = FastMap<size_t, std::shared_ptr<TimingOutline>>;
// a static shared_ptr to TimingOutline with nullptr as the pointer
const static std::shared_ptr<TimingOutline> 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<size_t, std::shared_ptr<TimingOutline> > 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>& TimingOutline::child(size_t child,
const std::string& label, const std::weak_ptr<TimingOutline>& thisPtr) {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
assert(thisPtr.lock().get() == this);
std::shared_ptr<TimingOutline>& result = children_[child];
if (!result) {
@ -172,7 +221,7 @@ const std::shared_ptr<TimingOutline>& 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<TimingOutline> 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<TimingOutline> current(gCurrentTimer.lock());
if (id != current->id_) {

39
gtsam/base/timing.h
View File

@ -21,7 +21,7 @@
#include <gtsam/dllexport.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
#include <boost/version.hpp>
#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<TimingOutline>&
child(size_t child, const std::string& label, const std::weak_ptr<TimingOutline>& 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(); }

2
gtsam/basis/Basis.h
View File

@ -291,7 +291,7 @@ class Basis {
*/
template <class T>
class ManifoldEvaluationFunctor : public VectorEvaluationFunctor {
enum { M = traits<T>::dimension };
inline constexpr static auto M = traits<T>::dimension;
using Base = VectorEvaluationFunctor;
public:

8
gtsam/basis/Chebyshev2.cpp
View File

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

8
gtsam/basis/Chebyshev2.h
View File

@ -61,13 +61,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
* @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) {

11
gtsam/config.h.in
View File

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

27
gtsam/discrete/AlgebraicDecisionTree.h
View File

@ -20,12 +20,12 @@
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/discrete/Ring.h>
#include <algorithm>
#include <iomanip>
#include <limits>
#include <map>
#include <string>
#include <iomanip>
#include <vector>
namespace gtsam {
@ -55,28 +55,11 @@ namespace gtsam {
public:
using Base = DecisionTree<L, double>;
/** 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) {}

4
gtsam/discrete/DecisionTree-inl.h
View File

@ -144,7 +144,7 @@ namespace gtsam {
private:
using Base = DecisionTree<L, Y>::Node;
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
@ -471,7 +471,7 @@ namespace gtsam {
private:
using Base = DecisionTree<L, Y>::Node;
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>

6
gtsam/discrete/DecisionTree.h
View File

@ -23,7 +23,7 @@
#include <gtsam/base/types.h>
#include <gtsam/discrete/Assignment.h>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#endif
#include <memory>
@ -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 <class ARCHIVE>
@ -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 <class ARCHIVE>

92
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -18,9 +18,10 @@
*/
#include <gtsam/base/FastSet.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/TableFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <utility>
@ -48,7 +49,7 @@ namespace gtsam {
return false;
} else {
const auto& f(static_cast<const DecisionTreeFactor&>(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<TableFactor>(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<TableFactor>((*tf) * TableFactor(*this));
} else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
// If `f` is a DecisionTreeFactor, simply call operator*.
result = std::make_shared<DecisionTreeFactor>(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<DecisionTreeFactor>(f->operator*(*this));
}
return result;
}
/* ************************************************************************ */
DiscreteFactor::shared_ptr DecisionTreeFactor::operator/(
const DiscreteFactor::shared_ptr& f) const {
if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
// Check if `f` is a TableFactor. If yes, then
// convert `this` to a TableFactor which is cheaper.
return std::make_shared<TableFactor>(tf->operator/(TableFactor(*this)));
} else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
// If `f` is a DecisionTreeFactor, divide normally.
return std::make_shared<DecisionTreeFactor>(this->operator/(*dtf));
} else {
// Else, convert `f` to a DecisionTreeFactor so we can divide
return std::make_shared<DecisionTreeFactor>(
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<Key, size_t> 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<std::pair<DiscreteValues, double>> 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<Key> allKeys(keys().begin(), keys().end());
std::set<Key> allKeys = this->labels();
MinHeap min_heap;
auto op = [&](const Assignment<Key>& 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;

96
gtsam/discrete/DecisionTreeFactor.h
View File

@ -21,11 +21,12 @@
#include <gtsam/discrete/AlgebraicDecisionTree.h>
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/Ring.h>
#include <gtsam/inference/Ordering.h>
#include <algorithm>
#include <memory>
#include <map>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>
@ -50,6 +51,11 @@ namespace gtsam {
typedef std::shared_ptr<DecisionTreeFactor> shared_ptr;
typedef AlgebraicDecisionTree<Key> 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<Key>& values) const {
virtual double evaluate(const Assignment<Key>& 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<std::pair<DiscreteValues, double>> enumerate() const;
@ -224,6 +257,17 @@ namespace gtsam {
/// Get all the probabilities in order of assignment values
std::vector<double> 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 <class ARCHIVE>

2
gtsam/discrete/DiscreteBayesNet.h
View File

@ -147,7 +147,7 @@ class GTSAM_EXPORT DiscreteBayesNet: public BayesNet<DiscreteConditional> {
/// @}
private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>

41
gtsam/discrete/DiscreteConditional.cpp
View File

@ -19,10 +19,12 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/debug.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/Ring.h>
#include <gtsam/discrete/Signature.h>
#include <gtsam/hybrid/HybridValues.h>
#include <algorithm>
#include <cassert>
#include <random>
#include <set>
#include <stdexcept>
@ -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<Key> 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<const DecisionTreeFactor*>(&other)) {
if (!dynamic_cast<const BaseFactor*>(&other)) {
return false;
} else {
const DecisionTreeFactor& f(static_cast<const DecisionTreeFactor&>(other));
return DecisionTreeFactor::equals(f, tol);
const BaseFactor& f(static_cast<const BaseFactor&>(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 << "</i></p>\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_;
}
/* ************************************************************************* */

28
gtsam/discrete/DiscreteConditional.h
View File

@ -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<const BaseConditional*>(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 <class Archive>

2
gtsam/discrete/DiscreteDistribution.h
View File

@ -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) {}

5
gtsam/discrete/DiscreteFactor.cpp
View File

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

57
gtsam/discrete/DiscreteFactor.h
View File

@ -22,6 +22,7 @@
#include <gtsam/discrete/AlgebraicDecisionTree.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/inference/Ordering.h>
#include <string>
namespace gtsam {
@ -46,6 +47,11 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
using Values = DiscreteValues; ///< backwards compatibility
using Unary = std::function<double(const double&)>;
using UnaryAssignment =
std::function<double(const Assignment<Key>&, const double&)>;
using Binary = std::function<double(const double, const double)>;
protected:
/// Map of Keys and their cardinalities.
std::map<Key, size_t> 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<Key> as it is the base class of DiscreteValues.
*
* @param values Discrete assignment.
* @return double
*/
virtual double evaluate(const Assignment<Key>& 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 <class ARCHIVE>

100
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -14,6 +14,7 @@
* @date Feb 14, 2011
* @author Duy-Nguyen Ta
* @author Frank Dellaert
* @author Varun Agrawal
*/
#include <gtsam/discrete/DiscreteBayesTree.h>
@ -35,13 +36,12 @@ namespace gtsam {
template class FactorGraph<DiscreteFactor>;
template class EliminateableFactorGraph<DiscreteFactorGraph>;
/* ************************************************************************* */
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<DecisionTreeFactor>(factor)) {
if (auto p = std::dynamic_pointer_cast<DiscreteFactor>(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<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
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<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr> //
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<DiscreteLookupTable>(nrFrontals,
orderedKeys, product);
auto lookup = std::make_shared<DiscreteLookupTable>(
nrFrontals, orderedKeys, product->toDecisionTreeFactor());
gttoc(lookup);
return {std::dynamic_pointer_cast<DiscreteConditional>(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<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr> //
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<DiscreteConditional>(product, *sum, orderedKeys);
auto conditional = std::make_shared<DiscreteConditional>(
product->toDecisionTreeFactor(), sum->toDecisionTreeFactor(),
orderedKeys);
gttoc(divide);
return {conditional, sum};

17
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -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<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>
EliminateDiscrete(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys);
@ -61,7 +62,7 @@ EliminateDiscrete(const DiscreteFactorGraph& factors,
* @ingroup discrete
*/
GTSAM_EXPORT
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>
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 <typename... Args>
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

4
gtsam/discrete/DiscreteKey.h
View File

@ -21,7 +21,7 @@
#include <gtsam/global_includes.h>
#include <gtsam/inference/Key.h>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/vector.hpp>
#endif
#include <map>
@ -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 <class ARCHIVE>

1
gtsam/discrete/DiscreteLookupDAG.cpp
View File

@ -23,6 +23,7 @@
#include <iterator>
#include <string>
#include <utility>
#include <cassert>
using std::pair;
using std::vector;

15
gtsam/discrete/DiscreteLookupDAG.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/discrete/DiscreteDistribution.h>
#include <gtsam/discrete/TableFactor.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/FactorGraph.h>
@ -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<DiscreteLookupTable> {
/// @}
private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>

37
gtsam/discrete/Ring.h Normal file
View File

@ -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 <algorithm>
/** 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; }
};

4
gtsam/discrete/SignatureParser.cpp
View File

@ -38,7 +38,7 @@ std::optional<Row> static ParseConditional(const std::string& token) {
} catch (...) {
return std::nullopt;
}
return std::move(row);
return row;
}
std::optional<Table> static ParseConditionalTable(
@ -62,7 +62,7 @@ std::optional<Table> static ParseConditionalTable(
}
}
}
return std::move(table);
return table;
}
std::vector<std::string> static Tokenize(const std::string& str) {

174
gtsam/discrete/TableDistribution.cpp Normal file
View File

@ -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 <gtsam/base/Testable.h>
#include <gtsam/base/debug.h>
#include <gtsam/discrete/Ring.h>
#include <gtsam/discrete/Signature.h>
#include <gtsam/discrete/TableDistribution.h>
#include <gtsam/hybrid/HybridValues.h>
#include <algorithm>
#include <cassert>
#include <random>
#include <set>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
using namespace std;
using std::pair;
using std::stringstream;
using std::vector;
namespace gtsam {
/// Normalize sparse_table
static Eigen::SparseVector<double> normalizeSparseTable(
const Eigen::SparseVector<double>& 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<TableFactor>(
f.sum(f.keys().size())))) {}
/* ************************************************************************** */
TableDistribution::TableDistribution(const DiscreteKeys& keys,
const std::vector<double>& 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<const TableDistribution*>(&other);
if (!dtc) {
return false;
} else {
const DiscreteConditional& f(
static_cast<const DiscreteConditional&>(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<double> 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<double> 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<size_t> distribution(p.begin(), p.end());
return distribution(rng);
}
} // namespace gtsam

177
gtsam/discrete/TableDistribution.h Normal file
View File

@ -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 <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/TableFactor.h>
#include <gtsam/inference/Conditional-inst.h>
#include <memory>
#include <string>
#include <vector>
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<double>::InnerIterator SparseIt;
public:
// typedefs needed to play nice with gtsam
typedef TableDistribution This; ///< Typedef to this class
typedef std::shared_ptr<This> 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<double>& potentials);
/**
* Construct from single DiscreteKey and std::vector.
*/
TableDistribution(const DiscreteKey& key,
const std::vector<double>& 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<Key>& 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 <class Archive>
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<TableDistribution> : public Testable<TableDistribution> {};
} // namespace gtsam

187
gtsam/discrete/TableFactor.cpp
View File

@ -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<double>
* @return Eigen::SparseVector<double>
*/
std::vector<double> ComputeLeafOrdering(const DiscreteKeys& dkeys,
const DecisionTreeFactor& dt) {
std::vector<double> probs = dt.probabilities();
std::vector<double> ordered;
static Eigen::SparseVector<double> 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<double> 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<Key, size_t> denominators;
double denom = sparseTable.size();
for (const DiscreteKey& dkey : dkeys) {
denom /= dkey.second;
denominators.insert(std::pair<Key, double>(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<Key>& 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<double> TableFactor::Convert(
const std::vector<double>& table) {
const DiscreteKeys& keys, const std::vector<double>& 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<double> 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<double> TableFactor::Convert(
}
/* ************************************************************************ */
Eigen::SparseVector<double> TableFactor::Convert(const std::string& table) {
Eigen::SparseVector<double> TableFactor::Convert(const DiscreteKeys& keys,
const std::string& table) {
// Convert string to doubles.
std::vector<double> ys;
std::istringstream iss(table);
std::copy(std::istream_iterator<double>(iss), std::istream_iterator<double>(),
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<const TableFactor&>(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<Key>& 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<TableFactor>(f)) {
// If `f` is a TableFactor, we can simply call `operator*`.
result = std::make_shared<TableFactor>(this->operator*(*tf));
} else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
// If `f` is a DecisionTreeFactor, we convert to a TableFactor which is
// cheaper than converting `this` to a DecisionTreeFactor.
result = std::make_shared<TableFactor>(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<DecisionTreeFactor>(
f->operator*(this->toDecisionTreeFactor()));
}
return result;
}
/* ************************************************************************ */
DiscreteFactor::shared_ptr TableFactor::operator/(
const DiscreteFactor::shared_ptr& f) const {
if (auto tf = std::dynamic_pointer_cast<TableFactor>(f)) {
return std::make_shared<TableFactor>(this->operator/(*tf));
} else if (auto dtf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
return std::make_shared<TableFactor>(
this->operator/(TableFactor(f->discreteKeys(), *dtf)));
} else {
TableFactor divisor(f->toDecisionTreeFactor());
return std::make_shared<TableFactor>(this->operator/(divisor));
}
}
/* ************************************************************************ */
DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
DiscreteKeys dkeys = discreteKeys();
std::vector<double> 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<Key> tree;
// We can have an empty sparse_table_ or one with a single value.
if (sparse_table_.size() != 0) {
tree = AlgebraicDecisionTree<Key>(sparse_table_.coeff(0));
}
return DecisionTreeFactor(dkeys, tree);
}
DecisionTreeFactor f(dkeys, table);
std::vector<double> table(sparse_table_.size(), 0.0);
for (SparseIt it(sparse_table_); it; ++it) {
table[it.index()] = it.value();
}
AlgebraicDecisionTree<Key> 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;
}

108
gtsam/discrete/TableFactor.h
View File

@ -17,8 +17,10 @@
#pragma once
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/Ring.h>
#include <gtsam/inference/Ordering.h>
#include <Eigen/Sparse>
@ -30,6 +32,12 @@
#include <utility>
#include <vector>
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <gtsam/base/MatrixSerialization.h>
#include <boost/serialization/nvp.hpp>
#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<double> Convert(const std::vector<double>& 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<double> Convert(const DiscreteKeys& keys,
const std::vector<double>& table);
/// Convert probability table given as string to SparseVector.
static Eigen::SparseVector<double> Convert(const std::string& table);
static Eigen::SparseVector<double> 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<TableFactor> shared_ptr;
typedef Eigen::SparseVector<double>::InnerIterator SparseIt;
typedef std::vector<std::pair<DiscreteValues, double>> AssignValList;
using Unary = std::function<double(const double&)>;
using UnaryAssignment =
std::function<double(const Assignment<Key>&, const double&)>;
using Binary = std::function<double(const double, const double)>;
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<double>& 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 <class SOURCE>
@ -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<Key, double>/AlgebraicDecisionTree
TableFactor(const DiscreteKeys& keys, const DecisionTree<Key, double>& 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<double> 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<Key>& 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 <class ARCHIVE>
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

37
gtsam/discrete/discrete.i
View File

@ -168,6 +168,43 @@ virtual class DiscreteDistribution : gtsam::DiscreteConditional {
std::vector<double> pmf() const;
};
#include <gtsam/discrete/TableFactor.h>
virtual class TableFactor : gtsam::DiscreteFactor {
TableFactor();
TableFactor(const gtsam::DiscreteKeys& keys,
const gtsam::TableFactor& potentials);
TableFactor(const gtsam::DiscreteKeys& keys, std::vector<double>& 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 <gtsam/discrete/TableDistribution.h>
virtual class TableDistribution : gtsam::DiscreteConditional {
TableDistribution();
TableDistribution(const gtsam::TableFactor& f);
TableDistribution(const gtsam::DiscreteKey& key, std::vector<double> spec);
TableDistribution(const gtsam::DiscreteKeys& keys, std::vector<double> 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 <gtsam/discrete/DiscreteBayesNet.h>
class DiscreteBayesNet {
DiscreteBayesNet();

9
gtsam/discrete/tests/testDecisionTree.cpp
View File

@ -23,6 +23,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/discrete/Ring.h>
#include <gtsam/discrete/Signature.h>
#include <gtsam/inference/Symbol.h>
@ -124,14 +125,6 @@ struct traits<DT> : public Testable<DT> {};
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) {

87
gtsam/discrete/tests/testDecisionTreeFactor.cpp
View File

@ -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<Key>;
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<DecisionTreeFactor>(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<DecisionTreeFactor>(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<DecisionTreeFactor>(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) {

4
gtsam/discrete/tests/testDiscreteConditional.cpp
View File

@ -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<DecisionTreeFactor>(actual2)));
std::vector<double> 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<DecisionTreeFactor>(actual2)));
}

16
gtsam/discrete/tests/testDiscreteFactorGraph.cpp
View File

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

15
gtsam/discrete/tests/testSerializationDiscrete.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteDistribution.h>
#include <gtsam/discrete/TableFactor.h>
#include <gtsam/inference/Symbol.h>
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<Key>;
BOOST_CLASS_EXPORT_GUID(ADT, "gtsam_AlgebraicDecisionTree");
@ -79,6 +81,19 @@ TEST(DiscreteSerialization, DecisionTreeFactor) {
EXPECT(equalsBinary<DecisionTreeFactor>(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<TableFactor>(tf));
EXPECT(equalsXML<TableFactor>(tf));
EXPECT(equalsBinary<TableFactor>(tf));
}
/* ************************************************************************* */
// Check serialization for DiscreteConditional & DiscreteDistribution
TEST(DiscreteSerialization, DiscreteConditional) {

82
gtsam/discrete/tests/testTableFactor.cpp
View File

@ -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<double>{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<double>{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<AlgebraicDecisionTree<Key>>(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<TableFactor>(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<TableFactor>(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<TableFactor>(f2.sum(1));
CHECK(actual22);
}
/* ************************************************************************* */

14
gtsam/geometry/BearingRange.h
View File

@ -21,7 +21,7 @@
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/OptionalJacobian.h>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#endif
#include <iostream>
@ -55,9 +55,9 @@ private:
R range_;
public:
enum { dimB = traits<B>::dimension };
enum { dimR = traits<R>::dimension };
enum { dimension = dimB + dimR };
constexpr static const size_t dimB = traits<B>::dimension;
constexpr static const size_t dimR = traits<R>::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 <class ARCHIVE>
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)
};

4
gtsam/geometry/Cal3.h
View File

@ -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<Cal3>;
@ -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 <class Archive>

4
gtsam/geometry/Cal3Bundler.h
View File

@ -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<Cal3Bundler>;
/// @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 <class Archive>

4
gtsam/geometry/Cal3DS2.h
View File

@ -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<Cal3DS2>;
@ -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 <class Archive>

4
gtsam/geometry/Cal3DS2_Base.h
View File

@ -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<Cal3DS2_Base>;
@ -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 <class Archive>

4
gtsam/geometry/Cal3Fisheye.h
View File

@ -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<Cal3Fisheye>;
@ -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 <class Archive>

4
gtsam/geometry/Cal3Unified.h
View File

@ -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<Cal3Unified>;
@ -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 <class Archive>

4
gtsam/geometry/Cal3_S2.h
View File

@ -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<Cal3_S2>;
@ -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 <class Archive>

4
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -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<Cal3_S2Stereo>;
@ -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 <class Archive>

1
gtsam/geometry/Cal3f.cpp
View File

@ -28,6 +28,7 @@
#include <gtsam/geometry/Cal3f.h>
#include <iostream>
#include <cassert>
namespace gtsam {

4
gtsam/geometry/Cal3f.h
View File

@ -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<Cal3f>;
/// @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 <class Archive>

10
gtsam/geometry/CalibratedCamera.h
View File

@ -25,7 +25,7 @@
#include <gtsam/base/Manifold.h>
#include <gtsam/base/ThreadsafeException.h>
#include <gtsam/dllexport.h>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#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<class Archive>
@ -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<class Archive>

3
gtsam/geometry/CameraSet.h
View File

@ -26,6 +26,7 @@
#include <gtsam/inference/Key.h>
#include <vector>
#include <cassert>
namespace gtsam {
@ -471,7 +472,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
}
private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ///
#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
/// Serialization function
friend class boost::serialization::access;
template <class ARCHIVE>

4
gtsam/geometry/EssentialMatrix.h
View File

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

28
gtsam/geometry/FundamentalMatrix.cpp
View File

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

11
gtsam/geometry/FundamentalMatrix.h
View File

@ -7,6 +7,7 @@
#pragma once
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
@ -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; }

1
gtsam/geometry/Line3.cpp
View File

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

2
gtsam/geometry/Line3.h
View File

@ -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() :

4
gtsam/geometry/OrientedPlane3.h
View File

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

6
gtsam/geometry/PinholeCamera.h
View File

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

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