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

Merge branch 'develop' into remove-nrAssignments

release/4.3a0
Varun Agrawal 2023-11-06 17:32:41 -05:00
parent b7deefd744 ba23e45722
commit 9b67c3a57a
345 changed files with 11424 additions and 3252 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
.github/scripts/python.sh vendored
View File

@ -9,14 +9,13 @@ set -x -e
# install TBB with _debug.so files
function install_tbb()
{
echo install_tbb
if [ "$(uname)" == "Linux" ]; then
sudo apt-get -y install libtbb-dev
elif [ "$(uname)" == "Darwin" ]; then
brew install tbb
fi
}
if [ -z ${PYTHON_VERSION+x} ]; then
@ -37,19 +36,18 @@ function install_dependencies()
export PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin
[ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
$PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt
if [ "${GTSAM_WITH_TBB:-OFF}" == "ON" ]; then
install_tbb
fi
}
function build()
{
mkdir $GITHUB_WORKSPACE/build
cd $GITHUB_WORKSPACE/build
export CMAKE_GENERATOR=Ninja
BUILD_PYBIND="ON"
cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
cmake $GITHUB_WORKSPACE \
-B build \
-DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
-DGTSAM_BUILD_TESTS=OFF \
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_USE_QUATERNIONS=OFF \
@ -65,16 +63,16 @@ function build()
# Set to 2 cores so that Actions does not error out during resource provisioning.
make -j2 install
cmake --build build -j2
cd $GITHUB_WORKSPACE/build/python
$PYTHON -m pip install --user .
$PYTHON -m pip install --user build/python
}
function test()
{
cd $GITHUB_WORKSPACE/python/gtsam/tests
$PYTHON -m unittest discover -v
cd $GITHUB_WORKSPACE
}
# select between build or test

43
.github/scripts/unix.sh vendored
View File

@ -5,33 +5,30 @@
# Specifically Linux and macOS.
##########################################################
set -e # Make sure any error makes the script to return an error code
set -x # echo
# install TBB with _debug.so files
function install_tbb()
{
echo install_tbb
if [ "$(uname)" == "Linux" ]; then
sudo apt-get -y install libtbb-dev
elif [ "$(uname)" == "Darwin" ]; then
brew install tbb
fi
}
# common tasks before either build or test
function configure()
{
set -e # Make sure any error makes the script to return an error code
set -x # echo
# delete old build
rm -rf build
SOURCE_DIR=$GITHUB_WORKSPACE
BUILD_DIR=$GITHUB_WORKSPACE/build
#env
rm -fr $BUILD_DIR || true
mkdir $BUILD_DIR && cd $BUILD_DIR
[ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb
if [ "${GTSAM_WITH_TBB:-OFF}" == "ON" ]; then
install_tbb
fi
if [ ! -z "$GCC_VERSION" ]; then
export CC=gcc-$GCC_VERSION
@ -40,7 +37,9 @@ function configure()
# GTSAM_BUILD_WITH_MARCH_NATIVE=OFF: to avoid crashes in builder VMs
# CMAKE_CXX_FLAGS="-w": Suppress warnings to avoid IO latency in CI logs
cmake $SOURCE_DIR \
export CMAKE_GENERATOR=Ninja
cmake $GITHUB_WORKSPACE \
-B build \
-DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \
-DCMAKE_CXX_FLAGS="-w" \
-DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
@ -62,9 +61,9 @@ function configure()
function finish ()
{
# Print ccache stats
[ -x "$(command -v ccache)" ] && ccache -s
cd $SOURCE_DIR
if [ -x "$(command -v ccache)" ]; then
ccache -s
fi
}
# compile the code with the intent of populating the cache
@ -77,12 +76,12 @@ function build ()
if [ "$(uname)" == "Linux" ]; then
if (($(nproc) > 2)); then
make -j4
cmake --build build -j4
else
make -j2
cmake --build build -j2
fi
elif [ "$(uname)" == "Darwin" ]; then
make -j$(sysctl -n hw.physicalcpu)
cmake --build build -j$(sysctl -n hw.physicalcpu)
fi
finish
@ -99,12 +98,12 @@ function test ()
# Actual testing
if [ "$(uname)" == "Linux" ]; then
if (($(nproc) > 2)); then
make -j$(nproc) check
cmake --build build -j$(nproc) --target check
else
make -j2 check
cmake --build build -j2 --target check
fi
elif [ "$(uname)" == "Darwin" ]; then
make -j$(sysctl -n hw.physicalcpu) check
cmake --build build -j$(sysctl -n hw.physicalcpu) --target check
fi
finish

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

@ -2,6 +2,12 @@ name: Linux CI
on: [pull_request]
# Every time you make a push to your PR, it cancel immediately the previous checks,
# and start a new one. The other runner will be available more quickly to your PR.
concurrency:
group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }}
cancel-in-progress: true
jobs:
build:
name: ${{ matrix.name }} ${{ matrix.build_type }}
@ -67,7 +73,7 @@ jobs:
fi
sudo apt-get -y update
sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev
sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev ninja-build
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib

6
.github/workflows/build-macos.yml vendored
View File

@ -2,6 +2,12 @@ name: macOS CI
on: [pull_request]
# Every time you make a push to your PR, it cancel immediately the previous checks,
# and start a new one. The other runner will be available more quickly to your PR.
concurrency:
group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }}
cancel-in-progress: true
jobs:
build:
name: ${{ matrix.name }} ${{ matrix.build_type }}

13
.github/workflows/build-python.yml vendored
View File

@ -2,6 +2,12 @@ name: Python CI
on: [pull_request]
# Every time you make a push to your PR, it cancel immediately the previous checks,
# and start a new one. The other runner will be available more quickly to your PR.
concurrency:
group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }}
cancel-in-progress: true
jobs:
build:
name: ${{ matrix.name }} ${{ matrix.build_type }} Python ${{ matrix.python_version }}
@ -69,7 +75,7 @@ jobs:
fi
sudo apt-get -y update
sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libboost-all-dev
sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libboost-all-dev ninja-build
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
@ -103,10 +109,13 @@ jobs:
with:
swap-size-gb: 6
- name: Install Dependencies
- name: Install System Dependencies
run: |
bash .github/scripts/python.sh -d
- name: Install Python Dependencies
run: python$PYTHON_VERSION -m pip install -r python/dev_requirements.txt
- name: Build
run: |
bash .github/scripts/python.sh -b

8
.github/workflows/build-special.yml vendored
View File

@ -2,6 +2,12 @@ name: Special Cases CI
on: [pull_request]
# Every time you make a push to your PR, it cancel immediately the previous checks,
# and start a new one. The other runner will be available more quickly to your PR.
concurrency:
group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }}
cancel-in-progress: true
jobs:
build:
name: ${{ matrix.name }} ${{ matrix.build_type }}
@ -97,7 +103,7 @@ jobs:
sudo add-apt-repository "deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy main"
fi
sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev
sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev ninja-build
if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib

55
.github/workflows/build-windows.yml vendored
View File

@ -2,6 +2,12 @@ name: Windows CI
on: [pull_request]
# Every time you make a push to your PR, it cancel immediately the previous checks,
# and start a new one. The other runner will be available more quickly to your PR.
concurrency:
group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }}
cancel-in-progress: true
jobs:
build:
name: ${{ matrix.name }} ${{ matrix.build_type }}
@ -28,6 +34,7 @@ jobs:
Debug,
Release
]
build_unstable: [ON]
include:
- name: windows-2019-cl
@ -87,32 +94,66 @@ jobs:
- name: Checkout
uses: actions/checkout@v3
- name: Setup msbuild
uses: ilammy/msvc-dev-cmd@v1
with:
arch: x${{ matrix.platform }}
- name: Configuration
shell: bash
run: |
export CMAKE_GENERATOR=Ninja
cmake -E remove_directory build
cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib"
cmake -B build \
-S . \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V43=OFF \
-DBOOST_ROOT="${BOOST_ROOT}" \
-DBOOST_INCLUDEDIR="${BOOST_ROOT}\boost\include" \
-DBOOST_LIBRARYDIR="${BOOST_ROOT}\lib"
- name: Build
shell: bash
run: |
# Since Visual Studio is a multi-generator, we need to use --config
# https://stackoverflow.com/a/24470998/1236990
cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam
cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam_unstable
cmake --build build -j 4 --config ${{ matrix.build_type }} --target wrap
- name: Test
shell: bash
run: |
# Run GTSAM tests
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.basis
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete
#cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.geometry
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.inference
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.navigation
#cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.nonlinear
#cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sam
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sam
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm
#cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.slam
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam
# Run GTSAM_UNSTABLE tests
#cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable
cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable
# Compile. Fail with exception
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable
# Compile. Fail with exception
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable
# Compile. Fail with exception
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable
# Compile. Fail with exception
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable
# Compile. Fail with exception
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable
# Compile. Fail with exception
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable
# Compile. Fail with exception
# cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition
# Run all tests
# cmake --build build -j1 --config ${{ matrix.build_type }} --target check

36
CMakeLists.txt
View File

@ -1,11 +1,5 @@
cmake_minimum_required(VERSION 3.0)
# new feature to Cmake Version > 2.8.12
# Mac ONLY. Define Relative Path on Mac OS
if(NOT DEFINED CMAKE_MACOSX_RPATH)
set(CMAKE_MACOSX_RPATH 0)
endif()
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 4)
set (GTSAM_VERSION_MINOR 3)
@ -13,18 +7,16 @@ set (GTSAM_VERSION_PATCH 0)
set (GTSAM_PRERELEASE_VERSION "a0")
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
if (${GTSAM_VERSION_PATCH} EQUAL 0)
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}${GTSAM_PRERELEASE_VERSION}")
if ("${GTSAM_PRERELEASE_VERSION}" STREQUAL "")
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
else()
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}${GTSAM_PRERELEASE_VERSION}")
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}${GTSAM_PRERELEASE_VERSION}")
endif()
project(GTSAM
LANGUAGES CXX C
VERSION "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
message(STATUS "GTSAM Version: ${GTSAM_VERSION_STRING}")
set (CMAKE_PROJECT_VERSION_MAJOR ${GTSAM_VERSION_MAJOR})
set (CMAKE_PROJECT_VERSION_MINOR ${GTSAM_VERSION_MINOR})
set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH})
@ -32,17 +24,18 @@ set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH})
###############################################################################
# Gather information, perform checks, set defaults
if(MSVC)
set(MSVC_LINKER_FLAGS "/FORCE:MULTIPLE")
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_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
include(GtsamMakeConfigFile)
include(GNUInstallDirs)
# Load build type flags and default to Debug mode
include(GtsamBuildTypes)
# Use macros for creating tests/timing scripts
include(GtsamTesting)
include(GtsamPrinting)
# guard against in-source builds
if(${GTSAM_SOURCE_DIR} STREQUAL ${GTSAM_BINARY_DIR})
message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ")
@ -50,6 +43,13 @@ endif()
include(cmake/HandleGeneralOptions.cmake) # CMake build options
# Load build type flags and default to Debug mode
include(GtsamBuildTypes)
# Use macros for creating tests/timing scripts
include(GtsamTesting)
include(GtsamPrinting)
############### Decide on BOOST ######################################
# Enable or disable serialization with GTSAM_ENABLE_BOOST_SERIALIZATION
option(GTSAM_ENABLE_BOOST_SERIALIZATION "Enable Boost serialization" ON)

2
INSTALL.md
View File

@ -182,7 +182,7 @@ Here are some tips to get the best possible performance out of GTSAM.
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. Add `-march=native` to `GTSAM_CMAKE_CXX_FLAGS`. A performance gain of
3. Use `GTSAM_BUILD_WITH_MARCH_NATIVE`. A performance gain of
25-30% can be expected on modern processors. Note that this affects the portability
of your executable. It may not run when copied to another system with older/different
processor architecture.

2
README.md
View File

@ -4,7 +4,7 @@
**As of January 2023, the `develop` branch is officially in "Pre 4.3" mode. We envision several API-breaking changes as we switch to C++17 and away from boost.**
In addition, features deprecated in 4.2 will be removed. Please use the last [4.2a8 release](https://github.com/borglab/gtsam/releases/tag/4.2a8) if you need those features. However, most are easily converted and can be tracked down (in 4.2) by disabling the cmake flag `GTSAM_ALLOW_DEPRECATED_SINCE_V42`.
In addition, features deprecated in 4.2 will be removed. Please use the stable [4.2 release](https://github.com/borglab/gtsam/releases/tag/4.2) if you need those features. However, most are easily converted and can be tracked down (in 4.2) by disabling the cmake flag `GTSAM_ALLOW_DEPRECATED_SINCE_V42`.
## What is GTSAM?

4
cmake/Config.cmake.in
View File

@ -21,6 +21,10 @@ else()
find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
endif()
if(@GTSAM_USE_TBB@)
find_dependency(TBB 4.4 COMPONENTS tbb tbbmalloc)
endif()
if(@GTSAM_USE_SYSTEM_EIGEN@)
find_dependency(Eigen3 REQUIRED)
endif()

323
cmake/FindTBB.cmake
View File

@ -1,323 +0,0 @@
# The MIT License (MIT)
#
# Copyright (c) 2015 Justus Calvin
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# FindTBB
# -------
#
# Find TBB include directories and libraries.
#
# Usage:
#
# find_package(TBB [major[.minor]] [EXACT]
# [QUIET] [REQUIRED]
# [[COMPONENTS] [components...]]
# [OPTIONAL_COMPONENTS components...])
#
# where the allowed components are tbbmalloc and tbb_preview. Users may modify
# the behavior of this module with the following variables:
#
# * TBB_ROOT_DIR - The base directory the of TBB installation.
# * TBB_INCLUDE_DIR - The directory that contains the TBB headers files.
# * TBB_LIBRARY - The directory that contains the TBB library files.
# * TBB_<library>_LIBRARY - The path of the TBB the corresponding TBB library.
# These libraries, if specified, override the
# corresponding library search results, where <library>
# may be tbb, tbb_debug, tbbmalloc, tbbmalloc_debug,
# tbb_preview, or tbb_preview_debug.
# * TBB_USE_DEBUG_BUILD - The debug version of tbb libraries, if present, will
# be used instead of the release version.
#
# Users may modify the behavior of this module with the following environment
# variables:
#
# * TBB_INSTALL_DIR
# * TBBROOT
# * LIBRARY_PATH
#
# This module will set the following variables:
#
# * TBB_FOUND - Set to false, or undefined, if we haven’t found, or
# don’t want to use TBB.
# * TBB_<component>_FOUND - If False, optional <component> part of TBB sytem is
# not available.
# * TBB_VERSION - The full version string
# * TBB_VERSION_MAJOR - The major version
# * TBB_VERSION_MINOR - The minor version
# * TBB_INTERFACE_VERSION - The interface version number defined in
# tbb/tbb_stddef.h.
# * TBB_<library>_LIBRARY_RELEASE - The path of the TBB release version of
# <library>, where <library> may be tbb, tbb_debug,
# tbbmalloc, tbbmalloc_debug, tbb_preview, or
# tbb_preview_debug.
# * TBB_<library>_LIBRARY_DEGUG - The path of the TBB release version of
# <library>, where <library> may be tbb, tbb_debug,
# tbbmalloc, tbbmalloc_debug, tbb_preview, or
# tbb_preview_debug.
#
# The following varibles should be used to build and link with TBB:
#
# * TBB_INCLUDE_DIRS - The include directory for TBB.
# * TBB_LIBRARIES - The libraries to link against to use TBB.
# * TBB_LIBRARIES_RELEASE - The release libraries to link against to use TBB.
# * TBB_LIBRARIES_DEBUG - The debug libraries to link against to use TBB.
# * TBB_DEFINITIONS - Definitions to use when compiling code that uses
# TBB.
# * TBB_DEFINITIONS_RELEASE - Definitions to use when compiling release code that
# uses TBB.
# * TBB_DEFINITIONS_DEBUG - Definitions to use when compiling debug code that
# uses TBB.
#
# This module will also create the "tbb" target that may be used when building
# executables and libraries.
include(FindPackageHandleStandardArgs)
if(NOT TBB_FOUND)
##################################
# Check the build type
##################################
if(NOT DEFINED TBB_USE_DEBUG_BUILD)
# Set build type to RELEASE by default for optimization.
set(TBB_BUILD_TYPE RELEASE)
elseif(TBB_USE_DEBUG_BUILD)
set(TBB_BUILD_TYPE DEBUG)
else()
set(TBB_BUILD_TYPE RELEASE)
endif()
##################################
# Set the TBB search directories
##################################
# Define search paths based on user input and environment variables
set(TBB_SEARCH_DIR ${TBB_ROOT_DIR} $ENV{TBB_INSTALL_DIR} $ENV{TBBROOT})
# Define the search directories based on the current platform
if(CMAKE_SYSTEM_NAME STREQUAL "Windows")
set(TBB_DEFAULT_SEARCH_DIR "C:/Program Files/Intel/TBB"
"C:/Program Files (x86)/Intel/TBB")
# Set the target architecture
if(CMAKE_SIZEOF_VOID_P EQUAL 8)
set(TBB_ARCHITECTURE "intel64")
else()
set(TBB_ARCHITECTURE "ia32")
endif()
# Set the TBB search library path search suffix based on the version of VC
if(WINDOWS_STORE)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11_ui")
elseif(MSVC14)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc14")
elseif(MSVC12)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc12")
elseif(MSVC11)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11")
elseif(MSVC10)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc10")
endif()
# Add the library path search suffix for the VC independent version of TBB
list(APPEND TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc_mt")
elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
# OS X
set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb"
"/usr/local/opt/tbb")
# TODO: Check to see which C++ library is being used by the compiler.
if(NOT ${CMAKE_SYSTEM_VERSION} VERSION_LESS 13.0)
# The default C++ library on OS X 10.9 and later is libc++
set(TBB_LIB_PATH_SUFFIX "lib/libc++" "lib")
else()
set(TBB_LIB_PATH_SUFFIX "lib")
endif()
elseif(CMAKE_SYSTEM_NAME STREQUAL "Linux")
# Linux
set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb")
# TODO: Check compiler version to see the suffix should be <arch>/gcc4.1 or
# <arch>/gcc4.1. For now, assume that the compiler is more recent than
# gcc 4.4.x or later.
if(CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64")
set(TBB_LIB_PATH_SUFFIX "lib/intel64/gcc4.4")
elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "^i.86$")
set(TBB_LIB_PATH_SUFFIX "lib/ia32/gcc4.4")
endif()
endif()
##################################
# Find the TBB include dir
##################################
find_path(TBB_INCLUDE_DIRS tbb/tbb.h
HINTS ${TBB_INCLUDE_DIR} ${TBB_SEARCH_DIR}
PATHS ${TBB_DEFAULT_SEARCH_DIR}
PATH_SUFFIXES include)
##################################
# Set version strings
##################################
if(TBB_INCLUDE_DIRS)
set(_tbb_version_file_prior_to_tbb_2021_1 "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h")
set(_tbb_version_file_after_tbb_2021_1 "${TBB_INCLUDE_DIRS}/oneapi/tbb/version.h")
if (EXISTS "${_tbb_version_file_prior_to_tbb_2021_1}")
file(READ "${_tbb_version_file_prior_to_tbb_2021_1}" _tbb_version_file )
elseif (EXISTS "${_tbb_version_file_after_tbb_2021_1}")
file(READ "${_tbb_version_file_after_tbb_2021_1}" _tbb_version_file )
else()
message(FATAL_ERROR "Found TBB installation: ${TBB_INCLUDE_DIRS} "
"missing version header.")
endif()
string(REGEX REPLACE ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1"
TBB_VERSION_MAJOR "${_tbb_version_file}")
string(REGEX REPLACE ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1"
TBB_VERSION_MINOR "${_tbb_version_file}")
string(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1"
TBB_INTERFACE_VERSION "${_tbb_version_file}")
set(TBB_VERSION "${TBB_VERSION_MAJOR}.${TBB_VERSION_MINOR}")
endif()
##################################
# Find TBB components
##################################
if(TBB_VERSION VERSION_LESS 4.3)
set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc tbb)
else()
set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc_proxy tbbmalloc tbb)
endif()
# Find each component
foreach(_comp ${TBB_SEARCH_COMPOMPONENTS})
if(";${TBB_FIND_COMPONENTS};tbb;" MATCHES ";${_comp};")
# Search for the libraries
find_library(TBB_${_comp}_LIBRARY_RELEASE ${_comp}
HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR}
PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH
PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX})
find_library(TBB_${_comp}_LIBRARY_DEBUG ${_comp}_debug
HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR}
PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH
PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX})
if(TBB_${_comp}_LIBRARY_DEBUG)
list(APPEND TBB_LIBRARIES_DEBUG "${TBB_${_comp}_LIBRARY_DEBUG}")
endif()
if(TBB_${_comp}_LIBRARY_RELEASE)
list(APPEND TBB_LIBRARIES_RELEASE "${TBB_${_comp}_LIBRARY_RELEASE}")
endif()
if(TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE} AND NOT TBB_${_comp}_LIBRARY)
set(TBB_${_comp}_LIBRARY "${TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE}}")
endif()
if(TBB_${_comp}_LIBRARY AND EXISTS "${TBB_${_comp}_LIBRARY}")
set(TBB_${_comp}_FOUND TRUE)
else()
set(TBB_${_comp}_FOUND FALSE)
endif()
# Mark internal variables as advanced
mark_as_advanced(TBB_${_comp}_LIBRARY_RELEASE)
mark_as_advanced(TBB_${_comp}_LIBRARY_DEBUG)
mark_as_advanced(TBB_${_comp}_LIBRARY)
endif()
endforeach()
##################################
# Set compile flags and libraries
##################################
set(TBB_DEFINITIONS_RELEASE "")
set(TBB_DEFINITIONS_DEBUG "-DTBB_USE_DEBUG=1")
if(TBB_LIBRARIES_${TBB_BUILD_TYPE})
set(TBB_DEFINITIONS "${TBB_DEFINITIONS_${TBB_BUILD_TYPE}}")
set(TBB_LIBRARIES "${TBB_LIBRARIES_${TBB_BUILD_TYPE}}")
elseif(TBB_LIBRARIES_RELEASE)
set(TBB_DEFINITIONS "${TBB_DEFINITIONS_RELEASE}")
set(TBB_LIBRARIES "${TBB_LIBRARIES_RELEASE}")
elseif(TBB_LIBRARIES_DEBUG)
set(TBB_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}")
set(TBB_LIBRARIES "${TBB_LIBRARIES_DEBUG}")
endif()
find_package_handle_standard_args(TBB
REQUIRED_VARS TBB_INCLUDE_DIRS TBB_LIBRARIES
HANDLE_COMPONENTS
VERSION_VAR TBB_VERSION)
##################################
# Create targets
##################################
if(NOT CMAKE_VERSION VERSION_LESS 3.0 AND TBB_FOUND)
# Start fix to support different targets for tbb, tbbmalloc, etc.
# (Jose Luis Blanco, Jan 2019)
# Iterate over tbb, tbbmalloc, etc.
foreach(libname ${TBB_SEARCH_COMPOMPONENTS})
if ((NOT TBB_${libname}_LIBRARY_RELEASE) AND (NOT TBB_${libname}_LIBRARY_DEBUG))
continue()
endif()
add_library(${libname} SHARED IMPORTED)
set_target_properties(${libname} PROPERTIES
INTERFACE_INCLUDE_DIRECTORIES ${TBB_INCLUDE_DIRS}
IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_RELEASE})
if(TBB_${libname}_LIBRARY_RELEASE AND TBB_${libname}_LIBRARY_DEBUG)
set_target_properties(${libname} PROPERTIES
INTERFACE_COMPILE_DEFINITIONS "$<$<OR:$<CONFIG:Debug>,$<CONFIG:RelWithDebInfo>>:TBB_USE_DEBUG=1>"
IMPORTED_LOCATION_DEBUG ${TBB_${libname}_LIBRARY_DEBUG}
IMPORTED_LOCATION_RELWITHDEBINFO ${TBB_${libname}_LIBRARY_DEBUG}
IMPORTED_LOCATION_RELEASE ${TBB_${libname}_LIBRARY_RELEASE}
IMPORTED_LOCATION_MINSIZEREL ${TBB_${libname}_LIBRARY_RELEASE}
)
elseif(TBB_${libname}_LIBRARY_RELEASE)
set_target_properties(${libname} PROPERTIES IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_RELEASE})
else()
set_target_properties(${libname} PROPERTIES
INTERFACE_COMPILE_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}"
IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_DEBUG}
)
endif()
endforeach()
# End of fix to support different targets
endif()
mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARIES)
unset(TBB_ARCHITECTURE)
unset(TBB_BUILD_TYPE)
unset(TBB_LIB_PATH_SUFFIX)
unset(TBB_DEFAULT_SEARCH_DIR)
endif()

18
cmake/GtsamBuildTypes.cmake
View File

@ -55,9 +55,6 @@ if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
"Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo." FORCE)
endif()
# Add option for using build type postfixes to allow installing multiple build modes
option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON)
# Define all cache variables, to be populated below depending on the OS/compiler:
set(GTSAM_COMPILE_OPTIONS_PRIVATE "" CACHE INTERNAL "(Do not edit) Private compiler flags for all build configurations." FORCE)
set(GTSAM_COMPILE_OPTIONS_PUBLIC "" CACHE INTERNAL "(Do not edit) Public compiler flags (exported to user projects) for all build configurations." FORCE)
@ -82,6 +79,13 @@ set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELWITHDEBINFO "NDEBUG" CACHE STRING "(Us
set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELEASE "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for Release configuration.")
set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_PROFILING "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for Profiling configuration.")
set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_TIMING "NDEBUG;ENABLE_TIMING" CACHE STRING "(User editable) Private preprocessor macros for Timing configuration.")
mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_DEBUG)
mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELWITHDEBINFO)
mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELEASE)
mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_PROFILING)
mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_TIMING)
if(MSVC)
# Common to all configurations:
list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE
@ -143,6 +147,13 @@ else()
set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING -g -O3 CACHE STRING "(User editable) Private compiler flags for Timing configuration.")
endif()
mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON)
mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG)
mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO)
mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE)
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.")
@ -198,7 +209,6 @@ if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
endif()
if (NOT MSVC)
option(GTSAM_BUILD_WITH_MARCH_NATIVE "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" OFF)
if(GTSAM_BUILD_WITH_MARCH_NATIVE)
# Check if Apple OS and compiler is [Apple]Clang
if(APPLE AND (${CMAKE_CXX_COMPILER_ID} MATCHES "^(Apple)?Clang$"))

16
cmake/GtsamTesting.cmake
View File

@ -42,7 +42,7 @@ endmacro()
# GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'.
#
# Usage example:
# gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
# gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib" ON)
#
# Arguments:
# globPatterns: The list of files or glob patterns from which to create examples, with
@ -51,8 +51,9 @@ endmacro()
# excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass
# an empty string "" if nothing needs to be excluded.
# linkLibraries: The list of libraries to link to.
macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries)
gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "examples" ${GTSAM_BUILD_EXAMPLES_ALWAYS})
# buildWithAll: Build examples with `make` and/or `make all`
macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries buildWithAll)
gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "examples" ${buildWithAll})
endmacro()
@ -76,8 +77,9 @@ endmacro()
# excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass
# an empty string "" if nothing needs to be excluded.
# linkLibraries: The list of libraries to link to.
macro(gtsamAddTimingGlob globPatterns excludedFiles linkLibraries)
gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "timing" ${GTSAM_BUILD_TIMING_ALWAYS})
# buildWithAll: Build examples with `make` and/or `make all`
macro(gtsamAddTimingGlob globPatterns excludedFiles linkLibraries buildWithAll)
gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "timing" ${buildWithAll})
endmacro()
@ -86,9 +88,8 @@ endmacro()
# Build macros for using tests
enable_testing()
#TODO(Varun) Move to HandlePrintConfiguration.cmake. This will require additional changes.
option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON)
option(GTSAM_BUILD_EXAMPLES_ALWAYS "Build examples with 'make all' (build with 'make examples' if not)" ON)
option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF)
# Add option for combining unit tests
if(MSVC OR XCODE_VERSION)
@ -123,6 +124,7 @@ add_custom_target(timing)
# Implementations of this file's macros:
macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
#TODO(Varun) Building of tests should not depend on global gtsam flag
if(GTSAM_BUILD_TESTS)
# Add group target if it doesn't already exist
if(NOT TARGET check.${groupName})

13
cmake/HandleGeneralOptions.cmake
View File

@ -8,6 +8,18 @@ else()
set(GTSAM_UNSTABLE_AVAILABLE 0)
endif()
### GtsamTesting related options
option(GTSAM_BUILD_EXAMPLES_ALWAYS "Build examples with 'make all' (build with 'make examples' if not)" ON)
option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF)
###
# Add option for using build type postfixes to allow installing multiple build modes
option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON)
if (NOT MSVC)
option(GTSAM_BUILD_WITH_MARCH_NATIVE "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" OFF)
endif()
# Configurable Options
if(GTSAM_UNSTABLE_AVAILABLE)
option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON)
@ -32,6 +44,7 @@ option(GTSAM_ALLOW_DEPRECATED_SINCE_V43 "Allow use of methods/functions depr
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
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)
message(STATUS "GTSAM is a shared library due to GTSAM_FORCE_SHARED_LIB")

2
cmake/HandleTBB.cmake
View File

@ -14,7 +14,7 @@ if (GTSAM_WITH_TBB)
endif()
# all definitions and link requisites will go via imported targets:
# tbb & tbbmalloc
list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
list(APPEND GTSAM_ADDITIONAL_LIBRARIES TBB::tbb TBB::tbbmalloc)
else()
set(GTSAM_USE_TBB 0) # This will go into config.h
endif()

5
cmake/README.html
View File

@ -47,9 +47,9 @@ linkLibraries: The list of libraries to link to in addition to CppUnitLite.
</code></pre>
</li>
<li>
<p><code>gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries)</code> Add scripts that will serve as examples of how to use the library. A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file. These executables will not be installed. They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'.</p>
<p><code>gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries buildWithAll)</code> Add scripts that will serve as examples of how to use the library. A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file. These executables will not be installed. They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'.</p>
<p>Usage example:</p>
<pre><code>gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
<pre><code>gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib" ON)
</code></pre>
<p>Arguments:</p>
<pre><code>globPatterns: The list of files or glob patterns from which to create unit tests, with
@ -58,6 +58,7 @@ linkLibraries: The list of libraries to link to in addition to CppUnitLite.
excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass
an empty string "" if nothing needs to be excluded.
linkLibraries: The list of libraries to link to.
buildWithAll: Build examples with `make` and/or `make all`
</code></pre>
</li>
</ul>

5
cmake/README.md
View File

@ -52,11 +52,11 @@ Defines two useful functions for creating CTest unit tests. Also immediately cr
Pass an empty string "" if nothing needs to be excluded.
linkLibraries: The list of libraries to link to in addition to CppUnitLite.
* `gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries)` Add scripts that will serve as examples of how to use the library. A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file. These executables will not be installed. They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'.
* `gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries buildWithAll)` Add scripts that will serve as examples of how to use the library. A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file. These executables will not be installed. They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'.
Usage example:
gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib" ON)
Arguments:
@ -66,6 +66,7 @@ Defines two useful functions for creating CTest unit tests. Also immediately cr
excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass
an empty string "" if nothing needs to be excluded.
linkLibraries: The list of libraries to link to.
buildWithAll: Build examples with `make` and/or `make all`
## GtsamMakeConfigFile

2
doc/CMakeLists.txt
View File

@ -56,7 +56,7 @@ if (GTSAM_BUILD_DOCS)
if (GTSAM_BUILD_UNSTABLE)
list(APPEND doc_subdirs ${gtsam_unstable_doc_subdirs})
endif()
if (GTSAM_BUILD_EXAMPLES)
if (GTSAM_BUILD_EXAMPLES_ALWAYS)
list(APPEND doc_subdirs examples)
endif()

131
doc/Hybrid.lyx
View File

@ -86,7 +86,7 @@ Hybrid Inference
\end_layout
\begin_layout Author
Frank Dellaert & Varun Agrawal
Frank Dellaert
\end_layout
\begin_layout Date
@ -264,12 +264,110 @@ If we take the log of
we get
\begin_inset Formula
\begin{equation}
\log P(x|y,m)=\log P_{m}(x|y)=K_{gcm}-E_{gcm}(x,y).\label{eq:gm_log}
\log P(x|y,m)=\log P_{m}(x|y)=K_{gc}(m)-E_{gcm}(x,y).\label{eq:gm_log}
\end{equation}
\end_inset
Equating
\end_layout
\begin_layout Standard
\noindent
For conciseness, we will write
\begin_inset Formula $K_{gc}(m)$
\end_inset
as
\begin_inset Formula $K_{gcm}$
\end_inset
.
\end_layout
\begin_layout Standard
\SpecialChar allowbreak
\end_layout
\begin_layout Standard
\noindent
The key point here is that
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\xout off
\uuline off
\uwave off
\noun off
\color none
\begin_inset Formula $K_{gm}$
\end_inset
\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\xout default
\uuline default
\uwave default
\noun default
\color inherit
is the log-normalization constant for the complete
\emph on
GaussianMixture
\emph default
across all values of
\begin_inset Formula $m$
\end_inset
, and cannot be dependent on the value of
\begin_inset Formula $m$
\end_inset
.
In contrast,
\begin_inset Formula $K_{gcm}$
\end_inset
is the log-normalization constant for a specific
\emph on
GaussianConditional
\emph default
mode (thus dependent on
\begin_inset Formula $m$
\end_inset
) and can have differing values based on the covariance matrices for each
mode.
Thus to obtain a constant
\begin_inset Formula $K_{gm}$
\end_inset
which satisfies the invariant, we need to specify
\begin_inset Formula $E_{gm}(x,y,m)$
\end_inset
accordingly.
\end_layout
\begin_layout Standard
\SpecialChar allowbreak
\end_layout
\begin_layout Standard
\noindent
By equating
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:gm_invariant"
@ -289,7 +387,7 @@ noprefix "false"
\end_inset
we see that this can be achieved by defining the error
, we see that this can be achieved by defining the error
\begin_inset Formula $E_{gm}(x,y,m)$
\end_inset
@ -541,9 +639,9 @@ noprefix "false"
:
\begin_inset Formula
\[
E_{mf}(y,m)=C+E_{gcm}(\bar{x},y)-K_{gcm}.
\]
\begin{equation}
E_{mf}(y,m)=C+E_{gcm}(\bar{x},y)-K_{gcm}\label{eq:mixture_factor}
\end{equation}
\end_inset
@ -555,7 +653,24 @@ Unfortunately, we can no longer choose
\begin_inset Formula $m$
\end_inset
to make the constant disappear.
to make the constant disappear, since
\begin_inset Formula $C$
\end_inset
has to be a constant applicable across all
\begin_inset Formula $m$
\end_inset
.
\end_layout
\begin_layout Standard
\SpecialChar allowbreak
\end_layout
\begin_layout Standard
\noindent
There are two possibilities:
\end_layout

BIN
doc/Hybrid.pdf
View File

Binary file not shown.

3
docker/ubuntu-gtsam-python/Dockerfile
View File

@ -6,9 +6,6 @@ FROM borglab/ubuntu-gtsam:bionic
# Install pip
RUN apt-get install -y python3-pip python3-dev
# Install python wrapper requirements
RUN python3 -m pip install -U -r /usr/src/gtsam/python/requirements.txt
# Run cmake again, now with python toolbox on
WORKDIR /usr/src/gtsam/build
RUN cmake \

2
examples/CMakeLists.txt
View File

@ -20,4 +20,4 @@ if (NOT GTSAM_USE_BOOST_FEATURES)
)
endif()
gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}")
gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}" ${GTSAM_BUILD_EXAMPLES_ALWAYS})

75
examples/GNCExample.cpp Normal file
View File

@ -0,0 +1,75 @@
/* ----------------------------------------------------------------------------
* 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 GNCExample.cpp
* @brief Simple example showcasing a Graduated Non-Convexity based solver
* @author Achintya Mohan
*/
/**
* A simple 2D pose graph optimization example
* - The robot is initially at origin (0.0, 0.0, 0.0)
* - We have full odometry measurements for 2 motions
* - The robot first moves to (1.0, 0.0, 0.1) and then to (1.0, 1.0, 0.2)
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/GncOptimizer.h>
#include <gtsam/nonlinear/GncParams.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <iostream>
using namespace std;
using namespace gtsam;
int main() {
cout << "Graduated Non-Convexity Example\n";
NonlinearFactorGraph graph;
// Add a prior to the first point, set to the origin
auto priorNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.addPrior(1, Pose2(0.0, 0.0, 0.0), priorNoise);
// Add additional factors, noise models must be Gaussian
Pose2 x1(1.0, 0.0, 0.1);
graph.emplace_shared<BetweenFactor<Pose2>>(1, 2, x1, noiseModel::Isotropic::Sigma(3, 0.2));
Pose2 x2(0.0, 1.0, 0.1);
graph.emplace_shared<BetweenFactor<Pose2>>(2, 3, x2, noiseModel::Isotropic::Sigma(3, 0.4));
// Initial estimates
Values initial;
initial.insert(1, Pose2(0.2, 0.5, -0.1));
initial.insert(2, Pose2(0.8, 0.3, 0.1));
initial.insert(3, Pose2(0.8, 0.2, 0.3));
// Set options for the non-minimal solver
LevenbergMarquardtParams lmParams;
lmParams.setMaxIterations(1000);
lmParams.setRelativeErrorTol(1e-5);
// Set GNC-specific options
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
gncParams.setLossType(GncLossType::TLS);
// Optimize the graph and print results
GncOptimizer<GncParams<LevenbergMarquardtParams>> optimizer(graph, initial, gncParams);
Values result = optimizer.optimize();
result.print("Final Result:");
return 0;
}

6
gtsam/CMakeLists.txt
View File

@ -112,7 +112,7 @@ if(GTSAM_SUPPORT_NESTED_DISSECTION)
endif()
# Versions
set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH})
set(gtsam_version ${GTSAM_VERSION_STRING})
set(gtsam_soversion ${GTSAM_VERSION_MAJOR})
message(STATUS "GTSAM Version: ${gtsam_version}")
message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
@ -145,10 +145,6 @@ if (GTSAM_USE_EIGEN_MKL)
target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})
endif()
if(GTSAM_USE_TBB)
target_include_directories(gtsam PUBLIC ${TBB_INCLUDE_DIRS})
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.

7
gtsam/base/Group.h
View File

@ -22,13 +22,6 @@
#include <gtsam/base/Testable.h>
#ifdef GTSAM_USE_BOOST_FEATURES
#include <boost/concept_check.hpp>
#include <boost/concept/requires.hpp>
#include <boost/type_traits/is_base_of.hpp>
#include <boost/static_assert.hpp>
#endif
#include <utility>
namespace gtsam {

2
gtsam/base/serialization.h
View File

@ -17,6 +17,7 @@
* @date Feb 7, 2012
*/
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#pragma once
#include <Eigen/Core>
@ -270,3 +271,4 @@ void deserializeBinary(const std::string& serialized, T& output,
///@}
} // namespace gtsam
#endif

3
gtsam/base/timing.cpp
View File

@ -19,9 +19,10 @@
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <cassert>
#include <iomanip>
#include <iostream>
#include <map>

6
gtsam/base/treeTraversal-inst.h
View File

@ -138,6 +138,8 @@ void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) {
/** Traverse a forest depth-first with pre-order and post-order visits.
* @param forest The forest of trees to traverse. The method \c forest.roots() should exist
* and return a collection of (shared) pointers to \c FOREST::Node.
* @param rootData The data to pass by reference to \c visitorPre when it is called on each
* root node.
* @param visitorPre \c visitorPre(node, parentData) will be called at every node, before
* visiting its children, and will be passed, by reference, the \c DATA object returned
* by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object
@ -147,8 +149,8 @@ void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) {
* @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting
* its children, and will be passed, by reference, the \c DATA object returned by the
* call to \c visitorPre (the \c DATA object may be modified by visiting the children).
* @param rootData The data to pass by reference to \c visitorPre when it is called on each
* root node. */
* @param problemSizeThreshold
*/
template<class FOREST, typename DATA, typename VISITOR_PRE,
typename VISITOR_POST>
void DepthFirstForestParallel(FOREST& forest, DATA& rootData,

7
gtsam/base/types.h
View File

@ -19,16 +19,11 @@
#pragma once
#include <gtsam/dllexport.h>
#ifdef GTSAM_USE_BOOST_FEATURES
#include <boost/concept/assert.hpp>
#include <boost/range/concepts.hpp>
#endif
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <gtsam/dllexport.h>
#include <cstddef>
#include <cstdint>
#include <exception>
#include <string>

2
gtsam/basis/Basis.h
View File

@ -80,7 +80,7 @@ using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
*
* @ingroup basis
*/
Matrix kroneckerProductIdentity(size_t M, const Weights& w);
Matrix GTSAM_EXPORT kroneckerProductIdentity(size_t M, const Weights& w);
/**
* CRTP Base class for function bases

2
gtsam/config.h.in
View File

@ -86,3 +86,5 @@
// Toggle switch for BetweenFactor jacobian computation
#cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
#cmakedefine GTSAM_SLOW_BUT_CORRECT_EXPMAP

64
gtsam/discrete/AlgebraicDecisionTree.h
View File

@ -29,14 +29,14 @@
namespace gtsam {
/**
* Algebraic Decision Trees fix the range to double
* Just has some nice constructors and some syntactic sugar
* TODO: consider eliminating this class altogether?
* An algebraic decision tree fixes the range of a DecisionTree to double.
* Just has some nice constructors and some syntactic sugar.
* TODO(dellaert): consider eliminating this class altogether?
*
* @ingroup discrete
*/
template <typename L>
class GTSAM_EXPORT AlgebraicDecisionTree : public DecisionTree<L, double> {
class AlgebraicDecisionTree : public DecisionTree<L, double> {
/**
* @brief Default method used by `labelFormatter` or `valueFormatter` when
* printing.
@ -81,12 +81,47 @@ namespace gtsam {
AlgebraicDecisionTree(const L& label, double y1, double y2)
: Base(label, y1, y2) {}
/** Create a new leaf function splitting on a variable */
/**
* @brief Create a new leaf function splitting on a variable
*
* @param labelC: The label with cardinality 2
* @param y1: The value for the first key
* @param y2: The value for the second key
*
* Example:
* @code{.cpp}
* std::pair<string, size_t> A {"a", 2};
* AlgebraicDecisionTree<string> a(A, 0.6, 0.4);
* @endcode
*/
AlgebraicDecisionTree(const typename Base::LabelC& labelC, double y1,
double y2)
: Base(labelC, y1, y2) {}
/** Create from keys and vector table */
/**
* @brief Create from keys with cardinalities and a vector table
*
* @param labelCs: The keys, with cardinalities, given as pairs
* @param ys: The vector table
*
* Example with three keys, A, B, and C, with cardinalities 2, 3, and 2,
* respectively, and a vector table of size 12:
* @code{.cpp}
* DiscreteKey A(0, 2), B(1, 3), C(2, 2);
* const vector<double> cpt{
* 1.0 / 3, 2.0 / 3, 3.0 / 7, 4.0 / 7, 5.0 / 11, 6.0 / 11, //
* 1.0 / 9, 8.0 / 9, 3.0 / 6, 3.0 / 6, 5.0 / 10, 5.0 / 10};
* AlgebraicDecisionTree<Key> expected(A & B & C, cpt);
* @endcode
* The table is given in the following order:
* A=0, B=0, C=0
* A=0, B=0, C=1
* ...
* A=1, B=1, C=1
* Hence, the first line in the table is for A==0, and the second for A==1.
* In each line, the first two entries are for B==0, the next two for B==1,
* and the last two for B==2. Each pair is for a C value of 0 and 1.
*/
AlgebraicDecisionTree //
(const std::vector<typename Base::LabelC>& labelCs,
const std::vector<double>& ys) {
@ -94,7 +129,14 @@ namespace gtsam {
Base::create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end());
}
/** Create from keys and string table */
/**
* @brief Create from keys and string table
*
* @param labelCs: The keys, with cardinalities, given as pairs
* @param table: The string table, given as a string of doubles.
*
* @note Table needs to be in same order as the vector table in the other constructor.
*/
AlgebraicDecisionTree //
(const std::vector<typename Base::LabelC>& labelCs,
const std::string& table) {
@ -109,7 +151,13 @@ namespace gtsam {
Base::create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end());
}
/** Create a new function splitting on a variable */
/**
* @brief Create a range of decision trees, splitting on a single variable.
*
* @param begin: Iterator to beginning of a range of decision trees
* @param end: Iterator to end of a range of decision trees
* @param label: The label to split on
*/
template <typename Iterator>
AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label)
: Base(nullptr) {

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

@ -660,7 +660,7 @@ namespace gtsam {
// B=1
// A=0: 3
// A=1: 4
// Note, through the magic of "compose", build([A B],[1 2 3 4]) will produce
// Note, through the magic of "compose", create([A B],[1 3 2 4]) will produce
// exactly the same tree as above: the highest label is always the root.
// However, it will be *way* faster if labels are given highest to lowest.
template<typename L, typename Y>

31
gtsam/discrete/DecisionTree.h
View File

@ -39,9 +39,23 @@
namespace gtsam {
/**
* Decision Tree
* L = label for variables
* Y = function range (any algebra), e.g., bool, int, double
* @brief a decision tree is a function from assignments to values.
* @tparam L label for variables
* @tparam Y function range (any algebra), e.g., bool, int, double
*
* After creating a decision tree on some variables, the tree can be evaluated
* on an assignment to those variables. Example:
*
* @code{.cpp}
* // Create a decision stump one one variable 'a' with values 10 and 20.
* DecisionTree<char, int> tree('a', 10, 20);
*
* // Evaluate the tree on an assignment to the variable.
* int value0 = tree({{'a', 0}}); // value0 = 10
* int value1 = tree({{'a', 1}}); // value1 = 20
* @endcode
*
* More examples can be found in testDecisionTree.cpp
*
* @ingroup discrete
*/
@ -136,7 +150,8 @@ namespace gtsam {
NodePtr root_;
protected:
/** Internal recursive function to create from keys, cardinalities,
/**
* Internal recursive function to create from keys, cardinalities,
* and Y values
*/
template <typename It, typename ValueIt>
@ -175,7 +190,13 @@ namespace gtsam {
/** Create a constant */
explicit DecisionTree(const Y& y);
/// Create tree with 2 assignments `y1`, `y2`, splitting on variable `label`
/**
* @brief Create tree with 2 assignments `y1`, `y2`, splitting on variable `label`
*
* @param label The variable to split on.
* @param y1 The value for the first assignment.
* @param y2 The value for the second assignment.
*/
DecisionTree(const L& label, const Y& y1, const Y& y2);
/** Allow Label+Cardinality for convenience */

112
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -34,15 +34,12 @@ namespace gtsam {
/* ************************************************************************ */
DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys& keys,
const ADT& potentials)
: DiscreteFactor(keys.indices()),
ADT(potentials),
cardinalities_(keys.cardinalities()) {}
: DiscreteFactor(keys.indices(), keys.cardinalities()), ADT(potentials) {}
/* ************************************************************************ */
DecisionTreeFactor::DecisionTreeFactor(const DiscreteConditional& c)
: DiscreteFactor(c.keys()),
AlgebraicDecisionTree<Key>(c),
cardinalities_(c.cardinalities_) {}
: DiscreteFactor(c.keys(), c.cardinalities()),
AlgebraicDecisionTree<Key>(c) {}
/* ************************************************************************ */
bool DecisionTreeFactor::equals(const DiscreteFactor& other,
@ -85,6 +82,22 @@ namespace gtsam {
ADT::print("", formatter);
}
/* ************************************************************************ */
DecisionTreeFactor DecisionTreeFactor::apply(ADT::Unary op) const {
// apply operand
ADT result = ADT::apply(op);
// Make a new factor
return DecisionTreeFactor(discreteKeys(), result);
}
/* ************************************************************************ */
DecisionTreeFactor DecisionTreeFactor::apply(ADT::UnaryAssignment op) const {
// apply operand
ADT result = ADT::apply(op);
// Make a new factor
return DecisionTreeFactor(discreteKeys(), result);
}
/* ************************************************************************ */
DecisionTreeFactor DecisionTreeFactor::apply(const DecisionTreeFactor& f,
ADT::Binary op) const {
@ -151,15 +164,23 @@ namespace gtsam {
}
// create new factor, note we collect keys that are not in frontalKeys
// TODO(frank): why do we need this??? result should contain correct keys!!!
/*
Due to branch merging, the labels in `result` may be missing some keys
E.g. After branch merging, we may get a ADT like:
Leaf [2] 1.0204082
This is missing the key values used for branching.
*/
KeyVector difference, frontalKeys_(frontalKeys), keys_(keys());
// Get the difference of the frontalKeys and the factor keys using set_difference
std::sort(keys_.begin(), keys_.end());
std::sort(frontalKeys_.begin(), frontalKeys_.end());
std::set_difference(keys_.begin(), keys_.end(), frontalKeys_.begin(),
frontalKeys_.end(), back_inserter(difference));
DiscreteKeys dkeys;
for (i = 0; i < keys().size(); i++) {
Key j = keys()[i];
// TODO(frank): inefficient!
if (std::find(frontalKeys.begin(), frontalKeys.end(), j) !=
frontalKeys.end())
continue;
dkeys.push_back(DiscreteKey(j, cardinality(j)));
for (Key key : difference) {
dkeys.push_back(DiscreteKey(key, cardinality(key)));
}
return std::make_shared<DecisionTreeFactor>(dkeys, result);
}
@ -182,15 +203,47 @@ namespace gtsam {
}
/* ************************************************************************ */
DiscreteKeys DecisionTreeFactor::discreteKeys() const {
DiscreteKeys result;
for (auto&& key : keys()) {
DiscreteKey dkey(key, cardinality(key));
if (std::find(result.begin(), result.end(), dkey) == result.end()) {
result.push_back(dkey);
std::vector<double> DecisionTreeFactor::probabilities() const {
// Set of all keys
std::set<Key> allKeys(keys().begin(), keys().end());
std::vector<double> probs;
/* An operation that takes each leaf probability, and computes the
* nrAssignments by checking the difference between the keys in the factor
* and the keys in the assignment.
* The nrAssignments is then used to append
* the correct number of leaf probability values to the `probs` vector
* defined above.
*/
auto op = [&](const Assignment<Key>& a, double p) {
// Get all the keys in the current assignment
std::set<Key> assignment_keys;
for (auto&& [k, _] : a) {
assignment_keys.insert(k);
}
// Find the keys missing in the assignment
std::vector<Key> diff;
std::set_difference(allKeys.begin(), allKeys.end(),
assignment_keys.begin(), assignment_keys.end(),
std::back_inserter(diff));
// Compute the total number of assignments in the (pruned) subtree
size_t nrAssignments = 1;
for (auto&& k : diff) {
nrAssignments *= cardinalities_.at(k);
}
return result;
// Add p `nrAssignments` times to the probs vector.
probs.insert(probs.end(), nrAssignments, p);
return p;
};
// Go through the tree
this->apply(op);
return probs;
}
/* ************************************************************************ */
@ -289,28 +342,21 @@ namespace gtsam {
/* ************************************************************************ */
DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys& keys,
const vector<double>& table)
: DiscreteFactor(keys.indices()),
AlgebraicDecisionTree<Key>(keys, table),
cardinalities_(keys.cardinalities()) {}
: DiscreteFactor(keys.indices(), keys.cardinalities()),
AlgebraicDecisionTree<Key>(keys, table) {}
/* ************************************************************************ */
DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys& keys,
const string& table)
: DiscreteFactor(keys.indices()),
AlgebraicDecisionTree<Key>(keys, table),
cardinalities_(keys.cardinalities()) {}
: DiscreteFactor(keys.indices(), keys.cardinalities()),
AlgebraicDecisionTree<Key>(keys, table) {}
/* ************************************************************************ */
DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const {
const size_t N = maxNrAssignments;
// Get the probabilities in the decision tree so we can threshold.
std::vector<double> probabilities;
this->visitLeaf([&](const Leaf& leaf) {
const size_t nrAssignments = leaf.nrAssignments();
double prob = leaf.constant();
probabilities.insert(probabilities.end(), nrAssignments, prob);
});
std::vector<double> probabilities = this->probabilities();
// The number of probabilities can be lower than max_leaves
if (probabilities.size() <= N) {

63
gtsam/discrete/DecisionTreeFactor.h
View File

@ -50,10 +50,6 @@ namespace gtsam {
typedef std::shared_ptr<DecisionTreeFactor> shared_ptr;
typedef AlgebraicDecisionTree<Key> ADT;
protected:
std::map<Key, size_t> cardinalities_;
public:
/// @name Standard Constructors
/// @{
@ -63,11 +59,46 @@ namespace gtsam {
/** Constructor from DiscreteKeys and AlgebraicDecisionTree */
DecisionTreeFactor(const DiscreteKeys& keys, const ADT& potentials);
/** Constructor from doubles */
/**
* @brief Constructor from doubles
*
* @param keys The discrete keys.
* @param table The table of values.
*
* @throw std::invalid_argument if the size of `table` does not match the
* number of assignments.
*
* Example:
* @code{.cpp}
* DiscreteKey X(0,2), Y(1,3);
* const std::vector<double> table {2, 5, 3, 6, 4, 7};
* DecisionTreeFactor f1({X, Y}, table);
* @endcode
*
* The values in the table should be laid out so that the first key varies
* the slowest, and the last key the fastest.
*/
DecisionTreeFactor(const DiscreteKeys& keys,
const std::vector<double>& table);
/** Constructor from string */
/**
* @brief Constructor from string
*
* @param keys The discrete keys.
* @param table The table of values.
*
* @throw std::invalid_argument if the size of `table` does not match the
* number of assignments.
*
* Example:
* @code{.cpp}
* DiscreteKey X(0,2), Y(1,3);
* DecisionTreeFactor factor({X, Y}, "2 5 3 6 4 7");
* @endcode
*
* The values in the table should be laid out so that the first key varies
* the slowest, and the last key the fastest.
*/
DecisionTreeFactor(const DiscreteKeys& keys, const std::string& table);
/// Single-key specialization
@ -119,8 +150,6 @@ namespace gtsam {
static double safe_div(const double& a, const double& b);
size_t cardinality(Key j) const { return cardinalities_.at(j); }
/// divide by factor f (safely)
DecisionTreeFactor operator/(const DecisionTreeFactor& f) const {
return apply(f, safe_div);
@ -153,6 +182,19 @@ namespace gtsam {
/// @name Advanced Interface
/// @{
/**
* Apply unary operator (*this) "op" f
* @param op a unary operator that operates on AlgebraicDecisionTree
*/
DecisionTreeFactor apply(ADT::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;
/**
* Apply binary operator (*this) "op" f
* @param f the second argument for op
@ -179,8 +221,8 @@ namespace gtsam {
/// Enumerate all values into a map from values to double.
std::vector<std::pair<DiscreteValues, double>> enumerate() const;
/// Return all the discrete keys associated with this factor.
DiscreteKeys discreteKeys() const;
/// Get all the probabilities in order of assignment values
std::vector<double> probabilities() const;
/**
* @brief Prune the decision tree of discrete variables.
@ -260,7 +302,6 @@ namespace gtsam {
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(ADT);
ar& BOOST_SERIALIZATION_NVP(cardinalities_);
}
#endif
};

13
gtsam/discrete/DiscreteBayesTree.h
View File

@ -58,6 +58,11 @@ class GTSAM_EXPORT DiscreteBayesTreeClique
//** evaluate conditional probability of subtree for given DiscreteValues */
double evaluate(const DiscreteValues& values) const;
//** (Preferred) sugar for the above for given DiscreteValues */
double operator()(const DiscreteValues& values) const {
return evaluate(values);
}
};
/* ************************************************************************* */
@ -105,4 +110,12 @@ class GTSAM_EXPORT DiscreteBayesTree
/// @}
};
/// traits
template <>
struct traits<DiscreteBayesTreeClique>
: public Testable<DiscreteBayesTreeClique> {};
template <>
struct traits<DiscreteBayesTree> : public Testable<DiscreteBayesTree> {};
} // namespace gtsam

12
gtsam/discrete/DiscreteConditional.h
View File

@ -77,6 +77,18 @@ class GTSAM_EXPORT DiscreteConditional
const Signature::Table& table)
: DiscreteConditional(Signature(key, parents, table)) {}
/**
* Construct from key, parents, and a vector<double> specifying the
* conditional probability table (CPT) in 00 01 10 11 order. For
* three-valued, it would be 00 01 02 10 11 12 20 21 22, etc....
*
* Example: DiscreteConditional P(D, {B,E}, table);
*/
DiscreteConditional(const DiscreteKey& key, const DiscreteKeys& parents,
const std::vector<double>& table)
: DiscreteConditional(1, DiscreteKeys{key} & parents,
ADT(DiscreteKeys{key} & parents, table)) {}
/**
* Construct from key, parents, and a string specifying the conditional
* probability table (CPT) in 00 01 10 11 order. For three-valued, it would

12
gtsam/discrete/DiscreteFactor.cpp
View File

@ -28,6 +28,18 @@ using namespace std;
namespace gtsam {
/* ************************************************************************ */
DiscreteKeys DiscreteFactor::discreteKeys() const {
DiscreteKeys result;
for (auto&& key : keys()) {
DiscreteKey dkey(key, cardinality(key));
if (std::find(result.begin(), result.end(), dkey) == result.end()) {
result.push_back(dkey);
}
}
return result;
}
/* ************************************************************************* */
double DiscreteFactor::error(const DiscreteValues& values) const {
return -std::log((*this)(values));

39
gtsam/discrete/DiscreteFactor.h
View File

@ -36,28 +36,35 @@ class HybridValues;
* @ingroup discrete
*/
class GTSAM_EXPORT DiscreteFactor: public Factor {
public:
// typedefs needed to play nice with gtsam
typedef DiscreteFactor This; ///< This class
typedef std::shared_ptr<DiscreteFactor> shared_ptr; ///< shared_ptr to this class
typedef std::shared_ptr<DiscreteFactor>
shared_ptr; ///< shared_ptr to this class
typedef Factor Base; ///< Our base class
using Values = DiscreteValues; ///< backwards compatibility
public:
protected:
/// Map of Keys and their cardinalities.
std::map<Key, size_t> cardinalities_;
public:
/// @name Standard Constructors
/// @{
/** Default constructor creates empty factor */
DiscreteFactor() {}
/** Construct from container of keys. This constructor is used internally from derived factor
* constructors, either from a container of keys or from a boost::assign::list_of. */
/**
* Construct from container of keys and map of cardinalities.
* This constructor is used internally from derived factor constructors,
* either from a container of keys or from a boost::assign::list_of.
*/
template <typename CONTAINER>
DiscreteFactor(const CONTAINER& keys) : Base(keys) {}
DiscreteFactor(const CONTAINER& keys,
const std::map<Key, size_t> cardinalities = {})
: Base(keys), cardinalities_(cardinalities) {}
/// @}
/// @name Testable
@ -77,6 +84,13 @@ public:
/// @name Standard Interface
/// @{
/// Return all the discrete keys associated with this factor.
DiscreteKeys discreteKeys() const;
std::map<Key, size_t> cardinalities() const { return cardinalities_; }
size_t cardinality(Key j) const { return cardinalities_.at(j); }
/// Find value for given assignment of values to variables
virtual double operator()(const DiscreteValues&) const = 0;
@ -124,6 +138,17 @@ public:
const Names& names = {}) const = 0;
/// @}
private:
#ifdef 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(cardinalities_);
}
#endif
};
// DiscreteFactor

37
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -42,15 +42,29 @@ class DiscreteJunctionTree;
/**
* @brief Main elimination function for DiscreteFactorGraph.
*
* @param factors
* @param keys
* @return GTSAM_EXPORT
* @param factors The factor graph to eliminate.
* @param frontalKeys An ordering for which variables to eliminate.
* @return A pair of the resulting conditional and the separator factor.
* @ingroup discrete
*/
GTSAM_EXPORT std::pair<std::shared_ptr<DiscreteConditional>, DecisionTreeFactor::shared_ptr>
EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& keys);
GTSAM_EXPORT
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
EliminateDiscrete(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys);
/**
* @brief Alternate elimination function for that creates non-normalized lookup tables.
*
* @param factors The factor graph to eliminate.
* @param frontalKeys An ordering for which variables to eliminate.
* @return A pair of the resulting lookup table and the separator factor.
* @ingroup discrete
*/
GTSAM_EXPORT
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
EliminateForMPE(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys);
/* ************************************************************************* */
template<> struct EliminationTraits<DiscreteFactorGraph>
{
typedef DiscreteFactor FactorType; ///< Type of factors in factor graph
@ -60,12 +74,14 @@ template<> struct EliminationTraits<DiscreteFactorGraph>
typedef DiscreteEliminationTree EliminationTreeType; ///< Type of elimination tree
typedef DiscreteBayesTree BayesTreeType; ///< Type of Bayes tree
typedef DiscreteJunctionTree JunctionTreeType; ///< Type of Junction tree
/// The default dense elimination function
static std::pair<std::shared_ptr<ConditionalType>,
std::shared_ptr<FactorType> >
DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
return EliminateDiscrete(factors, keys);
}
/// The default ordering generation function
static Ordering DefaultOrderingFunc(
const FactorGraphType& graph,
@ -74,7 +90,6 @@ template<> struct EliminationTraits<DiscreteFactorGraph>
}
};
/* ************************************************************************* */
/**
* A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
* Factor == DiscreteFactor
@ -108,8 +123,8 @@ class GTSAM_EXPORT DiscreteFactorGraph
/** Implicit copy/downcast constructor to override explicit template container
* constructor */
template <class DERIVEDFACTOR>
DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
template <class DERIVED_FACTOR>
DiscreteFactorGraph(const FactorGraph<DERIVED_FACTOR>& graph) : Base(graph) {}
/// @name Testable
/// @{
@ -227,10 +242,6 @@ class GTSAM_EXPORT DiscreteFactorGraph
/// @}
}; // \ DiscreteFactorGraph
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
EliminateForMPE(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys);
/// traits
template <>
struct traits<DiscreteFactorGraph> : public Testable<DiscreteFactorGraph> {};

2
gtsam/discrete/DiscreteJunctionTree.h
View File

@ -66,4 +66,6 @@ namespace gtsam {
DiscreteJunctionTree(const DiscreteEliminationTree& eliminationTree);
};
/// typedef for wrapper:
using DiscreteCluster = DiscreteJunctionTree::Cluster;
}

1
gtsam/discrete/DiscreteKey.cpp
View File

@ -49,6 +49,7 @@ namespace gtsam {
void DiscreteKeys::print(const std::string& s,
const KeyFormatter& keyFormatter) const {
std::cout << (s.empty() ? "" : s + " ") << std::endl;
for (auto&& dkey : *this) {
std::cout << DefaultKeyFormatter(dkey.first) << " " << dkey.second
<< std::endl;

6
gtsam/discrete/DiscreteKey.h
View File

@ -74,6 +74,12 @@ namespace gtsam {
return *this;
}
/// Add multiple keys (non-const!)
DiscreteKeys& operator&(const DiscreteKeys& keys) {
this->insert(this->end(), keys.begin(), keys.end());
return *this;
}
/// Print the keys and cardinalities.
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;

5
gtsam/discrete/DiscreteValues.h
View File

@ -120,6 +120,11 @@ class GTSAM_EXPORT DiscreteValues : public Assignment<Key> {
/// @}
};
/// Free version of CartesianProduct.
inline std::vector<DiscreteValues> cartesianProduct(const DiscreteKeys& keys) {
return DiscreteValues::CartesianProduct(keys);
}
/// Free version of markdown.
std::string markdown(const DiscreteValues& values,
const KeyFormatter& keyFormatter = DefaultKeyFormatter,

4
gtsam/discrete/SignatureParser.h
View File

@ -22,6 +22,8 @@
#include <string>
#include <vector>
#include <gtsam/dllexport.h>
namespace gtsam {
/**
* @brief A simple parser that replaces the boost spirit parser.
@ -47,7 +49,7 @@ namespace gtsam {
*
* Also fails if the rows are not of the same size.
*/
struct SignatureParser {
struct GTSAM_EXPORT SignatureParser {
using Row = std::vector<double>;
using Table = std::vector<Row>;

103
gtsam/discrete/TableFactor.cpp
View File

@ -13,11 +13,12 @@
* @file TableFactor.cpp
* @brief discrete factor
* @date May 4, 2023
* @author Yoonwoo Kim
* @author Yoonwoo Kim, Varun Agrawal
*/
#include <gtsam/base/FastSet.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/TableFactor.h>
#include <gtsam/hybrid/HybridValues.h>
@ -33,8 +34,7 @@ TableFactor::TableFactor() {}
/* ************************************************************************ */
TableFactor::TableFactor(const DiscreteKeys& dkeys,
const TableFactor& potentials)
: DiscreteFactor(dkeys.indices()),
cardinalities_(potentials.cardinalities_) {
: DiscreteFactor(dkeys.indices(), dkeys.cardinalities()) {
sparse_table_ = potentials.sparse_table_;
denominators_ = potentials.denominators_;
sorted_dkeys_ = discreteKeys();
@ -44,11 +44,11 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys,
/* ************************************************************************ */
TableFactor::TableFactor(const DiscreteKeys& dkeys,
const Eigen::SparseVector<double>& table)
: DiscreteFactor(dkeys.indices()), sparse_table_(table.size()) {
: DiscreteFactor(dkeys.indices(), dkeys.cardinalities()),
sparse_table_(table.size()) {
sparse_table_ = table;
double denom = table.size();
for (const DiscreteKey& dkey : dkeys) {
cardinalities_.insert(dkey);
denom /= dkey.second;
denominators_.insert(std::pair<Key, double>(dkey.first, denom));
}
@ -56,11 +56,51 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys,
sort(sorted_dkeys_.begin(), sorted_dkeys_.end());
}
/* ************************************************************************ */
TableFactor::TableFactor(const DiscreteKeys& dkeys,
const DecisionTree<Key, double>& dtree)
: TableFactor(dkeys, DecisionTreeFactor(dkeys, dtree)) {}
/**
* @brief Compute the correct ordering of the leaves in the decision tree.
*
* This is done by first taking all the values which have modulo 0 value with
* the cardinality of the innermost key `n`, and we go up to modulo n.
*
* @param dt The DecisionTree
* @return std::vector<double>
*/
std::vector<double> ComputeLeafOrdering(const DiscreteKeys& dkeys,
const DecisionTreeFactor& dt) {
std::vector<double> probs = dt.probabilities();
std::vector<double> ordered;
size_t n = dkeys[0].second;
for (size_t k = 0; k < n; ++k) {
for (size_t idx = 0; idx < probs.size(); ++idx) {
if (idx % n == k) {
ordered.push_back(probs[idx]);
}
}
}
return ordered;
}
/* ************************************************************************ */
TableFactor::TableFactor(const DiscreteKeys& dkeys,
const DecisionTreeFactor& dtf)
: TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {}
/* ************************************************************************ */
TableFactor::TableFactor(const DiscreteConditional& c)
: TableFactor(c.discreteKeys(), c) {}
/* ************************************************************************ */
Eigen::SparseVector<double> TableFactor::Convert(
const std::vector<double>& table) {
Eigen::SparseVector<double> sparse_table(table.size());
// Count number of nonzero elements in table and reserving the space.
// Count number of nonzero elements in table and reserve the space.
const uint64_t nnz = std::count_if(table.begin(), table.end(),
[](uint64_t i) { return i != 0; });
sparse_table.reserve(nnz);
@ -214,6 +254,45 @@ void TableFactor::print(const string& s, const KeyFormatter& formatter) const {
cout << "number of nnzs: " << sparse_table_.nonZeros() << endl;
}
/* ************************************************************************ */
TableFactor TableFactor::apply(Unary op) const {
// Initialize new factor.
uint64_t cardi = 1;
for (auto [key, c] : cardinalities_) cardi *= c;
Eigen::SparseVector<double> sparse_table(cardi);
sparse_table.reserve(sparse_table_.nonZeros());
// Populate
for (SparseIt it(sparse_table_); it; ++it) {
sparse_table.coeffRef(it.index()) = op(it.value());
}
// Free unused memory and return.
sparse_table.pruned();
sparse_table.data().squeeze();
return TableFactor(discreteKeys(), sparse_table);
}
/* ************************************************************************ */
TableFactor TableFactor::apply(UnaryAssignment op) const {
// Initialize new factor.
uint64_t cardi = 1;
for (auto [key, c] : cardinalities_) cardi *= c;
Eigen::SparseVector<double> sparse_table(cardi);
sparse_table.reserve(sparse_table_.nonZeros());
// Populate
for (SparseIt it(sparse_table_); it; ++it) {
DiscreteValues assignment = findAssignments(it.index());
sparse_table.coeffRef(it.index()) = op(assignment, it.value());
}
// Free unused memory and return.
sparse_table.pruned();
sparse_table.data().squeeze();
return TableFactor(discreteKeys(), sparse_table);
}
/* ************************************************************************ */
TableFactor TableFactor::apply(const TableFactor& f, Binary op) const {
if (keys_.empty() && sparse_table_.nonZeros() == 0)
@ -435,18 +514,6 @@ std::vector<std::pair<DiscreteValues, double>> TableFactor::enumerate() const {
return result;
}
/* ************************************************************************ */
DiscreteKeys TableFactor::discreteKeys() const {
DiscreteKeys result;
for (auto&& key : keys()) {
DiscreteKey dkey(key, cardinality(key));
if (std::find(result.begin(), result.end(), dkey) == result.end()) {
result.push_back(dkey);
}
}
return result;
}
// Print out header.
/* ************************************************************************ */
string TableFactor::markdown(const KeyFormatter& keyFormatter,

51
gtsam/discrete/TableFactor.h
View File

@ -12,7 +12,7 @@
/**
* @file TableFactor.h
* @date May 4, 2023
* @author Yoonwoo Kim
* @author Yoonwoo Kim, Varun Agrawal
*/
#pragma once
@ -32,6 +32,7 @@
namespace gtsam {
class DiscreteConditional;
class HybridValues;
/**
@ -44,8 +45,6 @@ class HybridValues;
*/
class GTSAM_EXPORT TableFactor : public DiscreteFactor {
protected:
/// Map of Keys and their cardinalities.
std::map<Key, size_t> cardinalities_;
/// SparseVector of nonzero probabilities.
Eigen::SparseVector<double> sparse_table_;
@ -75,7 +74,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
* @brief Return ith key in keys_ as a DiscreteKey
* @param i ith key in keys_
* @return DiscreteKey
* */
*/
DiscreteKey discreteKey(size_t i) const {
return DiscreteKey(keys_[i], cardinalities_.at(keys_[i]));
}
@ -94,6 +93,9 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
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:
@ -142,6 +144,15 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
TableFactor(const DiscreteKey& key, const std::vector<double>& row)
: TableFactor(DiscreteKeys{key}, row) {}
/// Constructor from DecisionTreeFactor
TableFactor(const DiscreteKeys& keys, const DecisionTreeFactor& dtf);
/// Constructor from DecisionTree<Key, double>/AlgebraicDecisionTree
TableFactor(const DiscreteKeys& keys, const DecisionTree<Key, double>& dtree);
/** Construct from a DiscreteConditional type */
explicit TableFactor(const DiscreteConditional& c);
/// @}
/// @name Testable
/// @{
@ -175,13 +186,11 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
return apply(f, Ring::mul);
};
/// multiple with DecisionTreeFactor
/// multiply with DecisionTreeFactor
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
static double safe_div(const double& a, const double& b);
size_t cardinality(Key j) const { return cardinalities_.at(j); }
/// divide by factor f (safely)
TableFactor operator/(const TableFactor& f) const {
return apply(f, safe_div);
@ -218,6 +227,18 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
/// @name Advanced Interface
/// @{
/**
* Apply unary operator `op(*this)` where `op` accepts the discrete value.
* @param op a unary operator that operates on TableFactor
*/
TableFactor apply(Unary op) const;
/**
* Apply unary operator `op(*this)` where `op` accepts the discrete assignment
* and the value at that assignment.
* @param op a unary operator that operates on TableFactor
*/
TableFactor apply(UnaryAssignment op) const;
/**
* Apply binary operator (*this) "op" f
* @param f the second argument for op
@ -225,10 +246,19 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
*/
TableFactor apply(const TableFactor& f, Binary op) const;
/// Return keys in contract mode.
/**
* Return keys in contract mode.
*
* Modes are each of the dimensions of a sparse tensor,
* and the contract modes represent which dimensions will
* be involved in contraction (aka tensor multiplication).
*/
DiscreteKeys contractDkeys(const TableFactor& f) const;
/// Return keys in free mode.
/**
* @brief Return keys in free mode which are the dimensions
* not involved in the contraction operation.
*/
DiscreteKeys freeDkeys(const TableFactor& f) const;
/// Return union of DiscreteKeys in two factors.
@ -274,9 +304,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
/// Enumerate all values into a map from values to double.
std::vector<std::pair<DiscreteValues, double>> enumerate() const;
/// Return all the discrete keys associated with this factor.
DiscreteKeys discreteKeys() const;
/**
* @brief Prune the decision tree of discrete variables.
*

114
gtsam/discrete/discrete.i
View File

@ -17,6 +17,8 @@ class DiscreteKeys {
};
// DiscreteValues is added in specializations/discrete.h as a std::map
std::vector<gtsam::DiscreteValues> cartesianProduct(
const gtsam::DiscreteKeys& keys);
string markdown(
const gtsam::DiscreteValues& values,
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
@ -31,13 +33,11 @@ string html(const gtsam::DiscreteValues& values,
std::map<gtsam::Key, std::vector<std::string>> names);
#include <gtsam/discrete/DiscreteFactor.h>
class DiscreteFactor {
virtual class DiscreteFactor : gtsam::Factor {
void print(string s = "DiscreteFactor\n",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::DiscreteFactor& other, double tol = 1e-9) const;
bool empty() const;
size_t size() const;
double operator()(const gtsam::DiscreteValues& values) const;
};
@ -49,7 +49,12 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
const std::vector<double>& spec);
DecisionTreeFactor(const gtsam::DiscreteKey& key, string table);
DecisionTreeFactor(const gtsam::DiscreteKeys& keys,
const std::vector<double>& table);
DecisionTreeFactor(const gtsam::DiscreteKeys& keys, string table);
DecisionTreeFactor(const std::vector<gtsam::DiscreteKey>& keys,
const std::vector<double>& table);
DecisionTreeFactor(const std::vector<gtsam::DiscreteKey>& keys, string table);
DecisionTreeFactor(const gtsam::DiscreteConditional& c);
@ -59,6 +64,8 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
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;
@ -66,6 +73,7 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
gtsam::DecisionTreeFactor* sum(size_t nrFrontals) const;
gtsam::DecisionTreeFactor* sum(const gtsam::Ordering& keys) const;
gtsam::DecisionTreeFactor* max(size_t nrFrontals) const;
gtsam::DecisionTreeFactor* max(const gtsam::Ordering& keys) const;
string dot(
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
@ -203,10 +211,16 @@ class DiscreteBayesTreeClique {
DiscreteBayesTreeClique(const gtsam::DiscreteConditional* conditional);
const gtsam::DiscreteConditional* conditional() const;
bool isRoot() const;
size_t nrChildren() const;
const gtsam::DiscreteBayesTreeClique* operator[](size_t i) const;
void print(string s = "DiscreteBayesTreeClique",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void printSignature(
const string& s = "Clique: ",
const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
double evaluate(const gtsam::DiscreteValues& values) const;
double operator()(const gtsam::DiscreteValues& values) const;
};
class DiscreteBayesTree {
@ -220,6 +234,9 @@ class DiscreteBayesTree {
bool empty() const;
const DiscreteBayesTreeClique* operator[](size_t j) const;
double evaluate(const gtsam::DiscreteValues& values) const;
double operator()(const gtsam::DiscreteValues& values) const;
string dot(const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void saveGraph(string s,
@ -242,9 +259,9 @@ class DiscreteBayesTree {
class DiscreteLookupTable : gtsam::DiscreteConditional{
DiscreteLookupTable(size_t nFrontals, const gtsam::DiscreteKeys& keys,
const gtsam::DecisionTreeFactor::ADT& potentials);
void print(
const std::string& s = "Discrete Lookup Table: ",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
void print(string s = "Discrete Lookup Table: ",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
size_t argmax(const gtsam::DiscreteValues& parentsValues) const;
};
@ -263,6 +280,14 @@ class DiscreteLookupDAG {
};
#include <gtsam/discrete/DiscreteFactorGraph.h>
pair<gtsam::DiscreteConditional*, gtsam::DecisionTreeFactor*>
EliminateDiscrete(const gtsam::DiscreteFactorGraph& factors,
const gtsam::Ordering& frontalKeys);
pair<gtsam::DiscreteConditional*, gtsam::DecisionTreeFactor*>
EliminateForMPE(const gtsam::DiscreteFactorGraph& factors,
const gtsam::Ordering& frontalKeys);
class DiscreteFactorGraph {
DiscreteFactorGraph();
DiscreteFactorGraph(const gtsam::DiscreteBayesNet& bayesNet);
@ -277,6 +302,7 @@ class DiscreteFactorGraph {
void add(const gtsam::DiscreteKey& j, const std::vector<double>& spec);
void add(const gtsam::DiscreteKeys& keys, string spec);
void add(const std::vector<gtsam::DiscreteKey>& keys, string spec);
void add(const std::vector<gtsam::DiscreteKey>& keys, const std::vector<double>& spec);
bool empty() const;
size_t size() const;
@ -290,25 +316,46 @@ class DiscreteFactorGraph {
double operator()(const gtsam::DiscreteValues& values) const;
gtsam::DiscreteValues optimize() const;
gtsam::DiscreteBayesNet sumProduct();
gtsam::DiscreteBayesNet sumProduct(gtsam::Ordering::OrderingType type);
gtsam::DiscreteBayesNet sumProduct(
gtsam::Ordering::OrderingType type = gtsam::Ordering::COLAMD);
gtsam::DiscreteBayesNet sumProduct(const gtsam::Ordering& ordering);
gtsam::DiscreteLookupDAG maxProduct();
gtsam::DiscreteLookupDAG maxProduct(gtsam::Ordering::OrderingType type);
gtsam::DiscreteLookupDAG maxProduct(
gtsam::Ordering::OrderingType type = gtsam::Ordering::COLAMD);
gtsam::DiscreteLookupDAG maxProduct(const gtsam::Ordering& ordering);
gtsam::DiscreteBayesNet* eliminateSequential();
gtsam::DiscreteBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type);
gtsam::DiscreteBayesNet* eliminateSequential(
gtsam::Ordering::OrderingType type = gtsam::Ordering::COLAMD);
gtsam::DiscreteBayesNet* eliminateSequential(
gtsam::Ordering::OrderingType type,
const gtsam::DiscreteFactorGraph::Eliminate& function);
gtsam::DiscreteBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
gtsam::DiscreteBayesNet* eliminateSequential(
const gtsam::Ordering& ordering,
const gtsam::DiscreteFactorGraph::Eliminate& function);
pair<gtsam::DiscreteBayesNet*, gtsam::DiscreteFactorGraph*>
eliminatePartialSequential(const gtsam::Ordering& ordering);
pair<gtsam::DiscreteBayesNet*, gtsam::DiscreteFactorGraph*>
eliminatePartialSequential(
const gtsam::Ordering& ordering,
const gtsam::DiscreteFactorGraph::Eliminate& function);
gtsam::DiscreteBayesTree* eliminateMultifrontal();
gtsam::DiscreteBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type);
gtsam::DiscreteBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);
gtsam::DiscreteBayesTree* eliminateMultifrontal(
gtsam::Ordering::OrderingType type = gtsam::Ordering::COLAMD);
gtsam::DiscreteBayesTree* eliminateMultifrontal(
gtsam::Ordering::OrderingType type,
const gtsam::DiscreteFactorGraph::Eliminate& function);
gtsam::DiscreteBayesTree* eliminateMultifrontal(
const gtsam::Ordering& ordering);
gtsam::DiscreteBayesTree* eliminateMultifrontal(
const gtsam::Ordering& ordering,
const gtsam::DiscreteFactorGraph::Eliminate& function);
pair<gtsam::DiscreteBayesTree*, gtsam::DiscreteFactorGraph*>
eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
pair<gtsam::DiscreteBayesTree*, gtsam::DiscreteFactorGraph*>
eliminatePartialMultifrontal(
const gtsam::Ordering& ordering,
const gtsam::DiscreteFactorGraph::Eliminate& function);
string dot(
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
@ -328,4 +375,41 @@ class DiscreteFactorGraph {
std::map<gtsam::Key, std::vector<std::string>> names) const;
};
#include <gtsam/discrete/DiscreteEliminationTree.h>
class DiscreteEliminationTree {
DiscreteEliminationTree(const gtsam::DiscreteFactorGraph& factorGraph,
const gtsam::VariableIndex& structure,
const gtsam::Ordering& order);
DiscreteEliminationTree(const gtsam::DiscreteFactorGraph& factorGraph,
const gtsam::Ordering& order);
void print(
string name = "EliminationTree: ",
const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::DiscreteEliminationTree& other,
double tol = 1e-9) const;
};
#include <gtsam/discrete/DiscreteJunctionTree.h>
class DiscreteCluster {
gtsam::Ordering orderedFrontalKeys;
gtsam::DiscreteFactorGraph factors;
const gtsam::DiscreteCluster& operator[](size_t i) const;
size_t nrChildren() const;
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
};
class DiscreteJunctionTree {
DiscreteJunctionTree(const gtsam::DiscreteEliminationTree& eliminationTree);
void print(
string name = "JunctionTree: ",
const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
size_t nrRoots() const;
const gtsam::DiscreteCluster& operator[](size_t i) const;
};
} // namespace gtsam

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

@ -75,6 +75,19 @@ struct traits<CrazyDecisionTree> : public Testable<CrazyDecisionTree> {};
GTSAM_CONCEPT_TESTABLE_INST(CrazyDecisionTree)
/* ************************************************************************** */
// Test char labels and int range
/* ************************************************************************** */
// Create a decision stump one one variable 'a' with values 10 and 20.
TEST(DecisionTree, Constructor) {
DecisionTree<char, int> tree('a', 10, 20);
// Evaluate the tree on an assignment to the variable.
EXPECT_LONGS_EQUAL(10, tree({{'a', 0}}));
EXPECT_LONGS_EQUAL(20, tree({{'a', 1}}));
}
/* ************************************************************************** */
// Test string labels and int range
/* ************************************************************************** */
@ -118,18 +131,47 @@ struct Ring {
static inline int mul(const int& a, const int& b) { return a * b; }
};
/* ************************************************************************** */
// Check that creating decision trees respects key order.
TEST(DecisionTree, ConstructorOrder) {
// Create labels
string A("A"), B("B");
const std::vector<int> ys1 = {1, 2, 3, 4};
DT tree1({{B, 2}, {A, 2}}, ys1); // faster version, as B is "higher" than A!
const std::vector<int> ys2 = {1, 3, 2, 4};
DT tree2({{A, 2}, {B, 2}}, ys2); // slower version !
// Both trees will be the same, tree is order from high to low labels.
// Choice(B)
// 0 Choice(A)
// 0 0 Leaf 1
// 0 1 Leaf 2
// 1 Choice(A)
// 1 0 Leaf 3
// 1 1 Leaf 4
EXPECT(tree2.equals(tree1));
// Check the values are as expected by calling the () operator:
EXPECT_LONGS_EQUAL(1, tree1({{A, 0}, {B, 0}}));
EXPECT_LONGS_EQUAL(3, tree1({{A, 0}, {B, 1}}));
EXPECT_LONGS_EQUAL(2, tree1({{A, 1}, {B, 0}}));
EXPECT_LONGS_EQUAL(4, tree1({{A, 1}, {B, 1}}));
}
/* ************************************************************************** */
// test DT
TEST(DecisionTree, example) {
TEST(DecisionTree, Example) {
// Create labels
string A("A"), B("B"), C("C");
// create a value
Assignment<string> x00, x01, x10, x11;
x00[A] = 0, x00[B] = 0;
x01[A] = 0, x01[B] = 1;
x10[A] = 1, x10[B] = 0;
x11[A] = 1, x11[B] = 1;
// Create assignments using brace initialization:
Assignment<string> x00{{A, 0}, {B, 0}};
Assignment<string> x01{{A, 0}, {B, 1}};
Assignment<string> x10{{A, 1}, {B, 0}};
Assignment<string> x11{{A, 1}, {B, 1}};
// empty
DT empty;
@ -245,8 +287,7 @@ TEST(DecisionTree, ConvertValuesOnly) {
StringBoolTree f2(f1, bool_of_int);
// Check a value
Assignment<string> x00;
x00["A"] = 0, x00["B"] = 0;
Assignment<string> x00 {{A, 0}, {B, 0}};
EXPECT(!f2(x00));
}
@ -270,10 +311,11 @@ TEST(DecisionTree, ConvertBoth) {
// Check some values
Assignment<Label> x00, x01, x10, x11;
x00[X] = 0, x00[Y] = 0;
x01[X] = 0, x01[Y] = 1;
x10[X] = 1, x10[Y] = 0;
x11[X] = 1, x11[Y] = 1;
x00 = {{X, 0}, {Y, 0}};
x01 = {{X, 0}, {Y, 1}};
x10 = {{X, 1}, {Y, 0}};
x11 = {{X, 1}, {Y, 1}};
EXPECT(!f2(x00));
EXPECT(!f2(x01));
EXPECT(f2(x10));

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

@ -27,6 +27,18 @@
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(DecisionTreeFactor, ConstructorsMatch) {
// Declare two keys
DiscreteKey X(0, 2), Y(1, 3);
// Create with vector and with string
const std::vector<double> table {2, 5, 3, 6, 4, 7};
DecisionTreeFactor f1({X, Y}, table);
DecisionTreeFactor f2({X, Y}, "2 5 3 6 4 7");
EXPECT(assert_equal(f1, f2));
}
/* ************************************************************************* */
TEST( DecisionTreeFactor, constructors)
{
@ -41,16 +53,18 @@ TEST( DecisionTreeFactor, constructors)
EXPECT_LONGS_EQUAL(2,f2.size());
EXPECT_LONGS_EQUAL(3,f3.size());
DiscreteValues values;
values[0] = 1; // x
values[1] = 2; // y
values[2] = 1; // z
EXPECT_DOUBLES_EQUAL(8, f1(values), 1e-9);
EXPECT_DOUBLES_EQUAL(7, f2(values), 1e-9);
EXPECT_DOUBLES_EQUAL(75, f3(values), 1e-9);
DiscreteValues x121{{0, 1}, {1, 2}, {2, 1}};
EXPECT_DOUBLES_EQUAL(8, f1(x121), 1e-9);
EXPECT_DOUBLES_EQUAL(7, f2(x121), 1e-9);
EXPECT_DOUBLES_EQUAL(75, f3(x121), 1e-9);
// Assert that error = -log(value)
EXPECT_DOUBLES_EQUAL(-log(f1(values)), f1.error(values), 1e-9);
EXPECT_DOUBLES_EQUAL(-log(f1(x121)), f1.error(x121), 1e-9);
// Construct from DiscreteConditional
DiscreteConditional conditional(X | Y = "1/1 2/3 1/4");
DecisionTreeFactor f4(conditional);
EXPECT_DOUBLES_EQUAL(0.8, f4(x121), 1e-9);
}
/* ************************************************************************* */

229
gtsam/discrete/tests/testDiscreteBayesTree.cpp
View File

@ -16,23 +16,24 @@
*/
#include <gtsam/base/Vector.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteBayesTree.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/inference/BayesNet.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
#include <vector>
using namespace std;
using namespace gtsam;
static constexpr bool debug = false;
/* ************************************************************************* */
struct TestFixture {
vector<DiscreteKey> keys;
DiscreteKeys keys;
std::vector<DiscreteValues> assignments;
DiscreteBayesNet bayesNet;
std::shared_ptr<DiscreteBayesTree> bayesTree;
@ -47,6 +48,9 @@ struct TestFixture {
keys.push_back(key_i);
}
// Enumerate all assignments.
assignments = DiscreteValues::CartesianProduct(keys);
// Create thin-tree Bayesnet.
bayesNet.add(keys[14] % "1/3");
@ -74,9 +78,9 @@ struct TestFixture {
};
/* ************************************************************************* */
// Check that BN and BT give the same answer on all configurations
TEST(DiscreteBayesTree, ThinTree) {
const TestFixture self;
const auto& keys = self.keys;
TestFixture self;
if (debug) {
GTSAM_PRINT(self.bayesNet);
@ -95,47 +99,56 @@ TEST(DiscreteBayesTree, ThinTree) {
EXPECT_LONGS_EQUAL(i, *(clique_i->conditional_->beginFrontals()));
}
auto R = self.bayesTree->roots().front();
// Check whether BN and BT give the same answer on all configurations
auto allPosbValues = DiscreteValues::CartesianProduct(
keys[0] & keys[1] & keys[2] & keys[3] & keys[4] & keys[5] & keys[6] &
keys[7] & keys[8] & keys[9] & keys[10] & keys[11] & keys[12] & keys[13] &
keys[14]);
for (size_t i = 0; i < allPosbValues.size(); ++i) {
DiscreteValues x = allPosbValues[i];
for (const auto& x : self.assignments) {
double expected = self.bayesNet.evaluate(x);
double actual = self.bayesTree->evaluate(x);
DOUBLES_EQUAL(expected, actual, 1e-9);
}
}
// Calculate all some marginals for DiscreteValues==all1
Vector marginals = Vector::Zero(15);
double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint_8_12 = 0,
joint82 = 0, joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0,
joint_4_11 = 0, joint_11_13 = 0, joint_11_13_14 = 0,
joint_11_12_13_14 = 0, joint_9_11_12_13 = 0, joint_8_11_12_13 = 0;
for (size_t i = 0; i < allPosbValues.size(); ++i) {
DiscreteValues x = allPosbValues[i];
/* ************************************************************************* */
// Check calculation of separator marginals
TEST(DiscreteBayesTree, SeparatorMarginals) {
TestFixture self;
// Calculate some marginals for DiscreteValues==all1
double marginal_14 = 0, joint_8_12 = 0;
for (auto& x : self.assignments) {
double px = self.bayesTree->evaluate(x);
for (size_t i = 0; i < 15; i++)
if (x[i]) marginals[i] += px;
if (x[12] && x[14]) {
joint_12_14 += px;
if (x[9]) joint_9_12_14 += px;
if (x[8]) joint_8_12_14 += px;
}
if (x[8] && x[12]) joint_8_12 += px;
if (x[2]) {
if (x[8]) joint82 += px;
if (x[1]) joint12 += px;
if (x[14]) marginal_14 += px;
}
if (x[4]) {
if (x[2]) joint24 += px;
if (x[5]) joint45 += px;
if (x[6]) joint46 += px;
if (x[11]) joint_4_11 += px;
DiscreteValues all1 = self.assignments.back();
// check separator marginal P(S0)
auto clique = (*self.bayesTree)[0];
DiscreteFactorGraph separatorMarginal0 =
clique->separatorMarginal(EliminateDiscrete);
DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9);
// check separator marginal P(S9), should be P(14)
clique = (*self.bayesTree)[9];
DiscreteFactorGraph separatorMarginal9 =
clique->separatorMarginal(EliminateDiscrete);
DOUBLES_EQUAL(marginal_14, separatorMarginal9(all1), 1e-9);
// check separator marginal of root, should be empty
clique = (*self.bayesTree)[11];
DiscreteFactorGraph separatorMarginal11 =
clique->separatorMarginal(EliminateDiscrete);
LONGS_EQUAL(0, separatorMarginal11.size());
}
/* ************************************************************************* */
// Check shortcuts in the tree
TEST(DiscreteBayesTree, Shortcuts) {
TestFixture self;
// Calculate some marginals for DiscreteValues==all1
double joint_11_13 = 0, joint_11_13_14 = 0, joint_11_12_13_14 = 0,
joint_9_11_12_13 = 0, joint_8_11_12_13 = 0;
for (auto& x : self.assignments) {
double px = self.bayesTree->evaluate(x);
if (x[11] && x[13]) {
joint_11_13 += px;
if (x[8] && x[12]) joint_8_11_12_13 += px;
@ -148,32 +161,12 @@ TEST(DiscreteBayesTree, ThinTree) {
}
}
}
DiscreteValues all1 = allPosbValues.back();
DiscreteValues all1 = self.assignments.back();
// check separator marginal P(S0)
auto clique = (*self.bayesTree)[0];
DiscreteFactorGraph separatorMarginal0 =
clique->separatorMarginal(EliminateDiscrete);
DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9);
DOUBLES_EQUAL(joint_12_14, 0.1875, 1e-9);
DOUBLES_EQUAL(joint_8_12_14, 0.0375, 1e-9);
DOUBLES_EQUAL(joint_9_12_14, 0.15, 1e-9);
// check separator marginal P(S9), should be P(14)
clique = (*self.bayesTree)[9];
DiscreteFactorGraph separatorMarginal9 =
clique->separatorMarginal(EliminateDiscrete);
DOUBLES_EQUAL(marginals[14], separatorMarginal9(all1), 1e-9);
// check separator marginal of root, should be empty
clique = (*self.bayesTree)[11];
DiscreteFactorGraph separatorMarginal11 =
clique->separatorMarginal(EliminateDiscrete);
LONGS_EQUAL(0, separatorMarginal11.size());
auto R = self.bayesTree->roots().front();
// check shortcut P(S9||R) to root
clique = (*self.bayesTree)[9];
auto clique = (*self.bayesTree)[9];
DiscreteBayesNet shortcut = clique->shortcut(R, EliminateDiscrete);
LONGS_EQUAL(1, shortcut.size());
DOUBLES_EQUAL(joint_11_13_14 / joint_11_13, shortcut.evaluate(all1), 1e-9);
@ -202,15 +195,67 @@ TEST(DiscreteBayesTree, ThinTree) {
shortcut.print("shortcut:");
}
}
}
/* ************************************************************************* */
// Check all marginals
TEST(DiscreteBayesTree, MarginalFactors) {
TestFixture self;
Vector marginals = Vector::Zero(15);
for (size_t i = 0; i < self.assignments.size(); ++i) {
DiscreteValues& x = self.assignments[i];
double px = self.bayesTree->evaluate(x);
for (size_t i = 0; i < 15; i++)
if (x[i]) marginals[i] += px;
}
// Check all marginals
DiscreteFactor::shared_ptr marginalFactor;
DiscreteValues all1 = self.assignments.back();
for (size_t i = 0; i < 15; i++) {
marginalFactor = self.bayesTree->marginalFactor(i, EliminateDiscrete);
auto marginalFactor = self.bayesTree->marginalFactor(i, EliminateDiscrete);
double actual = (*marginalFactor)(all1);
DOUBLES_EQUAL(marginals[i], actual, 1e-9);
}
}
/* ************************************************************************* */
// Check a number of joint marginals.
TEST(DiscreteBayesTree, Joints) {
TestFixture self;
// Calculate some marginals for DiscreteValues==all1
Vector marginals = Vector::Zero(15);
double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint82 = 0,
joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0, joint_4_11 = 0;
for (size_t i = 0; i < self.assignments.size(); ++i) {
DiscreteValues& x = self.assignments[i];
double px = self.bayesTree->evaluate(x);
for (size_t i = 0; i < 15; i++)
if (x[i]) marginals[i] += px;
if (x[12] && x[14]) {
joint_12_14 += px;
if (x[9]) joint_9_12_14 += px;
if (x[8]) joint_8_12_14 += px;
}
if (x[2]) {
if (x[8]) joint82 += px;
if (x[1]) joint12 += px;
}
if (x[4]) {
if (x[2]) joint24 += px;
if (x[5]) joint45 += px;
if (x[6]) joint46 += px;
if (x[11]) joint_4_11 += px;
}
}
// regression tests:
DOUBLES_EQUAL(joint_12_14, 0.1875, 1e-9);
DOUBLES_EQUAL(joint_8_12_14, 0.0375, 1e-9);
DOUBLES_EQUAL(joint_9_12_14, 0.15, 1e-9);
DiscreteValues all1 = self.assignments.back();
DiscreteBayesNet::shared_ptr actualJoint;
// Check joint P(8, 2)
@ -240,8 +285,8 @@ TEST(DiscreteBayesTree, ThinTree) {
/* ************************************************************************* */
TEST(DiscreteBayesTree, Dot) {
const TestFixture self;
string actual = self.bayesTree->dot();
TestFixture self;
std::string actual = self.bayesTree->dot();
EXPECT(actual ==
"digraph G{\n"
"0[label=\"13, 11, 6, 7\"];\n"
@ -268,6 +313,62 @@ TEST(DiscreteBayesTree, Dot) {
"}");
}
/* ************************************************************************* */
// Check that we can have a multi-frontal lookup table
TEST(DiscreteBayesTree, Lookup) {
using gtsam::symbol_shorthand::A;
using gtsam::symbol_shorthand::X;
// Make a small planning-like graph: 3 states, 2 actions
DiscreteFactorGraph graph;
const DiscreteKey x1{X(1), 3}, x2{X(2), 3}, x3{X(3), 3};
const DiscreteKey a1{A(1), 2}, a2{A(2), 2};
// Constraint on start and goal
graph.add(DiscreteKeys{x1}, std::vector<double>{1, 0, 0});
graph.add(DiscreteKeys{x3}, std::vector<double>{0, 0, 1});
// Should I stay or should I go?
// "Reward" (exp(-cost)) for an action is 10, and rewards multiply:
const double r = 10;
std::vector<double> table{
r, 0, 0, 0, r, 0, // x1 = 0
0, r, 0, 0, 0, r, // x1 = 1
0, 0, r, 0, 0, r // x1 = 2
};
graph.add(DiscreteKeys{x1, a1, x2}, table);
graph.add(DiscreteKeys{x2, a2, x3}, table);
// eliminate for MPE (maximum probable explanation).
Ordering ordering{A(2), X(3), X(1), A(1), X(2)};
auto lookup = graph.eliminateMultifrontal(ordering, EliminateForMPE);
// Check that the lookup table is correct
EXPECT_LONGS_EQUAL(2, lookup->size());
auto lookup_x1_a1_x2 = (*lookup)[X(1)]->conditional();
EXPECT_LONGS_EQUAL(3, lookup_x1_a1_x2->frontals().size());
// check that sum is 1.0 (not 100, as we now normalize)
DiscreteValues empty;
EXPECT_DOUBLES_EQUAL(1.0, (*lookup_x1_a1_x2->sum(3))(empty), 1e-9);
// And that only non-zero reward is for x1 a1 x2 == 0 1 1
EXPECT_DOUBLES_EQUAL(1.0, (*lookup_x1_a1_x2)({{X(1),0},{A(1),1},{X(2),1}}), 1e-9);
auto lookup_a2_x3 = (*lookup)[X(3)]->conditional();
// check that the sum depends on x2 and is non-zero only for x2 \in {1,2}
auto sum_x2 = lookup_a2_x3->sum(2);
EXPECT_DOUBLES_EQUAL(0, (*sum_x2)({{X(2),0}}), 1e-9);
EXPECT_DOUBLES_EQUAL(1.0, (*sum_x2)({{X(2),1}}), 1e-9);
EXPECT_DOUBLES_EQUAL(2.0, (*sum_x2)({{X(2),2}}), 1e-9);
EXPECT_LONGS_EQUAL(2, lookup_a2_x3->frontals().size());
// And that the non-zero rewards are for
// x2 a2 x3 == 1 1 2
EXPECT_DOUBLES_EQUAL(1.0, (*lookup_a2_x3)({{X(2),1},{A(2),1},{X(3),2}}), 1e-9);
// x2 a2 x3 == 2 0 2
EXPECT_DOUBLES_EQUAL(1.0, (*lookup_a2_x3)({{X(2),2},{A(2),0},{X(3),2}}), 1e-9);
// x2 a2 x3 == 2 1 2
EXPECT_DOUBLES_EQUAL(1.0, (*lookup_a2_x3)({{X(2),2},{A(2),1},{X(3),2}}), 1e-9);
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -46,6 +46,11 @@ TEST(DiscreteConditional, constructors) {
DiscreteConditional actual2(1, f2);
DecisionTreeFactor expected2 = f2 / *f2.sum(1);
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};
DiscreteConditional actual3(X, {Y, Z}, probs);
DecisionTreeFactor expected3 = f2;
EXPECT(assert_equal(expected3, static_cast<DecisionTreeFactor>(actual3)));
}
/* ************************************************************************* */

2
gtsam/discrete/tests/testDiscreteMarginals.cpp
View File

@ -121,7 +121,7 @@ TEST_UNSAFE( DiscreteMarginals, truss ) {
Clique expected0(std::make_shared<DiscreteConditional>((key[0] | key[2], key[4]) = "2/1 2/1 2/1 2/1"));
Clique::shared_ptr actual0 = (*bayesTree)[0];
// EXPECT(assert_equal(expected0, *actual0)); // TODO, correct but fails
EXPECT(assert_equal(expected0, *actual0));
Clique expected1(std::make_shared<DiscreteConditional>((key[1] | key[3], key[4]) = "1/2 1/2 1/2 1/2"));
Clique::shared_ptr actual1 = (*bayesTree)[1];

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

@ -19,6 +19,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/DiscreteDistribution.h>
#include <gtsam/discrete/Signature.h>
#include <gtsam/discrete/TableFactor.h>
@ -124,6 +125,23 @@ TEST(TableFactor, constructors) {
// Assert that error = -log(value)
EXPECT_DOUBLES_EQUAL(-log(f1(values)), f1.error(values), 1e-9);
// Construct from DiscreteConditional
DiscreteConditional conditional(X | Y = "1/1 2/3 1/4");
TableFactor f4(conditional);
// Manually constructed via inspection and comparison to DecisionTreeFactor
TableFactor expected(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8");
EXPECT(assert_equal(expected, f4));
// Test for 9=3x3 values.
DiscreteKey V(0, 3), W(1, 3);
DiscreteConditional conditional5(V | W = "1/2/3 5/6/7 9/10/11");
TableFactor f5(conditional5);
// GTSAM_PRINT(f5);
TableFactor expected_f5(
X & Y,
"0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667");
EXPECT(assert_equal(expected_f5, f5, 1e-6));
}
/* ************************************************************************* */
@ -156,7 +174,8 @@ TEST(TableFactor, multiplication) {
/* ************************************************************************* */
// Benchmark which compares runtime of multiplication of two TableFactors
// and two DecisionTreeFactors given sparsity from dense to 90% sparsity.
TEST(TableFactor, benchmark) {
// NOTE: Enable to run.
TEST_DISABLED(TableFactor, benchmark) {
DiscreteKey A(0, 5), B(1, 2), C(2, 5), D(3, 2), E(4, 5), F(5, 2), G(6, 3),
H(7, 2), I(8, 5), J(9, 7), K(10, 2), L(11, 3);
@ -352,6 +371,39 @@ TEST(TableFactor, htmlWithValueFormatter) {
EXPECT(actual == expected);
}
/* ************************************************************************* */
TEST(TableFactor, Unary) {
// Declare a bunch of keys
DiscreteKey X(0, 2), Y(1, 3);
// Create factors
TableFactor f(X & Y, "2 5 3 6 2 7");
auto op = [](const double x) { return 2 * x; };
auto g = f.apply(op);
TableFactor expected(X & Y, "4 10 6 12 4 14");
EXPECT(assert_equal(g, expected));
auto sq_op = [](const double x) { return x * x; };
auto g_sq = f.apply(sq_op);
TableFactor expected_sq(X & Y, "4 25 9 36 4 49");
EXPECT(assert_equal(g_sq, expected_sq));
}
/* ************************************************************************* */
TEST(TableFactor, UnaryAssignment) {
// Declare a bunch of keys
DiscreteKey X(0, 2), Y(1, 3);
// Create factors
TableFactor f(X & Y, "2 5 3 6 2 7");
auto op = [](const Assignment<Key>& key, const double x) { return 2 * x; };
auto g = f.apply(op);
TableFactor expected(X & Y, "4 10 6 12 4 14");
EXPECT(assert_equal(g, expected));
}
/* ************************************************************************* */
int main() {
TestResult tr;

12
gtsam/geometry/CameraSet.h
View File

@ -131,12 +131,15 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
return z;
}
/** An overload o the project2 function to accept
/** An overload of the project2 function to accept
* full matrices and vectors and pass it to the pointer
* version of the function
* version of the function.
*
* Use SFINAE to resolve overload ambiguity.
*/
template <class POINT, class... OptArgs>
ZVector project2(const POINT& point, OptArgs&... args) const {
typename std::enable_if<(sizeof...(OptArgs) != 0), ZVector>::type project2(
const POINT& point, OptArgs&... args) const {
// pass it to the pointer version of the function
return project2(point, (&args)...);
}
@ -435,8 +438,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
// (DxD) += (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
// add contribution of current factor
// TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for
// now...
// Eigen doesn't let us pass the expression so we call eval()
augmentedHessian.updateDiagonalBlock(
aug_i,
((FiT *

2
gtsam/geometry/Line3.h
View File

@ -146,7 +146,7 @@ class GTSAM_EXPORT Line3 {
* @param Dline - OptionalJacobian of transformed line with respect to l
* @return Transformed line in camera frame
*/
friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
GTSAM_EXPORT friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
OptionalJacobian<4, 6> Dpose,
OptionalJacobian<4, 4> Dline);
};

3
gtsam/geometry/PinholePose.h
View File

@ -332,7 +332,8 @@ public:
}
/// stream operator
friend std::ostream& operator<<(std::ostream &os, const PinholePose& camera) {
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const PinholePose& camera) {
os << "{R: " << camera.pose().rotation().rpy().transpose();
os << ", t: " << camera.pose().translation().transpose();
if (!camera.K_) os << ", K: none";

4
gtsam/geometry/Pose2.cpp
View File

@ -97,7 +97,7 @@ Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) {
/* ************************************************************************* */
Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) {
#ifdef SLOW_BUT_CORRECT_EXPMAP
#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP
return Expmap(v, H);
#else
if (H) {
@ -109,7 +109,7 @@ Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) {
}
/* ************************************************************************* */
Vector3 Pose2::ChartAtOrigin::Local(const Pose2& r, ChartJacobian H) {
#ifdef SLOW_BUT_CORRECT_EXPMAP
#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP
return Logmap(r, H);
#else
if (H) {

40
gtsam/geometry/Pose2.h
View File

@ -36,7 +36,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Pose2: public LieGroup<Pose2, 3> {
class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> {
public:
@ -112,10 +112,10 @@ public:
/// @{
/** print with optional string */
GTSAM_EXPORT void print(const std::string& s = "") const;
void print(const std::string& s = "") const;
/** assert equality up to a tolerance */
GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const;
bool equals(const Pose2& pose, double tol = 1e-9) const;
/// @}
/// @name Group
@ -125,7 +125,7 @@ public:
inline static Pose2 Identity() { return Pose2(); }
/// inverse
GTSAM_EXPORT Pose2 inverse() const;
Pose2 inverse() const;
/// compose syntactic sugar
inline Pose2 operator*(const Pose2& p2) const {
@ -137,16 +137,16 @@ public:
/// @{
///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {});
static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {});
///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = {});
static Vector3 Logmap(const Pose2& p, ChartJacobian H = {});
/**
* Calculate Adjoint map
* Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
*/
GTSAM_EXPORT Matrix3 AdjointMap() const;
Matrix3 AdjointMap() const;
/// Apply AdjointMap to twist xi
inline Vector3 Adjoint(const Vector3& xi) const {
@ -156,7 +156,7 @@ public:
/**
* Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19
*/
GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v);
static Matrix3 adjointMap(const Vector3& v);
/**
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
@ -192,15 +192,15 @@ public:
}
/// Derivative of Expmap
GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v);
static Matrix3 ExpmapDerivative(const Vector3& v);
/// Derivative of Logmap
GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v);
static Matrix3 LogmapDerivative(const Pose2& v);
// Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
struct ChartAtOrigin {
GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {});
struct GTSAM_EXPORT ChartAtOrigin {
static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
static Vector3 Local(const Pose2& r, ChartJacobian H = {});
};
using LieGroup<Pose2, 3>::inverse; // version with derivative
@ -210,7 +210,7 @@ public:
/// @{
/** Return point coordinates in pose coordinate frame */
GTSAM_EXPORT Point2 transformTo(const Point2& point,
Point2 transformTo(const Point2& point,
OptionalJacobian<2, 3> Dpose = {},
OptionalJacobian<2, 2> Dpoint = {}) const;
@ -222,7 +222,7 @@ public:
Matrix transformTo(const Matrix& points) const;
/** Return point coordinates in global frame */
GTSAM_EXPORT Point2 transformFrom(const Point2& point,
Point2 transformFrom(const Point2& point,
OptionalJacobian<2, 3> Dpose = {},
OptionalJacobian<2, 2> Dpoint = {}) const;
@ -273,14 +273,14 @@ public:
}
//// return transformation matrix
GTSAM_EXPORT Matrix3 matrix() const;
Matrix3 matrix() const;
/**
* Calculate bearing to a landmark
* @param point 2D location of landmark
* @return 2D rotation \f$ \in SO(2) \f$
*/
GTSAM_EXPORT Rot2 bearing(const Point2& point,
Rot2 bearing(const Point2& point,
OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const;
/**
@ -288,7 +288,7 @@ public:
* @param point SO(2) location of other pose
* @return 2D rotation \f$ \in SO(2) \f$
*/
GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
Rot2 bearing(const Pose2& pose,
OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const;
/**
@ -296,7 +296,7 @@ public:
* @param point 2D location of landmark
* @return range (double)
*/
GTSAM_EXPORT double range(const Point2& point,
double range(const Point2& point,
OptionalJacobian<1, 3> H1={},
OptionalJacobian<1, 2> H2={}) const;
@ -305,7 +305,7 @@ public:
* @param point 2D location of other pose
* @return range (double)
*/
GTSAM_EXPORT double range(const Pose2& point,
double range(const Pose2& point,
OptionalJacobian<1, 3> H1={},
OptionalJacobian<1, 3> H2={}) const;

2
gtsam/geometry/Pose3.h
View File

@ -204,7 +204,7 @@ public:
static Matrix6 LogmapDerivative(const Pose3& xi);
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
struct ChartAtOrigin {
struct GTSAM_EXPORT ChartAtOrigin {
static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {});
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {});
};

5
gtsam/geometry/Similarity2.h
View File

@ -74,9 +74,10 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
bool operator==(const Similarity2& other) const;
/// Print with optional string
void print(const std::string& s) const;
void print(const std::string& s = "") const;
friend std::ostream& operator<<(std::ostream& os, const Similarity2& p);
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Similarity2& p);
/// @}
/// @name Group

2
gtsam/geometry/Similarity3.cpp
View File

@ -171,7 +171,7 @@ Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
return internal::align(d_abPointPairs, aRb, centroids);
}
Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
Similarity3 Similarity3::Align(const Pose3Pairs &abPosePairs) {
const size_t n = abPosePairs.size();
if (n < 2)
throw std::runtime_error("input should have at least 2 pairs of poses");

7
gtsam/geometry/Similarity3.h
View File

@ -75,9 +75,10 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
bool operator==(const Similarity3& other) const;
/// Print with optional string
void print(const std::string& s) const;
void print(const std::string& s = "") const;
friend std::ostream& operator<<(std::ostream& os, const Similarity3& p);
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Similarity3& p);
/// @}
/// @name Group
@ -132,7 +133,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
* computed using the algorithm described here:
* http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
*/
static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
static Similarity3 Align(const Pose3Pairs& abPosePairs);
/// @}
/// @name Lie Group

4
gtsam/geometry/SphericalCamera.h
View File

@ -238,9 +238,9 @@ class GTSAM_EXPORT SphericalCamera {
// end of class SphericalCamera
template <>
struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {};
struct traits<SphericalCamera> : public internal::Manifold<SphericalCamera> {};
template <>
struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {};
struct traits<const SphericalCamera> : public internal::Manifold<SphericalCamera> {};
} // namespace gtsam

4
gtsam/geometry/StereoPoint2.h
View File

@ -46,7 +46,9 @@ public:
uL_(0), uR_(0), v_(0) {
}
/** constructor */
/** uL and uR represent the x-axis value of left and right frame coordinates respectively.
v represents the y coordinate value. The y-axis value should be the same under the
stereo constraint. */
StereoPoint2(double uL, double uR, double v) :
uL_(uL), uR_(uR), v_(v) {
}

3
gtsam/geometry/Unit3.h
View File

@ -105,7 +105,8 @@ public:
/// @name Testable
/// @{
friend std::ostream& operator<<(std::ostream& os, const Unit3& pair);
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Unit3& pair);
/// The print fuction
void print(const std::string& s = std::string()) const;

11
gtsam/geometry/geometry.i
View File

@ -166,7 +166,9 @@ class Rot2 {
// Manifold
gtsam::Rot2 retract(Vector v) const;
gtsam::Rot2 retract(Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
Vector localCoordinates(const gtsam::Rot2& p) const;
Vector localCoordinates(const gtsam::Rot2& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
// Lie Group
static gtsam::Rot2 Expmap(Vector v);
@ -397,19 +399,24 @@ class Pose2 {
static gtsam::Pose2 Identity();
gtsam::Pose2 inverse() const;
gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
gtsam::Pose2 compose(const gtsam::Pose2& p2, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
gtsam::Pose2 between(const gtsam::Pose2& p2) const;
gtsam::Pose2 between(const gtsam::Pose2& p2, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
// Operator Overloads
gtsam::Pose2 operator*(const gtsam::Pose2& p2) const;
// Manifold
gtsam::Pose2 retract(Vector v) const;
gtsam::Pose2 retract(Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
Vector localCoordinates(const gtsam::Pose2& p) const;
Vector localCoordinates(const gtsam::Pose2& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
// Lie Group
static gtsam::Pose2 Expmap(Vector v);
static Vector Logmap(const gtsam::Pose2& p);
Vector logmap(const gtsam::Pose2& p);
Vector logmap(const gtsam::Pose2& p, Eigen::Ref<Eigen::MatrixXd> H);
static Matrix ExpmapDerivative(Vector v);
static Matrix LogmapDerivative(const gtsam::Pose2& v);
Matrix AdjointMap() const;
@ -1075,6 +1082,7 @@ class Similarity2 {
// Standard Interface
bool equals(const gtsam::Similarity2& sim, double tol) const;
void print(const std::string& s = "") const;
Matrix matrix() const;
gtsam::Rot2& rotation();
gtsam::Point2& translation();
@ -1098,6 +1106,7 @@ class Similarity3 {
// Standard Interface
bool equals(const gtsam::Similarity3& sim, double tol) const;
void print(const std::string& s = "") const;
Matrix matrix() const;
gtsam::Rot3& rotation();
gtsam::Point3& translation();
@ -1173,11 +1182,13 @@ class TriangulationParameters {
bool enableEPI;
double landmarkDistanceThreshold;
double dynamicOutlierRejectionThreshold;
bool useLOST;
gtsam::SharedNoiseModel noiseModel;
TriangulationParameters(const double rankTolerance = 1.0,
const bool enableEPI = false,
double landmarkDistanceThreshold = -1,
double dynamicOutlierRejectionThreshold = -1,
const bool useLOST = false,
const gtsam::SharedNoiseModel& noiseModel = nullptr);
};

4
gtsam/geometry/tests/testPose2.cpp
View File

@ -66,7 +66,7 @@ TEST(Pose2, manifold) {
/* ************************************************************************* */
TEST(Pose2, retract) {
Pose2 pose(M_PI/2.0, Point2(1, 2));
#ifdef SLOW_BUT_CORRECT_EXPMAP
#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP
Pose2 expected(1.00811, 2.01528, 2.5608);
#else
Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01));
@ -204,7 +204,7 @@ TEST(Pose2, Adjoint_hat) {
TEST(Pose2, logmap) {
Pose2 pose0(M_PI/2.0, Point2(1, 2));
Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
#ifdef SLOW_BUT_CORRECT_EXPMAP
#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP
Vector3 expected(0.00986473, -0.0150896, 0.018);
#else
Vector3 expected(0.01, -0.015, 0.018);

19
gtsam/geometry/tests/testRot3.cpp
View File

@ -597,6 +597,25 @@ TEST(Rot3, quaternion) {
EXPECT(assert_equal(expected2, actual2));
}
/* ************************************************************************* */
TEST(Rot3, ConvertQuaternion) {
Eigen::Quaterniond eigenQuaternion;
eigenQuaternion.w() = 1.0;
eigenQuaternion.x() = 2.0;
eigenQuaternion.y() = 3.0;
eigenQuaternion.z() = 4.0;
EXPECT_DOUBLES_EQUAL(1, eigenQuaternion.w(), 1e-9);
EXPECT_DOUBLES_EQUAL(2, eigenQuaternion.x(), 1e-9);
EXPECT_DOUBLES_EQUAL(3, eigenQuaternion.y(), 1e-9);
EXPECT_DOUBLES_EQUAL(4, eigenQuaternion.z(), 1e-9);
Rot3 R(eigenQuaternion);
EXPECT_DOUBLES_EQUAL(1, R.toQuaternion().w(), 1e-9);
EXPECT_DOUBLES_EQUAL(2, R.toQuaternion().x(), 1e-9);
EXPECT_DOUBLES_EQUAL(3, R.toQuaternion().y(), 1e-9);
EXPECT_DOUBLES_EQUAL(4, R.toQuaternion().z(), 1e-9);
}
/* ************************************************************************* */
Matrix Cayley(const Matrix& A) {
Matrix::Index n = A.cols();

25
gtsam/geometry/tests/testSphericalCamera.cpp
View File

@ -24,6 +24,13 @@
#include <cmath>
#include <iostream>
#if defined(__i686__) || defined(__i386__)
// See issue discussion: https://github.com/borglab/gtsam/issues/1605
constexpr double TEST_THRESHOLD = 1e-5;
#else
constexpr double TEST_THRESHOLD = 1e-7;
#endif
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -104,8 +111,8 @@ TEST(SphericalCamera, Dproject) {
Matrix numerical_pose = numericalDerivative21(project3, pose, point1);
Matrix numerical_point = numericalDerivative22(project3, pose, point1);
EXPECT(assert_equal(bearing1, result));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
EXPECT(assert_equal(numerical_pose, Dpose, TEST_THRESHOLD));
EXPECT(assert_equal(numerical_point, Dpoint, TEST_THRESHOLD));
}
/* ************************************************************************* */
@ -123,8 +130,8 @@ TEST(SphericalCamera, reprojectionError) {
Matrix numerical_point =
numericalDerivative32(reprojectionError2, pose, point1, bearing1);
EXPECT(assert_equal(Vector2(0.0, 0.0), result));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
EXPECT(assert_equal(numerical_pose, Dpose, TEST_THRESHOLD));
EXPECT(assert_equal(numerical_point, Dpoint, TEST_THRESHOLD));
}
/* ************************************************************************* */
@ -137,9 +144,9 @@ TEST(SphericalCamera, reprojectionError_noisy) {
numericalDerivative31(reprojectionError2, pose, point1, bearing_noisy);
Matrix numerical_point =
numericalDerivative32(reprojectionError2, pose, point1, bearing_noisy);
EXPECT(assert_equal(Vector2(-0.050282, 0.00833482), result, 1e-5));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
EXPECT(assert_equal(Vector2(-0.050282, 0.00833482), result, 1e2*TEST_THRESHOLD));
EXPECT(assert_equal(numerical_pose, Dpose, TEST_THRESHOLD));
EXPECT(assert_equal(numerical_point, Dpoint, TEST_THRESHOLD));
}
/* ************************************************************************* */
@ -151,8 +158,8 @@ TEST(SphericalCamera, Dproject2) {
camera.project2(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project3, pose1, point1);
Matrix numerical_point = numericalDerivative22(project3, pose1, point1);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
CHECK(assert_equal(numerical_pose, Dpose, TEST_THRESHOLD));
CHECK(assert_equal(numerical_point, Dpoint, TEST_THRESHOLD));
}
/* ************************************************************************* */

14
gtsam/geometry/triangulation.h
View File

@ -574,6 +574,11 @@ struct GTSAM_EXPORT TriangulationParameters {
*/
double dynamicOutlierRejectionThreshold;
/**
* if true, will use the LOST algorithm instead of DLT
*/
bool useLOST;
SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation
/**
@ -588,10 +593,12 @@ struct GTSAM_EXPORT TriangulationParameters {
TriangulationParameters(const double _rankTolerance = 1.0,
const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
double _dynamicOutlierRejectionThreshold = -1,
const bool _useLOST = false,
const SharedNoiseModel& _noiseModel = nullptr) :
rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
landmarkDistanceThreshold(_landmarkDistanceThreshold), //
dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
useLOST(_useLOST),
noiseModel(_noiseModel){
}
@ -604,6 +611,7 @@ struct GTSAM_EXPORT TriangulationParameters {
<< std::endl;
os << "dynamicOutlierRejectionThreshold = "
<< p.dynamicOutlierRejectionThreshold << std::endl;
os << "useLOST = " << p.useLOST << std::endl;
os << "noise model" << std::endl;
return os;
}
@ -665,8 +673,8 @@ class TriangulationResult : public std::optional<Point3> {
return value();
}
// stream to output
friend std::ostream& operator<<(std::ostream& os,
const TriangulationResult& result) {
GTSAM_EXPORT friend std::ostream& operator<<(
std::ostream& os, const TriangulationResult& result) {
if (result)
os << "point = " << *result << std::endl;
else
@ -701,7 +709,7 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
try {
Point3 point =
triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
params.enableEPI, params.noiseModel);
params.enableEPI, params.noiseModel, params.useLOST);
// Check landmark distance and re-projection errors to avoid outliers
size_t i = 0;

26
gtsam/hybrid/GaussianMixture.cpp
View File

@ -228,19 +228,19 @@ std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) {
/**
* @brief Helper function to get the pruner functional.
*
* @param decisionTree The probability decision tree of only discrete keys.
* @param discreteProbs The probabilities of only discrete keys.
* @return std::function<GaussianConditional::shared_ptr(
* const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
*/
std::function<GaussianConditional::shared_ptr(
const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) {
GaussianMixture::prunerFunc(const DecisionTreeFactor &discreteProbs) {
// Get the discrete keys as sets for the decision tree
// and the gaussian mixture.
auto decisionTreeKeySet = DiscreteKeysAsSet(decisionTree.discreteKeys());
auto discreteProbsKeySet = DiscreteKeysAsSet(discreteProbs.discreteKeys());
auto gaussianMixtureKeySet = DiscreteKeysAsSet(this->discreteKeys());
auto pruner = [decisionTree, decisionTreeKeySet, gaussianMixtureKeySet](
auto pruner = [discreteProbs, discreteProbsKeySet, gaussianMixtureKeySet](
const Assignment<Key> &choices,
const GaussianConditional::shared_ptr &conditional)
-> GaussianConditional::shared_ptr {
@ -249,8 +249,8 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) {
// Case where the gaussian mixture has the same
// discrete keys as the decision tree.
if (gaussianMixtureKeySet == decisionTreeKeySet) {
if (decisionTree(values) == 0.0) {
if (gaussianMixtureKeySet == discreteProbsKeySet) {
if (discreteProbs(values) == 0.0) {
// empty aka null pointer
std::shared_ptr<GaussianConditional> null;
return null;
@ -259,9 +259,9 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) {
}
} else {
std::vector<DiscreteKey> set_diff;
std::set_difference(decisionTreeKeySet.begin(), decisionTreeKeySet.end(),
gaussianMixtureKeySet.begin(),
gaussianMixtureKeySet.end(),
std::set_difference(
discreteProbsKeySet.begin(), discreteProbsKeySet.end(),
gaussianMixtureKeySet.begin(), gaussianMixtureKeySet.end(),
std::back_inserter(set_diff));
const std::vector<DiscreteValues> assignments =
@ -272,7 +272,7 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) {
// If any one of the sub-branches are non-zero,
// we need this conditional.
if (decisionTree(augmented_values) > 0.0) {
if (discreteProbs(augmented_values) > 0.0) {
return conditional;
}
}
@ -285,12 +285,10 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) {
}
/* *******************************************************************************/
void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) {
auto decisionTreeKeySet = DiscreteKeysAsSet(decisionTree.discreteKeys());
auto gmKeySet = DiscreteKeysAsSet(this->discreteKeys());
void GaussianMixture::prune(const DecisionTreeFactor &discreteProbs) {
// Functional which loops over all assignments and create a set of
// GaussianConditionals
auto pruner = prunerFunc(decisionTree);
auto pruner = prunerFunc(discreteProbs);
auto pruned_conditionals = conditionals_.apply(pruner);
conditionals_.root_ = pruned_conditionals.root_;

11
gtsam/hybrid/GaussianMixture.h
View File

@ -74,13 +74,13 @@ class GTSAM_EXPORT GaussianMixture
/**
* @brief Helper function to get the pruner functor.
*
* @param decisionTree The pruned discrete probability decision tree.
* @param discreteProbs The pruned discrete probabilities.
* @return std::function<GaussianConditional::shared_ptr(
* const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
*/
std::function<GaussianConditional::shared_ptr(
const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
prunerFunc(const DecisionTreeFactor &decisionTree);
prunerFunc(const DecisionTreeFactor &discreteProbs);
public:
/// @name Constructors
@ -234,12 +234,11 @@ class GTSAM_EXPORT GaussianMixture
/**
* @brief Prune the decision tree of Gaussian factors as per the discrete
* `decisionTree`.
* `discreteProbs`.
*
* @param decisionTree A pruned decision tree of discrete keys where the
* leaves are probabilities.
* @param discreteProbs A pruned set of probabilities for the discrete keys.
*/
void prune(const DecisionTreeFactor &decisionTree);
void prune(const DecisionTreeFactor &discreteProbs);
/**
* @brief Merge the Gaussian Factor Graphs in `this` and `sum` while

104
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -37,43 +37,25 @@ bool HybridBayesNet::equals(const This &bn, double tol) const {
return Base::equals(bn, tol);
}
/* ************************************************************************* */
DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const {
AlgebraicDecisionTree<Key> decisionTree;
// The canonical decision tree factor which will get
// the discrete conditionals added to it.
DecisionTreeFactor dtFactor;
for (auto &&conditional : *this) {
if (conditional->isDiscrete()) {
// Convert to a DecisionTreeFactor and add it to the main factor.
DecisionTreeFactor f(*conditional->asDiscrete());
dtFactor = dtFactor * f;
}
}
return std::make_shared<DecisionTreeFactor>(dtFactor);
}
/* ************************************************************************* */
/**
* @brief Helper function to get the pruner functional.
*
* @param prunedDecisionTree The prob. decision tree of only discrete keys.
* @param prunedDiscreteProbs The prob. decision tree of only discrete keys.
* @param conditional Conditional to prune. Used to get full assignment.
* @return std::function<double(const Assignment<Key> &, double)>
*/
std::function<double(const Assignment<Key> &, double)> prunerFunc(
const DecisionTreeFactor &prunedDecisionTree,
const DecisionTreeFactor &prunedDiscreteProbs,
const HybridConditional &conditional) {
// Get the discrete keys as sets for the decision tree
// and the Gaussian mixture.
std::set<DiscreteKey> decisionTreeKeySet =
DiscreteKeysAsSet(prunedDecisionTree.discreteKeys());
std::set<DiscreteKey> discreteProbsKeySet =
DiscreteKeysAsSet(prunedDiscreteProbs.discreteKeys());
std::set<DiscreteKey> conditionalKeySet =
DiscreteKeysAsSet(conditional.discreteKeys());
auto pruner = [prunedDecisionTree, decisionTreeKeySet, conditionalKeySet](
auto pruner = [prunedDiscreteProbs, discreteProbsKeySet, conditionalKeySet](
const Assignment<Key> &choices,
double probability) -> double {
// This corresponds to 0 probability
@ -83,8 +65,8 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
DiscreteValues values(choices);
// Case where the Gaussian mixture has the same
// discrete keys as the decision tree.
if (conditionalKeySet == decisionTreeKeySet) {
if (prunedDecisionTree(values) == 0) {
if (conditionalKeySet == discreteProbsKeySet) {
if (prunedDiscreteProbs(values) == 0) {
return pruned_prob;
} else {
return probability;
@ -114,11 +96,12 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
}
// Now we generate the full assignment by enumerating
// over all keys in the prunedDecisionTree.
// over all keys in the prunedDiscreteProbs.
// First we find the differing keys
std::vector<DiscreteKey> set_diff;
std::set_difference(decisionTreeKeySet.begin(), decisionTreeKeySet.end(),
conditionalKeySet.begin(), conditionalKeySet.end(),
std::set_difference(discreteProbsKeySet.begin(),
discreteProbsKeySet.end(), conditionalKeySet.begin(),
conditionalKeySet.end(),
std::back_inserter(set_diff));
// Now enumerate over all assignments of the differing keys
@ -130,7 +113,7 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
// If any one of the sub-branches are non-zero,
// we need this probability.
if (prunedDecisionTree(augmented_values) > 0.0) {
if (prunedDiscreteProbs(augmented_values) > 0.0) {
return probability;
}
}
@ -143,44 +126,49 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
}
/* ************************************************************************* */
void HybridBayesNet::updateDiscreteConditionals(
const DecisionTreeFactor &prunedDecisionTree) {
KeyVector prunedTreeKeys = prunedDecisionTree.keys();
DecisionTreeFactor HybridBayesNet::pruneDiscreteConditionals(
size_t maxNrLeaves) {
// Get the joint distribution of only the discrete keys
// The joint discrete probability.
DiscreteConditional discreteProbs;
std::vector<size_t> discrete_factor_idxs;
// Record frontal keys so we can maintain ordering
Ordering discrete_frontals;
// Loop with index since we need it later.
for (size_t i = 0; i < this->size(); i++) {
HybridConditional::shared_ptr conditional = this->at(i);
auto conditional = this->at(i);
if (conditional->isDiscrete()) {
auto discrete = conditional->asDiscrete();
discreteProbs = discreteProbs * (*conditional->asDiscrete());
// Apply prunerFunc to the underlying AlgebraicDecisionTree
auto discreteTree =
std::dynamic_pointer_cast<DecisionTreeFactor::ADT>(discrete);
DecisionTreeFactor::ADT prunedDiscreteTree =
discreteTree->apply(prunerFunc(prunedDecisionTree, *conditional));
// Create the new (hybrid) conditional
KeyVector frontals(discrete->frontals().begin(),
discrete->frontals().end());
auto prunedDiscrete = std::make_shared<DiscreteLookupTable>(
frontals.size(), conditional->discreteKeys(), prunedDiscreteTree);
conditional = std::make_shared<HybridConditional>(prunedDiscrete);
// Add it back to the BayesNet
this->at(i) = conditional;
Ordering conditional_keys(conditional->frontals());
discrete_frontals += conditional_keys;
discrete_factor_idxs.push_back(i);
}
}
const DecisionTreeFactor prunedDiscreteProbs =
discreteProbs.prune(maxNrLeaves);
// Eliminate joint probability back into conditionals
DiscreteFactorGraph dfg{prunedDiscreteProbs};
DiscreteBayesNet::shared_ptr dbn = dfg.eliminateSequential(discrete_frontals);
// Assign pruned discrete conditionals back at the correct indices.
for (size_t i = 0; i < discrete_factor_idxs.size(); i++) {
size_t idx = discrete_factor_idxs.at(i);
this->at(idx) = std::make_shared<HybridConditional>(dbn->at(i));
}
return prunedDiscreteProbs;
}
/* ************************************************************************* */
HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
// Get the decision tree of only the discrete keys
auto discreteConditionals = this->discreteConditionals();
const auto decisionTree = discreteConditionals->prune(maxNrLeaves);
DecisionTreeFactor prunedDiscreteProbs =
this->pruneDiscreteConditionals(maxNrLeaves);
this->updateDiscreteConditionals(decisionTree);
/* To Prune, we visitWith every leaf in the GaussianMixture.
/* To prune, we visitWith every leaf in the GaussianMixture.
* For each leaf, using the assignment we can check the discrete decision tree
* for 0.0 probability, then just set the leaf to a nullptr.
*
@ -190,12 +178,12 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
HybridBayesNet prunedBayesNetFragment;
// Go through all the conditionals in the
// Bayes Net and prune them as per decisionTree.
// Bayes Net and prune them as per prunedDiscreteProbs.
for (auto &&conditional : *this) {
if (auto gm = conditional->asMixture()) {
// Make a copy of the Gaussian mixture and prune it!
auto prunedGaussianMixture = std::make_shared<GaussianMixture>(*gm);
prunedGaussianMixture->prune(decisionTree); // imperative :-(
prunedGaussianMixture->prune(prunedDiscreteProbs); // imperative :-(
// Type-erase and add to the pruned Bayes Net fragment.
prunedBayesNetFragment.push_back(prunedGaussianMixture);

13
gtsam/hybrid/HybridBayesNet.h
View File

@ -136,13 +136,6 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
*/
VectorValues optimize(const DiscreteValues &assignment) const;
/**
* @brief Get all the discrete conditionals as a decision tree factor.
*
* @return DecisionTreeFactor::shared_ptr
*/
DecisionTreeFactor::shared_ptr discreteConditionals() const;
/**
* @brief Sample from an incomplete BayesNet, given missing variables.
*
@ -222,11 +215,11 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
private:
/**
* @brief Update the discrete conditionals with the pruned versions.
* @brief Prune all the discrete conditionals.
*
* @param prunedDecisionTree
* @param maxNrLeaves
*/
void updateDiscreteConditionals(const DecisionTreeFactor &prunedDecisionTree);
DecisionTreeFactor pruneDiscreteConditionals(size_t maxNrLeaves);
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */

16
gtsam/hybrid/HybridBayesTree.cpp
View File

@ -173,18 +173,18 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const {
/* ************************************************************************* */
void HybridBayesTree::prune(const size_t maxNrLeaves) {
auto decisionTree = this->roots_.at(0)->conditional()->asDiscrete();
auto discreteProbs = this->roots_.at(0)->conditional()->asDiscrete();
DecisionTreeFactor prunedDecisionTree = decisionTree->prune(maxNrLeaves);
decisionTree->root_ = prunedDecisionTree.root_;
DecisionTreeFactor prunedDiscreteProbs = discreteProbs->prune(maxNrLeaves);
discreteProbs->root_ = prunedDiscreteProbs.root_;
/// Helper struct for pruning the hybrid bayes tree.
struct HybridPrunerData {
/// The discrete decision tree after pruning.
DecisionTreeFactor prunedDecisionTree;
HybridPrunerData(const DecisionTreeFactor& prunedDecisionTree,
DecisionTreeFactor prunedDiscreteProbs;
HybridPrunerData(const DecisionTreeFactor& prunedDiscreteProbs,
const HybridBayesTree::sharedNode& parentClique)
: prunedDecisionTree(prunedDecisionTree) {}
: prunedDiscreteProbs(prunedDiscreteProbs) {}
/**
* @brief A function used during tree traversal that operates on each node
@ -204,13 +204,13 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) {
if (conditional->isHybrid()) {
auto gaussianMixture = conditional->asMixture();
gaussianMixture->prune(parentData.prunedDecisionTree);
gaussianMixture->prune(parentData.prunedDiscreteProbs);
}
return parentData;
}
};
HybridPrunerData rootData(prunedDecisionTree, 0);
HybridPrunerData rootData(prunedDiscreteProbs, 0);
{
treeTraversal::no_op visitorPost;
// Limits OpenMP threads since we're mixing TBB and OpenMP

4
gtsam/hybrid/HybridBayesTree.h
View File

@ -123,6 +123,10 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
};
/// traits
template <>
struct traits<HybridBayesTreeClique> : public Testable<HybridBayesTreeClique> {
};
template <>
struct traits<HybridBayesTree> : public Testable<HybridBayesTree> {};

1
gtsam/hybrid/HybridFactor.h
View File

@ -20,6 +20,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DecisionTree.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/TableFactor.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Values.h>

5
gtsam/hybrid/HybridFactorGraph.cpp
View File

@ -17,7 +17,6 @@
* @date January, 2023
*/
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
namespace gtsam {
@ -26,7 +25,7 @@ namespace gtsam {
std::set<DiscreteKey> HybridFactorGraph::discreteKeys() const {
std::set<DiscreteKey> keys;
for (auto& factor : factors_) {
if (auto p = std::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
if (auto p = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
for (const DiscreteKey& key : p->discreteKeys()) {
keys.insert(key);
}
@ -67,6 +66,8 @@ const KeySet HybridFactorGraph::continuousKeySet() const {
for (const Key& key : p->continuousKeys()) {
keys.insert(key);
}
} else if (auto p = std::dynamic_pointer_cast<GaussianFactor>(factor)) {
keys.insert(p->keys().begin(), p->keys().end());
}
}
return keys;

2
gtsam/hybrid/HybridFactorGraph.h
View File

@ -35,7 +35,7 @@ using SharedFactor = std::shared_ptr<Factor>;
* Hybrid Factor Graph
* Factor graph with utilities for hybrid factors.
*/
class HybridFactorGraph : public FactorGraph<Factor> {
class GTSAM_EXPORT HybridFactorGraph : public FactorGraph<Factor> {
public:
using Base = FactorGraph<Factor>;
using This = HybridFactorGraph; ///< this class

113
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -48,8 +48,6 @@
#include <utility>
#include <vector>
// #define HYBRID_TIMING
namespace gtsam {
/// Specialize EliminateableFactorGraph for HybridGaussianFactorGraph:
@ -98,7 +96,6 @@ static GaussianFactorGraphTree addGaussian(
// TODO(dellaert): it's probably more efficient to first collect the discrete
// keys, and then loop over all assignments to populate a vector.
GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
gttic(assembleGraphTree);
GaussianFactorGraphTree result;
@ -120,7 +117,7 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
// TODO(dellaert): in C++20, we can use std::visit.
continue;
}
} else if (dynamic_pointer_cast<DecisionTreeFactor>(f)) {
} else if (dynamic_pointer_cast<DiscreteFactor>(f)) {
// Don't do anything for discrete-only factors
// since we want to eliminate continuous values only.
continue;
@ -131,8 +128,6 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
}
}
gttoc(assembleGraphTree);
return result;
}
@ -167,8 +162,8 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
DiscreteFactorGraph dfg;
for (auto &f : factors) {
if (auto dtf = dynamic_pointer_cast<DecisionTreeFactor>(f)) {
dfg.push_back(dtf);
if (auto df = dynamic_pointer_cast<DiscreteFactor>(f)) {
dfg.push_back(df);
} else if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) {
// Ignore orphaned clique.
// TODO(dellaert): is this correct? If so explain here.
@ -190,7 +185,8 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
/* ************************************************************************ */
// If any GaussianFactorGraph in the decision tree contains a nullptr, convert
// that leaf to an empty GaussianFactorGraph. Needed since the DecisionTree will
// otherwise create a GFG with a single (null) factor, which doesn't register as null.
// otherwise create a GFG with a single (null) factor,
// which doesn't register as null.
GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) {
auto emptyGaussian = [](const GaussianFactorGraph &graph) {
bool hasNull =
@ -230,26 +226,14 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
return {nullptr, nullptr};
}
#ifdef HYBRID_TIMING
gttic_(hybrid_eliminate);
#endif
auto result = EliminatePreferCholesky(graph, frontalKeys);
#ifdef HYBRID_TIMING
gttoc_(hybrid_eliminate);
#endif
return result;
};
// Perform elimination!
DecisionTree<Key, Result> eliminationResults(factorGraphTree, eliminate);
#ifdef HYBRID_TIMING
tictoc_print_();
#endif
// Separate out decision tree into conditionals and remaining factors.
const auto [conditionals, newFactors] = unzip(eliminationResults);
@ -273,6 +257,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
};
DecisionTree<Key, double> probabilities(eliminationResults, probability);
return {
std::make_shared<HybridConditional>(gaussianMixture),
std::make_shared<DecisionTreeFactor>(discreteSeparator, probabilities)};
@ -359,64 +344,68 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
// When the number of assignments is large we may encounter stack overflows.
// However this is also the case with iSAM2, so no pressure :)
// PREPROCESS: Identify the nature of the current elimination
// TODO(dellaert): just check the factors:
// Check the factors:
// 1. if all factors are discrete, then we can do discrete elimination:
// 2. if all factors are continuous, then we can do continuous elimination:
// 3. if not, we do hybrid elimination:
// First, identify the separator keys, i.e. all keys that are not frontal.
KeySet separatorKeys;
bool only_discrete = true, only_continuous = true;
for (auto &&factor : factors) {
separatorKeys.insert(factor->begin(), factor->end());
if (auto hybrid_factor = std::dynamic_pointer_cast<HybridFactor>(factor)) {
if (hybrid_factor->isDiscrete()) {
only_continuous = false;
} else if (hybrid_factor->isContinuous()) {
only_discrete = false;
} else if (hybrid_factor->isHybrid()) {
only_continuous = false;
only_discrete = false;
}
// remove frontals from separator
for (auto &k : frontalKeys) {
separatorKeys.erase(k);
}
// Build a map from keys to DiscreteKeys
auto mapFromKeyToDiscreteKey = factors.discreteKeyMap();
// Fill in discrete frontals and continuous frontals.
std::set<DiscreteKey> discreteFrontals;
KeySet continuousFrontals;
for (auto &k : frontalKeys) {
if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) {
discreteFrontals.insert(mapFromKeyToDiscreteKey.at(k));
} else {
continuousFrontals.insert(k);
} else if (auto cont_factor =
std::dynamic_pointer_cast<GaussianFactor>(factor)) {
only_discrete = false;
} else if (auto discrete_factor =
std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
only_continuous = false;
}
}
// Fill in discrete discrete separator keys and continuous separator keys.
std::set<DiscreteKey> discreteSeparatorSet;
KeyVector continuousSeparator;
for (auto &k : separatorKeys) {
if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) {
discreteSeparatorSet.insert(mapFromKeyToDiscreteKey.at(k));
} else {
continuousSeparator.push_back(k);
}
}
// Check if we have any continuous keys:
const bool discrete_only =
continuousFrontals.empty() && continuousSeparator.empty();
// NOTE: We should really defer the product here because of pruning
if (discrete_only) {
if (only_discrete) {
// Case 1: we are only dealing with discrete
return discreteElimination(factors, frontalKeys);
} else if (mapFromKeyToDiscreteKey.empty()) {
} else if (only_continuous) {
// Case 2: we are only dealing with continuous
return continuousElimination(factors, frontalKeys);
} else {
// Case 3: We are now in the hybrid land!
KeySet frontalKeysSet(frontalKeys.begin(), frontalKeys.end());
// Find all the keys in the set of continuous keys
// which are not in the frontal keys. This is our continuous separator.
KeyVector continuousSeparator;
auto continuousKeySet = factors.continuousKeySet();
std::set_difference(
continuousKeySet.begin(), continuousKeySet.end(),
frontalKeysSet.begin(), frontalKeysSet.end(),
std::inserter(continuousSeparator, continuousSeparator.begin()));
// Similarly for the discrete separator.
KeySet discreteSeparatorSet;
std::set<DiscreteKey> discreteSeparator;
auto discreteKeySet = factors.discreteKeySet();
std::set_difference(
discreteKeySet.begin(), discreteKeySet.end(), frontalKeysSet.begin(),
frontalKeysSet.end(),
std::inserter(discreteSeparatorSet, discreteSeparatorSet.begin()));
// Convert from set of keys to set of DiscreteKeys
auto discreteKeyMap = factors.discreteKeyMap();
for (auto key : discreteSeparatorSet) {
discreteSeparator.insert(discreteKeyMap.at(key));
}
return hybridElimination(factors, frontalKeys, continuousSeparator,
discreteSeparatorSet);
discreteSeparator);
}
}
@ -440,7 +429,7 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
// Add the gaussian factor error to every leaf of the error tree.
error_tree = error_tree.apply(
[error](double leaf_value) { return leaf_value + error; });
} else if (dynamic_pointer_cast<DecisionTreeFactor>(f)) {
} else if (dynamic_pointer_cast<DiscreteFactor>(f)) {
// If factor at `idx` is discrete-only, we skip.
continue;
} else {

8
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -40,6 +40,7 @@ class HybridEliminationTree;
class HybridBayesTree;
class HybridJunctionTree;
class DecisionTreeFactor;
class TableFactor;
class JacobianFactor;
class HybridValues;
@ -112,8 +113,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
public:
using Base = HybridFactorGraph;
using This = HybridGaussianFactorGraph; ///< this class
using BaseEliminateable =
EliminateableFactorGraph<This>; ///< for elimination
///< for elimination
using BaseEliminateable = EliminateableFactorGraph<This>;
using shared_ptr = std::shared_ptr<This>; ///< shared_ptr to This
using Values = gtsam::Values; ///< backwards compatibility
@ -148,7 +149,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
/// @name Standard Interface
/// @{
using Base::error; // Expose error(const HybridValues&) method..
/// Expose error(const HybridValues&) method.
using Base::error;
/**
* @brief Compute error for each discrete assignment,

3
gtsam/hybrid/HybridGaussianISAM.h
View File

@ -29,7 +29,8 @@
namespace gtsam {
/**
* @brief
* @brief Incremental Smoothing and Mapping (ISAM) algorithm
* for hybrid factor graphs.
*
* @ingroup hybrid
*/

2
gtsam/hybrid/HybridJunctionTree.cpp
View File

@ -66,7 +66,7 @@ struct HybridConstructorTraversalData {
for (auto& k : hf->discreteKeys()) {
data.discreteKeys.insert(k.first);
}
} else if (auto hf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
} else if (auto hf = std::dynamic_pointer_cast<DiscreteFactor>(f)) {
for (auto& k : hf->discreteKeys()) {
data.discreteKeys.insert(k.first);
}

5
gtsam/hybrid/HybridNonlinearFactorGraph.cpp
View File

@ -17,6 +17,7 @@
*/
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/TableFactor.h>
#include <gtsam/hybrid/GaussianMixture.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
@ -55,8 +56,6 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
for (auto& f : factors_) {
// First check if it is a valid factor
if (!f) {
// TODO(dellaert): why?
linearFG->push_back(GaussianFactor::shared_ptr());
continue;
}
// Check if it is a nonlinear mixture factor
@ -67,7 +66,7 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
} else if (auto nlf = dynamic_pointer_cast<NonlinearFactor>(f)) {
const GaussianFactor::shared_ptr& gf = nlf->linearize(continuousValues);
linearFG->push_back(gf);
} else if (dynamic_pointer_cast<DecisionTreeFactor>(f)) {
} else if (dynamic_pointer_cast<DiscreteFactor>(f)) {
// If discrete-only: doesn't need linearization.
linearFG->push_back(f);
} else if (auto gmf = dynamic_pointer_cast<GaussianMixtureFactor>(f)) {

6
gtsam/hybrid/HybridSmoother.cpp
View File

@ -72,7 +72,8 @@ void HybridSmoother::update(HybridGaussianFactorGraph graph,
addConditionals(graph, hybridBayesNet_, ordering);
// Eliminate.
auto bayesNetFragment = graph.eliminateSequential(ordering);
HybridBayesNet::shared_ptr bayesNetFragment =
graph.eliminateSequential(ordering);
/// Prune
if (maxNrLeaves) {
@ -96,7 +97,8 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph,
HybridGaussianFactorGraph graph(originalGraph);
HybridBayesNet hybridBayesNet(originalHybridBayesNet);
// If we are not at the first iteration, means we have conditionals to add.
// If hybridBayesNet is not empty,
// it means we have conditionals to add to the factor graph.
if (!hybridBayesNet.empty()) {
// We add all relevant conditional mixtures on the last continuous variable
// in the previous `hybridBayesNet` to the graph

2
gtsam/hybrid/HybridSmoother.h
View File

@ -24,7 +24,7 @@
namespace gtsam {
class HybridSmoother {
class GTSAM_EXPORT HybridSmoother {
private:
HybridBayesNet hybridBayesNet_;
HybridGaussianFactorGraph remainingFactorGraph_;

6
gtsam/hybrid/hybrid.i
View File

@ -35,14 +35,11 @@ class HybridValues {
};
#include <gtsam/hybrid/HybridFactor.h>
virtual class HybridFactor {
virtual class HybridFactor : gtsam::Factor {
void print(string s = "HybridFactor\n",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::HybridFactor& other, double tol = 1e-9) const;
bool empty() const;
size_t size() const;
gtsam::KeyVector keys() const;
// Standard interface:
double error(const gtsam::HybridValues &values) const;
@ -179,6 +176,7 @@ class HybridGaussianFactorGraph {
void push_back(const gtsam::HybridBayesTree& bayesTree);
void push_back(const gtsam::GaussianMixtureFactor* gmm);
void push_back(gtsam::DecisionTreeFactor* factor);
void push_back(gtsam::TableFactor* factor);
void push_back(gtsam::JacobianFactor* factor);
bool empty() const;

27
gtsam/hybrid/tests/Switching.h
View File

@ -202,31 +202,16 @@ struct Switching {
* @brief Add "mode chain" to HybridNonlinearFactorGraph from M(0) to M(K-2).
* E.g. if K=4, we want M0, M1 and M2.
*
* @param fg The nonlinear factor graph to which the mode chain is added.
* @param fg The factor graph to which the mode chain is added.
*/
void addModeChain(HybridNonlinearFactorGraph *fg,
template <typename FACTORGRAPH>
void addModeChain(FACTORGRAPH *fg,
std::string discrete_transition_prob = "1/2 3/2") {
fg->emplace_shared<DiscreteDistribution>(modes[0], "1/1");
fg->template emplace_shared<DiscreteDistribution>(modes[0], "1/1");
for (size_t k = 0; k < K - 2; k++) {
auto parents = {modes[k]};
fg->emplace_shared<DiscreteConditional>(modes[k + 1], parents,
discrete_transition_prob);
}
}
/**
* @brief Add "mode chain" to HybridGaussianFactorGraph from M(0) to M(K-2).
* E.g. if K=4, we want M0, M1 and M2.
*
* @param fg The gaussian factor graph to which the mode chain is added.
*/
void addModeChain(HybridGaussianFactorGraph *fg,
std::string discrete_transition_prob = "1/2 3/2") {
fg->emplace_shared<DiscreteDistribution>(modes[0], "1/1");
for (size_t k = 0; k < K - 2; k++) {
auto parents = {modes[k]};
fg->emplace_shared<DiscreteConditional>(modes[k + 1], parents,
discrete_transition_prob);
fg->template emplace_shared<DiscreteConditional>(
modes[k + 1], parents, discrete_transition_prob);
}
}
};

26
gtsam/hybrid/tests/testHybridBayesNet.cpp
View File

@ -231,7 +231,7 @@ TEST(HybridBayesNet, Pruning) {
auto prunedTree = prunedBayesNet.evaluate(delta.continuous());
// Regression test on pruned logProbability tree
std::vector<double> pruned_leaves = {0.0, 20.346113, 0.0, 19.738098};
std::vector<double> pruned_leaves = {0.0, 32.713418, 0.0, 31.735823};
AlgebraicDecisionTree<Key> expected_pruned(discrete_keys, pruned_leaves);
EXPECT(assert_equal(expected_pruned, prunedTree, 1e-6));
@ -248,8 +248,10 @@ TEST(HybridBayesNet, Pruning) {
logProbability +=
posterior->at(4)->asDiscrete()->logProbability(hybridValues);
// Regression
double density = exp(logProbability);
EXPECT_DOUBLES_EQUAL(density, actualTree(discrete_values), 1e-9);
EXPECT_DOUBLES_EQUAL(density,
1.6078460548731697 * actualTree(discrete_values), 1e-6);
EXPECT_DOUBLES_EQUAL(density, prunedTree(discrete_values), 1e-9);
EXPECT_DOUBLES_EQUAL(logProbability, posterior->logProbability(hybridValues),
1e-9);
@ -283,10 +285,16 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
EXPECT_LONGS_EQUAL(7, posterior->size());
size_t maxNrLeaves = 3;
auto discreteConditionals = posterior->discreteConditionals();
DiscreteConditional discreteConditionals;
for (auto&& conditional : *posterior) {
if (conditional->isDiscrete()) {
discreteConditionals =
discreteConditionals * (*conditional->asDiscrete());
}
}
const DecisionTreeFactor::shared_ptr prunedDecisionTree =
std::make_shared<DecisionTreeFactor>(
discreteConditionals->prune(maxNrLeaves));
discreteConditionals.prune(maxNrLeaves));
#ifdef GTSAM_DT_MERGING
EXPECT_LONGS_EQUAL(maxNrLeaves + 2 /*2 zero leaves*/,
@ -295,12 +303,16 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
EXPECT_LONGS_EQUAL(8 /*full tree*/, prunedDecisionTree->nrLeaves());
#endif
auto original_discrete_conditionals = *(posterior->at(4)->asDiscrete());
// regression
DiscreteKeys dkeys{{M(0), 2}, {M(1), 2}, {M(2), 2}};
DecisionTreeFactor::ADT potentials(
dkeys, std::vector<double>{0, 0, 0, 0.505145423, 0, 1, 0, 0.494854577});
DiscreteConditional expected_discrete_conditionals(1, dkeys, potentials);
// Prune!
posterior->prune(maxNrLeaves);
// Functor to verify values against the original_discrete_conditionals
// Functor to verify values against the expected_discrete_conditionals
auto checker = [&](const Assignment<Key>& assignment,
double probability) -> double {
// typecast so we can use this to get probability value
@ -308,7 +320,7 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
if (prunedDecisionTree->operator()(choices) == 0) {
EXPECT_DOUBLES_EQUAL(0.0, probability, 1e-9);
} else {
EXPECT_DOUBLES_EQUAL(original_discrete_conditionals(choices), probability,
EXPECT_DOUBLES_EQUAL(expected_discrete_conditionals(choices), probability,
1e-9);
}
return 0.0;

2
gtsam/hybrid/tests/testHybridBayesTree.cpp
View File

@ -146,7 +146,7 @@ TEST(HybridBayesTree, Optimize) {
DiscreteFactorGraph dfg;
for (auto&& f : *remainingFactorGraph) {
auto discreteFactor = dynamic_pointer_cast<DecisionTreeFactor>(f);
auto discreteFactor = dynamic_pointer_cast<DiscreteFactor>(f);
assert(discreteFactor);
dfg.push_back(discreteFactor);
}

55
gtsam/hybrid/tests/testHybridEstimation.cpp
View File

@ -140,6 +140,61 @@ TEST(HybridEstimation, IncrementalSmoother) {
EXPECT(assert_equal(expected_continuous, result));
}
/****************************************************************************/
// Test approximate inference with an additional pruning step.
TEST(HybridEstimation, ISAM) {
size_t K = 15;
std::vector<double> measurements = {0, 1, 2, 2, 2, 2, 3, 4, 5, 6, 6,
7, 8, 9, 9, 9, 10, 11, 11, 11, 11};
// Ground truth discrete seq
std::vector<size_t> discrete_seq = {1, 1, 0, 0, 0, 1, 1, 1, 1, 0,
1, 1, 1, 0, 0, 1, 1, 0, 0, 0};
// Switching example of robot moving in 1D
// with given measurements and equal mode priors.
Switching switching(K, 1.0, 0.1, measurements, "1/1 1/1");
HybridNonlinearISAM isam;
HybridNonlinearFactorGraph graph;
Values initial;
// gttic_(Estimation);
// Add the X(0) prior
graph.push_back(switching.nonlinearFactorGraph.at(0));
initial.insert(X(0), switching.linearizationPoint.at<double>(X(0)));
HybridGaussianFactorGraph linearized;
for (size_t k = 1; k < K; k++) {
// Motion Model
graph.push_back(switching.nonlinearFactorGraph.at(k));
// Measurement
graph.push_back(switching.nonlinearFactorGraph.at(k + K - 1));
initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
isam.update(graph, initial, 3);
// isam.bayesTree().print("\n\n");
graph.resize(0);
initial.clear();
}
Values result = isam.estimate();
DiscreteValues assignment = isam.assignment();
DiscreteValues expected_discrete;
for (size_t k = 0; k < K - 1; k++) {
expected_discrete[M(k)] = discrete_seq[k];
}
EXPECT(assert_equal(expected_discrete, assignment));
Values expected_continuous;
for (size_t k = 0; k < K; k++) {
expected_continuous.insert(X(k), measurements[k]);
}
EXPECT(assert_equal(expected_continuous, result));
}
/**
* @brief A function to get a specific 1D robot motion problem as a linearized
* factor graph. This is the problem P(X|Z, M), i.e. estimating the continuous

28
gtsam/hybrid/tests/testHybridFactorGraph.cpp
View File

@ -18,7 +18,9 @@
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/utilities.h>
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
using namespace std;
@ -37,6 +39,32 @@ TEST(HybridFactorGraph, Constructor) {
HybridFactorGraph fg;
}
/* ************************************************************************* */
// Test if methods to get keys work as expected.
TEST(HybridFactorGraph, Keys) {
HybridGaussianFactorGraph hfg;
// Add prior on x0
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
// Add factor between x0 and x1
hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
// Add a gaussian mixture factor ϕ(x1, c1)
DiscreteKey m1(M(1), 2);
DecisionTree<Key, GaussianFactor::shared_ptr> dt(
M(1), std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
hfg.add(GaussianMixtureFactor({X(1)}, {m1}, dt));
KeySet expected_continuous{X(0), X(1)};
EXPECT(
assert_container_equality(expected_continuous, hfg.continuousKeySet()));
KeySet expected_discrete{M(1)};
EXPECT(assert_container_equality(expected_discrete, hfg.discreteKeySet()));
}
/* ************************************************************************* */
int main() {
TestResult tr;

7
gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
View File

@ -42,6 +42,7 @@
#include <functional>
#include <iostream>
#include <iterator>
#include <numeric>
#include <vector>
#include "Switching.h"
@ -612,11 +613,11 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) {
// Create expected decision tree with two factor graphs:
// Get mixture factor:
auto mixture = std::dynamic_pointer_cast<GaussianMixtureFactor>(fg.at(0));
auto mixture = fg.at<GaussianMixtureFactor>(0);
CHECK(mixture);
// Get prior factor:
const auto gf = std::dynamic_pointer_cast<HybridConditional>(fg.at(1));
const auto gf = fg.at<HybridConditional>(1);
CHECK(gf);
using GF = GaussianFactor::shared_ptr;
const GF prior = gf->asGaussian();
@ -902,7 +903,7 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
// Test resulting posterior Bayes net has correct size:
EXPECT_LONGS_EQUAL(8, posterior->size());
// TODO(dellaert): this test fails - no idea why.
// Ratio test
EXPECT(ratioTest(bn, measurements, *posterior));
}

2
gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
View File

@ -420,7 +420,7 @@ TEST(HybridFactorGraph, Full_Elimination) {
DiscreteFactorGraph discrete_fg;
// TODO(Varun) Make this a function of HybridGaussianFactorGraph?
for (auto& factor : (*remainingFactorGraph_partial)) {
auto df = dynamic_pointer_cast<DecisionTreeFactor>(factor);
auto df = dynamic_pointer_cast<DiscreteFactor>(factor);
assert(df);
discrete_fg.push_back(df);
}

8
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -140,9 +140,15 @@ namespace gtsam {
/** Access the conditional */
const sharedConditional& conditional() const { return conditional_; }
/** is this the root of a Bayes tree ? */
/// Return true if this clique is the root of a Bayes tree.
inline bool isRoot() const { return parent_.expired(); }
/// Return the number of children.
size_t nrChildren() const { return children.size(); }
/// Return the child at index i.
const derived_ptr operator[](size_t i) const { return children.at(i); }
/** The size of subtree rooted at this clique, i.e., nr of Cliques */
size_t treeSize() const;

4
gtsam/inference/ClusterTree.h
View File

@ -49,7 +49,7 @@ class ClusterTree {
virtual ~Cluster() {}
const Cluster& operator[](size_t i) const {
return *(children[i]);
return *(children.at(i));
}
/// Construct from factors associated with a single key
@ -161,7 +161,7 @@ class ClusterTree {
}
const Cluster& operator[](size_t i) const {
return *(roots_[i]);
return *(roots_.at(i));
}
/// @}

10
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -74,8 +74,9 @@ namespace gtsam {
EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
const auto [bayesNet, factorGraph] = etree.eliminate(function);
// If any factors are remaining, the ordering was incomplete
if(!factorGraph->empty())
throw InconsistentEliminationRequested();
if(!factorGraph->empty()) {
throw InconsistentEliminationRequested(factorGraph->keys());
}
// Return the Bayes net
return bayesNet;
}
@ -136,8 +137,9 @@ namespace gtsam {
JunctionTreeType junctionTree(etree);
const auto [bayesTree, factorGraph] = junctionTree.eliminate(function);
// If any factors are remaining, the ordering was incomplete
if(!factorGraph->empty())
throw InconsistentEliminationRequested();
if(!factorGraph->empty()) {
throw InconsistentEliminationRequested(factorGraph->keys());
}
// Return the Bayes tree
return bayesTree;
}

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