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

Merge branch 'develop' into fix-1496

release/4.3a0
Varun Agrawal 2023-07-17 23:49:55 -04:00
parent e5a7bace55 016f77ba55
commit 3fe9f1a346
44 changed files with 562 additions and 609 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.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 }}

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

6
.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 }}

6
.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 }}

6
.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 }}

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

1
cmake/HandleGeneralOptions.cmake
View File

@ -32,6 +32,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()

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 {

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

41
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,
@ -182,15 +179,12 @@ 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 {
std::vector<double> probs;
for (auto&& [key, value] : enumerate()) {
probs.push_back(value);
}
}
return result;
return probs;
}
/* ************************************************************************ */
@ -289,16 +283,14 @@ 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 {
@ -306,11 +298,10 @@ namespace gtsam {
// 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);
});
// NOTE(Varun) this is potentially slow due to the cartesian product
for (auto&& [assignment, prob] : this->enumerate()) {
probabilities.push_back(prob);
}
// The number of probabilities can be lower than max_leaves
if (probabilities.size() <= N) {

11
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
/// @{
@ -119,8 +115,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);
@ -179,8 +173,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 +254,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
};

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

43
gtsam/discrete/DiscreteFactor.h
View File

@ -36,28 +36,35 @@ class HybridValues;
* @ingroup discrete
*/
class GTSAM_EXPORT DiscreteFactor: public Factor {
public:
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. */
template<typename CONTAINER>
DiscreteFactor(const CONTAINER& keys) : Base(keys) {}
/**
* 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,
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

26
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,6 +56,10 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys,
sort(sorted_dkeys_.begin(), sorted_dkeys_.end());
}
/* ************************************************************************ */
TableFactor::TableFactor(const DiscreteConditional& c)
: TableFactor(c.discreteKeys(), c.probabilities()) {}
/* ************************************************************************ */
Eigen::SparseVector<double> TableFactor::Convert(
const std::vector<double>& table) {
@ -435,18 +439,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,

15
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]));
}
@ -142,6 +141,9 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
TableFactor(const DiscreteKey& key, const std::vector<double>& row)
: TableFactor(DiscreteKeys{key}, row) {}
/** Construct from a DiscreteConditional type */
explicit TableFactor(const DiscreteConditional& c);
/// @}
/// @name Testable
/// @{
@ -180,8 +182,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
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);
@ -274,9 +274,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.
*

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

@ -51,6 +51,11 @@ TEST( DecisionTreeFactor, 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");
DecisionTreeFactor f4(conditional);
EXPECT_DOUBLES_EQUAL(0.8, f4(values), 1e-9);
}
/* ************************************************************************* */

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

@ -93,7 +93,8 @@ void printTime(map<double, pair<chrono::microseconds, chrono::microseconds>>
for (auto&& kv : measured_time) {
cout << "dropout: " << kv.first
<< " | TableFactor time: " << kv.second.first.count()
<< " | DecisionTreeFactor time: " << kv.second.second.count() << endl;
<< " | DecisionTreeFactor time: " << kv.second.second.count() <<
endl;
}
}
@ -124,6 +125,13 @@ 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));
}
/* ************************************************************************* */
@ -156,7 +164,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);

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

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

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

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

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,12 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) {
}
/* *******************************************************************************/
void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) {
auto decisionTreeKeySet = DiscreteKeysAsSet(decisionTree.discreteKeys());
void GaussianMixture::prune(const DecisionTreeFactor &discreteProbs) {
auto discreteProbsKeySet = DiscreteKeysAsSet(discreteProbs.discreteKeys());
auto gmKeySet = DiscreteKeysAsSet(this->discreteKeys());
// 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

60
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -39,41 +39,41 @@ bool HybridBayesNet::equals(const This &bn, double tol) const {
/* ************************************************************************* */
DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const {
AlgebraicDecisionTree<Key> decisionTree;
AlgebraicDecisionTree<Key> discreteProbs;
// The canonical decision tree factor which will get
// the discrete conditionals added to it.
DecisionTreeFactor dtFactor;
DecisionTreeFactor discreteProbsFactor;
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;
discreteProbsFactor = discreteProbsFactor * f;
}
}
return std::make_shared<DecisionTreeFactor>(dtFactor);
return std::make_shared<DecisionTreeFactor>(discreteProbsFactor);
}
/* ************************************************************************* */
/**
* @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 +83,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 +114,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 +131,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;
}
}
@ -144,8 +145,8 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
/* ************************************************************************* */
void HybridBayesNet::updateDiscreteConditionals(
const DecisionTreeFactor &prunedDecisionTree) {
KeyVector prunedTreeKeys = prunedDecisionTree.keys();
const DecisionTreeFactor &prunedDiscreteProbs) {
KeyVector prunedTreeKeys = prunedDiscreteProbs.keys();
// Loop with index since we need it later.
for (size_t i = 0; i < this->size(); i++) {
@ -153,18 +154,21 @@ void HybridBayesNet::updateDiscreteConditionals(
if (conditional->isDiscrete()) {
auto discrete = conditional->asDiscrete();
// Apply prunerFunc to the underlying AlgebraicDecisionTree
// Convert pointer from conditional to factor
auto discreteTree =
std::dynamic_pointer_cast<DecisionTreeFactor::ADT>(discrete);
// Apply prunerFunc to the underlying AlgebraicDecisionTree
DecisionTreeFactor::ADT prunedDiscreteTree =
discreteTree->apply(prunerFunc(prunedDecisionTree, *conditional));
discreteTree->apply(prunerFunc(prunedDiscreteProbs, *conditional));
gttic_(HybridBayesNet_MakeConditional);
// 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);
gttoc_(HybridBayesNet_MakeConditional);
// Add it back to the BayesNet
this->at(i) = conditional;
@ -175,10 +179,16 @@ void HybridBayesNet::updateDiscreteConditionals(
/* ************************************************************************* */
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);
gttic_(HybridBayesNet_PruneDiscreteConditionals);
DecisionTreeFactor::shared_ptr discreteConditionals =
this->discreteConditionals();
const DecisionTreeFactor prunedDiscreteProbs =
discreteConditionals->prune(maxNrLeaves);
gttoc_(HybridBayesNet_PruneDiscreteConditionals);
this->updateDiscreteConditionals(decisionTree);
gttic_(HybridBayesNet_UpdateDiscreteConditionals);
this->updateDiscreteConditionals(prunedDiscreteProbs);
gttoc_(HybridBayesNet_UpdateDiscreteConditionals);
/* To Prune, we visitWith every leaf in the GaussianMixture.
* For each leaf, using the assignment we can check the discrete decision tree
@ -189,13 +199,14 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
HybridBayesNet prunedBayesNetFragment;
gttic_(HybridBayesNet_PruneMixtures);
// 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);
@ -205,6 +216,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
prunedBayesNetFragment.push_back(conditional);
}
}
gttoc_(HybridBayesNet_PruneMixtures);
return prunedBayesNetFragment;
}

4
gtsam/hybrid/HybridBayesNet.h
View File

@ -224,9 +224,9 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
/**
* @brief Update the discrete conditionals with the pruned versions.
*
* @param prunedDecisionTree
* @param prunedDiscreteProbs
*/
void updateDiscreteConditionals(const DecisionTreeFactor &prunedDecisionTree);
void updateDiscreteConditionals(const DecisionTreeFactor &prunedDiscreteProbs);
#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

19
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -98,7 +98,7 @@ 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);
gttic_(assembleGraphTree);
GaussianFactorGraphTree result;
@ -131,7 +131,7 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
}
}
gttoc(assembleGraphTree);
gttoc_(assembleGraphTree);
return result;
}
@ -190,7 +190,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 +231,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);

7
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -112,8 +112,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 +148,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,

77
gtsam/inference/FactorGraph.h
View File

@ -29,10 +29,6 @@
#include <Eigen/Core> // for Eigen::aligned_allocator
#ifdef GTSAM_USE_BOOST_FEATURES
#include <boost/assign/list_inserter.hpp>
#endif
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/vector.hpp>
@ -53,45 +49,6 @@ class BayesTree;
class HybridValues;
/** Helper */
template <class C>
class CRefCallPushBack {
C& obj;
public:
explicit CRefCallPushBack(C& obj) : obj(obj) {}
template <typename A>
void operator()(const A& a) {
obj.push_back(a);
}
};
/** Helper */
template <class C>
class RefCallPushBack {
C& obj;
public:
explicit RefCallPushBack(C& obj) : obj(obj) {}
template <typename A>
void operator()(A& a) {
obj.push_back(a);
}
};
/** Helper */
template <class C>
class CRefCallAddCopy {
C& obj;
public:
explicit CRefCallAddCopy(C& obj) : obj(obj) {}
template <typename A>
void operator()(const A& a) {
obj.addCopy(a);
}
};
/**
* A factor graph is a bipartite graph with factor nodes connected to variable
* nodes. In this class, however, only factor nodes are kept around.
@ -215,17 +172,26 @@ class FactorGraph {
push_back(factor);
}
#ifdef GTSAM_USE_BOOST_FEATURES
/// `+=` works well with boost::assign list inserter.
/// Append factor to factor graph
template <class DERIVEDFACTOR>
typename std::enable_if<
std::is_base_of<FactorType, DERIVEDFACTOR>::value,
boost::assign::list_inserter<RefCallPushBack<This>>>::type
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value,
This>::type&
operator+=(std::shared_ptr<DERIVEDFACTOR> factor) {
return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(
factor);
push_back(factor);
return *this;
}
/**
* @brief Overload comma operator to allow for append chaining.
*
* E.g. fg += factor1, factor2, ...
*/
template <class DERIVEDFACTOR>
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value, This>::type& operator,(
std::shared_ptr<DERIVEDFACTOR> factor) {
push_back(factor);
return *this;
}
#endif
/// @}
/// @name Adding via iterators
@ -276,18 +242,15 @@ class FactorGraph {
push_back(factorOrContainer);
}
#ifdef GTSAM_USE_BOOST_FEATURES
/**
* Add a factor or container of factors, including STL collections,
* BayesTrees, etc.
*/
template <class FACTOR_OR_CONTAINER>
boost::assign::list_inserter<CRefCallPushBack<This>> operator+=(
const FACTOR_OR_CONTAINER& factorOrContainer) {
return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(
factorOrContainer);
This& operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) {
push_back(factorOrContainer);
return *this;
}
#endif
/// @}
/// @name Specialized versions

12
gtsam/inference/Ordering.cpp
View File

@ -281,6 +281,18 @@ void Ordering::print(const std::string& str,
cout.flush();
}
/* ************************************************************************* */
Ordering::This& Ordering::operator+=(Key key) {
this->push_back(key);
return *this;
}
/* ************************************************************************* */
Ordering::This& Ordering::operator,(Key key) {
this->push_back(key);
return *this;
}
/* ************************************************************************* */
Ordering::This& Ordering::operator+=(KeyVector& keys) {
this->insert(this->end(), keys.begin(), keys.end());

20
gtsam/inference/Ordering.h
View File

@ -25,10 +25,6 @@
#include <gtsam/inference/MetisIndex.h>
#include <gtsam/base/FastSet.h>
#ifdef GTSAM_USE_BOOST_FEATURES
#include <boost/assign/list_inserter.hpp>
#endif
#include <algorithm>
#include <vector>
@ -61,15 +57,13 @@ public:
Base(keys.begin(), keys.end()) {
}
#ifdef GTSAM_USE_BOOST_FEATURES
/// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling
/// push_back.
boost::assign::list_inserter<boost::assign_detail::call_push_back<This> > operator+=(
Key key) {
return boost::assign::make_list_inserter(
boost::assign_detail::call_push_back<This>(*this))(key);
}
#endif
/// Add new variables to the ordering as
/// `ordering += key1, key2, ...`.
This& operator+=(Key key);
/// Overloading the comma operator allows for chaining appends
// e.g. keys += key1, key2
This& operator,(Key key);
/**
* @brief Append new keys to the ordering as `ordering += keys`.

14
gtsam/inference/tests/testOrdering.cpp
View File

@ -196,6 +196,20 @@ TEST(Ordering, csr_format_3) {
EXPECT(adjExpected == adjAcutal);
}
/* ************************************************************************* */
TEST(Ordering, AppendKey) {
using symbol_shorthand::X;
Ordering actual;
actual += X(0);
Ordering expected1{X(0)};
EXPECT(assert_equal(expected1, actual));
actual += X(1), X(2), X(3);
Ordering expected2{X(0), X(1), X(2), X(3)};
EXPECT(assert_equal(expected2, actual));
}
/* ************************************************************************* */
TEST(Ordering, AppendVector) {
using symbol_shorthand::X;

26
gtsam/linear/LossFunctions.h
View File

@ -91,7 +91,7 @@ class GTSAM_EXPORT Base {
* functions. It would be better for this function to accept the vector and
* internally call the norm if necessary.
*
* This returns \rho(x) in \ref mEstimator
* This returns \f$\rho(x)\f$ in \ref mEstimator
*/
virtual double loss(double distance) const { return 0; }
@ -143,9 +143,9 @@ class GTSAM_EXPORT Base {
*
* This model has no additional parameters.
*
* - Loss \rho(x) = 0.5 x²
* - Derivative \phi(x) = x
* - Weight w(x) = \phi(x)/x = 1
* - Loss \f$ \rho(x) = 0.5 x² \f$
* - Derivative \f$ \phi(x) = x \f$
* - Weight \f$ w(x) = \phi(x)/x = 1 \f$
*/
class GTSAM_EXPORT Null : public Base {
public:
@ -285,9 +285,9 @@ class GTSAM_EXPORT Cauchy : public Base {
*
* This model has a scalar parameter "c".
*
* - Loss \rho(x) = c² (1 - (1-x²/c²)³)/6 if |x|<c, c²/6 otherwise
* - Derivative \phi(x) = x(1-x²/c²)² if |x|<c, 0 otherwise
* - Weight w(x) = \phi(x)/x = (1-x²/c²)² if |x|<c, 0 otherwise
* - Loss \f$ \rho(x) = c² (1 - (1-x²/c²)³)/6 \f$ if |x|<c, c²/6 otherwise
* - Derivative \f$ \phi(x) = x(1-x²/c²)² if |x|<c \f$, 0 otherwise
* - Weight \f$ w(x) = \phi(x)/x = (1-x²/c²)² \f$ if |x|<c, 0 otherwise
*/
class GTSAM_EXPORT Tukey : public Base {
protected:
@ -320,9 +320,9 @@ class GTSAM_EXPORT Tukey : public Base {
*
* This model has a scalar parameter "c".
*
* - Loss \rho(x) = -0.5 c² (exp(-x²/c²) - 1)
* - Derivative \phi(x) = x exp(-x²/c²)
* - Weight w(x) = \phi(x)/x = exp(-x²/c²)
* - Loss \f$ \rho(x) = -0.5 c² (exp(-x²/c²) - 1) \f$
* - Derivative \f$ \phi(x) = x exp(-x²/c²) \f$
* - Weight \f$ w(x) = \phi(x)/x = exp(-x²/c²) \f$
*/
class GTSAM_EXPORT Welsch : public Base {
protected:
@ -439,9 +439,9 @@ class GTSAM_EXPORT DCS : public Base {
*
* This model has a scalar parameter "k".
*
* - Loss \rho(x) = 0 if |x|<k, 0.5(k-|x|)² otherwise
* - Derivative \phi(x) = 0 if |x|<k, (-k+x) if x>k, (k+x) if x<-k
* - Weight w(x) = \phi(x)/x = 0 if |x|<k, (-k+x)/x if x>k, (k+x)/x if x<-k
* - Loss \f$ \rho(x) = 0 \f$ if |x|<k, 0.5(k-|x|)² otherwise
* - Derivative \f$ \phi(x) = 0 \f$ if |x|<k, (-k+x) if x>k, (k+x) if x<-k
* - Weight \f$ w(x) = \phi(x)/x = 0 \f$ if |x|<k, (-k+x)/x if x>k, (k+x)/x if x<-k
*/
class GTSAM_EXPORT L2WithDeadZone : public Base {
protected:

22
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -70,6 +70,28 @@ TEST(GaussianFactorGraph, initialization) {
EQUALITY(expectedIJS, actualIJS);
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, Append) {
// Create empty graph
GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
auto f1 =
make_shared<JacobianFactor>(0, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2);
auto f2 = make_shared<JacobianFactor>(0, -10 * I_2x2, 1, 10 * I_2x2,
Vector2(2.0, -1.0), unit2);
auto f3 = make_shared<JacobianFactor>(0, -5 * I_2x2, 2, 5 * I_2x2,
Vector2(0.0, 1.0), unit2);
fg += f1;
fg += f2;
EXPECT_LONGS_EQUAL(2, fg.size());
fg = GaussianFactorGraph();
fg += f1, f2, f3;
EXPECT_LONGS_EQUAL(3, fg.size());
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, sparseJacobian) {
// Create factor graph:

34
gtsam/nonlinear/Values-inl.h
View File

@ -243,16 +243,50 @@ namespace gtsam {
insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
}
// partial specialization to insert an expression involving unary operators
template <typename UnaryOp, typename ValueType>
void Values::insert(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val) {
insert(j, val.eval());
}
// partial specialization to insert an expression involving binary operators
template <typename BinaryOp, typename ValueType1, typename ValueType2>
void Values::insert(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val) {
insert(j, val.eval());
}
// update with templated value
template <typename ValueType>
void Values::update(Key j, const ValueType& val) {
update(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
}
// partial specialization to update with an expression involving unary operators
template <typename UnaryOp, typename ValueType>
void Values::update(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val) {
update(j, val.eval());
}
// partial specialization to update with an expression involving binary operators
template <typename BinaryOp, typename ValueType1, typename ValueType2>
void Values::update(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val) {
update(j, val.eval());
}
// insert_or_assign with templated value
template <typename ValueType>
void Values::insert_or_assign(Key j, const ValueType& val) {
insert_or_assign(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
}
template <typename UnaryOp, typename ValueType>
void Values::insert_or_assign(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val) {
insert_or_assign(j, val.eval());
}
template <typename BinaryOp, typename ValueType1, typename ValueType2>
void Values::insert_or_assign(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val) {
insert_or_assign(j, val.eval());
}
}

51
gtsam/nonlinear/Values.h
View File

@ -245,6 +245,31 @@ namespace gtsam {
template <typename ValueType>
void insert(Key j, const ValueType& val);
/** Partial specialization that allows passing a unary Eigen expression for val.
*
* A unary expression is an expression such as 2*a or -a, where a is a valid Vector or Matrix type.
* The typical usage is for types Point2 (i.e. Eigen::Vector2d) or Point3 (i.e. Eigen::Vector3d).
* For example, together with the partial specialization for binary operators, a user may call insert(j, 2*a + M*b - c),
* where M is an appropriately sized matrix (such as a rotation matrix).
* Thus, it isn't necessary to explicitly evaluate the Eigen expression, as in insert(j, (2*a + M*b - c).eval()),
* nor is it necessary to first assign the expression to a separate variable.
*/
template <typename UnaryOp, typename ValueType>
void insert(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val);
/** Partial specialization that allows passing a binary Eigen expression for val.
*
* A binary expression is an expression such as a + b, where a and b are valid Vector or Matrix
* types of compatible size.
* The typical usage is for types Point2 (i.e. Eigen::Vector2d) or Point3 (i.e. Eigen::Vector3d).
* For example, together with the partial specialization for binary operators, a user may call insert(j, 2*a + M*b - c),
* where M is an appropriately sized matrix (such as a rotation matrix).
* Thus, it isn't necessary to explicitly evaluate the Eigen expression, as in insert(j, (2*a + M*b - c).eval()),
* nor is it necessary to first assign the expression to a separate variable.
*/
template <typename BinaryOp, typename ValueType1, typename ValueType2>
void insert(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val);
/// version for double
void insertDouble(Key j, double c) { insert<double>(j,c); }
@ -258,6 +283,18 @@ namespace gtsam {
template <typename T>
void update(Key j, const T& val);
/** Partial specialization that allows passing a unary Eigen expression for val,
* similar to the partial specialization for insert.
*/
template <typename UnaryOp, typename ValueType>
void update(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val);
/** Partial specialization that allows passing a binary Eigen expression for val,
* similar to the partial specialization for insert.
*/
template <typename BinaryOp, typename ValueType1, typename ValueType2>
void update(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val);
/** update the current available values without adding new ones */
void update(const Values& values);
@ -266,7 +303,7 @@ namespace gtsam {
/**
* Update a set of variables.
* If any variable key doe not exist, then perform an insert.
* If any variable key does not exist, then perform an insert.
*/
void insert_or_assign(const Values& values);
@ -274,6 +311,18 @@ namespace gtsam {
template <typename ValueType>
void insert_or_assign(Key j, const ValueType& val);
/** Partial specialization that allows passing a unary Eigen expression for val,
* similar to the partial specialization for insert.
*/
template <typename UnaryOp, typename ValueType>
void insert_or_assign(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val);
/** Partial specialization that allows passing a binary Eigen expression for val,
* similar to the partial specialization for insert.
*/
template <typename BinaryOp, typename ValueType1, typename ValueType2>
void insert_or_assign(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val);
/** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
void erase(Key j);

15
gtsam/nonlinear/nonlinear.i
View File

@ -507,26 +507,22 @@ class ISAM2 {
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
const gtsam::Values& newTheta,
const gtsam::FactorIndices& removeFactorIndices,
gtsam::KeyGroupMap& constrainedKeys,
const gtsam::KeyGroupMap& constrainedKeys,
const gtsam::KeyList& noRelinKeys);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
const gtsam::Values& newTheta,
const gtsam::FactorIndices& removeFactorIndices,
gtsam::KeyGroupMap& constrainedKeys,
const gtsam::KeyList& noRelinKeys,
const gtsam::KeyList& extraReelimKeys);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
const gtsam::Values& newTheta,
const gtsam::FactorIndices& removeFactorIndices,
gtsam::KeyGroupMap& constrainedKeys,
const gtsam::KeyList& noRelinKeys,
const gtsam::KeyList& extraReelimKeys,
bool force_relinearize);
bool force_relinearize = false);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
const gtsam::Values& newTheta,
const gtsam::ISAM2UpdateParams& updateParams);
double error(const gtsam::VectorValues& values) const;
gtsam::Values getLinearizationPoint() const;
bool valueExists(gtsam::Key key) const;
gtsam::Values calculateEstimate() const;
@ -552,8 +548,7 @@ class ISAM2 {
string dot(const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void saveGraph(string s,
const gtsam::KeyFormatter& keyFormatter =
void saveGraph(string s, const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
};

74
gtsam/nonlinear/tests/testValues.cpp
View File

@ -134,6 +134,44 @@ TEST( Values, insert_good )
CHECK(assert_equal(expected, cfg1));
}
/* ************************************************************************* */
TEST( Values, insert_expression )
{
Point2 p1(0.1, 0.2);
Point2 p2(0.3, 0.4);
Point2 p3(0.5, 0.6);
Point2 p4(p1 + p2 + p3);
Point2 p5(-p1);
Point2 p6(2.0*p1);
Values cfg1, cfg2;
cfg1.insert(key1, p1 + p2 + p3);
cfg1.insert(key2, -p1);
cfg1.insert(key3, 2.0*p1);
cfg2.insert(key1, p4);
cfg2.insert(key2, p5);
cfg2.insert(key3, p6);
CHECK(assert_equal(cfg1, cfg2));
Point3 p7(0.1, 0.2, 0.3);
Point3 p8(0.4, 0.5, 0.6);
Point3 p9(0.7, 0.8, 0.9);
Point3 p10(p7 + p8 + p9);
Point3 p11(-p7);
Point3 p12(2.0*p7);
Values cfg3, cfg4;
cfg3.insert(key1, p7 + p8 + p9);
cfg3.insert(key2, -p7);
cfg3.insert(key3, 2.0*p7);
cfg4.insert(key1, p10);
cfg4.insert(key2, p11);
cfg4.insert(key3, p12);
CHECK(assert_equal(cfg3, cfg4));
}
/* ************************************************************************* */
TEST( Values, insert_bad )
{
@ -167,6 +205,23 @@ TEST( Values, update_element )
CHECK(assert_equal((Vector)v2, cfg.at<Vector3>(key1)));
}
/* ************************************************************************* */
TEST(Values, update_element_with_expression)
{
Values cfg;
Vector3 v1(5.0, 6.0, 7.0);
Vector3 v2(8.0, 9.0, 1.0);
cfg.insert(key1, v1);
CHECK(cfg.size() == 1);
CHECK(assert_equal((Vector)v1, cfg.at<Vector3>(key1)));
cfg.update(key1, 2.0*v1 + v2);
CHECK(cfg.size() == 1);
CHECK(assert_equal((2.0*v1 + v2).eval(), cfg.at<Vector3>(key1)));
}
/* ************************************************************************* */
TEST(Values, InsertOrAssign) {
Values values;
Key X(0);
@ -183,6 +238,25 @@ TEST(Values, InsertOrAssign) {
EXPECT(assert_equal(values.at<double>(X), y));
}
/* ************************************************************************* */
TEST(Values, InsertOrAssignWithExpression) {
Values values,expected;
Key X(0);
Vector3 x{1.0, 2.0, 3.0};
Vector3 y{4.0, 5.0, 6.0};
CHECK(values.size() == 0);
// This should perform an insert.
Vector3 z = x + y;
values.insert_or_assign(X, x + y);
EXPECT(assert_equal(values.at<Vector3>(X), z));
// This should perform an update.
z = 2.0*x - 3.0*y;
values.insert_or_assign(X, 2.0*x - 3.0*y);
EXPECT(assert_equal(values.at<Vector3>(X), z));
}
/* ************************************************************************* */
TEST(Values, basic_functions)
{

87
python/gtsam/tests/test_VisualISAMExample.py
View File

@ -6,24 +6,29 @@ All Rights Reserved
See LICENSE for the license information
visual_isam unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
Author: Frank Dellaert & Duy Nguyen Ta & Varun Agrawal (Python)
"""
# pylint: disable=maybe-no-member,invalid-name
import unittest
import gtsam.utils.visual_data_generator as generator
import gtsam.utils.visual_isam as visual_isam
from gtsam import symbol
from gtsam.utils.test_case import GtsamTestCase
import gtsam
from gtsam import symbol
class TestVisualISAMExample(GtsamTestCase):
"""Test class for ISAM2 with visual landmarks."""
def test_VisualISAMExample(self):
"""Test to see if ISAM works as expected for a simple visual SLAM example."""
def setUp(self):
# Data Options
options = generator.Options()
options.triangle = False
options.nrCameras = 20
self.options = options
# iSAM Options
isamOptions = visual_isam.Options()
@ -32,26 +37,82 @@ class TestVisualISAMExample(GtsamTestCase):
isamOptions.batchInitialization = True
isamOptions.reorderInterval = 10
isamOptions.alwaysRelinearize = False
self.isamOptions = isamOptions
# Generate data
data, truth = generator.generate_data(options)
self.data, self.truth = generator.generate_data(options)
def test_VisualISAMExample(self):
"""Test to see if ISAM works as expected for a simple visual SLAM example."""
# Initialize iSAM with the first pose and points
isam, result, nextPose = visual_isam.initialize(
data, truth, isamOptions)
self.data, self.truth, self.isamOptions)
# Main loop for iSAM: stepping through all poses
for currentPose in range(nextPose, options.nrCameras):
isam, result = visual_isam.step(data, isam, result, truth,
currentPose)
for currentPose in range(nextPose, self.options.nrCameras):
isam, result = visual_isam.step(self.data, isam, result,
self.truth, currentPose)
for i, _ in enumerate(truth.cameras):
for i, true_camera in enumerate(self.truth.cameras):
pose_i = result.atPose3(symbol('x', i))
self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
self.gtsamAssertEquals(pose_i, true_camera.pose(), 1e-5)
for j, _ in enumerate(truth.points):
for j, expected_point in enumerate(self.truth.points):
point_j = result.atPoint3(symbol('l', j))
self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
self.gtsamAssertEquals(point_j, expected_point, 1e-5)
def test_isam2_error(self):
"""Test for isam2 error() method."""
# Initialize iSAM with the first pose and points
isam, result, nextPose = visual_isam.initialize(
self.data, self.truth, self.isamOptions)
# Main loop for iSAM: stepping through all poses
for currentPose in range(nextPose, self.options.nrCameras):
isam, result = visual_isam.step(self.data, isam, result,
self.truth, currentPose)
values = gtsam.VectorValues()
estimate = isam.calculateBestEstimate()
for key in estimate.keys():
try:
v = gtsam.Pose3.Logmap(estimate.atPose3(key))
except RuntimeError:
v = estimate.atPoint3(key)
values.insert(key, v)
self.assertAlmostEqual(isam.error(values), 34212421.14731998)
def test_isam2_update(self):
"""
Test for full version of ISAM2::update method
"""
# Initialize iSAM with the first pose and points
isam, result, nextPose = visual_isam.initialize(
self.data, self.truth, self.isamOptions)
remove_factor_indices = []
constrained_keys = gtsam.KeyGroupMap()
no_relin_keys = gtsam.KeyList()
extra_reelim_keys = gtsam.KeyList()
isamArgs = (remove_factor_indices, constrained_keys, no_relin_keys,
extra_reelim_keys, False)
# Main loop for iSAM: stepping through all poses
for currentPose in range(nextPose, self.options.nrCameras):
isam, result = visual_isam.step(self.data, isam, result,
self.truth, currentPose, isamArgs)
for i in range(len(self.truth.cameras)):
pose_i = result.atPose3(symbol('x', i))
self.gtsamAssertEquals(pose_i, self.truth.cameras[i].pose(), 1e-5)
for j in range(len(self.truth.points)):
point_j = result.atPoint3(symbol('l', j))
self.gtsamAssertEquals(point_j, self.truth.points[j], 1e-5)
if __name__ == "__main__":

4
python/gtsam/utils/visual_isam.py
View File

@ -79,7 +79,7 @@ def initialize(data, truth, options):
return isam, result, nextPoseIndex
def step(data, isam, result, truth, currPoseIndex):
def step(data, isam, result, truth, currPoseIndex, isamArgs=()):
'''
Do one step isam update
@param[in] data: measurement data (odometry and visual measurements and their noiseModels)
@ -123,7 +123,7 @@ def step(data, isam, result, truth, currPoseIndex):
# Update ISAM
# figure(1)tic
isam.update(newFactors, initialEstimates)
isam.update(newFactors, initialEstimates, *isamArgs)
# t=toc plot(frame_i,t,'r.') tic
newResult = isam.calculateEstimate()
# t=toc plot(frame_i,t,'g.')