fixing spacing issue

.github/workflows/build-linux.yml

@@ -27,7 +27,7 @@ jobs:
         name: [
           ubuntu-20.04-gcc-9,
           ubuntu-20.04-clang-9,
-          ubuntu-22.04-gcc-11,
+          ubuntu-22.04-gcc-12,
           ubuntu-22.04-clang-14,
         ]
 
@@ -44,7 +44,7 @@ jobs:
             compiler: clang
             version: "9"
 
-          - name: ubuntu-22.04-gcc-11
+          - name: ubuntu-22.04-gcc-12
             os: ubuntu-22.04
             compiler: gcc
             version: "11"

CMakeLists.txt

@@ -1,23 +1,8 @@
-cmake_minimum_required(VERSION 3.9)
+cmake_minimum_required(VERSION 3.9...3.29)
-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()
 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)

CppUnitLite/Test.h

@@ -129,7 +129,7 @@ protected:
     result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); }
 
 #define CHECK_EQUAL(expected,actual)\
-{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); }
+{ if (!((expected) == (actual))) { result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); return; } }
 
 #define LONGS_EQUAL(expected,actual)\
 { long actualTemp = actual; \

GTSAM-Concepts.md

@@ -22,7 +22,7 @@ In GTSAM, all properties and operations needed to use a type must be defined thr
 In detail, we ask that the following items are defined in the traits object (although, not all are needed for optimization):
 
 * values:
-   * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1.
+   * `inline constexpr static auto dimension = D;`, a constexpr that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1.
 * types:
     * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix<double,n,1>`.
     * `ChartJacobian`, a typedef for `OptionalJacobian<dimension, dimension>`.

INSTALL.md

@@ -27,7 +27,7 @@ $ make install
        downloaded from https://www.threadingbuildingblocks.org/
      - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and
        `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually
-       achieved with MKL disabled. We therefore advise you to benchmark your problem 
+       achieved with MKL disabled. We therefore advise you to benchmark your problem
        before using MKL.
 
     Tested compilers:
@@ -128,12 +128,12 @@ We support several build configurations for GTSAM (case insensitive)
 
 #### CMAKE_INSTALL_PREFIX
 
-The install folder. The default is typically `/usr/local/`. 
+The install folder. The default is typically `/usr/local/`.
 To configure to install to your home directory, you could execute:
 
 ```cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME ..```
 
-#### GTSAM_TOOLBOX_INSTALL_PATH 
+#### GTSAM_TOOLBOX_INSTALL_PATH
 
 The Matlab toolbox will be installed in a subdirectory
 of this folder, called 'gtsam'.
@@ -142,7 +142,7 @@ of this folder, called 'gtsam'.
 
 #### GTSAM_BUILD_CONVENIENCE_LIBRARIES
 
-This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam. 
+This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam.
 Set with the command line as follows:
 
 ```cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..```
@@ -159,6 +159,16 @@ Set with the command line as follows:
   ON:             When enabled, libgtsam_unstable will be built and installed with the same options as libgtsam.  In addition, if tests are enabled, the unit tests will be built as well.  The Matlab toolbox will also be generated if the matlab toolbox is enabled, installing into a folder called `gtsam_unstable`.
   OFF (Default)  If disabled, no `gtsam_unstable` code will be included in build or install.
 
+## Convenience Options:
+
+#### GTSAM_BUILD_EXAMPLES_ALWAYS
+
+Whether or not to force building examples, can be true or false.
+
+#### GTSAM_BUILD_TESTS
+
+Whether or not to build tests, can be true or false.
+
 ## Check
 
 `make check` will build and run all of the tests. Note that the tests will only be
@@ -179,7 +189,7 @@ Here are some tips to get the best possible performance out of GTSAM.
 
 1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode.
 2. Enable TBB. On modern processors with multiple cores, this can easily speed up
-    optimization by 30-50%. Please note that this may not be true for very small 
+    optimization by 30-50%. Please note that this may not be true for very small
     problems where the overhead of dispatching work to multiple threads outweighs
     the benefit. We recommend that you benchmark your problem with/without TBB.
 3. Use `GTSAM_BUILD_WITH_MARCH_NATIVE`. A performance gain of

cmake/FindGooglePerfTools.cmake

@@ -1,42 +1,40 @@
 # -*- cmake -*-
 
-# - Find Google perftools
+# - Find GPerfTools (formerly Google perftools)
-# Find the Google perftools includes and libraries
+# Find the GPerfTools libraries
-# This module defines
+# If false, do not try to use Google perftools.
-#  GOOGLE_PERFTOOLS_INCLUDE_DIR, where to find heap-profiler.h, etc.
+# Also defined for general use are
-#  GOOGLE_PERFTOOLS_FOUND, If false, do not try to use Google perftools.
+# - GPERFTOOLS_TCMALLOC: where to find the tcmalloc library
-# also defined for general use are
+# - GPERFTOOLS_PROFILER: where to find the profiler library
-#  TCMALLOC_LIBRARY, where to find the tcmalloc library.
-
-FIND_PATH(GOOGLE_PERFTOOLS_INCLUDE_DIR google/heap-profiler.h
-/usr/local/include
-/usr/include
-)
 
 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)
+IF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
-    SET(TCMALLOC_LIBRARIES ${TCMALLOC_LIBRARY})
+    SET(TCMALLOC_LIBRARIES ${GPERFTOOLS_TCMALLOC})
-    SET(GOOGLE_PERFTOOLS_FOUND "YES")
+    SET(GPERFTOOLS_FOUND "YES")
-ELSE (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
+ELSE (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
-  SET(GOOGLE_PERFTOOLS_FOUND "NO")
+  SET(GPERFTOOLS_FOUND "NO")
-ENDIF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
+ENDIF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
 
-IF (GOOGLE_PERFTOOLS_FOUND)
+IF (GPERFTOOLS_FOUND)
-   IF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY)
+   MESSAGE(STATUS "Found Gperftools: ${GPERFTOOLS_PROFILER}")
-      MESSAGE(STATUS "Found Google perftools: ${GOOGLE_PERFTOOLS_LIBRARIES}")
+ELSE (GPERFTOOLS_FOUND)
-   ENDIF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY)
-ELSE (GOOGLE_PERFTOOLS_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
+  GPERFTOOLS_TCMALLOC
-  GOOGLE_PERFTOOLS_INCLUDE_DIR
+  GPERFTOOLS_PROFILER
-  )
+)
+
+option(GTSAM_ENABLE_GPERFTOOLS                   "Enable/Disable Gperftools" OFF)

cmake/GtsamBuildTypes.cmake

@@ -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_COMMON          /W3 /WX /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_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  /MD /O2 /D /Zi /d2Zi+  CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo 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         /MD /O2  CACHE STRING "(User editable) Private compiler flags for Release 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       /MD /O2  /Zi  CACHE STRING "(User editable) Private compiler flags for Profiling 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          /MD /O2  CACHE STRING "(User editable) Private compiler flags for Timing 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
+    -Wpedantic                                     # Enable pedantic warnings
-    $<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr -Werror=return-local-addr>            # Error: return local address
+    $<$<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:Clang>:-Wreturn-stack-address   -Werror=return-stack-address>  # Error: return local address
+    $<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr>            # Error: return local address
-    $<$<CXX_COMPILER_ID:Clang>:-Wno-weak-template-vtables>  # TODO(dellaert): don't know how to resolve
+    $<$<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.")
-    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
-    # See: https://cmake.org/cmake/help/latest/prop_tgt/CXX_EXTENSIONS.html
+set(CMAKE_CXX_EXTENSIONS OFF)
-    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()
 
 # 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(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/bin")
-  set(EXECUTABLE_OUTPUT_PATH "${GTSAM_BINARY_DIR}/bin")
+  set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/lib")
-  set(LIBRARY_OUTPUT_PATH "${GTSAM_BINARY_DIR}/lib")
+  set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/lib")
 endif()
 
 # Set up build type list for cmake-gui

cmake/HandleAllocators.cmake

@@ -7,7 +7,7 @@ else()
     list(APPEND possible_allocators BoostPool STL)
     set(preferred_allocator STL)
 endif()
-if(GOOGLE_PERFTOOLS_FOUND)
+if(GPERFTOOLS_FOUND)
     list(APPEND possible_allocators tcmalloc)
 endif()
 

cmake/HandleEigen.cmake

@@ -13,12 +13,6 @@ if(GTSAM_USE_SYSTEM_EIGEN)
     # Since Eigen 3.3.0 a Eigen3Config.cmake is available so use it.
     find_package(Eigen3 CONFIG REQUIRED) # need to find again as REQUIRED
 
-    # The actual include directory (for BUILD cmake target interface):
-    # Note: EIGEN3_INCLUDE_DIR points to some random location on some eigen
-    # versions.  So here I use the target itself to get the proper include
-    # directory (it is generated by cmake, thus has the correct path)
-    get_target_property(GTSAM_EIGEN_INCLUDE_FOR_BUILD Eigen3::Eigen INTERFACE_INCLUDE_DIRECTORIES)
-
     # check if MKL is also enabled - can have one or the other, but not both!
     # Note: Eigen >= v3.2.5 includes our patches
     if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5))
@@ -30,6 +24,8 @@ if(GTSAM_USE_SYSTEM_EIGEN)
     if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4))
         message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.")
     endif()
+    
+    set(GTSAM_EIGEN_VERSION "${EIGEN3_VERSION}")
 else()
     # Use bundled Eigen include path.
     # Clear any variables set by FindEigen3
@@ -46,7 +42,7 @@ else()
 
     add_library(gtsam_eigen3 INTERFACE)
 
-    target_include_directories(gtsam_eigen3 INTERFACE
+    target_include_directories(gtsam_eigen3 SYSTEM INTERFACE
       $<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")
-# Detect Eigen version:
-set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h")
-if (EXISTS ${EIGEN_VER_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}")
+message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}")
-else()
-    message(WARNING "Cannot determine Eigen version, missing file: `${EIGEN_VER_H}`")
-endif ()
 
 if (MSVC)
     if (GTSAM_SHARED_LIB)

cmake/HandleGeneralOptions.cmake

@@ -21,17 +21,20 @@ if (NOT MSVC)
 endif()
 
 # Configurable Options
+option(BUILD_SHARED_LIBS                     "Build shared libraries" ON)
 if(GTSAM_UNSTABLE_AVAILABLE)
     option(GTSAM_BUILD_UNSTABLE              "Enable/Disable libgtsam_unstable"          ON)
     option(GTSAM_UNSTABLE_BUILD_PYTHON       "Enable/Disable Python wrapper for libgtsam_unstable" ON)
     option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF)
 endif()
-option(GTSAM_FORCE_SHARED_LIB               "Force gtsam to be a shared library, overriding BUILD_SHARED_LIBS" ON)
+option(GTSAM_FORCE_SHARED_LIB               "Force gtsam to be a shared library, overriding BUILD_SHARED_LIBS" OFF)
 option(GTSAM_FORCE_STATIC_LIB               "Force gtsam to be a static library, overriding BUILD_SHARED_LIBS" OFF)
 option(GTSAM_USE_QUATERNIONS                "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
 option(GTSAM_POSE3_EXPMAP                   "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
 option(GTSAM_ROT3_EXPMAP                    "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
 option(GTSAM_DT_MERGING                     "Enable/Disable merging of equal leaf nodes in DecisionTrees. This leads to significant speed up and memory savings." ON)
+option(GTSAM_ENABLE_TIMING                  "Enable the timing tools (gttic/gttoc)" OFF)
+option(GTSAM_HYBRID_TIMING                  "Enable the timing of hybrid factor graph machinery" OFF)
 option(GTSAM_ENABLE_CONSISTENCY_CHECKS      "Enable/Disable expensive consistency checks" OFF)
 option(GTSAM_ENABLE_MEMORY_SANITIZER        "Enable/Disable memory sanitizer" OFF)
 option(GTSAM_WITH_TBB                       "Use Intel Threaded Building Blocks (TBB) if available" ON)
@@ -46,7 +49,9 @@ option(GTSAM_TANGENT_PREINTEGRATION         "Use new ImuFactor with integration
 option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF)
 option(GTSAM_SLOW_BUT_CORRECT_EXPMAP        "Use slower but correct expmap for Pose2"  OFF)
 
-if (GTSAM_FORCE_SHARED_LIB)
+if (GTSAM_FORCE_SHARED_LIB AND GTSAM_FORCE_STATIC_LIB)
+    message(FATAL_ERROR "GTSAM_FORCE_SHARED_LIB and GTSAM_FORCE_STATIC_LIB are both true. Please, to unambiguously select the desired library type to use to build GTSAM, set one of GTSAM_FORCE_SHARED_LIB=ON, GTSAM_FORCE_STATIC_LIB=ON, or BUILD_SHARED_LIBS={ON/OFF}")
+elseif (GTSAM_FORCE_SHARED_LIB)
     message(STATUS "GTSAM is a shared library due to GTSAM_FORCE_SHARED_LIB")
     set(GTSAM_LIBRARY_TYPE SHARED CACHE STRING "" FORCE)
     set(GTSAM_SHARED_LIB 1 CACHE BOOL "" FORCE)
@@ -55,10 +60,9 @@ elseif (GTSAM_FORCE_STATIC_LIB)
     set(GTSAM_LIBRARY_TYPE STATIC CACHE STRING "" FORCE)
     set(GTSAM_SHARED_LIB 0 CACHE BOOL "" FORCE)
 elseif (BUILD_SHARED_LIBS)
-    message(STATUS "GTSAM is a shared library due to BUILD_SHARED_LIBS is ON")
     set(GTSAM_LIBRARY_TYPE SHARED CACHE STRING "" FORCE)
     set(GTSAM_SHARED_LIB 1 CACHE BOOL "" FORCE)
-elseif((DEFINED BUILD_SHARED_LIBS) AND (NOT BUILD_SHARED_LIBS))
+elseif(NOT BUILD_SHARED_LIBS)
     message(STATUS "GTSAM is a static library due to BUILD_SHARED_LIBS is OFF")
     set(GTSAM_LIBRARY_TYPE STATIC CACHE STRING "" FORCE)
     set(GTSAM_SHARED_LIB 0 CACHE BOOL "" FORCE)

cmake/HandlePerfTools.cmake

@@ -1,4 +1,4 @@
 
 ###############################################################################
 # Find Google perftools
-find_package(GooglePerfTools)
+find_package(GooglePerfTools)

cmake/HandlePrintConfiguration.cmake

@@ -91,6 +91,7 @@ print_enabled_config(${GTSAM_ENABLE_MEMORY_SANITIZER}     "Build with Memory San
 print_enabled_config(${GTSAM_ROT3_EXPMAP}                 "Rot3 retract is full ExpMap     ")
 print_enabled_config(${GTSAM_POSE3_EXPMAP}                "Pose3 retract is full ExpMap    ")
 print_enabled_config(${GTSAM_DT_MERGING}                  "Enable branch merging in DecisionTree")
+print_enabled_config(${GTSAM_ENABLE_TIMING}               "Enable timing machinery")
 print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V43}  "Allow features deprecated in GTSAM 4.3")
 print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION}   "Metis-based Nested Dissection   ")
 print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION}      "Use tangent-space preintegration")

examples/CombinedImuFactorsExample.cpp

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

examples/ImuFactorsExample.cpp

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

examples/elaboratePoint2KalmanFilter.cpp

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

gtsam/3rdparty/cephes/CMakeLists.txt

@@ -89,7 +89,7 @@ set(CEPHES_SOURCES
     cephes/zetac.c)
 
 # Add library source files
-add_library(cephes-gtsam SHARED ${CEPHES_SOURCES})
+add_library(cephes-gtsam ${GTSAM_LIBRARY_TYPE} ${CEPHES_SOURCES})
 
 # Add include directory (aka headers)
 target_include_directories(
@@ -103,11 +103,7 @@ set_target_properties(
              C_STANDARD 99)
 
 if(WIN32)
-  set_target_properties(
+  set_target_properties(cephes-gtsam PROPERTIES COMPILE_FLAGS /w)
-    cephes-gtsam
-    PROPERTIES PREFIX ""
-               COMPILE_FLAGS /w
-               RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin")
 endif()
 
 if(APPLE)

gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake

@@ -15,7 +15,6 @@ option(GKRAND "enable GKRAND support" OFF)
 
 # Add compiler flags.
 if(MSVC)
-  set(GKlib_COPTS "/Ox")
   set(GKlib_COPTIONS "-DWIN32 -DMSC -D_CRT_SECURE_NO_DEPRECATE -DUSE_GKREGEX")
 elseif(MINGW)
   set(GKlib_COPTS "-DUSE_GKREGEX")

gtsam/3rdparty/metis/libmetis/CMakeLists.txt

@@ -3,7 +3,6 @@ include_directories(.)
 # Find sources.
 file(GLOB metis_sources *.c)
 # Build libmetis.
-add_definitions(-fPIC)
 add_library(metis-gtsam ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources})
 if(UNIX)
   target_link_libraries(metis-gtsam m)
@@ -11,9 +10,8 @@ endif()
 
 if(WIN32)
 		set_target_properties(metis-gtsam PROPERTIES
-			PREFIX ""
 			COMPILE_FLAGS /w
-			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin")
+			WINDOWS_EXPORT_ALL_SYMBOLS ON)
 endif()
 
 if (APPLE)

gtsam/CMakeLists.txt

@@ -151,6 +151,10 @@ if (GTSAM_USE_EIGEN_MKL)
   target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})
 endif()
 
+if (GTSAM_ENABLE_GPERFTOOLS AND GPERFTOOLS_FOUND)
+  target_link_libraries(gtsam PRIVATE ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER})
+endif()
+
 # Add includes for source directories 'BEFORE' boost and any system include
 # paths so that the compiler uses GTSAM headers in our source directory instead
 # of any previously installed GTSAM headers.
@@ -183,10 +187,7 @@ if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with
 			PREFIX "lib")
 		target_compile_definitions(gtsam PRIVATE GTSAM_IMPORT_STATIC)
 	else()
-		set_target_properties(gtsam PROPERTIES
+		set_target_properties(gtsam PROPERTIES DEFINE_SYMBOL GTSAM_EXPORTS)
-			PREFIX ""
-			DEFINE_SYMBOL GTSAM_EXPORTS
-			RUNTIME_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/bin")
 	endif()
 endif()
 

gtsam/base/ConcurrentMap.h

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

gtsam/base/FastList.h

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

gtsam/base/FastMap.h

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

gtsam/base/FastSet.h

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

gtsam/base/GenericValue.h

@@ -56,9 +56,10 @@ public:
   GenericValue(){}
 
   /// Construct from value
-  GenericValue(const T& value) :
+  GenericValue(const T& value) : Value(),
-      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)
 };

gtsam/base/Lie.h

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

gtsam/base/Manifold.h

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

gtsam/base/Matrix.cpp

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

gtsam/base/Matrix.h

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

gtsam/base/MatrixSerialization.h

@@ -19,11 +19,12 @@
 // \callgraph
 
 // Defined only if boost serialization is enabled
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #pragma once
 
 #include <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

gtsam/base/ProductLieGroup.h

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

gtsam/base/SymmetricBlockMatrix.h

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

gtsam/base/TestableAssertions.h

@@ -32,7 +32,8 @@ namespace gtsam {
 /**
  * Equals testing for basic types
  */
-inline bool assert_equal(const Key& expected, const Key& actual, double tol = 0.0) {
+inline bool assert_equal(const Key& expected, const Key& actual) {
+  // TODO - why isn't tol used?
   if(expected != actual) {
     std::cout << "Not equal:\nexpected: " << expected << "\nactual: " << actual << std::endl;
     return false;

gtsam/base/Value.h

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

gtsam/base/Vector.cpp

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

gtsam/base/Vector.h

@@ -16,6 +16,7 @@
  * @author  Frank Dellaert
  * @author  Alex Hagiopol
  * @author  Varun Agrawal
+ * @author  Fan Jiang
  */
 
 // \callgraph
@@ -193,14 +194,12 @@ GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b);
  */
 template<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);
 }
 

gtsam/base/VectorSerialization.h

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

gtsam/base/VectorSpace.h

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

gtsam/base/VerticalBlockMatrix.h

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

gtsam/base/cholesky.cpp

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

gtsam/base/concepts.h

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

gtsam/base/serialization.h

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

gtsam/base/serializationTestHelpers.h

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

gtsam/base/std_optional_serialization.h

@@ -11,7 +11,7 @@
 #pragma once
 
 // Defined only if boost serialization is enabled
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <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
 

gtsam/base/tests/testMatrix.cpp

@@ -1151,7 +1151,7 @@ TEST(Matrix, Matrix24IsVectorSpace) {
 }
 
 TEST(Matrix, RowMajorIsVectorSpace) {
-#ifdef GTSAM_USE_BOOST_FEATURES
+#if GTSAM_USE_BOOST_FEATURES
   typedef Eigen::Matrix<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>);

gtsam/base/tests/testNumericalDerivative.cpp

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

gtsam/base/tests/testVector.cpp

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

gtsam/base/timing.cpp

@@ -31,7 +31,9 @@
 
 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_) {

gtsam/base/timing.h

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

gtsam/basis/Basis.h

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

gtsam/basis/Chebyshev2.cpp

@@ -20,6 +20,14 @@
 
 namespace gtsam {
 
+double Chebyshev2::Point(size_t N, int j, double a, double b) {
+  assert(j >= 0 && size_t(j) < N);
+  const double dtheta = M_PI / (N > 1 ? (N - 1) : 1);
+  // We add -PI so that we get values ordered from -1 to +1
+  // sin(-M_PI_2 + dtheta*j); also works
+  return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
+}
+
 Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
   // Allocate space for weights
   Weights weights(N);

gtsam/basis/Chebyshev2.h

@@ -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) {
+  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;
-  }
 
   /// All Chebyshev points
   static Vector Points(size_t N) {

gtsam/config.h.in

@@ -26,7 +26,11 @@
 
 // Paths to example datasets distributed with GTSAM
 #define GTSAM_SOURCE_TREE_DATASET_DIR "@GTSAM_SOURCE_DIR@/examples/Data"
+<<<<<<< HEAD
 #ifndef __QNX__  
+=======
+#if !defined(__QNX__) 
+>>>>>>> 93f463ddbf8e990e6dccc622fcb8ecb67f21549a
 #define GTSAM_INSTALLED_DATASET_DIR "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data"
 #else
 //Set toolbox path to the path on the target.
@@ -47,6 +51,9 @@
 // Whether to enable merging of equal leaf nodes in the Discrete Decision Tree.
 #cmakedefine GTSAM_DT_MERGING
 
+// Whether to enable timing in hybrid factor graph machinery
+#cmakedefine01 GTSAM_HYBRID_TIMING
+
 // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake)
 #cmakedefine GTSAM_USE_TBB
 
@@ -93,3 +100,7 @@
 #cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
 
 #cmakedefine GTSAM_SLOW_BUT_CORRECT_EXPMAP
+
+// Boost flags
+#cmakedefine01 GTSAM_ENABLE_BOOST_SERIALIZATION
+#cmakedefine01 GTSAM_USE_BOOST_FEATURES

gtsam/discrete/AlgebraicDecisionTree.h

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

gtsam/discrete/DecisionTree-inl.h

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

gtsam/discrete/DecisionTree.h

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

gtsam/discrete/DecisionTreeFactor.cpp

@@ -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(
+  DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(size_t nrFrontals,
-      size_t nrFrontals, ADT::Binary op) const {
+                                                             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 {
+  double DecisionTreeFactor::computeThreshold(const size_t N) const {
-    const size_t N = maxNrAssignments;
-
     // 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,
+        for (size_t i = 0; i < std::min(nrAssignments, N); ++i) {
-        // then we insert into the max heap.
+          // If p is larger than the smallest element,
-        if (p > min_heap.top()) {
+          // then we insert into the min heap.
-          for (size_t i = 0; i < std::min(nrAssignments, N); ++i) {
+          // 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;

gtsam/discrete/DecisionTreeFactor.h

@@ -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.
+    /// Disambiguate to use DiscreteFactor version. Mainly for wrapper
-    double operator()(const DiscreteValues& values) const override {
+    using DiscreteFactor::operator();
-      return ADT::operator()(values);
-    }
 
     /// 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 {
+    DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
-      return combine(nrFrontals, ADT::Ring::add);
+      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, ADT::Ring::add);
+      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, ADT::Ring::max);
+      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, ADT::Ring::max);
+      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>

gtsam/discrete/DiscreteBayesNet.h

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

gtsam/discrete/DiscreteConditional.cpp

@@ -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)
+                                         const DiscreteFactor& f)
-    : BaseFactor(f / (*f.sum(nrFrontals))), BaseConditional(nrFrontals) {}
+    : 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));
+    const BaseFactor& f(static_cast<const BaseFactor&>(other));
-    return DecisionTreeFactor::equals(f, tol);
+    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_;
 }
 
 /* ************************************************************************* */

gtsam/discrete/DiscreteConditional.h

@@ -54,7 +54,7 @@ class GTSAM_EXPORT DiscreteConditional
   DiscreteConditional() {}
 
   /// Construct from factor, taking the first `nFrontals` keys as frontals.
-  DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f);
+  DiscreteConditional(size_t nFrontals, const DiscreteFactor& f);
 
   /**
    * Construct from DiscreteKeys and AlgebraicDecisionTree, taking the first
@@ -168,13 +168,9 @@ class GTSAM_EXPORT DiscreteConditional
     static_cast<const BaseConditional*>(this)->print(s, formatter);
   }
 
-  /// Evaluate, just look up in AlgebraicDecisionTree
+  using BaseFactor::error;       ///< DiscreteValues version
-  double evaluate(const DiscreteValues& values) const {
+  using BaseFactor::evaluate;    ///< DiscreteValues version
-    return ADT::operator()(values);
+  using BaseFactor::operator();  ///< DiscreteValues version
-  }
-
-  using DecisionTreeFactor::error;       ///< DiscreteValues version
-  using DecisionTreeFactor::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>

gtsam/discrete/DiscreteDistribution.h

@@ -40,7 +40,7 @@ class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional {
   /// Default constructor needed for serialization.
   DiscreteDistribution() {}
 
-  /// Constructor from factor.
+  /// Constructor from DecisionTreeFactor.
   explicit DiscreteDistribution(const DecisionTreeFactor& f)
       : Base(f.size(), f) {}
 

gtsam/discrete/DiscreteFactor.cpp

@@ -28,6 +28,11 @@ using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+bool DiscreteFactor::equals(const DiscreteFactor& lf, double tol) const {
+  return Base::equals(lf, tol) && cardinalities_ == lf.cardinalities_;
+}
+
 /* ************************************************************************ */
 DiscreteKeys DiscreteFactor::discreteKeys() const {
   DiscreteKeys result;

gtsam/discrete/DiscreteFactor.h

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

gtsam/discrete/DiscreteFactorGraph.cpp

@@ -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 {
+  DiscreteFactor::shared_ptr DiscreteFactorGraph::product() const {
-    DecisionTreeFactor result;
+    DiscreteFactor::shared_ptr result;
-    for(const sharedFactor& factor: *this)
+    for (auto it = this->begin(); it != this->end(); ++it) {
-      if (factor) result = (*factor) * result;
+      if (*it) {
+        if (result) {
+          result = result->multiply(*it);
+        } else {
+          // Assign to the first non-null factor
+          result = *it;
+        }
+      }
+    }
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
-  double DiscreteFactorGraph::operator()(
+  double DiscreteFactorGraph::operator()(const DiscreteValues& values) const {
-      const DiscreteValues &values) const {
     double product = 1.0;
-    for( const sharedFactor& factor: factors_ )
+    for (const sharedFactor& factor : factors_) {
-      product *= (*factor)(values);
+      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
+  DiscreteFactor::shared_ptr DiscreteFactorGraph::scaledProduct() const {
-  std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>  //
-  EliminateForMPE(const DiscreteFactorGraph& factors,
-                  const Ordering& frontalKeys) {
     // PRODUCT: multiply all factors
     gttic(product);
-    DecisionTreeFactor product;
+    DiscreteFactor::shared_ptr product = this->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());
+    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,
+    auto lookup = std::make_shared<DiscreteLookupTable>(
-                                                          orderedKeys, product);
+        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(
+  DiscreteValues DiscreteFactorGraph::optimize(const Ordering& ordering) const {
-      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
+    DiscreteFactor::shared_ptr product = factors.scaledProduct();
-    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);
 
     // 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 =
+    auto conditional = std::make_shared<DiscreteConditional>(
-        std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
+        product->toDecisionTreeFactor(), sum->toDecisionTreeFactor(),
+        orderedKeys);
     gttoc(divide);
 
     return {conditional, sum};

gtsam/discrete/DiscreteFactorGraph.h

@@ -14,6 +14,7 @@
  * @date Feb 14, 2011
  * @author Duy-Nguyen Ta
  * @author Frank Dellaert
+ * @author Varun Agrawal
  */
 
 #pragma once
@@ -48,7 +49,7 @@ class DiscreteJunctionTree;
  * @ingroup discrete
  */
 GTSAM_EXPORT
-std::pair<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

gtsam/discrete/DiscreteKey.h

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

gtsam/discrete/DiscreteLookupDAG.cpp

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

gtsam/discrete/DiscreteLookupDAG.h

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

gtsam/discrete/Ring.h

@@ -0,0 +1,37 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    Ring.h
+ * @brief   Real Ring definition
+ * @author  Varun Agrawal
+ * @date    Dec 8, 2024
+ */
+
+#pragma once
+
+#include <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; }
+};

gtsam/discrete/SignatureParser.cpp

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

gtsam/discrete/TableDistribution.cpp

@@ -0,0 +1,174 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file TableDistribution.cpp
+ *  @date Dec 22, 2024
+ *  @author Varun Agrawal
+ */
+
+#include <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

gtsam/discrete/TableDistribution.h

@@ -0,0 +1,177 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file TableDistribution.h
+ *  @date Dec 22, 2024
+ *  @author Varun Agrawal
+ */
+
+#pragma once
+
+#include <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

gtsam/discrete/TableFactor.cpp

@@ -62,35 +62,104 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys,
     : TableFactor(dkeys, DecisionTreeFactor(dkeys, dtree)) {}
 
 /**
- * @brief Compute the correct ordering of the leaves in the decision tree.
+ * @brief Compute the indexing of the leaves in the decision tree based on the
+ * assignment and add the (index, leaf) pair to a SparseVector.
  *
- * This is done by first taking all the values which have modulo 0 value with
+ * We visit each leaf in the tree, and using the cardinalities of the keys,
- * the cardinality of the innermost key `n`, and we go up to modulo n.
+ * 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,
+static Eigen::SparseVector<double> ComputeSparseTable(
-                                        const DecisionTreeFactor& dt) {
+    const DiscreteKeys& dkeys, const DecisionTreeFactor& dt) {
-  std::vector<double> probs = dt.probabilities();
+  // SparseVector needs to know the maximum possible index,
-  std::vector<double> ordered;
+  // 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) {
+  // Compute denominators to be used in computing sparse table indices
-    for (size_t idx = 0; idx < probs.size(); ++idx) {
+  std::map<Key, size_t> denominators;
-      if (idx % n == k) {
+  double denom = sparseTable.size();
-        ordered.push_back(probs[idx]);
+  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++) {
+  // If no keys, then return empty DecisionTreeFactor
-    table.push_back(sparse_table_.coeff(i));
+  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;
 }

gtsam/discrete/TableFactor.h

@@ -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.
+ public:
-  /// 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);
+   * 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`,
+  /// Getter for the underlying sparse vector
-  /// is just look up in TableFactor.
+  Eigen::SparseVector<double> sparseTable() const { return sparse_table_; }
-  double evaluate(const DiscreteValues& values) const {
-    return operator()(values);
-  }
 
-  /// Evaluate probability distribution, sugar.
+  /// Evaluate probability distribution, is just look up in TableFactor.
-  double operator()(const DiscreteValues& values) const override;
+  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

gtsam/discrete/discrete.i

@@ -61,14 +61,14 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
   DecisionTreeFactor(const std::vector<gtsam::DiscreteKey>& keys, string table);
 
   DecisionTreeFactor(const gtsam::DiscreteConditional& c);
-  
+
   void print(string s = "DecisionTreeFactor\n",
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::DecisionTreeFactor& other, double tol = 1e-9) const;
 
   size_t cardinality(gtsam::Key j) const;
-  
+
   double operator()(const gtsam::DiscreteValues& values) const;
   gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const;
   size_t cardinality(gtsam::Key j) const;
@@ -168,6 +168,43 @@ virtual class DiscreteDistribution : gtsam::DiscreteConditional {
   std::vector<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();

gtsam/discrete/tests/testDecisionTree.cpp

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

gtsam/discrete/tests/testDecisionTreeFactor.cpp

@@ -30,6 +30,12 @@
 using namespace std;
 using namespace gtsam;
 
+/** Convert Signature into CPT */
+DecisionTreeFactor create(const Signature& signature) {
+  DecisionTreeFactor p(signature.discreteKeys(), signature.cpt());
+  return p;
+}
+
 /* ************************************************************************* */
 TEST(DecisionTreeFactor, ConstructorsMatch) {
   // Declare two keys
@@ -105,21 +111,45 @@ TEST(DecisionTreeFactor, multiplication) {
   CHECK(assert_equal(expected2, actual));
 }
 
+/* ************************************************************************* */
+TEST(DecisionTreeFactor, Divide) {
+  DiscreteKey A(0, 2), S(1, 2);
+  DecisionTreeFactor pA = create(A % "99/1"), pS = create(S % "50/50");
+  DecisionTreeFactor joint = pA * pS;
+
+  DecisionTreeFactor s = joint / pA;
+
+  // Factors are not equal due to difference in keys
+  EXPECT(assert_inequal(pS, s));
+
+  // The underlying data should be the same
+  using ADT = AlgebraicDecisionTree<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);
+  using namespace pruning_fixture;
-  DecisionTreeFactor f(A & B & C, "1 5 3 7 2 6 4 8");
 
   // 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) {

gtsam/discrete/tests/testDiscreteConditional.cpp

@@ -46,7 +46,7 @@ TEST(DiscreteConditional, constructors) {
   DecisionTreeFactor f2(
       X & Y & Z, "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
   DiscreteConditional actual2(1, f2);
-  DecisionTreeFactor expected2 = f2 / *f2.sum(1);
+  DecisionTreeFactor expected2 = f2 / f2.sum(1)->toDecisionTreeFactor();
   EXPECT(assert_equal(expected2, static_cast<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)));
 }
 

gtsam/discrete/tests/testDiscreteFactorGraph.cpp

@@ -94,7 +94,7 @@ TEST_UNSAFE( DiscreteFactorGraph, DiscreteFactorGraphEvaluationTest) {
   EXPECT_DOUBLES_EQUAL( 1.944, graph(values), 1e-9);
 
   // Check if graph product works
-  DecisionTreeFactor product = graph.product();
+  DecisionTreeFactor product = graph.product()->toDecisionTreeFactor();
   EXPECT_DOUBLES_EQUAL( 1.944, product(values), 1e-9);
 }
 
@@ -113,12 +113,13 @@ TEST(DiscreteFactorGraph, test) {
   const Ordering frontalKeys{0};
   const auto [conditional, newFactorPtr] = EliminateDiscrete(graph, frontalKeys);
 
-  DecisionTreeFactor newFactor = *newFactorPtr;
+  DecisionTreeFactor newFactor =
+      *std::dynamic_pointer_cast<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());
+  denominator =
-  // Ensure normalization is correct.
+      expectedFactor.max(expectedFactor.size())->toDecisionTreeFactor();
-  expectedFactor = expectedFactor / *normalization;
+  // Ensure denominator is correct.
+  expectedFactor = expectedFactor / denominator;
   EXPECT(assert_equal(expectedFactor, newFactor));
 
   // Test using elimination tree

gtsam/discrete/tests/testSerializationDiscrete.cpp

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

gtsam/discrete/tests/testTableFactor.cpp

@@ -134,14 +134,77 @@ TEST(TableFactor, constructors) {
   EXPECT(assert_equal(expected, f4));
 
   // Test for 9=3x3 values.
-  DiscreteKey V(0, 3), W(1, 3);
+  DiscreteKey V(0, 3), W(1, 3), O(100, 3);
   DiscreteConditional conditional5(V | W = "1/2/3 5/6/7 9/10/11");
   TableFactor f5(conditional5);
-  // GTSAM_PRINT(f5);
+
-  TableFactor expected_f5(
+  std::string expected_values =
-      X & Y,
+      "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667";
-      "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);
 }
 
 /* ************************************************************************* */

gtsam/geometry/BearingRange.h

@@ -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 };
+  constexpr static const size_t dimB = traits<B>::dimension;
-  enum { dimR = traits<R>::dimension };
+  constexpr static const size_t dimR = traits<R>::dimension;
-  enum { dimension = dimB + dimR };
+  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 {
+  inline constexpr static auto NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0;
-    NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
-  };
 public:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };

gtsam/geometry/Cal3.h

@@ -73,7 +73,7 @@ class GTSAM_EXPORT Cal3 {
   double u0_ = 0.0f, v0_ = 0.0f;  ///< principal point
 
  public:
-  enum { dimension = 5 };
+  inline constexpr static auto dimension = 5;
   ///< shared pointer to calibration object
   using shared_ptr = std::shared_ptr<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>

gtsam/geometry/Cal3Bundler.h

@@ -37,7 +37,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3f {
   // Note: u0 and v0 are constants and not optimized.
 
  public:
-  enum { dimension = 3 };
+  inline constexpr static auto dimension = 3;
   using shared_ptr = std::shared_ptr<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>

gtsam/geometry/Cal3DS2.h

@@ -36,7 +36,7 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
   using Base = Cal3DS2_Base;
 
  public:
-  enum { dimension = 9 };
+  inline constexpr static auto dimension = 9;
 
   ///< shared pointer to stereo calibration object
   using shared_ptr = std::shared_ptr<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>

gtsam/geometry/Cal3DS2_Base.h

@@ -46,7 +46,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
   double tol_ = 1e-5;             ///< tolerance value when calibrating
 
  public:
-  enum { dimension = 9 };
+  inline constexpr static auto dimension = 9;
 
   ///< shared pointer to stereo calibration object
   using shared_ptr = std::shared_ptr<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>

gtsam/geometry/Cal3Fisheye.h

@@ -55,7 +55,7 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
   double tol_ = 1e-5;             ///< tolerance value when calibrating
 
  public:
-  enum { dimension = 9 };
+  inline constexpr static auto dimension = 9;
   ///< shared pointer to fisheye calibration object
   using shared_ptr = std::shared_ptr<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>

gtsam/geometry/Cal3Unified.h

@@ -50,7 +50,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
   double xi_ = 0.0f;  ///< mirror parameter
 
  public:
-  enum { dimension = 10 };
+  inline constexpr static auto dimension = 10;
 
   ///< shared pointer to stereo calibration object
   using shared_ptr = std::shared_ptr<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>

gtsam/geometry/Cal3_S2.h

@@ -33,7 +33,7 @@ namespace gtsam {
  */
 class GTSAM_EXPORT Cal3_S2 : public Cal3 {
  public:
-  enum { dimension = 5 };
+  inline constexpr static auto dimension = 5;
 
   ///< shared pointer to calibration object
   using shared_ptr = std::shared_ptr<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>

gtsam/geometry/Cal3_S2Stereo.h

@@ -32,7 +32,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
   double b_ = 1.0f;  ///< Stereo baseline.
 
  public:
-  enum { dimension = 6 };
+  inline constexpr static auto dimension = 6;
 
   ///< shared pointer to stereo calibration object
   using shared_ptr = std::shared_ptr<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>

gtsam/geometry/Cal3f.cpp

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

gtsam/geometry/Cal3f.h

@@ -34,7 +34,7 @@ namespace gtsam {
  */
 class GTSAM_EXPORT Cal3f : public Cal3 {
  public:
-  enum { dimension = 1 };
+  inline constexpr static auto dimension = 1;
   using shared_ptr = std::shared_ptr<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>

gtsam/geometry/CalibratedCamera.h

@@ -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 {
+  inline constexpr static auto dimension = 6;
-    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>

gtsam/geometry/CameraSet.h

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

gtsam/geometry/EssentialMatrix.h

@@ -82,7 +82,7 @@ class EssentialMatrix {
 
   /// @name Manifold
   /// @{
-  enum { dimension = 5 };
+  inline constexpr static auto dimension = 5;
   inline static size_t Dim() { return dimension;}
   inline size_t dim() const { return dimension;}
 
@@ -180,7 +180,7 @@ class EssentialMatrix {
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>

gtsam/geometry/FundamentalMatrix.cpp

@@ -74,6 +74,20 @@ Matrix3 FundamentalMatrix::matrix() const {
          V_.transpose().matrix();
 }
 
+Vector3 FundamentalMatrix::epipolarLine(const Point2& p,
+                                        OptionalJacobian<3, 7> H) {
+  Vector3 point(p.x(), p.y(), 1);  // Create a point in homogeneous coordinates
+  Vector3 line = matrix() * point;  // Compute the epipolar line
+
+  if (H) {
+    // Compute the Jacobian if requested
+    throw std::runtime_error(
+        "FundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
+  }
+
+  return line;  // Return the epipolar line
+}
+
 void FundamentalMatrix::print(const std::string& s) const {
   std::cout << s << matrix() << std::endl;
 }
@@ -116,6 +130,20 @@ Matrix3 SimpleFundamentalMatrix::matrix() const {
   return Ka().transpose().inverse() * E_.matrix() * Kb().inverse();
 }
 
+Vector3 SimpleFundamentalMatrix::epipolarLine(const Point2& p,
+                                              OptionalJacobian<3, 7> H) {
+  Vector3 point(p.x(), p.y(), 1);  // Create a point in homogeneous coordinates
+  Vector3 line = matrix() * point;  // Compute the epipolar line
+
+  if (H) {
+    // Compute the Jacobian if requested
+    throw std::runtime_error(
+        "SimpleFundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
+  }
+
+  return line;  // Return the epipolar line
+}
+
 void SimpleFundamentalMatrix::print(const std::string& s) const {
   std::cout << s << " E:\n"
             << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_

gtsam/geometry/FundamentalMatrix.h

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

gtsam/geometry/Line3.cpp

@@ -97,7 +97,6 @@ Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
   Rot3 cRw = wRc.inverse();
   Rot3 cRl = cRw * wL.R_;
 
-  Vector2 w_ab;
   Vector3 t = ((wL.R_).transpose() * wTc.translation());
   Vector2 c_ab(wL.a_ - t[0], wL.b_ - t[1]);
 

gtsam/geometry/Line3.h

@@ -47,7 +47,7 @@ class GTSAM_EXPORT Line3 {
   double a_, b_;  // Intersection of line with the world x-y plane rotated by R_
   // Also the closest point on line to origin
  public:
-  enum { dimension = 4 };
+  inline constexpr static auto dimension = 4;
 
   /** Default constructor is the Z axis **/
   Line3() :

gtsam/geometry/OrientedPlane3.h

@@ -39,9 +39,7 @@ private:
   double d_;    ///< The perpendicular distance to this plane
 
 public:
-  enum {
+  inline constexpr static auto dimension = 3;
-    dimension = 3
-  };
 
   /// @name Constructors
   /// @{