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

Merge branch 'hybrid/error' into hybrid/tests

release/4.3a0
Varun Agrawal 2022-11-04 15:20:45 -04:00
parent 96afdffae8 8fa7f44361
commit 5bfce89b65
418 changed files with 15438 additions and 9705 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -101,8 +101,6 @@ if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
# Copy matlab.h to the correct folder.
configure_file(${PROJECT_SOURCE_DIR}/wrap/matlab.h
${PROJECT_BINARY_DIR}/wrap/matlab.h COPYONLY)
# Add the include directories so that matlab.h can be found
include_directories("${PROJECT_BINARY_DIR}" "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}")
add_subdirectory(wrap)
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")

2
INSTALL.md
View File

@ -50,7 +50,7 @@ will run up to 10x faster in Release mode! See the end of this document for
additional debugging tips.
3. GTSAM has Doxygen documentation. To generate, run 'make doc' from your
build directory.
build directory after setting the `GTSAM_BUILD_DOCS` and `GTSAM_BUILD_[HTML|LATEX]` cmake flags.
4. The instructions below install the library to the default system install path and
build all components. From a terminal, starting in the root library folder,

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_SYSTEM_EIGEN@)
find_dependency(Eigen3 REQUIRED)
endif()
# Load exports
include(${OUR_CMAKE_DIR}/@PACKAGE_NAME@-exports.cmake)

81
cmake/FindEigen3.cmake
View File

@ -1,81 +0,0 @@
# - Try to find Eigen3 lib
#
# This module supports requiring a minimum version, e.g. you can do
# find_package(Eigen3 3.1.2)
# to require version 3.1.2 or newer of Eigen3.
#
# Once done this will define
#
# EIGEN3_FOUND - system has eigen lib with correct version
# EIGEN3_INCLUDE_DIR - the eigen include directory
# EIGEN3_VERSION - eigen version
# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
if(NOT Eigen3_FIND_VERSION)
if(NOT Eigen3_FIND_VERSION_MAJOR)
set(Eigen3_FIND_VERSION_MAJOR 2)
endif(NOT Eigen3_FIND_VERSION_MAJOR)
if(NOT Eigen3_FIND_VERSION_MINOR)
set(Eigen3_FIND_VERSION_MINOR 91)
endif(NOT Eigen3_FIND_VERSION_MINOR)
if(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION_PATCH 0)
endif(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
endif(NOT Eigen3_FIND_VERSION)
macro(_eigen3_check_version)
file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK FALSE)
else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK TRUE)
endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
if(NOT EIGEN3_VERSION_OK)
message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
"but at least version ${Eigen3_FIND_VERSION} is required")
endif(NOT EIGEN3_VERSION_OK)
endmacro(_eigen3_check_version)
if (EIGEN3_INCLUDE_DIR)
# in cache already
_eigen3_check_version()
set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
else (EIGEN3_INCLUDE_DIR)
find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
PATHS
${CMAKE_INSTALL_PREFIX}/include
${KDE4_INCLUDE_DIR}
PATH_SUFFIXES eigen3 eigen
)
if(EIGEN3_INCLUDE_DIR)
_eigen3_check_version()
endif(EIGEN3_INCLUDE_DIR)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
mark_as_advanced(EIGEN3_INCLUDE_DIR)
endif(EIGEN3_INCLUDE_DIR)

34
cmake/HandleEigen.cmake
View File

@ -1,10 +1,6 @@
###############################################################################
# Option for using system Eigen or GTSAM-bundled Eigen
# Default: Use system's Eigen if found automatically:
find_package(Eigen3 QUIET)
set(USE_SYSTEM_EIGEN_INITIAL_VALUE ${Eigen3_FOUND})
option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" ${USE_SYSTEM_EIGEN_INITIAL_VALUE})
unset(USE_SYSTEM_EIGEN_INITIAL_VALUE)
option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
if(NOT GTSAM_USE_SYSTEM_EIGEN)
# This option only makes sense if using the embedded copy of Eigen, it is
@ -14,10 +10,14 @@ endif()
# Switch for using system Eigen or GTSAM-bundled Eigen
if(GTSAM_USE_SYSTEM_EIGEN)
find_package(Eigen3 REQUIRED) # need to find again as REQUIRED
# Since Eigen 3.3.0 a Eigen3Config.cmake is available so use it.
find_package(Eigen3 CONFIG REQUIRED) # need to find again as REQUIRED
# Use generic Eigen include paths e.g. <Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
# The actual include directory (for BUILD cmake target interface):
# Note: EIGEN3_INCLUDE_DIR points to some random location on some eigen
# versions. So here I use the target itself to get the proper include
# directory (it is generated by cmake, thus has the correct path)
get_target_property(GTSAM_EIGEN_INCLUDE_FOR_BUILD Eigen3::Eigen INTERFACE_INCLUDE_DIRECTORIES)
# check if MKL is also enabled - can have one or the other, but not both!
# Note: Eigen >= v3.2.5 includes our patches
@ -30,9 +30,6 @@ if(GTSAM_USE_SYSTEM_EIGEN)
if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4))
message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.")
endif()
# The actual include directory (for BUILD cmake target interface):
set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}")
else()
# Use bundled Eigen include path.
# Clear any variables set by FindEigen3
@ -45,7 +42,20 @@ else()
set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/")
# The actual include directory (for BUILD cmake target interface):
set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${GTSAM_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${GTSAM_SOURCE_DIR}/gtsam/3rdparty/Eigen")
add_library(gtsam_eigen3 INTERFACE)
target_include_directories(gtsam_eigen3 INTERFACE
$<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
$<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
)
add_library(Eigen3::Eigen ALIAS gtsam_eigen3)
install(TARGETS gtsam_eigen3 EXPORT GTSAM-exports PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
list(APPEND GTSAM_EXPORTED_TARGETS gtsam_eigen3)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}")
endif()
# Detect Eigen version:

6
doc/CMakeLists.txt
View File

@ -26,6 +26,7 @@ if (GTSAM_BUILD_DOCS)
gtsam/basis
gtsam/discrete
gtsam/geometry
gtsam/hybrid
gtsam/inference
gtsam/linear
gtsam/navigation
@ -33,7 +34,6 @@ if (GTSAM_BUILD_DOCS)
gtsam/sam
gtsam/sfm
gtsam/slam
gtsam/smart
gtsam/symbolic
gtsam
)
@ -42,10 +42,12 @@ if (GTSAM_BUILD_DOCS)
set(gtsam_unstable_doc_subdirs
gtsam_unstable/base
gtsam_unstable/discrete
gtsam_unstable/dynamics
gtsam_unstable/geometry
gtsam_unstable/linear
gtsam_unstable/nonlinear
gtsam_unstable/partition
gtsam_unstable/slam
gtsam_unstable/dynamics
gtsam_unstable
)

2729
doc/Doxyfile.in
View File

File diff suppressed because it is too large Load Diff

1
doc/DoxygenLayout.xml
View File

@ -18,7 +18,6 @@
<tab type="files" visible="yes" title="" intro=""/>
<tab type="globals" visible="yes" title="" intro=""/>
</tab>
<tab type="dirs" visible="yes" title="" intro=""/>
<tab type="examples" visible="yes" title="" intro=""/>
</navindex>

8
gtsam/CMakeLists.txt
View File

@ -117,12 +117,10 @@ set_target_properties(gtsam PROPERTIES
VERSION ${gtsam_version}
SOVERSION ${gtsam_soversion})
# Append Eigen include path, set in top-level CMakeLists.txt to either
# Append Eigen include path to either
# system-eigen, or GTSAM eigen path
target_include_directories(gtsam PUBLIC
$<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
$<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
)
target_link_libraries(gtsam PUBLIC Eigen3::Eigen)
# MKL include dir:
if (GTSAM_USE_EIGEN_MKL)
target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})

2
gtsam/base/ConcurrentMap.h
View File

@ -62,7 +62,7 @@ namespace gtsam {
* convenience to avoid having lengthy types in the code. Through timing,
* we've seen that the fast_pool_allocator can lead to speedups of several
* percent.
* @addtogroup base
* @ingroup base
*/
template<typename KEY, typename VALUE>
class ConcurrentMap : public ConcurrentMapBase<KEY,VALUE> {

2
gtsam/base/DSFMap.h
View File

@ -28,7 +28,7 @@ namespace gtsam {
/**
* Disjoint set forest using an STL map data structure underneath
* Uses rank compression and union by rank, iterator version
* @addtogroup base
* @ingroup base
*/
template <class KEY>
class DSFMap {

4
gtsam/base/DSFVector.h
View File

@ -33,7 +33,7 @@ namespace gtsam {
* A fast implementation of disjoint set forests that uses vector as underly data structure.
* This is the absolute minimal DSF data structure, and only allows size_t keys
* Uses rank compression but not union by rank :-(
* @addtogroup base
* @ingroup base
*/
class GTSAM_EXPORT DSFBase {
@ -59,7 +59,7 @@ public:
/**
* DSFVector additionally keeps a vector of keys to support more expensive operations
* @addtogroup base
* @ingroup base
*/
class GTSAM_EXPORT DSFVector: public DSFBase {

2
gtsam/base/FastList.h
View File

@ -34,7 +34,7 @@ namespace gtsam {
* convenience to avoid having lengthy types in the code. Through timing,
* we've seen that the fast_pool_allocator can lead to speedups of several
* percent.
* @addtogroup base
* @ingroup base
*/
template<typename VALUE>
class FastList: public std::list<VALUE, typename internal::FastDefaultAllocator<VALUE>::type> {

2
gtsam/base/FastMap.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
* convenience to avoid having lengthy types in the code. Through timing,
* we've seen that the fast_pool_allocator can lead to speedups of several
* percent.
* @addtogroup base
* @ingroup base
*/
template<typename KEY, typename VALUE>
class FastMap : public std::map<KEY, VALUE, std::less<KEY>,

2
gtsam/base/FastSet.h
View File

@ -43,7 +43,7 @@ namespace gtsam {
* fast_pool_allocator instead of the default STL allocator. This is just a
* convenience to avoid having lengthy types in the code. Through timing,
* we've seen that the fast_pool_allocator can lead to speedups of several %.
* @addtogroup base
* @ingroup base
*/
template<typename VALUE>
class FastSet: public std::set<VALUE, std::less<VALUE>,

2
gtsam/base/FastVector.h
View File

@ -27,7 +27,7 @@ namespace gtsam {
/**
* FastVector is a type alias to a std::vector with a custom memory allocator.
* The particular allocator depends on GTSAM's cmake configuration.
* @addtogroup base
* @ingroup base
*/
template <typename T>
using FastVector =

2
gtsam/base/SymmetricBlockMatrix.h
View File

@ -47,7 +47,7 @@ namespace gtsam {
* matrix view. firstBlock() determines the block that appears to have index 0 for all operations
* (except re-setting firstBlock()).
*
* @addtogroup base */
* @ingroup base */
class GTSAM_EXPORT SymmetricBlockMatrix
{
public:

2
gtsam/base/Testable.h
View File

@ -51,7 +51,7 @@ namespace gtsam {
* tests and in generic algorithms.
*
* See macros for details on using this structure
* @addtogroup base
* @ingroup base
* @tparam T is the objectype this constrains to be testable - assumes print() and equals()
*/
template <class T>

2
gtsam/base/ThreadsafeException.h
View File

@ -14,7 +14,7 @@
* @brief Base exception type that uses tbb_allocator if GTSAM is compiled with TBB
* @author Richard Roberts
* @date Aug 21, 2010
* @addtogroup base
* @ingroup base
*/
#pragma once

2
gtsam/base/VerticalBlockMatrix.h
View File

@ -38,7 +38,7 @@ namespace gtsam {
* row for all operations. To include all rows, rowEnd() should be set to the number of rows in
* the matrix (i.e. one after the last true row index).
*
* @addtogroup base */
* @ingroup base */
class GTSAM_EXPORT VerticalBlockMatrix
{
public:

2
gtsam/base/make_shared.h
View File

@ -26,7 +26,7 @@
#include <type_traits>
namespace gtsam {
/// An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly
/// An shorthand alias for accessing the \::type inside std::enable_if that can be used in a template directly
template<bool B, class T = void>
using enable_if_t = typename std::enable_if<B, T>::type;
}

2
gtsam/base/timing.h
View File

@ -147,7 +147,7 @@ namespace gtsam {
size_t id_;
size_t t_;
size_t tWall_;
double t2_ ; ///< cache the \sum t_i^2
double t2_ ; ///< cache the \f$ \sum t_i^2 \f$
size_t tIt_;
size_t tMax_;
size_t tMin_;

2
gtsam/base/types.cpp
View File

@ -14,7 +14,7 @@
* @brief Functions for handling type information
* @author Varun Agrawal
* @date May 18, 2020
* @addtogroup base
* @ingroup base
*/
#include <gtsam/base/types.h>

2
gtsam/base/types.h
View File

@ -14,7 +14,7 @@
* @brief Typedefs for easier changing of types
* @author Richard Roberts
* @date Aug 21, 2010
* @addtogroup base
* @ingroup base
*/
#pragma once

7
gtsam/basis/Basis.h
View File

@ -78,6 +78,8 @@ using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
* @tparam M Size of the identity matrix.
* @param w The weights of the polynomial.
* @return Mx(M*N) kronecker product [w(0)*I, w(1)*I, ..., w(N-1)*I]
*
* @ingroup basis
*/
template <size_t M>
Matrix kroneckerProductIdentity(const Weights& w) {
@ -90,7 +92,10 @@ Matrix kroneckerProductIdentity(const Weights& w) {
return result;
}
/// CRTP Base class for function bases
/**
* CRTP Base class for function bases
* @ingroup basis
*/
template <typename DERIVED>
class Basis {
public:

6
gtsam/basis/BasisFactors.h
View File

@ -32,6 +32,8 @@ namespace gtsam {
*
* Example, degree 8 Chebyshev polynomial measured at x=0.5:
* EvaluationFactor<Chebyshev2> factor(key, measured, model, 8, 0.5);
*
* @ingroup basis
*/
template <class BASIS>
class EvaluationFactor : public FunctorizedFactor<double, Vector> {
@ -86,6 +88,8 @@ class EvaluationFactor : public FunctorizedFactor<double, Vector> {
*
* @param BASIS: The basis class to use e.g. Chebyshev2
* @param M: Size of the evaluated state vector.
*
* @ingroup basis
*/
template <class BASIS, int M>
class VectorEvaluationFactor
@ -149,6 +153,8 @@ class VectorEvaluationFactor
* VectorComponentFactor<BASIS, P> controlPrior(key, measured, model,
* N, i, t, a, b);
* where N is the degree and i is the component index.
*
* @ingroup basis
*/
template <class BASIS, size_t P>
class VectorComponentFactor

2
gtsam/basis/ParameterMatrix.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file ParamaterMatrix.h
* @file ParameterMatrix.h
* @brief Define ParameterMatrix class which is used to store values at
* interpolation points.
* @author Varun Agrawal, Frank Dellaert

4
gtsam/discrete/AlgebraicDecisionTree.h
View File

@ -31,6 +31,8 @@ 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?
*
* @ingroup discrete
*/
template <typename L>
class GTSAM_EXPORT AlgebraicDecisionTree : public DecisionTree<L, double> {
@ -156,7 +158,7 @@ namespace gtsam {
}
/// print method customized to value type `double`.
void print(const std::string& s,
void print(const std::string& s = "",
const typename Base::LabelFormatter& labelFormatter =
&DefaultFormatter) const {
auto valueFormatter = [](const double& v) {

1
gtsam/discrete/Assignment.h
View File

@ -31,6 +31,7 @@ namespace gtsam {
* An assignment from labels to value index (size_t).
* Assigns to each label a value. Implemented as a simple map.
* A discrete factor takes an Assignment and returns a value.
* @ingroup discrete
*/
template <class L>
class Assignment : public std::map<L, size_t> {

2
gtsam/discrete/DecisionTree.h
View File

@ -38,6 +38,8 @@ namespace gtsam {
* Decision Tree
* L = label for variables
* Y = function range (any algebra), e.g., bool, int, double
*
* @ingroup discrete
*/
template<typename L, typename Y>
class DecisionTree {

4
gtsam/discrete/DecisionTreeFactor.h
View File

@ -36,7 +36,9 @@ namespace gtsam {
class DiscreteConditional;
/**
* A discrete probabilistic factor
* A discrete probabilistic factor.
*
* @ingroup discrete
*/
class GTSAM_EXPORT DecisionTreeFactor : public DiscreteFactor,
public AlgebraicDecisionTree<Key> {

2
gtsam/discrete/DiscreteBayesNet.h
View File

@ -33,7 +33,7 @@ namespace gtsam {
/**
* A Bayes net made from discrete conditional distributions.
* @addtogroup discrete
* @ingroup discrete
*/
class GTSAM_EXPORT DiscreteBayesNet: public BayesNet<DiscreteConditional> {
public:

5
gtsam/discrete/DiscreteBayesTree.h
View File

@ -62,7 +62,10 @@ class GTSAM_EXPORT DiscreteBayesTreeClique
};
/* ************************************************************************* */
/** A Bayes tree representing a Discrete density */
/**
* @brief A Bayes tree representing a Discrete density.
* @ingroup discrete
*/
class GTSAM_EXPORT DiscreteBayesTree
: public BayesTree<DiscreteBayesTreeClique> {
private:

2
gtsam/discrete/DiscreteConditional.h
View File

@ -32,6 +32,8 @@ namespace gtsam {
/**
* Discrete Conditional Density
* Derives from DecisionTreeFactor
*
* @ingroup discrete
*/
class GTSAM_EXPORT DiscreteConditional
: public DecisionTreeFactor,

2
gtsam/discrete/DiscreteDistribution.h
View File

@ -27,6 +27,8 @@ namespace gtsam {
/**
* A prior probability on a set of discrete variables.
* Derives from DiscreteConditional
*
* @ingroup discrete
*/
class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional {
public:

4
gtsam/discrete/DiscreteEliminationTree.h
View File

@ -24,6 +24,10 @@
namespace gtsam {
/**
* @brief Elimination tree for discrete factors.
* @ingroup discrete
*/
class GTSAM_EXPORT DiscreteEliminationTree :
public EliminationTree<DiscreteBayesNet, DiscreteFactorGraph>
{

2
gtsam/discrete/DiscreteFactor.h
View File

@ -31,6 +31,8 @@ class DiscreteConditional;
/**
* Base class for discrete probabilistic factors
* The most general one is the derived DecisionTreeFactor
*
* @ingroup discrete
*/
class GTSAM_EXPORT DiscreteFactor: public Factor {

10
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -40,7 +40,14 @@ class DiscreteEliminationTree;
class DiscreteBayesTree;
class DiscreteJunctionTree;
/** Main elimination function for DiscreteFactorGraph */
/**
* @brief Main elimination function for DiscreteFactorGraph.
*
* @param factors
* @param keys
* @return GTSAM_EXPORT
* @ingroup discrete
*/
GTSAM_EXPORT std::pair<boost::shared_ptr<DiscreteConditional>, DecisionTreeFactor::shared_ptr>
EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& keys);
@ -64,6 +71,7 @@ template<> struct EliminationTraits<DiscreteFactorGraph>
/**
* A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
* Factor == DiscreteFactor
* @ingroup discrete
*/
class GTSAM_EXPORT DiscreteFactorGraph
: public FactorGraph<DiscreteFactor>,

3
gtsam/discrete/DiscreteJunctionTree.h
View File

@ -44,7 +44,8 @@ namespace gtsam {
* The tree structure and elimination method are exactly analogous to the EliminationTree,
* except that in the JunctionTree, at each node multiple variables are eliminated at a time.
*
* \addtogroup Multifrontal
* \ingroup Multifrontal
* @ingroup discrete
* \nosubgrouping
*/
class GTSAM_EXPORT DiscreteJunctionTree :

1
gtsam/discrete/DiscreteKey.h
View File

@ -31,6 +31,7 @@ namespace gtsam {
/**
* Key type for discrete variables.
* Includes Key and cardinality.
* @ingroup discrete
*/
using DiscreteKey = std::pair<Key,size_t>;

1
gtsam/discrete/DiscreteLookupDAG.h
View File

@ -32,6 +32,7 @@ class DiscreteBayesNet;
/**
* @brief DiscreteLookupTable table for max-product
* @ingroup discrete
*
* Inherits from discrete conditional for convenience, but is not normalized.
* Is used in the max-product algorithm.

1
gtsam/discrete/DiscreteMarginals.h
View File

@ -28,6 +28,7 @@ namespace gtsam {
/**
* A class for computing marginals of variables in a DiscreteFactorGraph
* @ingroup discrete
*/
class DiscreteMarginals {

1
gtsam/discrete/DiscreteValues.h
View File

@ -36,6 +36,7 @@ namespace gtsam {
* Another good thing is we don't need to have the special DiscreteKey which
* stores cardinality of a Discrete variable. It should be handled naturally in
* the new class DiscreteValue, as the variable's type (domain)
* @ingroup discrete
*/
class GTSAM_EXPORT DiscreteValues : public Assignment<Key> {
public:

2
gtsam/discrete/Signature.h
View File

@ -48,6 +48,8 @@ namespace gtsam {
* (E|T,L) = "F F F 1"
* X|E = "95/5 2/98"
* (D|E,B) = "9/1 2/8 3/7 1/9"
*
* @ingroup discrete
*/
class GTSAM_EXPORT Signature {

4
gtsam/geometry/Cal3.h
View File

@ -16,7 +16,7 @@
*/
/**
* @addtogroup geometry
* @ingroup geometry
*/
#pragma once
@ -63,7 +63,7 @@ void calibrateJacobians(const Cal& calibration, const Point2& pn,
/**
* @brief Common base class for all calibration models.
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3 {

2
gtsam/geometry/Cal3Bundler.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
/**
* @brief Calibration used by Bundler
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3Bundler : public Cal3 {

2
gtsam/geometry/Cal3DS2.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
* @brief Calibration of a camera with radial distortion that also supports
* Lie-group behaviors for optimization.
* \sa Cal3DS2_Base
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {

2
gtsam/geometry/Cal3DS2_Base.h
View File

@ -27,7 +27,7 @@ namespace gtsam {
/**
* @brief Calibration of a camera with radial distortion
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*
* Uses same distortionmodel as OpenCV, with

2
gtsam/geometry/Cal3Fisheye.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
/**
* @brief Calibration of a fisheye camera
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*
* Uses same distortionmodel as OpenCV, with

4
gtsam/geometry/Cal3Unified.h
View File

@ -18,7 +18,7 @@
*/
/**
* @addtogroup geometry
* @ingroup geometry
*/
#pragma once
@ -30,7 +30,7 @@ namespace gtsam {
/**
* @brief Calibration of a omni-directional camera with mirror + lens radial
* distortion
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*
* Similar to Cal3DS2, does distortion but has additional mirror parameter xi

4
gtsam/geometry/Cal3_S2.h
View File

@ -16,7 +16,7 @@
*/
/**
* @addtogroup geometry
* @ingroup geometry
*/
#pragma once
@ -28,7 +28,7 @@ namespace gtsam {
/**
* @brief The most common 5DOF 3D->2D calibration
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3_S2 : public Cal3 {

7
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -24,7 +24,7 @@ namespace gtsam {
/**
* @brief The most common 5DOF 3D->2D calibration, stereo version
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
@ -38,7 +38,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
/// @name Standard Constructors
/// @
/// @{
/// default calibration leaves coordinates unchanged
Cal3_S2Stereo() = default;
@ -55,6 +55,8 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
Cal3_S2Stereo(double fov, int w, int h, double b)
: Cal3_S2(fov, w, h), b_(b) {}
/// @}
/**
* Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
* @param p point in intrinsic coordinates
@ -82,7 +84,6 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
*/
Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); }
/// @}
/// @name Testable
/// @{

4
gtsam/geometry/CalibratedCamera.h
View File

@ -46,7 +46,7 @@ private:
/**
* A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT PinholeBase {
@ -241,7 +241,7 @@ private:
* A Calibrated camera class [R|-R't], calibration K=I.
* If calibration is known, it is more computationally efficient
* to calibrate the measurements rather than try to predict in pixels.
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT CalibratedCamera: public PinholeBase {

2
gtsam/geometry/Line3.h
View File

@ -38,7 +38,7 @@ GTSAM_EXPORT Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
/**
* A 3D line (R,a,b) : (Rot3,Scalar,Scalar)
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Line3 {

2
gtsam/geometry/OrientedPlane3.h
View File

@ -133,8 +133,6 @@ public:
inline double distance() const {
return d_;
}
/// @}
};
template<> struct traits<OrientedPlane3> : public internal::Manifold<

2
gtsam/geometry/PinholeCamera.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
/**
* A pinhole camera class that has a Pose3 and a Calibration.
* Use PinholePose if you will not be optimizing for Calibration
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
template<typename Calibration>

4
gtsam/geometry/PinholePose.h
View File

@ -27,7 +27,7 @@ namespace gtsam {
/**
* A pinhole camera class that has a Pose3 and a *fixed* Calibration.
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
template<typename CALIBRATION>
@ -236,7 +236,7 @@ public:
* A pinhole camera class that has a Pose3 and a *fixed* Calibration.
* Instead of using this class, one might consider calibrating the measurements
* and using CalibratedCamera, which would then be faster.
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
template<typename CALIBRATION>

2
gtsam/geometry/Pose2.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
/**
* A 2D pose (Point2,Rot2)
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
class Pose2: public LieGroup<Pose2, 3> {

1
gtsam/geometry/Pose3.cpp
View File

@ -27,7 +27,6 @@
namespace gtsam {
using std::vector;
using Point3Pairs = vector<Point3Pair>;
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3)

4
gtsam/geometry/Pose3.h
View File

@ -31,7 +31,7 @@ class Pose2;
/**
* A 3D pose (R,t) : (Rot3,Point3)
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
@ -83,7 +83,7 @@ public:
* A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point
* Note this allows for noise on the points but in that case the mapping will not be exact.
*/
static boost::optional<Pose3> Align(const std::vector<Point3Pair>& abPointPairs);
static boost::optional<Pose3> Align(const Point3Pairs& abPointPairs);
// Version of Pose3::Align that takes 2 matrices.
static boost::optional<Pose3> Align(const Matrix& a, const Matrix& b);

4
gtsam/geometry/Rot2.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
/**
* Rotation matrix
* NOTE: the angle theta is in radians unless explicitly stated
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Rot2 : public LieGroup<Rot2, 1> {
@ -86,7 +86,7 @@ namespace gtsam {
static Rot2 atan2(double y, double x);
/**
* Random, generates random angle \in [-p,pi]
* Random, generates random angle \f$\in\f$ [-pi,pi]
* Example:
* std::mt19937 engine(42);
* Unit3 unit = Unit3::Random(engine);

4
gtsam/geometry/Rot3.h
View File

@ -53,7 +53,7 @@ namespace gtsam {
* @brief Rot3 is a 3D rotation represented as a rotation matrix if the
* preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion
* if it is defined.
* @addtogroup geometry
* @ingroup geometry
*/
class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
private:
@ -129,7 +129,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
Rot3(double w, double x, double y, double z) : Rot3(Quaternion(w, x, y, z)) {}
/**
* Random, generates a random axis, then random angle \in [-p,pi]
* Random, generates a random axis, then random angle \f$\in\f$ [-pi,pi]
* Example:
* std::mt19937 engine(42);
* Unit3 unit = Unit3::Random(engine);

2
gtsam/geometry/SO4.h
View File

@ -74,7 +74,7 @@ GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::non
/**
* Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q)
* -> S \in St(3,4).
* -> \f$ S \in St(3,4) \f$.
*/
GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none);

3
gtsam/geometry/SOn.h
View File

@ -121,7 +121,8 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
/// currently only defined for SO3.
static SO ClosestTo(const MatrixNN& M);
/// Named constructor that finds chordal mean = argmin_R \sum sqr(|R-R_i|_F),
/// Named constructor that finds chordal mean
/// \f$ mu = argmin_R \sum sqr(|R-R_i|_F) \f$,
/// currently only defined for SO3.
static SO ChordalMean(const std::vector<SO>& rotations);

2
gtsam/geometry/Similarity2.h
View File

@ -132,7 +132,7 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
* using the algorithm described here:
* http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
*/
static Similarity2 Align(const std::vector<Pose2Pair>& abPosePairs);
static Similarity2 Align(const Pose2Pairs& abPosePairs);
/// @}
/// @name Lie Group

2
gtsam/geometry/Similarity3.h
View File

@ -120,7 +120,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
/**
* Create Similarity3 by aligning at least three point pairs
*/
static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
static Similarity3 Align(const Point3Pairs& abPointPairs);
/**
* Create the Similarity3 object that aligns at least two pose pairs.

5
gtsam/geometry/SphericalCamera.h
View File

@ -34,7 +34,7 @@ namespace gtsam {
* Empty calibration. Only needed to play well with other cameras
* (e.g., when templating functions wrt cameras), since other cameras
* have constuctors in the form ‘camera(pose,calibration)’
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT EmptyCal {
@ -64,7 +64,7 @@ class GTSAM_EXPORT EmptyCal {
/**
* A spherical camera class that has a Pose3 and measures bearing vectors.
* The camera has an ‘Empty’ calibration and the only 6 dof are the pose
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT SphericalCamera {
@ -82,7 +82,6 @@ class GTSAM_EXPORT SphericalCamera {
EmptyCal::shared_ptr emptyCal_;
public:
/// @}
/// @name Standard Constructors
/// @{

2
gtsam/geometry/StereoCamera.h
View File

@ -42,7 +42,7 @@ private:
/**
* A stereo camera class, parameterize by left camera pose and stereo calibration
* @addtogroup geometry
* @ingroup geometry
*/
class GTSAM_EXPORT StereoCamera {

2
gtsam/geometry/StereoPoint2.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
/**
* A 2D stereo point, v will be same for rectified images
* @addtogroup geometry
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT StereoPoint2 {

4
gtsam/geometry/geometry.i
View File

@ -1123,7 +1123,7 @@ class StereoCamera {
#include <gtsam/geometry/triangulation.h>
class TriangulationResult {
enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
Status status;
gtsam::TriangulationResult::Status status;
TriangulationResult(const gtsam::Point3& p);
const gtsam::Point3& get() const;
static gtsam::TriangulationResult Degenerate();
@ -1142,7 +1142,7 @@ class TriangulationParameters {
bool enableEPI;
double landmarkDistanceThreshold;
double dynamicOutlierRejectionThreshold;
SharedNoiseModel noiseModel;
gtsam::SharedNoiseModel noiseModel;
TriangulationParameters(const double rankTolerance = 1.0,
const bool enableEPI = false,
double landmarkDistanceThreshold = -1,

2
gtsam/global_includes.h
View File

@ -13,7 +13,7 @@
* @file global_includes.h
* @brief Included from all GTSAM files
* @author Richard Roberts
* @addtogroup base
* @ingroup base
*/
#pragma once

43
gtsam/groups.dox
View File

@ -1,7 +1,7 @@
/**
\defgroup LinearSolving Solving of sparse linear systems with least-squares
@{
@{ @}
\defgroup Multifrontal Solving by multifrontal variable elimination (QR and Cholesky)
@{ @}
@ -9,9 +9,48 @@
\defgroup Sequential Solving by sequential variable elimination (QR and Cholesky)
@{ @}
\defgroup base Base
@{ @}
\defgroup basis Basis
@{ @}
\defgroup discrete Discrete
@{ @}
\defgroup geometry Geometry
@{
Geometric primitives.
@}
\defgroup SLAM Useful SLAM components
\defgroup hybrid Hybrid
@{ @}
\defgroup inference Inference
@{ @}
\defgroup linear Linear
@{ @}
\defgroup navigation Navigation
@{ @}
\defgroup nonlinear Nonlinear
@{ @}
\defgroup sam SAM
@{ @}
\defgroup sfm SFM
@{ @}
\defgroup slam SLAM
@{ Useful SLAM components @}
\defgroup symbolic Symbolic
@{ @}
\defgroup isam2 ISAM2
@{ @}
*/

24
gtsam/hybrid/GaussianMixture.cpp
View File

@ -207,4 +207,28 @@ void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) {
conditionals_.root_ = pruned_conditionals.root_;
}
/* *******************************************************************************/
AlgebraicDecisionTree<Key> GaussianMixture::error(
const VectorValues &continuousVals) const {
// functor to convert from GaussianConditional to double error value.
auto errorFunc =
[continuousVals](const GaussianConditional::shared_ptr &conditional) {
if (conditional) {
return conditional->error(continuousVals);
} else {
// return arbitrarily large error
return 1e50;
}
};
DecisionTree<Key, double> errorTree(conditionals_, errorFunc);
return errorTree;
}
/* *******************************************************************************/
double GaussianMixture::error(const VectorValues &continuousVals,
const DiscreteValues &discreteValues) const {
auto conditional = conditionals_(discreteValues);
return conditional->error(continuousVals);
}
} // namespace gtsam

21
gtsam/hybrid/GaussianMixture.h
View File

@ -45,6 +45,7 @@ namespace gtsam {
* where i indexes the components and k_i is a component-wise normalization
* constant.
*
* @ingroup hybrid
*/
class GTSAM_EXPORT GaussianMixture
: public HybridFactor,
@ -143,6 +144,26 @@ class GTSAM_EXPORT GaussianMixture
/// Getter for the underlying Conditionals DecisionTree
const Conditionals &conditionals();
/**
* @brief Compute error of the GaussianMixture as a tree.
*
* @param continuousVals The continuous VectorValues.
* @return AlgebraicDecisionTree<Key> A decision tree with corresponding keys
* as the factor but leaf values as the error.
*/
AlgebraicDecisionTree<Key> error(const VectorValues &continuousVals) const;
/**
* @brief Compute the error of this Gaussian Mixture given the continuous
* values and a discrete assignment.
*
* @param continuousVals The continuous values at which to compute the error.
* @param discreteValues The discrete assignment for a specific mode sequence.
* @return double
*/
double error(const VectorValues &continuousVals,
const DiscreteValues &discreteValues) const;
/**
* @brief Prune the decision tree of Gaussian factors as per the discrete
* `decisionTree`.

20
gtsam/hybrid/GaussianMixtureFactor.cpp
View File

@ -95,4 +95,24 @@ GaussianMixtureFactor::Sum GaussianMixtureFactor::asGaussianFactorGraphTree()
};
return {factors_, wrap};
}
/* *******************************************************************************/
AlgebraicDecisionTree<Key> GaussianMixtureFactor::error(
const VectorValues &continuousVals) const {
// functor to convert from sharedFactor to double error value.
auto errorFunc = [continuousVals](const GaussianFactor::shared_ptr &factor) {
return factor->error(continuousVals);
};
DecisionTree<Key, double> errorTree(factors_, errorFunc);
return errorTree;
}
/* *******************************************************************************/
double GaussianMixtureFactor::error(
const VectorValues &continuousVals,
const DiscreteValues &discreteValues) const {
auto factor = factors_(discreteValues);
return factor->error(continuousVals);
}
} // namespace gtsam

25
gtsam/hybrid/GaussianMixtureFactor.h
View File

@ -20,15 +20,19 @@
#pragma once
#include <gtsam/discrete/AlgebraicDecisionTree.h>
#include <gtsam/discrete/DecisionTree.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam {
class GaussianFactorGraph;
// Needed for wrapper.
using GaussianFactorVector = std::vector<gtsam::GaussianFactor::shared_ptr>;
/**
@ -40,6 +44,7 @@ using GaussianFactorVector = std::vector<gtsam::GaussianFactor::shared_ptr>;
* Represents the underlying Gaussian Mixture as a Decision Tree, where the set
* of discrete variables indexes to the continuous gaussian distribution.
*
* @ingroup hybrid
*/
class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
public:
@ -125,6 +130,26 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
*/
Sum add(const Sum &sum) const;
/**
* @brief Compute error of the GaussianMixtureFactor as a tree.
*
* @param continuousVals The continuous VectorValues.
* @return AlgebraicDecisionTree<Key> A decision tree with corresponding keys
* as the factor but leaf values as the error.
*/
AlgebraicDecisionTree<Key> error(const VectorValues &continuousVals) const;
/**
* @brief Compute the error of this Gaussian Mixture given the continuous
* values and a discrete assignment.
*
* @param continuousVals The continuous values at which to compute the error.
* @param discreteValues The discrete assignment for a specific mode sequence.
* @return double
*/
double error(const VectorValues &continuousVals,
const DiscreteValues &discreteValues) const;
/// Add MixtureFactor to a Sum, syntactic sugar.
friend Sum &operator+=(Sum &sum, const GaussianMixtureFactor &factor) {
sum = factor.add(sum);

41
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -232,4 +232,45 @@ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const {
return gbn.optimize();
}
/* ************************************************************************* */
double HybridBayesNet::error(const VectorValues &continuousValues,
const DiscreteValues &discreteValues) const {
GaussianBayesNet gbn = this->choose(discreteValues);
return gbn.error(continuousValues);
}
/* ************************************************************************* */
AlgebraicDecisionTree<Key> HybridBayesNet::error(
const VectorValues &continuousValues) const {
AlgebraicDecisionTree<Key> error_tree;
for (size_t idx = 0; idx < size(); idx++) {
AlgebraicDecisionTree<Key> conditional_error;
if (factors_.at(idx)->isHybrid()) {
// If factor is hybrid, select based on assignment.
GaussianMixture::shared_ptr gm = this->atMixture(idx);
conditional_error = gm->error(continuousValues);
if (idx == 0) {
error_tree = conditional_error;
} else {
error_tree = error_tree + conditional_error;
}
} else if (factors_.at(idx)->isContinuous()) {
// If continuous only, get the (double) error
// and add it to the error_tree
double error = this->atGaussian(idx)->error(continuousValues);
error_tree = error_tree.apply(
[error](double leaf_value) { return leaf_value + error; });
} else if (factors_.at(idx)->isDiscrete()) {
// If factor at `idx` is discrete-only, we skip.
continue;
}
}
return error_tree;
}
} // namespace gtsam

22
gtsam/hybrid/HybridBayesNet.h
View File

@ -29,6 +29,8 @@ namespace gtsam {
/**
* A hybrid Bayes net is a collection of HybridConditionals, which can have
* discrete conditionals, Gaussian mixtures, or pure Gaussian conditionals.
*
* @ingroup hybrid
*/
class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
public:
@ -122,6 +124,26 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
/// Prune the Hybrid Bayes Net such that we have at most maxNrLeaves leaves.
HybridBayesNet prune(size_t maxNrLeaves);
/**
* @brief 0.5 * sum of squared Mahalanobis distances
* for a specific discrete assignment.
*
* @param continuousValues Continuous values at which to compute the error.
* @param discreteValues Discrete assignment for a specific mode sequence.
* @return double
*/
double error(const VectorValues &continuousValues,
const DiscreteValues &discreteValues) const;
/**
* @brief Compute conditional error for each discrete assignment,
* and return as a tree.
*
* @param continuousValues Continuous values at which to compute the error.
* @return AlgebraicDecisionTree<Key>
*/
AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
/// @}
private:

5
gtsam/hybrid/HybridBayesTree.h
View File

@ -34,8 +34,11 @@ class HybridConditional;
class VectorValues;
/* ************************************************************************* */
/** A clique in a HybridBayesTree
/**
* @brief A clique in a HybridBayesTree
* which is a HybridConditional internally.
*
* @ingroup hybrid
*/
class GTSAM_EXPORT HybridBayesTreeClique
: public BayesTreeCliqueBase<HybridBayesTreeClique,

2
gtsam/hybrid/HybridConditional.h
View File

@ -54,6 +54,8 @@ namespace gtsam {
*
* A great reference to the type-erasure pattern is Eduaado Madrid's CppCon
* talk (https://www.youtube.com/watch?v=s082Qmd_nHs).
*
* @ingroup hybrid
*/
class GTSAM_EXPORT HybridConditional
: public HybridFactor,

2
gtsam/hybrid/HybridDiscreteFactor.h
View File

@ -28,6 +28,8 @@ namespace gtsam {
* A HybridDiscreteFactor is a thin container for DiscreteFactor, which allows
* us to hide the implementation of DiscreteFactor and thus avoid diamond
* inheritance.
*
* @ingroup hybrid
*/
class GTSAM_EXPORT HybridDiscreteFactor : public HybridFactor {
private:

2
gtsam/hybrid/HybridEliminationTree.h
View File

@ -25,6 +25,8 @@ namespace gtsam {
/**
* Elimination Tree type for Hybrid
*
* @ingroup hybrid
*/
class GTSAM_EXPORT HybridEliminationTree
: public EliminationTree<HybridBayesNet, HybridGaussianFactorGraph> {

2
gtsam/hybrid/HybridFactor.h
View File

@ -40,6 +40,8 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
* - HybridDiscreteFactor
* - GaussianMixtureFactor
* - GaussianMixture
*
* @ingroup hybrid
*/
class GTSAM_EXPORT HybridFactor : public Factor {
private:

2
gtsam/hybrid/HybridGaussianFactor.h
View File

@ -27,6 +27,8 @@ namespace gtsam {
* A HybridGaussianFactor is a layer over GaussianFactor so that we do not have
* a diamond inheritance i.e. an extra factor type that inherits from both
* HybridFactor and GaussianFactor.
*
* @ingroup hybrid
*/
class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
private:

17
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -39,7 +39,14 @@ class DecisionTreeFactor;
class JacobianFactor;
/** Main elimination function for HybridGaussianFactorGraph */
/**
* @brief Main elimination function for HybridGaussianFactorGraph.
*
* @param factors The factor graph to eliminate.
* @param keys The elimination ordering.
* @return The conditional on the ordering keys and the remaining factors.
* @ingroup hybrid
*/
GTSAM_EXPORT
std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys);
@ -68,10 +75,12 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
};
/**
* Gaussian Hybrid Factor Graph
* Hybrid Gaussian Factor Graph
* -----------------------
* This is the linearized version of a hybrid factor graph.
* Everything inside needs to be hybrid factor or hybrid conditional.
*
* @ingroup hybrid
*/
class GTSAM_EXPORT HybridGaussianFactorGraph
: public HybridFactorGraph,
@ -120,13 +129,13 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
void add(JacobianFactor&& factor);
/// Add a Jacobian factor as a shared ptr.
void add(boost::shared_ptr<JacobianFactor> factor);
void add(JacobianFactor::shared_ptr factor);
/// Add a DecisionTreeFactor to the factor graph.
void add(DecisionTreeFactor&& factor);
/// Add a DecisionTreeFactor as a shared ptr.
void add(boost::shared_ptr<DecisionTreeFactor> factor);
void add(DecisionTreeFactor::shared_ptr factor);
/**
* Add a gaussian factor *pointer* to the internal gaussian factor graph

53
gtsam/hybrid/HybridGaussianISAM.cpp
View File

@ -38,6 +38,35 @@ HybridGaussianISAM::HybridGaussianISAM() {}
HybridGaussianISAM::HybridGaussianISAM(const HybridBayesTree& bayesTree)
: Base(bayesTree) {}
/* ************************************************************************* */
Ordering HybridGaussianISAM::GetOrdering(
HybridGaussianFactorGraph& factors,
const HybridGaussianFactorGraph& newFactors) {
// Get all the discrete keys from the factors
KeySet allDiscrete = factors.discreteKeys();
// Create KeyVector with continuous keys followed by discrete keys.
KeyVector newKeysDiscreteLast;
const KeySet newFactorKeys = newFactors.keys();
// Insert continuous keys first.
for (auto& k : newFactorKeys) {
if (!allDiscrete.exists(k)) {
newKeysDiscreteLast.push_back(k);
}
}
// Insert discrete keys at the end
std::copy(allDiscrete.begin(), allDiscrete.end(),
std::back_inserter(newKeysDiscreteLast));
const VariableIndex index(factors);
// Get an ordering where the new keys are eliminated last
Ordering ordering = Ordering::ColamdConstrainedLast(
index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()),
true);
return ordering;
}
/* ************************************************************************* */
void HybridGaussianISAM::updateInternal(
const HybridGaussianFactorGraph& newFactors,
@ -54,7 +83,7 @@ void HybridGaussianISAM::updateInternal(
}
// Add the removed top and the new factors
FactorGraphType factors;
HybridGaussianFactorGraph factors;
factors += bn;
factors += newFactors;
@ -63,32 +92,12 @@ void HybridGaussianISAM::updateInternal(
factors += boost::make_shared<BayesTreeOrphanWrapper<Node>>(orphan);
}
// Get all the discrete keys from the factors
KeySet allDiscrete = factors.discreteKeys();
// Create KeyVector with continuous keys followed by discrete keys.
KeyVector newKeysDiscreteLast;
// Insert continuous keys first.
for (auto& k : newFactorKeys) {
if (!allDiscrete.exists(k)) {
newKeysDiscreteLast.push_back(k);
}
}
// Insert discrete keys at the end
std::copy(allDiscrete.begin(), allDiscrete.end(),
std::back_inserter(newKeysDiscreteLast));
// Get an ordering where the new keys are eliminated last
const VariableIndex index(factors);
Ordering elimination_ordering;
if (ordering) {
elimination_ordering = *ordering;
} else {
elimination_ordering = Ordering::ColamdConstrainedLast(
index,
KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()),
true);
elimination_ordering = GetOrdering(factors, newFactors);
}
// eliminate all factors (top, added, orphans) into a new Bayes tree

16
gtsam/hybrid/HybridGaussianISAM.h
View File

@ -26,6 +26,11 @@
namespace gtsam {
/**
* @brief
*
* @ingroup hybrid
*/
class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
public:
typedef ISAM<HybridBayesTree> Base;
@ -67,6 +72,17 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
const boost::optional<Ordering>& ordering = boost::none,
const HybridBayesTree::Eliminate& function =
HybridBayesTree::EliminationTraitsType::DefaultEliminate);
/**
* @brief Helper method to get an ordering given the existing factors and any
* new factors added.
*
* @param factors The existing factors in the BayesTree.
* @param newFactors New factors added during the update step.
* @return Ordering
*/
static Ordering GetOrdering(HybridGaussianFactorGraph& factors,
const HybridGaussianFactorGraph& newFactors);
};
/// traits

3
gtsam/hybrid/HybridJunctionTree.h
View File

@ -45,7 +45,8 @@ class HybridEliminationTree;
* EliminationTree, except that in the JunctionTree, at each node multiple
* variables are eliminated at a time.
*
* \addtogroup Multifrontal
* \ingroup Multifrontal
* \ingroup hybrid
* \nosubgrouping
*/
class GTSAM_EXPORT HybridJunctionTree

26
gtsam/hybrid/MixtureFactor.h
View File

@ -107,8 +107,12 @@ class MixtureFactor : public HybridFactor {
std::copy(f->keys().begin(), f->keys().end(),
std::inserter(factor_keys_set, factor_keys_set.end()));
nonlinear_factors.push_back(
boost::dynamic_pointer_cast<NonlinearFactor>(f));
if (auto nf = boost::dynamic_pointer_cast<NonlinearFactor>(f)) {
nonlinear_factors.push_back(nf);
} else {
throw std::runtime_error(
"Factors passed into MixtureFactor need to be nonlinear!");
}
}
factors_ = Factors(discreteKeys, nonlinear_factors);
@ -121,6 +125,22 @@ class MixtureFactor : public HybridFactor {
~MixtureFactor() = default;
/**
* @brief Compute error of the MixtureFactor as a tree.
*
* @param continuousVals The continuous values for which to compute the error.
* @return AlgebraicDecisionTree<Key> A decision tree with corresponding keys
* as the factor but leaf values as the error.
*/
AlgebraicDecisionTree<Key> error(const Values& continuousVals) const {
// functor to convert from sharedFactor to double error value.
auto errorFunc = [continuousVals](const sharedFactor& factor) {
return factor->error(continuousVals);
};
DecisionTree<Key, double> errorTree(factors_, errorFunc);
return errorTree;
}
/**
* @brief Compute error of factor given both continuous and discrete values.
*
@ -149,7 +169,7 @@ class MixtureFactor : public HybridFactor {
/// print to stdout
void print(
const std::string& s = "MixtureFactor",
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << (s.empty() ? "" : s + " ");
Base::print("", keyFormatter);

40
gtsam/hybrid/tests/testGaussianMixture.cpp
View File

@ -78,15 +78,51 @@ TEST(GaussianMixture, Equals) {
GaussianMixture::Conditionals conditionals(
{m1},
vector<GaussianConditional::shared_ptr>{conditional0, conditional1});
GaussianMixture mixtureFactor({X(1)}, {X(2)}, {m1}, conditionals);
GaussianMixture mixture({X(1)}, {X(2)}, {m1}, conditionals);
// Let's check that this worked:
DiscreteValues mode;
mode[m1.first] = 1;
auto actual = mixtureFactor(mode);
auto actual = mixture(mode);
EXPECT(actual == conditional1);
}
/* ************************************************************************* */
/// Test error method of GaussianMixture.
TEST(GaussianMixture, Error) {
Matrix22 S1 = Matrix22::Identity();
Matrix22 S2 = Matrix22::Identity() * 2;
Matrix22 R1 = Matrix22::Ones();
Matrix22 R2 = Matrix22::Ones();
Vector2 d1(1, 2), d2(2, 1);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34));
auto conditional0 = boost::make_shared<GaussianConditional>(X(1), d1, R1,
X(2), S1, model),
conditional1 = boost::make_shared<GaussianConditional>(X(1), d2, R2,
X(2), S2, model);
// Create decision tree
DiscreteKey m1(1, 2);
GaussianMixture::Conditionals conditionals(
{m1},
vector<GaussianConditional::shared_ptr>{conditional0, conditional1});
GaussianMixture mixture({X(1)}, {X(2)}, {m1}, conditionals);
VectorValues values;
values.insert(X(1), Vector2::Ones());
values.insert(X(2), Vector2::Zero());
auto error_tree = mixture.error(values);
std::vector<DiscreteKey> discrete_keys = {m1};
std::vector<double> leaves = {0.5, 4.3252595};
AlgebraicDecisionTree<Key> expected_error(discrete_keys, leaves);
// regression
EXPECT(assert_equal(expected_error, error_tree, 1e-6));
}
/* ************************************************************************* */
int main() {
TestResult tr;

43
gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file GaussianMixtureFactor.cpp
* @file testGaussianMixtureFactor.cpp
* @brief Unit tests for GaussianMixtureFactor
* @author Varun Agrawal
* @author Fan Jiang
@ -135,7 +135,7 @@ TEST(GaussianMixtureFactor, Printing) {
EXPECT(assert_print_equal(expected, mixtureFactor));
}
TEST_UNSAFE(GaussianMixtureFactor, GaussianMixture) {
TEST(GaussianMixtureFactor, GaussianMixture) {
KeyVector keys;
keys.push_back(X(0));
keys.push_back(X(1));
@ -151,6 +151,45 @@ TEST_UNSAFE(GaussianMixtureFactor, GaussianMixture) {
EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size());
}
/* ************************************************************************* */
// Test the error of the GaussianMixtureFactor
TEST(GaussianMixtureFactor, Error) {
DiscreteKey m1(1, 2);
auto A01 = Matrix2::Identity();
auto A02 = Matrix2::Identity();
auto A11 = Matrix2::Identity();
auto A12 = Matrix2::Identity() * 2;
auto b = Vector2::Zero();
auto f0 = boost::make_shared<JacobianFactor>(X(1), A01, X(2), A02, b);
auto f1 = boost::make_shared<JacobianFactor>(X(1), A11, X(2), A12, b);
std::vector<GaussianFactor::shared_ptr> factors{f0, f1};
GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
VectorValues continuousVals;
continuousVals.insert(X(1), Vector2(0, 0));
continuousVals.insert(X(2), Vector2(1, 1));
// error should return a tree of errors, with nodes for each discrete value.
AlgebraicDecisionTree<Key> error_tree = mixtureFactor.error(continuousVals);
std::vector<DiscreteKey> discrete_keys = {m1};
std::vector<double> errors = {1, 4};
AlgebraicDecisionTree<Key> expected_error(discrete_keys, errors);
EXPECT(assert_equal(expected_error, error_tree));
// Test for single leaf given discrete assignment P(X|M,Z).
DiscreteValues discreteVals;
discreteVals[m1.first] = 1;
EXPECT_DOUBLES_EQUAL(4.0, mixtureFactor.error(continuousVals, discreteVals),
1e-9);
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -183,6 +183,61 @@ TEST(HybridBayesNet, OptimizeMultifrontal) {
EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5));
}
/* ****************************************************************************/
// Test bayes net error
TEST(HybridBayesNet, Error) {
Switching s(3);
Ordering hybridOrdering = s.linearizedFactorGraph.getHybridOrdering();
HybridBayesNet::shared_ptr hybridBayesNet =
s.linearizedFactorGraph.eliminateSequential(hybridOrdering);
HybridValues delta = hybridBayesNet->optimize();
auto error_tree = hybridBayesNet->error(delta.continuous());
std::vector<DiscreteKey> discrete_keys = {{M(0), 2}, {M(1), 2}};
std::vector<double> leaves = {0.0097568009, 3.3973404e-31, 0.029126214,
0.0097568009};
AlgebraicDecisionTree<Key> expected_error(discrete_keys, leaves);
// regression
EXPECT(assert_equal(expected_error, error_tree, 1e-9));
// Error on pruned bayes net
auto prunedBayesNet = hybridBayesNet->prune(2);
auto pruned_error_tree = prunedBayesNet.error(delta.continuous());
std::vector<double> pruned_leaves = {2e50, 3.3973404e-31, 2e50, 0.0097568009};
AlgebraicDecisionTree<Key> expected_pruned_error(discrete_keys,
pruned_leaves);
// regression
EXPECT(assert_equal(expected_pruned_error, pruned_error_tree, 1e-9));
// Verify error computation and check for specific error value
DiscreteValues discrete_values;
discrete_values[M(0)] = 1;
discrete_values[M(1)] = 1;
double total_error = 0;
for (size_t idx = 0; idx < hybridBayesNet->size(); idx++) {
if (hybridBayesNet->at(idx)->isHybrid()) {
double error = hybridBayesNet->atMixture(idx)->error(delta.continuous(),
discrete_values);
total_error += error;
} else if (hybridBayesNet->at(idx)->isContinuous()) {
double error = hybridBayesNet->atGaussian(idx)->error(delta.continuous());
total_error += error;
}
}
EXPECT_DOUBLES_EQUAL(
total_error, hybridBayesNet->error(delta.continuous(), discrete_values),
1e-9);
EXPECT_DOUBLES_EQUAL(total_error, error_tree(discrete_values), 1e-9);
EXPECT_DOUBLES_EQUAL(total_error, pruned_error_tree(discrete_values), 1e-9);
}
/* ****************************************************************************/
// Test bayes net pruning
TEST(HybridBayesNet, Prune) {

108
gtsam/hybrid/tests/testMixtureFactor.cpp Normal file
View File

@ -0,0 +1,108 @@
/* ----------------------------------------------------------------------------
* 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 testMixtureFactor.cpp
* @brief Unit tests for MixtureFactor
* @author Varun Agrawal
* @date October 2022
*/
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/MixtureFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>
// Include for test suite
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using noiseModel::Isotropic;
using symbol_shorthand::M;
using symbol_shorthand::X;
/* ************************************************************************* */
// Check iterators of empty mixture.
TEST(MixtureFactor, Constructor) {
MixtureFactor factor;
MixtureFactor::const_iterator const_it = factor.begin();
CHECK(const_it == factor.end());
MixtureFactor::iterator it = factor.begin();
CHECK(it == factor.end());
}
TEST(MixtureFactor, Printing) {
DiscreteKey m1(1, 2);
double between0 = 0.0;
double between1 = 1.0;
Vector1 sigmas = Vector1(1.0);
auto model = noiseModel::Diagonal::Sigmas(sigmas, false);
auto f0 =
boost::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
auto f1 =
boost::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
std::string expected =
R"(Hybrid [x1 x2; 1]
MixtureFactor
Choice(1)
0 Leaf Nonlinear factor on 2 keys
1 Leaf Nonlinear factor on 2 keys
)";
EXPECT(assert_print_equal(expected, mixtureFactor));
}
/* ************************************************************************* */
// Test the error of the MixtureFactor
TEST(MixtureFactor, Error) {
DiscreteKey m1(1, 2);
double between0 = 0.0;
double between1 = 1.0;
Vector1 sigmas = Vector1(1.0);
auto model = noiseModel::Diagonal::Sigmas(sigmas, false);
auto f0 =
boost::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
auto f1 =
boost::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
Values continuousVals;
continuousVals.insert<double>(X(1), 0);
continuousVals.insert<double>(X(2), 1);
AlgebraicDecisionTree<Key> error_tree = mixtureFactor.error(continuousVals);
std::vector<DiscreteKey> discrete_keys = {m1};
std::vector<double> errors = {0.5, 0};
AlgebraicDecisionTree<Key> expected_error(discrete_keys, errors);
EXPECT(assert_equal(expected_error, error_tree));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

2
gtsam/inference/BayesNet.h
View File

@ -27,7 +27,7 @@ namespace gtsam {
/**
* A BayesNet is a tree of conditionals, stored in elimination order.
* @addtogroup inference
* @ingroup inference
*/
template <class CONDITIONAL>
class BayesNet : public FactorGraph<CONDITIONAL> {

2
gtsam/inference/BayesTree-inst.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file BayesTree-inl.h
* @file BayesTree-inst.h
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
* @author Frank Dellaert
* @author Michael Kaess

2
gtsam/inference/BayesTree.h
View File

@ -59,7 +59,7 @@ namespace gtsam {
* @tparam CLIQUE The type of the clique data structure, defaults to BayesTreeClique, normally do not change this
* as it is only used when developing special versions of BayesTree, e.g. for ISAM2.
*
* \addtogroup Multifrontal
* \ingroup Multifrontal
* \nosubgrouping
*/
template<class CLIQUE>

2
gtsam/inference/ClusterTree-inst.h
View File

@ -1,5 +1,5 @@
/**
* @file EliminatableClusterTree-inst.h
* @file ClusterTree-inst.h
* @date Oct 8, 2013
* @author Kai Ni
* @author Richard Roberts

5
gtsam/inference/ClusterTree.h
View File

@ -1,5 +1,5 @@
/**
* @file EliminatableClusterTree.h
* @file ClusterTree.h
* @date Oct 8, 2013
* @author Kai Ni
* @author Richard Roberts
@ -139,7 +139,6 @@ class ClusterTree {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}
/// @name Advanced Interface
/// @{
@ -169,6 +168,7 @@ class ClusterTree {
protected:
/// @name Details
/// @{
/// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors
/// are copied, factors are not cloned.
@ -236,6 +236,7 @@ class EliminatableClusterTree : public ClusterTree<GRAPH> {
protected:
/// @name Details
/// @{
/// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors
/// are copied, factors are not cloned.

2
gtsam/inference/DotWriter.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
/**
* @brief DotWriter is a helper class for writing graphviz .dot files.
* @addtogroup inference
* @ingroup inference
*/
struct GTSAM_EXPORT DotWriter {
double figureWidthInches; ///< The figure width on paper in inches

2
gtsam/inference/EliminationTree-inst.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file EliminationTree-inl.h
* @file EliminationTree-inst.h
* @author Frank Dellaert
* @author Richard Roberts
* @date Oct 13, 2010

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